Provide a way to set the optical frame convention used for rendering sensors

[azeey]

Desired behavior

Currently, Gazebo uses the +x forward convention for all rendering sensors. This was done to keep the same behavior as Gazebo Classic. However, this is at odds with the convention in ROS, which is +z forward. Users run into this issue (e.g. #29) and the current workaround is to set a static transform in ROS to create a separate optical frame. There's an old PR in gazebosim/ros_gz#257 to add a dedicated publisher, but I think it would be best if Gazebo itself provided a way to set the convention used.

Implementation suggestion

We could do one of the following:

1. Add an SDFormat tag to all rendering sensors that sets whether the +z convention should be used.
2. Add an SDFormat under <sensor> that sets whether the +z convention should be used. This would be easier since we'd only modify one SDFormat description file, but it would affect all sensors
3. One of the two options above, but the tag would allow setting a custom transformation matrix.

[Sukhvansh2004]

Hi, can I work on this issue?

[azeey]

@Sukhvansh2004, thanks for volunteering. I'll assign the ticket to you. @iche033, @mjcarroll any thoughts on which option we should follow?

[iche033]

I wrote some notes down not long ago on what sensors need optical frames:

Regarding optical frames, I looked back at what we did in subt and mbzirc and this is what I think:

• we only need to use optical frame for regular RGB camera and depth camera images, e.g. /camera/image_raw and /camera/depth topics. As an example, this was done in mbzirc: https://github.com/osrf/mbzirc/blob/main/mbzirc_ign/src/mbzirc_ign/model.py#L235-L260. We only created optical frames for image_raw and depth topics.
• we don't need optical frames for point cloud topics, i.e. lidar point clouds and RGBD camera's point clouds, e.g. /lidar/points or /rgbd_camera/points topics

The optical_frame_publisher sets this transformation pose to create a z forward optical frame.

So if the above is true, instead of updating the generated sensor data itself, I think we just need a way to add an optical link with the above transformation inside gz-sim and set the msg frame id to that optical link name.

Here's a proposal that's similar to option 2.

<sensor>
  <!-- Use the existing <frame_id> element (sdf 1.12+) to specify frame as usual -->
  <frame_id>my_z_optical_frame</frame_id>
  <!-- Support a new //sensor/frame element.
       Gazebo sees this and creates a new frame (link?) with the specified pose -->
  <frame name="my_z_optical_frame">
    <pose>0 0 0 -1.5708 0 -1.5708</pose>
  </frame>
</sensor>

Open to other ideas.

[JorgePH]

I think the issue in #29 was that the frame_id in the RGBD pointcloud was populating with the optical_frame_id too (there is only one field to define the frame in RGBD sensor). As long as the pcd (following ROS convention) and cameras (following cameras convention) don't get published on different frames, their information is going to be conflicting. Thanks to ROS tf tools, as long as you have the tf tree well defined, it does not matter too much the frame where the data is published as long as it is consistent.

IMO at least another frame tag should be added for RGBD.

[Danilrivero]

I think the issue in #29 was that the frame_id in the RGBD pointcloud was populating with the optical_frame_id too (there is only one field to define the frame in RGBD sensor). As long as the pcd (following ROS convention) and cameras (following cameras convention) don't get published on different frames, their information is going to be conflicting. Thanks to ROS tf tools, as long as you have the tf tree well defined, it does not matter too much the frame where the data is published as long as it is consistent.

IMO at least another frame tag should be added for RGBD.

I agree on this. Your camera (e.g) should provide you with the required transformations following the proper conventions and use this information to properly transform to any frame required (which would be on your side).

In this case Gazebo would be expecting this format and another frame would be required to fully define your RGBD camera.

Open to discuss this and see if it could be a possibility.

[iche033]

Based on the comments above and in #29, the issue came from #458 (and #362 for DepthCameraSensor) since they incorrectly set the point cloud msg to optical frame. These PRs were supposedly submitted to fix "optical frame issues" for their use case. However, after gathering these recent comments I agree we should provide the ability to use a separate frame for the point cloud msg.

One example would be to use <frame_id> and <optical_frame_id> for the separate frames:

<!-- sdf 1.12 -->
<sensor name="my_sensor" type="rgbd">
  <!-- point clouds will use this frame -->
  <frame_id>sensor_frame</frame_id>
  <camera>
    <!-- images will use this frame -->
    <optical_frame_id>optical_frame<opical_frame_id>
    ...
  </camera>
...
</sensor>

To achieve the above, we'll unfortunately need to revert #458 and #362, and un-deprecate the <optical_frame_id> tag in sdf 1.12. Not ideal since we're changing behavior yet again so I want to be very sure this is the right fix. Open to suggestions.

[Sukhvansh2004]

I think the issue in #29 was that the frame_id in the RGBD pointcloud was populating with the optical_frame_id too (there is only one field to define the frame in RGBD sensor). As long as the pcd (following ROS convention) and cameras (following cameras convention) don't get published on different frames, their information is going to be conflicting. Thanks to ROS tf tools, as long as you have the tf tree well defined, it does not matter too much the frame where the data is published as long as it is consistent.
IMO at least another frame tag should be added for RGBD.

I agree on this. Your camera (e.g) should provide you with the required transformations following the proper conventions and use this information to properly transform to any frame required (which would be on your side).

In this case Gazebo would be expecting this format and another frame would be required to fully define your RGBD camera.

Open to discuss this and see if it could be a possibility.

Yeah I myself have worked on depth camera once and had to figure out that the external sensor frame tag needs to be as per the XYZ convention while the internal plugin frame needs to follow the ROS convention of ZYX

[Sukhvansh2004]

I have some doubts too regarding the implementations, currently I am a bit occuped with some work will try to get in contact and start working on this issue ASAP this weekend

[iche033]

Thanks for helping to work on this. Before going straight into implementation, I would like to get some feedback on the proposal first. I talked to @azeey about this and I think he has other thoughts on how we may want to address this from ros_gz.

[Danilrivero]

Based on the comments above and in #29, the issue came from #458 (and #362 for DepthCameraSensor) since they incorrectly set the point cloud msg to optical frame. These PRs were supposedly submitted to fix "optical frame issues" for their use case. However, after gathering these recent comments I agree we should provide the ability to use a separate frame for the point cloud msg.

To achieve the above, we'll unfortunately need to revert #458 and #362, and un-deprecate the <optical_frame_id> tag in sdf 1.12. Not ideal since we're changing behavior yet again so I want to be very sure this is the right fix. Open to suggestions.

I agree with you, even though the revert could cause some trouble it seems like the path we would go with, having the <optical_frame_id> tag implemented. Some sort of backwards compatibility could be discussed if required for any possible breaking changes.

Thanks for helping to work on this. Before going straight into implementation, I would like to get some feedback on the proposal first. I talked to @azeey about this and I think he has other thoughts on how we may want to address this from ros_gz.

Any information on this?