ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Handling a delayed localization pose?

asked 2019-08-21 05:21:31 -0500

madmax gravatar image

Basically I am getting a global pose for localization which is 60ms delayed.
So just setting this pose to current pose will add an error of 6cm by not compensating for the driven distance ( e.g. 1m/s) in this 60ms.

So what I am doing is taking the old odom ( now -60ms) and the current odom, subtracting each other and add the resulting transform to the incoming localization pose.

And in code it looks like this:

 auto oldOdomTf = tf->lookupTransform( ODOM, currentTime, BASE_LINK, currentTime - 60ms,
                                       ODOM );
 auto yaw = tf_utils::getYawFromQuaternion( oldOdomTf.transform.rotation );
 RCLCPP_INFO( node.get_logger(), "get transform from old %s to %s: x: %.2f y: %.2f yaw: %.2f",
             BASE_LINK.c_str(), ODOM.c_str(), oldOdomTf.transform.translation.x,
              oldOdomTf.transform.translation.y, yaw );
 auto currentOdomTf = tf->lookupTransform( ODOM, currentTime, BASE_LINK, currentTime,
                                 ODOM );
 yaw = tf_utils::getYawFromQuaternion( currentOdomTf.transform.rotation );
 RCLCPP_INFO( node.get_logger(), "get transform from current %s to %s: x: %.2f y: %.2f yaw: %.2f",
              BASE_LINK.c_str(),ODOM.c_str(), currentOdomTf.transform.translation.x,
              currentOdomTf.transform.translation.y, yaw );

 tf2::Transform odomDelta, old, current;
 tf2::fromMsg( oldOdomTf.transform, old );
 tf2::fromMsg( currentOdomTf.transform, current );
 odomDelta = current * old.inverse(); 
 latest_tf_ = latest_tf_ * odomDelta; // get new map pose

Output is this:

[INFO 1566381525.298737439] [testNode]: transform from map to odom: x: 1.00 y: 1.61 yaw: -1.57
[INFO 1566381525.298759261] [testNode]: get transform from old base_link to odom: x: 0.60 y: 0.00 yaw: 0.00
[INFO 1566381525.298953778] [testNode]: get transform from current base_link to odom: x: 0.70 y: 0.00 yaw: 0.00
[INFO 1566381525.299004058] [testNode]: transform from old odom to new odom: x: 0.10 y: 0.00 yaw: 0.00
[INFO 1566381525.299014972] [testNode]: transform from map to odom with time compensation: x: 1.00 y: 1.51 yaw: -1.57

So, it looks good but my question is if this could be done simpler? Like with a direct tf call to transform spae and time? ;)

I was looking at the ros2 amcl pkg, where they write also something about integrating the odom data but in written code I only see a odom - base link transformation...

edit retag flag offensive close merge delete



Why not use EKF to fuse the results? robot_localization EKF can handle pose sources with old timestamp.

scottnothing gravatar image scottnothing  ( 2019-08-28 13:42:37 -0500 )edit

From what I know it's not ported to ROS2 yet.

madmax gravatar image madmax  ( 2019-08-29 02:01:44 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2019-08-21 20:07:56 -0500

tfoote gravatar image

For what you're asking it to do that looks about right. Unfortunately tf doesn't have full support for general relativity solutions.

But I would suggest that you rethink about what you're asking for. If you do truely need the global position with 6cm with less that 100ms latency this is what you need to do. But if you step back and think about your problem it's unlikely that you need super precise global position with very low latency. In general global fixes are moderately high latency and often are discontinuous.

As such the standard techinque is to decouple the low latency high speed positioning requirements and do all of those computations in the odometric frame. And then only deal with global positioning at a slower rate updating setpoint and goals in the odometric frame when new global information is received it updates the map -> odom transform with any drift observed. But between updates the only errors is the accumulation of drift in your odometry, which should be moderately small if you have a global fix streaming in, even if it's latent by 60ms and coming in at 10Hz.

For reference also:

edit flag offensive delete link more


Thanks for the kind and insigntful answer! you are right, we have a Camera system which reads datamatrix tags on the floor, so sometimes you see them and sometimes you dont. Problem in the first place was that we added the outdated pose to current odometry which delocalized us;) so the 60ms delay shouldnt be a problem if we just publish it on the map frame.

madmax gravatar image madmax  ( 2019-08-22 16:28:05 -0500 )edit

Question Tools



Asked: 2019-08-21 05:21:31 -0500

Seen: 442 times

Last updated: Aug 21 '19