Ask Your Question
0

Add a tf frame in a simulator [closed]

asked 2015-05-04 22:54:13 -0600

scopus gravatar image

updated 2015-05-05 00:57:00 -0600

Hi, all. I want to add a frame between "odom" and "base_footprint" in a simulator by imitating navigation_stage. Then it is connected to fakelocalization. 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!

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by scopus
close date 2015-05-05 02:21:14.130375

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-05-05 02:20:56 -0600

scopus gravatar image
odomPublisher=rospy.Publisher("/base_pose_ground_truth", Odometry, 1)

should be

 odomPublisher=rospy.Publisher("/base_pose_ground_truth", Odometry)
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2015-05-04 22:54:13 -0600

Seen: 1,025 times

Last updated: May 05 '15