Hi, all. I want to add a frame between "odom" and "base_footprint" in a simulator by imitating [navigation_stage](http://wiki.ros.org/navigation_stage). Then it is connected to [fakelocalization](http://wiki.ros.org/fake_localization). In this way, the tf tree for this simulator is constructed.
It is a python script which subscribes a "/robot_pose" topic to compute the transform between odom and base_footprint, then it publishes another "/base_pose_ground_truth" topic under frame "/odom".
The codes of the script are listed below.
#!/usr/bin/env python
import roslib
roslib.load_manifest('simulator')
import rospy, tf
from geometry_msgs.msg import Point, Quaternion, PoseStamped
from nav_msgs.msg import Odometry
import xml.dom.minidom
def newDataAvailable(robotPose):
robotPosition=robotPose.pose.pose.position.x - x, robotPose.pose.pose.position.y-y, 0.0
robotOrientation=(robotPose.pose.pose.orientation.x, robotPose.pose.pose.orientation.y, robotPose.pose.pose.orientation.z, robotPose.pose.pose.orientation.w)
robotVelocity=(robotPose.twist.twist.linear.x,robotPose.twist.twist.linear.y,robotPose.twist.twist.linear.z)
currentTime = rospy.Time.now()
br.sendTransform(robotPosition, robotOrientation, currentTime, "base_footprint", "odom")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = parentFrameId
odom.child_frame_id = frameId
odom.pose.pose.position = Point(*robotPosition)
odom.pose.pose.orientation =Quaternion(*robotOrientation)
odom.twist.twist.linear=Point(*robotVelocity)
odomPublisher.publish(odom)
def main():
rospy.init_node('simulator_robot_tf_broadcaster')
global frameId, parentFrameId, br, x, y, odomPublisher
frameId = rospy.get_param("frame", "base_footprint")
parentFrameId = rospy.get_param("parent_frame", "odom")
robotgroundtruthTopic = "/robot_pose"
rospy.Subscriber(robotgroundtruthTopic, Odometry, newDataAvailable)
odomPublisher=rospy.Publisher("/base_pose_ground_truth", Odometry, 1)
scene_file_param=rospy.get_param("/scene_file", "scene.xml")
dom = xml.dom.minidom.parse(scene_file_param)
root = dom.documentElement
itemlist = dom.getElementsByTagName('agent')
for i in itemlist:
pn = i.getAttribute("type")
if pn=="2": # robot:2
print "I got the robot's initial postion"
x = i.getAttribute("x")
#print x
y = i.getAttribute("y")
#print y
br = tf.TransformBroadcaster()
x=float(x)
y=float(y)
rospy.set_param("/fake_localization/delta_x",-x)
rospy.set_param("/fake_localization/delta_y",-y)
print x,y
rospy.spin()
if __name__ == '__main__':
main()
When I run it, all coordinates works as expected. However, there exists serious delay for computing tf. In rviz, the TF tree doesn't displayed in real-time. For example, the displayed coordinate "base_footprint" in rviz is "following" the robot ( I am quite sure that this is time delay, not the transform error in distance, because the displayed "base_footprint" axes can get the robot's position).
And an warning comes out all the time:
[WARN] [WallTime: 1430749249.205509] Inbound TCP/IP connection failed: 'int' object has no attribute 'peer_subscribe'
Another phenomenon is the message outputing from topic "/base_pose_ground_truth" is in a lower speed comparing to topic "/robot_pose"( why?)
Please tell me the reason why. Thank you!
↧