Add ROS parameters for configuring GZ to ROS optical frame conversion

[iche033]

Marked as draft to get feedback

🎉 New feature

Summary

Adds a number of ROS parameters for configuring GZ to ROS frame conversion.

The main feature is the publish_optical_frame ROS parameter, which applies to the GZ to ROS direction. When set to true, the bridge automatically converts the messages to z-forward optical frame. The outgoing messages will have a new frame_id with "_optical" suffix string, and a TF with x-forward to z-forward transform is published using a static transform broadcaster.

Example usage:

ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image --ros-args -p publish_optical_frame:=true -r /rgbd_camera/image:=/rgbd_camera/optical/image

The bridge sets the publish_optical_frame ROS param to true and also remaps the original topic to a new name with an optical sub-namespace.

If the user wants to set a different frame id or transform, they can override them using a couple of other ROS parameters:

• override_frame_id_string (string) - Overrides frame_id of the outgoing messages
• override_frame_transform (double array) - Custom transform [x, y, z, roll, pitch, yaw]

See the API section of README.md for more info.

Note that this feature adds 2 new ROS package dependencies:

• tf2
• tf2_ros

Test it

See the GZ to ROS Optical frame conversion section of README.md for more instructions.

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• 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.

[ahcorde]

Code is not compiling:

    tf2::tf2

  but the target was not found.  Possible reasons include:

    * There is a typo in the target name.
    * A find_package call is missing for an IMPORTED target.
    * An ALIAS target is missing.

[iche033]

Code is not compiling:

    tf2::tf2

  but the target was not found.  Possible reasons include:

    * There is a typo in the target name.
    * A find_package call is missing for an IMPORTED target.
    * An ALIAS target is missing.

fixed.

[ahcorde]

@iche033 is this ready for review?

[iche033]

@iche033 is this ready for review?

Not yet, I'm planning to add more tests. But I am looking for feedback on the proposed ROS parameters for converting to optical frame, mainly If the general direction is ok.

[peci1]

I think this is going in good direction!

1. Why is the static TF being re-broadcasted every second? Is there a reason for that (it cannot ever change as it is defined now)?
2. The name of the parameter publish_optical_frame is a bit misleading for me (because it not only publishes the frame, but also overwrites it in messages).
3. In the example in readme, isn't the frame (needlessly) published twice - once by the image bridge and once by the camera_info bridge? If the intended way of use includes just setting override_frame_id_string, shouldn't one of the bridges use just this instead of publish_optical_frame?
4. One thing to note is that this solution pushes the SDF further away from being a complete source of information for a simulation with ROS. With the PR, the SDF would always need the bridges along with it to be usable in ROS. And the configuration of the bridges could not be automatically determined (except blindly enabling publish_optical_frame for all image topics). The alternative solution with an extra SDF tag would be more explicit about this. On the other hand, the usage of optical frames is something ROS-specific, so if Gazebo wants to be middleware-agnostic, it is probably okay.

[iche033]

I think this is going in good direction!

Thanks for the feedback!

1. Why is the static TF being re-broadcasted every second? Is there a reason for that (it cannot ever change as it is defined now)?

Currently the static tf broadcaster sends a transform and goes out of scope so I think this caused a problem I ran into, which is that the static TF is not latched, and new subscriber couldn't get the TF. I'll look into getting rid of throttling.

2. The name of the parameter publish_optical_frame is a bit misleading for me (because it not only publishes the frame, but also overwrites it in messages).

Maybe override_and_publish_optical_frame?

3. In the example in readme, isn't the frame (needlessly) published twice - once by the image bridge and once by the camera_info bridge? If the intended way of use includes just setting override_frame_id_string, shouldn't one of the bridges use just this instead of publish_optical_frame?

We can remove the need to publish the frame twice if

1. the Image bridge enables publish_optical_frame, and sets override_frame_id_string to a fixed string value, e.g. "my_optical_frame"
2. the CameraInfo bridge will then only need to set override_frame_id_string:="my_optical_frame"

The problem is that if the user does not set the extra optional override_frame_id_string param in the first Image bridge, the optical frame id will be an auto-generated string (done by adding a _optical suffix to the original frame_id). This generated string will be difficult to guess by the user. One idea around this is to have a parameter, e.g. override_frame_id_append_suffix_string. So something like this will work:

1. the Image bridge enables publish_optical_frame only
2. the CameraInfo bridge set override_frame_id_append_suffix_string:="_optical"

4. One thing to note is that this solution pushes the SDF further away from being a complete source of information for a simulation with ROS. With the PR, the SDF would always need the bridges along with it to be usable in ROS. And the configuration of the bridges could not be automatically determined (except blindly enabling publish_optical_frame for all image topics). The alternative solution with an extra SDF tag would be more explicit about this. On the other hand, the usage of optical frames is something ROS-specific, so if Gazebo wants to be middleware-agnostic, it is probably okay.

yeah this is a ROS-specific solution to the problem. I think having a way to deal with this in SDF would be nice but it was decided to scope it down and go with this for now.

[peci1]

This all seems super complicated. I can imagine lots of frustrated users. Even if they just copy code from tutorials (or ChatGPT does it for them), they forget one thing and the whole camera machinery doesn't work in ROS.

I have an alternative proposition: what if this package provides a convenience launch file that would just get the two Gz and two ROS topic names as arguments and an optional argument with the new frame ID (defaulting to <topic>_optical_frame), and this launch file would launch:

1. Image bridge (with override_frame_id_string)
2. Camera info bridge (with override_frame_id_string)
3. Static TF broadcaster (this would probably have to be a custom node so that it autodetects the GZ frame ID). For efficiency reasons, this should subscribe to the camera info topic and not to the image topic. This node could even subscribe just for one message and the shut down the subscription.
4. Remaps of the GZ topic names to the ROS topic names.

What do you think?

[iche033]

I think this does not need to be that complicated for most users. To migrate their existing bridge to use the new optical frame conversion feature, I expect most users to only set the publish_optical_frame:=true for both Image and CameraInfo topics (and ignore any override_frame_id_string params). This will 'magically' work for them. The caveat here is that the _optical frame is needlessly published twice like you noted. Not ideal but I think that should be ok?

We can also provide a convenience launch file like the one you proposed. It could be implemented on top of this PR and uses the ROS parameters introduced here.

How does this sound?

[Danilrivero]

I think this does not need to be that complicated for most users. To migrate their existing bridge to use the new optical frame conversion feature, I expect most users to only set the publish_optical_frame:=true for both Image and CameraInfo topics (and ignore any override_frame_id_string params). This will 'magically' work for them. The caveat here is that the _optical frame is needlessly published twice like you noted. Not ideal but I think that should be ok?

Agree on this, most users will care about setting up publish_optical_frame, the proposals to override are required though, for flexibility and avoid future fixes directly related to that.