Tf publishing under multiple robot_localizations (world_frame = map_frame)

[pmukherj]

Quick question! In the guide it is suggested that we could use two instances of robot_localization to make sure things are aligned with REP-105. Specifically,

"If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then: Set your world_frame to your map_frame value MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of robot_localization! However, that instance should not fuse the global data."

However, the tf is always published between "world_frame" and "base_link". We can't really set the base_link to odom (repeat names are not allowed). I feel like I'm missing a solution here?.

[ayrton04]

Ah, so REP-105 states that the chain should be map->odom->base_link. One node should be responsible for odom->base_link and the other should technically be responsible for map->base_link. Since tf doesn't allow multiple parents to a given frame, though, the second node should listen for the odom->base_link transform and use that to generate a map->odom transform.

For you, both of your frame setups would be identical for both instances of ekf_localization_node, but one of them should have "world frame" set to "map," and the other should have it set to "odom." ekf_localization_node will handle all the tf stuff for you.

I should do a better job of explaining this on the wiki page.

[pmukherj]

Yes! I think that makes sense to me. However, in the second node where I set the world_frame to map, the base_link frame has to be set to odom? Otherwise ekf_localization_node will try to publish between map and base_link, which will do odd things because the other node is already publishing odom to base_link (base_link will have two parents!)

[ayrton04]

No, the base_link frame is still base link.

Instance 1: map_frame is map odom_frame is odom base_link_frame is base_link world_frame is odom

Instance 2: map_frame is map odom_frame is odom base_link_frame is base_link world_frame is map

In Instance 2, it will automatically detect that your world frame is map, and adjust its behavior accordingly. Instead of spitting out a map->base_link transform (which is what is computes first), it will listen for the odom->base_link transform from Instance 1, and use that (in conjunction with its internal, unpublished map->base_link transform), to output a map->odom transform. So Instance 1 will be producing odom->base_link, and instance 2 will produce map->odom. Check out line ~1196 of ros_filter.h (in the run function). That's where it handles this situation.

[pmukherj]

Okay! Yes, I see what you mean now. This all makes sense. I have it running like you prescribe. The data is coming out correctly, and the filtered pose output is consistent with the input into my the map_ekf (instance 2).

However my TF tree still seems incorrect! I know you calculate the required tf between map->odom for the tree to be consistent (line 1223 in ros_filter.h), however it seems it is the inverse that is required! I also appreciate how complex the tf tree setup and the lookupTransform function is, but this is the kind of situation I ran into.

Example situation I ran into output of filter in instance 1: x=0.5, y=z=0, rpy=0.
odom->base_link: x=0.5, y=z=0, rpy=0, output of filter at instance 1 was consistent with tf tree output of filter in instance 2 (filteredpose): x=1,y=z=0, rpy=0. map->base_link: x=0, y=z=0, rpy=0, output of filter at instance 2 was INconsistent with the tf tree.

If I probe the map->odom link, it is set to x=-0.5, y=z=0, rpy=0. It seems that the inverse of map->odom is what needs to be output here. I switched line 1223 to provide the inverse and everything started working fine.

Does this make sense? If so, I can provide a PR.

[ayrton04]

Hmmm, that does sound suspect. Let me look at it and draw some pictures. Any chance you'd have a bag and launch files? This is one test that takes a while to set up. Thanks for looking into this!

[ayrton04]

Actually, three quick questions:

(1) Are you saying you printed out the (unpublished) value of map->base_link, and it didn't match its own map pose estimate? (2) What exactly did you invert on line 1223? (3) How hard would it be to call tf::transformPose (with a target frame of odom) on the map pose? See if the value that comes out is actually the odom pose.

[pmukherj]

Sorry! I meant to answer this before sending the PR 1)I tf_echo'd the map->base_link tf and it didn't match the EKF output :(. 2) Inversion is shown in the PR! 3) I could try that! But I think tf_echo shows the same thing?

[ayrton04]

Gotcha. And with your change, map->base_link now correctly matches the EKF output?

[pmukherj]

Yes! I looked at the math, it felt like I needed the inverse of what was being published. But I'd love for you to check that over.

[ayrton04]

Fixed by @pmukherj in PR #101. Will roll out in the x.1.6.