Add a tf frame in a simulator [closed]

edit

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!