ros_gz_bridge improvements

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>

Author: ahcorde

ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp

@@ -42,9 +42,9 @@ class RosGzBridge : public rclcpp::Node
   void add_bridge(const BridgeConfig & config);
 
   /// \brief Create a new ROS-GZ bridge for a service
-  /// \param[in] ros_type_name Name of the ROS service (eg ros_bz_interfaces/srv/ControlWorld)
+  /// \param[in] ros_type_name Name of the ROS service (eg ros_gz_interfaces/srv/ControlWorld)
   /// \param[in] gz_req_type_name Gazebo service request type
-  /// \param[in] gz_req_type_name Gazebo service response type
+  /// \param[in] gz_rep_type_name Gazebo service response type
   /// \param[in] service_name Address of the service to be bridged
   void add_service_bridge(
     const std::string & ros_type_name,

ros_gz_bridge/src/bridge_handle_gz_to_ros_parameters.hpp

@@ -1,4 +1,4 @@
-// Copyright 2026 Open Source Robotics Foundation, Inc.
+// Copyright 2022 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.

ros_gz_bridge/src/convert/geometry_msgs.cpp

@@ -161,12 +161,9 @@ convert_gz_to_ros(
 {
   convert_gz_to_ros(gz_msg.pose().position(), ros_msg.pose.position);
   convert_gz_to_ros(gz_msg.pose().orientation(), ros_msg.pose.orientation);
-  int data_size = gz_msg.covariance().data_size();
-  if (data_size == 36) {
-    for (int i = 0; i < data_size; i++) {
-      auto data = gz_msg.covariance().data()[i];
-      ros_msg.covariance[i] = data;
-    }
+  if (gz_msg.covariance().data_size() == 36) {
+    const auto & cov = gz_msg.covariance().data();
+    std::copy(cov.begin(), cov.end(), ros_msg.covariance.begin());
   }
 }
 
@@ -323,12 +320,9 @@ convert_gz_to_ros(
 {
   convert_gz_to_ros(gz_msg.twist().linear(), ros_msg.twist.linear);
   convert_gz_to_ros(gz_msg.twist().angular(), ros_msg.twist.angular);
-  int data_size = gz_msg.covariance().data_size();
-  if (data_size == 36) {
-    for (int i = 0; i < data_size; i++) {
-      auto data = gz_msg.covariance().data()[i];
-      ros_msg.covariance[i] = data;
-    }
+  if (gz_msg.covariance().data_size() == 36) {
+    const auto & cov = gz_msg.covariance().data();
+    std::copy(cov.begin(), cov.end(), ros_msg.covariance.begin());
   }
 }
 

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -232,7 +232,7 @@ convert_ros_to_gz(
   gz::msgs::Contact & gz_msg)
 {
   convert_ros_to_gz(ros_msg.collision1, (*gz_msg.mutable_collision1()));
-  convert_ros_to_gz(ros_msg.collision1, (*gz_msg.mutable_collision2()));
+  convert_ros_to_gz(ros_msg.collision2, (*gz_msg.mutable_collision2()));
   gz_msg.clear_position();
   for (auto const & ros_position : ros_msg.positions) {
     auto gz_position = gz_msg.add_position();
@@ -261,19 +261,23 @@ convert_gz_to_ros(
 {
   convert_gz_to_ros(gz_msg.collision1(), ros_msg.collision1);
   convert_gz_to_ros(gz_msg.collision2(), ros_msg.collision2);
+  ros_msg.positions.reserve(gz_msg.position_size());
   for (auto i = 0; i < gz_msg.position_size(); ++i) {
     geometry_msgs::msg::Vector3 ros_position;
     convert_gz_to_ros(gz_msg.position(i), ros_position);
     ros_msg.positions.push_back(ros_position);
   }
+  ros_msg.normals.reserve(gz_msg.normal_size());
   for (auto i = 0; i < gz_msg.normal_size(); ++i) {
     geometry_msgs::msg::Vector3 ros_normal;
     convert_gz_to_ros(gz_msg.normal(i), ros_normal);
     ros_msg.normals.push_back(ros_normal);
   }
+  ros_msg.depths.reserve(gz_msg.depth_size());
   for (auto i = 0; i < gz_msg.depth_size(); ++i) {
     ros_msg.depths.push_back(gz_msg.depth(i));
   }
+  ros_msg.wrenches.reserve(gz_msg.wrench_size());
   for (auto i = 0; i < gz_msg.wrench_size(); ++i) {
     ros_gz_interfaces::msg::JointWrench ros_joint_wrench;
     convert_gz_to_ros(gz_msg.wrench(i), ros_joint_wrench);
@@ -302,6 +306,7 @@ convert_gz_to_ros(
   ros_gz_interfaces::msg::Contacts & ros_msg)
 {
   convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  ros_msg.contacts.reserve(gz_msg.contact_size());
   for (auto i = 0; i < gz_msg.contact_size(); ++i) {
     ros_gz_interfaces::msg::Contact ros_contact;
     convert_gz_to_ros(gz_msg.contact(i), ros_contact);

ros_gz_bridge/src/convert/sensor_msgs.cpp

@@ -336,26 +336,20 @@ convert_gz_to_ros(
   convert_gz_to_ros(gz_msg.linear_acceleration(), ros_msg.linear_acceleration);
 
 #ifdef GZ_MSGS_IMU_HAS_COVARIANCE
-  int data_size = gz_msg.linear_acceleration_covariance().data_size();
-  if (data_size == 9) {
-    for (int i = 0; i < data_size; ++i) {
-      auto data = gz_msg.linear_acceleration_covariance().data()[i];
-      ros_msg.linear_acceleration_covariance[i] = data;
-    }
+  if (gz_msg.linear_acceleration_covariance().data_size() == 9) {
+    const auto & accel_cov = gz_msg.linear_acceleration_covariance().data();
+    std::copy(accel_cov.begin(), accel_cov.end(),
+      ros_msg.linear_acceleration_covariance.begin());
   }
-  data_size = gz_msg.angular_velocity_covariance().data_size();
-  if (data_size == 9) {
-    for (int i = 0; i < data_size; ++i) {
-      auto data = gz_msg.angular_velocity_covariance().data()[i];
-      ros_msg.angular_velocity_covariance[i] = data;
-    }
+  if (gz_msg.angular_velocity_covariance().data_size() == 9) {
+    const auto & gyro_cov = gz_msg.angular_velocity_covariance().data();
+    std::copy(gyro_cov.begin(), gyro_cov.end(),
+      ros_msg.angular_velocity_covariance.begin());
   }
-  data_size = gz_msg.orientation_covariance().data_size();
-  if (data_size == 9) {
-    for (int i = 0; i < data_size; ++i) {
-      auto data = gz_msg.orientation_covariance().data()[i];
-      ros_msg.orientation_covariance[i] = data;
-    }
+  if (gz_msg.orientation_covariance().data_size() == 9) {
+    const auto & orient_cov = gz_msg.orientation_covariance().data();
+    std::copy(orient_cov.begin(), orient_cov.end(),
+      ros_msg.orientation_covariance.begin());
   }
 #endif
 }
@@ -418,10 +412,12 @@ convert_gz_to_ros(
 {
   convert_gz_to_ros(gz_msg.header(), ros_msg.header);
 
+  ros_msg.axes.reserve(gz_msg.axes_size());
   for (auto i = 0; i < gz_msg.axes_size(); ++i) {
     ros_msg.axes.push_back(gz_msg.axes(i));
   }
 
+  ros_msg.buttons.reserve(gz_msg.buttons_size());
   for (auto i = 0; i < gz_msg.buttons_size(); ++i) {
     ros_msg.buttons.push_back(gz_msg.buttons(i));
   }

ros_gz_bridge/src/convert/utils.cpp

@@ -32,7 +32,7 @@ std::string replace_delimiter(
 
   while (last_pos < input.size()) {
     std::size_t pos = input.find(old_delim, last_pos);
-    output += input.substr(last_pos, pos - last_pos);
+    output.append(input, last_pos, pos - last_pos);
     if (pos != std::string::npos) {
       output += new_delim;
       pos += old_delim.size();

ros_gz_bridge/src/factory.hpp

@@ -217,6 +217,7 @@ class Factory : public FactoryInterface
     const GZ_T & gz_msg,
     ROS_T & ros_msg);
 
+private:
   std::string ros_type_name_;
   std::string gz_type_name_;
 };

ros_gz_bridge/src/service_factory.hpp

@@ -73,6 +73,7 @@ class ServiceFactory : public ServiceFactoryInterface
             if (send_response_on_error(ros_res)) {
               srv_handle->send_response(*reqid, ros_res);
             }
+            return;
           }
           convert_gz_to_ros(reply, ros_res);
           srv_handle->send_response(*reqid, ros_res);