Fix frame_id in rgbd_camera

[fmrico]

🦟 Bug fix

Fixes [No number]

Summary

When using rgbd_camera as sensor, frame_id is filled with FrameId() (for example tiago/head_2_link/head_front_camera_frame_sensor), instead of OpticalFrameId() (for example, head_front_camera_rgb_optical_frame). So, the frame_id in the messages is wrong.

This PR fixes it.

I hope it helps. Any comment is welcome :)

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[iche033]

Thanks for the fix. I was able to reproduce the problem and confirms that this fixes it for Harmonic when the frame id is specified via <optical_frame_id>.

Just one minor cpplint issue

[ahcorde]

@iche033 do we need to backport this to other distros?

[sea-bass]

Doing a drive-by comment here since I saw the merge notification. I'm not entirely sure if this is a bug or a feature needed to realign images from Gazebo to show up correctly in RViz through TF lookups.

Before going down the backport path, I would validate that this does not break the example used in the original PR #259

[fmrico]

Hi,

Actually, this PR fixes to publish in a valid frame, but this is not the correct frame.

If I use FrameId() I get the wrong frame (tiago/head_2_link/head_front_camera_frame_sensor), so the options could be:

• To add a new method getCameraFrame() in RgbdCameraSensor to get the last part of what FrameId() returns.
• A more general method, maybe in the base class, getSimpleFrame() or similar, to get the same.

Best

[iche033]

I created a backport PR in #461 that includes the fix from this PR.

#259 fixes the issue for the camera_info topic, while this PR fixes it for the frame_id field in the msg header in other topics. So it should not break the example used in #259