tf.transformPose() throws because of timing issues

[lsouchet]

Hello, I am trying to make a listener of tf that sends commands to my robot.

I plugged it to /move_base_simple/goal to use the rviz built in panel.

But when I try to send a command, the python node throws with this error:

[ERROR] [WallTime: 1425288368.508838] bad callback: <bound method MoveToListener.callback of <MoveToListener(Thread-1, initial)>> Traceback (most recent call last): File "/home/lsouchet/catkin_ws/src/ros_comm/clients/rospy/src/rospy/topics.py", line 697, in _invoke_callback cb(msg) File "/home/lsouchet/catkin_ws/src/naoqi_bridge/naoqi_driver/src/naoqi_driver/naoqi_moveto.py", line 48, in callback robotToTarget1 = self.listener.transformPose("/base_footprint", poseStamped) File "/opt/ros/indigo/lib/python2.7/dist-packages/tf/listener.py", line 188, in transformPose mat44 = self.asMatrix(target_frame, ps.header) File "/opt/ros/indigo/lib/python2.7/dist-packages/tf/listener.py", line 75, in asMatrix translation,rotation = self.lookupTransform(target_frame, hdr.frame_id, hdr.stamp) ExtrapolationException: Lookup would require extrapolation into the future. Requested time 1425288368.508244038 but the latest data is at time 1425288368.469716072, when looking up transform from frame [odom] to frame [base_footprint]

Here is the output of the message in question:

z$ rostopic echo /move_base_simple/goal [15-03-02 10:26] header: seq: 4 stamp: secs: 1425288368 nsecs: 508243953 frame_id: odom pose: position: x: -0.0192804336548 y: 0.425947189331 z: 0.0 orientation: x: 0.0 y: 0.0 z: -0.0167020614301 w: 0.999860510843

And here is the code of my callback (note that I commented the try/except shit to let it throw):

def callback(self, poseStamped):

Adding this line to reset the message timestamp fixes the issue.

    #poseStamped.header.stamp = rospy.Time(0)
    #try:
    robotToTarget1 = self.listener.transformPose("/base_footprint", poseStamped)
    #except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
    #    print "Error in transformPose: " + str(e)
    #    return
    quat = robotToTarget1.pose.orientation
    (roll,pitch,yaw) = euler_from_quaternion((quat.x, quat.y, quat.z, quat.w))
    self.motionProxy.moveTo(robotToTarget1.pose.position.x, robotToTarget1.pose.position.y, yaw)

There appears to be an issue about the timing between the message is published and the transform is processed but I don't understand why. Is is a real bug or something I did wrong?

Thanks in advance

[hdeeken]

As far as I can see the 2D Nav Goal Tool is perfectly fine.

I would recommend to use a waitForTransform statement before calling lookupTransform. It takes a (really) small amount of time until tf has distributed the incoming updates. For details consult the wiki.

Hope this helps, if there's still a problem follow up questions (concerning your code) are probably better placed at ROS answers. ;)

[wjwwood]

I think this is intended behavior, as @hdeeken suggested you should use waitForTransform before calling lookupTransform if you don't want it to potentially raise that exception.

I'll let @tfoote make the call on this issue though.

[tfoote]

@lsouchet This does look like expected behavior. I suggest that you read through the tf tutorials to understand what is happening. Especially the 4th about tf and Time to understand this error.

If this is a low throughput callback a waitForTransform is reasonable. If it's a high throughput callback you should use a tf::MessageFilter