Add ROS parameters for configuring GZ to ROS optical frame conversion

ros_gz_bridge/CMakeLists.txt

@@ -22,6 +22,9 @@ find_package(gz-transport REQUIRED)
 find_package(gz_msgs_vendor REQUIRED)
 find_package(gz-msgs REQUIRED)
 
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
 # Install the python module for this package
 ament_python_install_package(${PROJECT_NAME})
 
@@ -117,6 +120,8 @@ target_link_libraries(${bridge_lib}
     ${trajectory_msgs_TARGETS}
     ${vision_msgs_TARGETS}
   PRIVATE
+    tf2::tf2
+    tf2_ros::tf2_ros
     yaml-cpp::yaml-cpp
 )
 

ros_gz_bridge/README.md

@@ -190,6 +190,55 @@ The screenshot shows all the shell windows and their expected content
 
 ![Gazebo Transport images and ROS rqt](images/bridge_image_exchange.png)
 
+### GZ to ROS Optical frame conversion
+
+For sensors like cameras, it is commonly expected that ROS image data are in a
+z-forward optical frame, see [REP-0103](https://www.ros.org/reps/rep-0103.html).
+Historically, when bridging GZ to ROS `Image` and `CameraInfo` topics, users
+would typically create a new optical frame with a x to z-forward transformation,
+e.g. by using a static transform publisher. The sensor's frame id in SDF would
+then be set to point to the new optical frame.
+
+The bridge now has a parameter named `publish_optical_frame` to automate
+this process. In the above example for bridging image topics, you can run the
+bridge and pass these extra ROS arguments:
+
+```bash
+. ~/bridge_ws/install/setup.bash
+ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/[email protected] --ros-args -p publish_optical_frame:=true -r /rgbd_camera/image:=/rgbd_camera/optical/image
+ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/camera_info@sensor_msgs/msg/[email protected] --ros-args -p publish_optical_frame:=true -r /rgbd_camera/camera_info:=/rgbd_camera/optical/camera_info
+```
+
+Each command above sets the `publish_optical_frame` parameter to `true`, and
+it also remaps the original topic to a new name with an `optical` sub-namespace:
+
+```bash
+# Original topics:
+# /rgbd_camera/image
+# /rgbd_camera/camera_info
+# New remapped optical frame topics
+/rgbd_camera/optical/image
+/rgbd_camera/optical/camera_info
+```
+
+The messages from these topics will include a header with its `frame_id` set
+to a new optical frame that contains a `_optical` suffix string. This new
+optical frame is published by the bridge using a static transform broadcaster.
+
+You can introspect the TF tree using `tf2_tools` and viewing the generated pdf:
+
+```bash
+ros2 run tf2_tools view_frames
+evince frames_<timestamp>.pdf
+```
+
+The TF tree should look like:
+
+```bash
+# New TF tree. A new frame with `_optical` suffix is added
+/rgbd_camera/link/rgbd_camera -> /rgbd_camera/link/rgbd_camera_optical
+```
+
 ## Example 3: Static bridge
 
 In this example, we're going to run an executable that starts a bidirectional
@@ -333,6 +382,45 @@ By changing `chatter` to `/chatter` or `~/chatter` you can obtain different resu
 
 ROS 2 Parameters:
 
- * `subscription_heartbeat` - Period at which the node checks for new subscribers for lazy bridges.
- * `config_file` - YAML file to be loaded as the bridge configuration
- * `expand_gz_topic_names` - Enable or disable ROS namespace applied on GZ topics.
+* `subscription_heartbeat`
+    * type: double
+    * default: 1000
+    * description: Period (ms) at which the node checks for new subscribers for
+      lazy bridges.
+* `config_file`
+    * type: string
+    * default: ""
+    * description: YAML file to be loaded as the bridge configuration
+* `expand_gz_topic_names`
+    * type: bool
+    * default: false
+    * description: Enable or disable ROS namespace applied on GZ topics.
+* `override_timestamps_with_wall_time`
+    * type: bool
+    * default: false
+    * direction: GZ to ROS
+    * description: Override the header.stamp field of outgoing messages with
+      wall time.
+* `publish_optical_frame`
+    * type: bool
+    * default: false
+    * direction: GZ to ROS
+    * description: Apply x-forward to z-forward transformation to outgoing
+      messages. A new frame with a `_optical` suffix will be broadcasted.
+ * `override_frame_id_string`
+    * type: string
+    * default: ""
+    * direction: GZ to ROS
+    * description: Override the `header.frame_id` field with a new string value.
+ * `override_frame_transform`
+    * type: double array
+    * default: {}
+    * direction: GZ to ROS
+    * description: Publish messages under a new frame which has the specified
+      transformation from the original frame. This is done by broadcasting a
+      tf with the `frame_id` set to the frame id in the original message, and
+      the `child_frame_id` set to the new frame id in the updated outgoing
+      message. The new frame id of the outgoing messages must be set via the
+      `override_frame_id_string` parameter. The array of doubles are:
+      `[x, y, z, roll, pitch, yaw]`, where the translational components are
+      in meters and the rotational components are in radians.

ros_gz_bridge/include/ros_gz_bridge/bridge_config.hpp

@@ -68,6 +68,9 @@ struct BridgeConfig
 
   /// \brief Flag to change the "laziness" of the bridge
   bool is_lazy = kDefaultLazy;
+
+  /// \brief True to publish message from GZ to ROS in z-forward optical frame.
+  bool publish_optical_frame{false};
 };
 
 /// \brief Generate a group of BridgeConfigs from a YAML String

ros_gz_bridge/package.xml

@@ -30,7 +30,9 @@
   <depend>rosgraph_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>tf2</depend>
   <depend>tf2_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>trajectory_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
   <depend>vision_msgs</depend>

ros_gz_bridge/src/bridge_config.cpp

@@ -35,6 +35,7 @@ constexpr const char kDirection[] = "direction";
 constexpr const char kPublisherQueue[] = "publisher_queue";
 constexpr const char kSubscriberQueue[] = "subscriber_queue";
 constexpr const char kLazy[] = "lazy";
+constexpr const char kPublishOpticalFrame[] = "publish_optical_frame";
 
 // Comparison strings for bridge directions
 constexpr const char kBidirectional[] = "BIDIRECTIONAL";
@@ -142,6 +143,20 @@ std::optional<BridgeConfig> parseEntry(const YAML::Node & yaml_node)
     ret.is_lazy = yaml_node[kLazy].as<bool>();
   }
 
+  if (yaml_node[kPublishOpticalFrame]) {
+    bool publish_optical_frame = yaml_node[kPublishOpticalFrame].as<bool>();
+    if (publish_optical_frame) {
+      if (direction == kGzToRos) {
+        ret.publish_optical_frame = publish_optical_frame;
+      } else {
+        RCLCPP_ERROR(
+          logger,
+          "The %s parameter is only applicable to the [%s] direction",
+          kPublishOpticalFrame, direction.c_str());
+      }
+    }
+  }
+
   return ret;
 }
 

ros_gz_bridge/src/bridge_handle.cpp

@@ -30,8 +30,6 @@ BridgeHandle::BridgeHandle(
   config_(config),
   factory_(get_factory(config.ros_type_name, config.gz_type_name))
 {
-  ros_node_->get_parameter("override_timestamps_with_wall_time",
-    override_timestamps_with_wall_time_);
 }
 
 BridgeHandle::~BridgeHandle() = default;

ros_gz_bridge/src/bridge_handle.hpp

@@ -101,9 +101,6 @@ class BridgeHandle
 
   /// \brief Typed factory used to create publishers/subscribers
   std::shared_ptr<FactoryInterface> factory_;
-
-  /// \brief Override the header.stamp field of the outgoing messages with the wall time
-  bool override_timestamps_with_wall_time_ = false;
 };
 
 }  // namespace ros_gz_bridge

ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp

@@ -12,13 +12,84 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 //
+
+#include <tf2/LinearMath/Quaternion.h>
+
 #include <memory>
+#include <string>
+#include <vector>
 
 #include "bridge_handle_gz_to_ros.hpp"
 
 namespace ros_gz_bridge
 {
 
+BridgeHandleGzToRos::BridgeHandleGzToRos(
+  rclcpp::Node::SharedPtr ros_node,
+  std::shared_ptr<gz::transport::Node> gz_node,
+  const BridgeConfig & config)
+: BridgeHandle(ros_node, gz_node, config)
+{
+  ros_node_->get_parameter("override_timestamps_with_wall_time",
+    gz_to_ros_parameters_.override_timestamps_with_wall_time);
+
+  ros_node_->get_parameter("override_frame_id_string",
+      gz_to_ros_parameters_.override_frame_id_string);
+
+  std::vector<double> frame_tf;
+  ros_node_->get_parameter("override_frame_transform", frame_tf);
+  if (!frame_tf.empty() && frame_tf.size() != 6) {
+    RCLCPP_ERROR(
+      ros_node_->get_logger(),
+      "The 'override_frame_transform' parameter must be an array of 6 "
+      "floating point values: [x, y, z, roll, pitch, yaw].");
+    frame_tf.clear();
+  }
+
+  // Publish_optical_optical_frame is a convenient ROS parameter that will
+  // populates the override_frame_transform and override_frame_id_string
+  // params with default values for converting x-forward to z-forward optical
+  // frame. Note that they can still be overridden by the user if they decide
+  // to set these params individually.
+  bool publish_optical_frame = false;
+  ros_node_->get_parameter("publish_optical_frame",
+      publish_optical_frame);
+  publish_optical_frame |= config.publish_optical_frame;
+  std::vector<double> optical_frame_tf{0, 0, 0, -M_PI / 2.0, 0, -M_PI / 2.0};
+  if (publish_optical_frame) {
+    gz_to_ros_parameters_.override_frame_id_suffix_string = "optical";
+    if (frame_tf.empty()) {
+      frame_tf = optical_frame_tf;
+    }
+  }
+
+  if (!frame_tf.empty()) {
+    geometry_msgs::msg::Transform transform;
+    transform.translation.x = frame_tf[0];
+    transform.translation.y = frame_tf[1];
+    transform.translation.z = frame_tf[2];
+    tf2::Quaternion q;
+    q.setRPY(frame_tf[3], frame_tf[4], frame_tf[5]);
+    transform.rotation.x = q.x();
+    transform.rotation.y = q.y();
+    transform.rotation.z = q.z();
+    transform.rotation.w = q.w();
+    gz_to_ros_parameters_.override_frame_transform = transform;
+  }
+
+  if (gz_to_ros_parameters_.override_frame_transform.has_value() &&
+    gz_to_ros_parameters_.override_frame_id_string.empty() &&
+    gz_to_ros_parameters_.override_frame_id_suffix_string.empty())
+  {
+    RCLCPP_ERROR(
+      ros_node_->get_logger(),
+      "The 'override_frame_id_string' parameter cannot be empty "
+      "when 'override_frame_transform' is set. Disabling "
+      "'override_frame_transform'.");
+    gz_to_ros_parameters_.override_frame_transform.reset();
+  }
+}
+
 BridgeHandleGzToRos::~BridgeHandleGzToRos() = default;
 
 size_t BridgeHandleGzToRos::NumSubscriptions() const
@@ -71,7 +142,8 @@ void BridgeHandleGzToRos::StartSubscriber()
     this->config_.gz_topic_name,
     this->config_.subscriber_queue_size,
     this->ros_publisher_,
-    override_timestamps_with_wall_time_);
+    this->ros_node_,
+    gz_to_ros_parameters_);
 
   this->gz_subscriber_ = this->gz_node_;
 }

ros_gz_bridge/src/bridge_handle_gz_to_ros.hpp

@@ -21,6 +21,7 @@
 #include <rclcpp/subscription_base.hpp>
 
 #include "bridge_handle.hpp"
+#include "bridge_handle_gz_to_ros_parameters.hpp"
 
 namespace ros_gz_bridge
 {
@@ -32,7 +33,10 @@ class BridgeHandleGzToRos : public BridgeHandle
 {
 public:
   /// \brief Constructor
-  using BridgeHandle::BridgeHandle;
+  BridgeHandleGzToRos(
+    rclcpp::Node::SharedPtr ros_node,
+    std::shared_ptr<gz::transport::Node> gz_node,
+    const BridgeConfig & config);
 
   /// \brief Destructor
   ~BridgeHandleGzToRos() override;
@@ -62,6 +66,9 @@ class BridgeHandleGzToRos : public BridgeHandle
 
   /// \brief ROS publisher, populated when publisher active
   rclcpp::PublisherBase::SharedPtr ros_publisher_ = {nullptr};
+
+  /// \brief GZ to ROS parameters
+  BridgeHandleGzToRosParameters gz_to_ros_parameters_;
 };
 
 }  // namespace ros_gz_bridge

ros_gz_bridge/src/bridge_handle_gz_to_ros_parameters.hpp

@@ -0,0 +1,51 @@
+// Copyright 2025 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BRIDGE_HANDLE_GZ_TO_ROS_PARAMETERS_HPP_
+#define BRIDGE_HANDLE_GZ_TO_ROS_PARAMETERS_HPP_
+
+#include <optional>
+#include <string>
+
+#include <geometry_msgs/msg/transform.hpp>
+
+namespace ros_gz_bridge
+{
+
+struct BridgeHandleGzToRosParameters
+{
+  /// \brief Override the header.stamp field of the outgoing messages with
+  /// the wall time
+  bool override_timestamps_with_wall_time = false;
+
+  /// Publish messages under a new frame which has the specified transformation
+  /// from the original frame. This is done by broadcasting a tf with the
+  /// frame_id set to the frame_id in the original message, and the
+  /// child_frame_id set to the new frame id in the updated outgoing message.
+  /// The new frame id of the outgoing messages must be set via the
+  /// override_frame_id_string parameter.
+  std::optional<geometry_msgs::msg::Transform> override_frame_transform;
+
+  /// \brief Override the header.frame_id field of the outgoing messages with
+  /// this new frame_id string
+  std::string override_frame_id_string;
+
+  /// \brief Append the header.frame_id field of the outgoing messages with
+  /// a suffix string. Used internally for publishing optical frames.
+  std::string override_frame_id_suffix_string;
+};
+
+}  // namespace ros_gz_bridge
+
+#endif  // BRIDGE_HANDLE_GZ_TO_ROS_PARAMETERS_HPP_