tf lookup timing issues in AmclNode::handleInitialPoseMessage // ("Failed to transform initial pose in time") #424

stwirth · 2016-02-22T09:59:10Z

In AmclNode::handleInitialPoseMessage (see here), the tf lookup uses ros::Time::now() to get the robot motion since the initial pose's time stamp.
Within this function, tf2::BufferCore::lookupTransform(base_frame_id_, global_frame_id_, ros::Time::now()) is called. That last call fails often if the odometry update rate is low as it needs to have updated the tf from global_frame_id_ to base_frame_id_ since the call to ros::Time::now() to succeed.

As a solution I would propose to add a waitForTransform() before the lookup call.

The text was updated successfully, but these errors were encountered:

stwirth · 2016-02-22T11:01:37Z

Additionally, I think the lookup should use odom_frame_id_ as "fixed frame" argument to get the relative motion.

stwirth · 2016-02-22T20:29:15Z

After working on this a little I think this multiplication is the wrong way around. tx_odom is the delta the robot moved between the stamp of the incoming intial pose message and ros::Time::now(). It should be multiplied with pose_old from the right.

message. Modifies the set_pose.py script to be able to send an initial pose with a user defined time stamp at a user defined time. Adds a rostest to exercise this new option. This reveals the issues mentioned in ros-planning#424 (the new test fails).

…ng#424). The tf lookup as it was before this change was very likely to fail as ros::Time::now() was used to look up a tf without waiting on the tf's availability. Additionally, the computation of the "new pose" by multiplying the delta that the robot moved from the initialpose's timestamp to ros::Time::now() was wrong. That delta has to by multiplied from the right to the "old pose". This commit also changes the reference frame to look up this delta to be the odom frame as this one is supposed to be smooth and therefore the best reference to get relative robot motion in the robot (base link) frame.

…age. Modifies the set_pose.py script to be able to send an initial pose with a user defined time stamp at a user defined time. Adds a rostest to exercise this new option. This reveals the issues mentioned in ros-planning#424 (the new test fails).

…ng#424). The tf lookup as it was before this change was very likely to fail as ros::Time::now() was used to look up a tf without waiting on the tf's availability. Additionally, the computation of the "new pose" by multiplying the delta that the robot moved from the initialpose's timestamp to ros::Time::now() was wrong. That delta has to by multiplied from the right to the "old pose". This commit also changes the reference frame to look up this delta to be the odom frame as this one is supposed to be smooth and therefore the best reference to get relative robot motion in the robot (base link) frame.

…age. Modifies the set_pose.py script to be able to send an initial pose with a user defined time stamp at a user defined time. Adds a rostest to exercise this new option. This reveals the issues mentioned in ros-planning#424 (the new test fails).

…ng#424). The tf lookup as it was before this change was very likely to fail as ros::Time::now() was used to look up a tf without waiting on the tf's availability. Additionally, the computation of the "new pose" by multiplying the delta that the robot moved from the initialpose's timestamp to ros::Time::now() was wrong. That delta has to by multiplied from the right to the "old pose". This commit also changes the reference frame to look up this delta to be the odom frame as this one is supposed to be smooth and therefore the best reference to get relative robot motion in the robot (base link) frame.

Fixes #424 for indigo: New test and fix for proper interpretation of a delayed initial pose.

…age. Modifies the set_pose.py script to be able to send an initial pose with a user defined time stamp at a user defined time. Adds a rostest to exercise this new option. This reveals the issues mentioned in ros-planning#424 (the new test fails).

…ng#424). The tf lookup as it was before this change was very likely to fail as ros::Time::now() was used to look up a tf without waiting on the tf's availability. Additionally, the computation of the "new pose" by multiplying the delta that the robot moved from the initialpose's timestamp to ros::Time::now() was wrong. That delta has to by multiplied from the right to the "old pose". This commit also changes the reference frame to look up this delta to be the odom frame as this one is supposed to be smooth and therefore the best reference to get relative robot motion in the robot (base link) frame.

mikeferguson added a commit that referenced this issue Feb 26, 2016

Merge pull request #426 from stwirth/fix-424-indigo

f63cf76

Fixes #424 for indigo: New test and fix for proper interpretation of a delayed initial pose.

mikeferguson closed this as completed in 610a42f Feb 26, 2016

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

tf lookup timing issues in AmclNode::handleInitialPoseMessage // ("Failed to transform initial pose in time") #424

tf lookup timing issues in AmclNode::handleInitialPoseMessage // ("Failed to transform initial pose in time") #424

stwirth commented Feb 22, 2016

stwirth commented Feb 22, 2016

stwirth commented Feb 22, 2016

tf lookup timing issues in AmclNode::handleInitialPoseMessage // ("Failed to transform initial pose in time") #424

tf lookup timing issues in AmclNode::handleInitialPoseMessage // ("Failed to transform initial pose in time") #424

Comments

stwirth commented Feb 22, 2016

stwirth commented Feb 22, 2016

stwirth commented Feb 22, 2016