Fix odometry msgs with child frame other than baseLink

[frygge]

This PR addresses #726

It fixes the bug that ignores the child frame of odometry messages. This is relevant for processing odometry messages in differential mode (since there, it computes the differential in the child frame, hence it needs to know its name) and odometry poses in relative mode.

[SteveMacenski]

LGTM

[ayrton04]

Moving the conversation from #726 over here.

First, thank you for the PR and the conversation in the original ticket.

I get what you're attempting to do here (and it's clever), but I can't help feeling that we're abusing the frame IDs in messages a bit. It also changes the filter behaviour: if you enable differential mode on a nav_msgs/Odometry input but your child_frame_id is filled out, it will always use that. I'm trying to determine if that will be a problem.

Moreover, anyone who has their transform tree set up such that the frame_id of the nav_msgs/Odometry input is set to the sensor's frame could see a change in behaviour.

Having said that, I like having this as a solution to pose data being measured from a sensor that is onboard the vehicle and reported in a frame that does not align with the robot's world frame.

I wonder if it makes sense to gate the behaviour behind a parameter that is only relevant for nav_msgs/Odometry inputs? Something like differential_use_child_frame or something?

Just for my own curiosity, what happens if you don't use your code change, but change the frame_id of your message to be alb_sensor_frame? Technically, the data from your sensor is not reported in the odom frame, but if we're not using it directly, it doesn't matter.

[SteveMacenski]

I think adding a parameter would be a reasonable move given what you describe above on differential mode.

[frygge]

Sorry for the delay. I agree with you both, There might be use cases that break with this patch; thus, a gating parameter is a good approach.

Since the PR solves a similar problem with relative enabled, I'd rather propose a parameter name such as pose_from_child_frame, pose_use_child_frame, diff_rel_use_child_frame, or something alike. What do you think?

@ayrton04 I changed the header.frame_id to alb_sensor_frame in the bag file with a short python script. The differential use case did not show any changes (it's still going sideways). The relative use case now does not work at all since it can't find the transformation between alb_sensor_frame and odom.

[ayrton04]

Never apologise to me for taking a long time to respond! I am notorious for it here.

I am really happy about this PR, as it enables the package to support a use case that it didn't support previously. I like diff_rel_use_child_frame the most. Naming parameters with very specific purposes is hard.

In any case, thank you for the PR!

[ayrton04]

Zero rush, just checking in on this. Will you be willing/able to add the parameter in question?

[frygge]

Yes, sure. I'm happy to finish it as we agreed. I just have an urgent paper deadline ahead for next week monday :)

[frygge]

Aiming at writing easy-to-read code for better maintainability, I could set the child_frame parameter in the pose-callback functions, i. e.

• on geometry_msgs::PoseWithCovarianceStamped, the callback passes the child frame baseLinkFrameId_ (as it was before)
• on sensor_msgs::Imu, the callback passes the child frame baseLinkFrameId_ (as it was before; only relevant if differential is enabled since there is a imuData flag also passed through, which changes the logic)
• on nav_msgs::Odometry, the callback either passes the child frame baseLinkFrameId_ or header.child_frame_id depending on a parameter.

The last point yields that the newly introduced parameter also changes the child frame without enabling differential or relative. It seems to me that this is a) the easier implementation and b) a more consistent implementation. In that case, I would suggest a parameter odomN_pose_uses_child_frame, which also is easier to read while precisely describing what it does.

Implementing the new behavior only for differential and relative odometry messages includes a lot of if-else conditions, which makes the code much harder to read and maintain.

What do you think about this?

[frygge]

I implemented the proposal and compared the behavior with the parameter enabled and disabled.

• with odom0_diff_rel_use_child_frame=False, everything looks as without the patch
• with odom0_diff_rel_use_child_frame=True, the new feature looks just fine

I found a bug in my previous implementation: it always interpreted the pose from child_frame_id to header.frame when differential and relative was disabled, which could break certain use cases. With the way of gating the behavior as described in the previous post, this bug is fixed.

Now, the parameter odomN_pose_uses_child_frame (or something alike) makes even more sense than diff_rel_use_child_frame. I'd change that if you'd agree.

[frygge]

FYI: I posted a related question on stackoverflow regarding the interpretation of the child_frame. No one seems to know the answer. So I took the freedom to link this pull request there since it basically creates the answer "You're free to choose." :)

[ayrton04]

Thanks for the PR! Just a couple things I want to iron out.

[frygge]

The debian based pipeline failed with the following error message:

08:27:52 time="2022-04-12T06:30:47Z" level=error msg="error waiting for container: unexpected EOF"

This seems to be no bug related to this PR. Can you look into it or maybe just restart this particular pipeline?

[ayrton04]

We don't have the ability to restart jobs on the ROS build farm. It would likely just get kicked off again in a bit, but we can force its hand by just closing and re-opening your PR.

[ayrton04]

OK, merging this. I don't suppose you have a ROS 2 workspace? It would be good to get this change in there. I can also manually pull the change over.

[ayrton04]

Thank you for the PR!

[frygge]

Unfortunately, I don't yet.

Thanks a lot to you as well. We're happy to use the software in our cars :)

[HaoguangYang]

OK, merging this. I don't suppose you have a ROS 2 workspace? It would be good to get this change in there. I can also manually pull the change over.

I would be happy to port this over to ros2 branches as well.

[roncapat]

Any news on a ros2 port?

[HaoguangYang]

Any news on a ros2 port?

Actually, I do have a compiling version but I got side-tracked by other projects recently. I will test my modifications when I have time and keep you posted.