Merge branch 'jazzy' into ros2-jazzy-backport

README.md

@@ -59,7 +59,7 @@ This repository holds packages that provide integration between
 
 ## Install
 
-This branch supports ROS Rolling. See above for other ROS versions.
+This branch supports ROS Jazzy. See above for other ROS versions.
 
 ### Binaries
 
@@ -74,24 +74,24 @@ They are hosted at https://packages.ros.org.
 
 1. Install `ros_gz`
 
-        sudo apt install ros-rolling-ros-gz
+        sudo apt install ros-jazzy-ros-gz
 
 ### From source
 
 #### ROS
 
 Be sure you've installed
-[ROS Rolling](https://index.ros.org/doc/ros2/Installation/)
+[ROS Jazzy](https://docs.ros.org/en/jazzy/Installation.html)
 (at least ROS-Base). More ROS dependencies will be installed below.
 
 #### Gazebo
 
-Install either [Edifice, Fortress, or Garden](https://gazebosim.org/docs).
+Install either [Garden or Harmonic](https://gazebosim.org/docs).
 
 Set the `GZ_VERSION` environment variable to the Gazebo version you'd
 like to compile against. For example:
 
-    export GZ_VERSION=edifice # IMPORTANT: Replace with correct version
+    export GZ_VERSION=harmonic # IMPORTANT: Replace with correct version
 
 > You only need to set this variable when compiling, not when running.
 
@@ -107,14 +107,14 @@ The following steps are for Linux and OSX.
     cd ~/ws/src
 
     # Download needed software
-    git clone https://github.com/gazebosim/ros_gz.git -b ros2
+    git clone https://github.com/gazebosim/ros_gz.git -b jazzy
     ```
 
 1. Install dependencies (this may also install Gazebo):
 
     ```
     cd ~/ws
-    rosdep install -r --from-paths src -i -y --rosdistro humble
+    rosdep install -r --from-paths src -i -y --rosdistro jazzy
     ```
 
     > If `rosdep` fails to install Gazebo libraries and you have not installed them before, please follow [Gazebo installation instructions](https://gazebosim.org/docs/latest/install).

ros_gz/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package ros_gz
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.4 (2024-08-29)
+------------------
+
+1.0.3 (2024-07-22)
+------------------
+
 1.0.2 (2024-07-03)
 ------------------
 * Prepare for 1.0.0 Release (`#495 <https://github.com/gazebosim/ros_gz//issues/495>`_)

ros_gz/package.xml

@@ -4,7 +4,7 @@
   <!-- TODO: Make this a metapackage, see
        https://github.com/ros2/ros2/issues/408 -->
   <name>ros_gz</name>
-  <version>1.0.0</version>
+  <version>1.0.4</version>
   <description>Meta-package containing interfaces for using ROS 2 with <a href="https://gazebosim.org">Gazebo</a> simulation.</description>
   <maintainer email="[email protected]">Aditya Pande</maintainer>
   <maintainer email="[email protected]">Alejandro Hernandez</maintainer>

ros_gz_bridge/CHANGELOG.rst

@@ -2,6 +2,25 @@
 Changelog for package ros_gz_bridge
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.4 (2024-08-29)
+------------------
+* feat: `override_timestamps_with_wall_time` parameter (backport `#562 <https://github.com/gazebosim/ros_gz/issues/562>`_) (`#584 <https://github.com/gazebosim/ros_gz/issues/584>`_)
+  Co-authored-by: Rein Appeldoorn <[email protected]>
+* Use memcpy instead of std::copy when bridging images (`#565 <https://github.com/gazebosim/ros_gz/issues/565>`_) (`#585 <https://github.com/gazebosim/ros_gz/issues/585>`_)
+  While testing ros <-> gz communication using the bridge I noticed that the bridge was talking quite a bit of time copying images from Gazebo to ROS. I found that the std::copy operation that we're doing is substantially slower than the memcpy alternative. I think that in principle this shouldn't happen but the numbers are quite clear. Perhaps std::copy is doing something that doesn't use cache effectively
+  ---------
+  Co-authored-by: Jose Luis Rivero <[email protected]>
+  (cherry picked from commit a781b78852112246245c05481db6335388d4f736)
+  Co-authored-by: Carlos Agüero <[email protected]>
+* Contributors: mergify[bot]
+
+1.0.3 (2024-07-22)
+------------------
+* Add support for gz.msgs.EntityWrench (base branch: ros2) (`#573 <https://github.com/gazebosim/ros_gz/issues/573>`_) (`#574 <https://github.com/gazebosim/ros_gz/issues/574>`_)
+  (cherry picked from commit f9afb69d1163633dd978024bb7271a28cf7b551a)
+  Co-authored-by: Victor T. Noppeney <[email protected]>
+* Contributors: mergify[bot]
+
 1.0.2 (2024-07-03)
 ------------------
 * Merge pull request `#569 <https://github.com/gazebosim/ros_gz//issues/569>`_ from azeey/iron_to_jazzy

ros_gz_bridge/README.md

@@ -34,6 +34,7 @@ The following message types can be bridged for topics:
 | ros_gz_interfaces/msg/Contacts                 | gz.msgs.Contacts                    |
 | ros_gz_interfaces/msg/Dataframe                | gz.msgs.Dataframe                   |
 | ros_gz_interfaces/msg/Entity                   | gz.msgs.Entity                      |
+| ros_gz_interfaces/msg/EntityWrench             | gz.msgs.EntityWrench                |
 | ros_gz_interfaces/msg/Float32Array             | gz.msgs.Float_V                     |
 | ros_gz_interfaces/msg/GuiCamera                | gz.msgs.GUICamera                   |
 | ros_gz_interfaces/msg/JointWrench              | gz.msgs.JointWrench                 |
@@ -89,6 +90,9 @@ You should create an unidirectional `/clock` bridge:
 ros2 run ros_gz_bridge parameter_bridge /clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock
 ```
 
+An alternative set-up can be using the bridge with the `override_timestamps_with_wall_time` ros parameter set to `true` (default=`false`). In this set-up,
+all header timestamps of the outgoing messages will be stamped with the wall time. This can be useful when the simulator has to communicate with an external system that requires wall times.
+
 ## Example 1a: Gazebo Transport talker and ROS 2 listener
 
 Start the parameter bridge which will watch the specified topics.

ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

@@ -18,6 +18,7 @@
 // Gazebo Msgs
 #include <gz/msgs/altimeter.pb.h>
 #include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_wrench.pb.h>
 #include <gz/msgs/joint_wrench.pb.h>
 #include <gz/msgs/contact.pb.h>
 #include <gz/msgs/contacts.pb.h>
@@ -37,6 +38,7 @@
 // ROS 2 messages
 #include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
+#include <ros_gz_interfaces/msg/entity_wrench.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
 #include <ros_gz_interfaces/msg/contact.hpp>
 #include <ros_gz_interfaces/msg/contacts.hpp>
@@ -95,6 +97,18 @@ convert_gz_to_ros(
   const gz::msgs::Entity & gz_msg,
   ros_gz_interfaces::msg::Entity & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityWrench & ros_msg,
+  gz::msgs::EntityWrench & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityWrench & gz_msg,
+  ros_gz_interfaces::msg::EntityWrench & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ros_gz_bridge</name>
-  <version>1.0.0</version>
+  <version>1.0.4</version>
   <description>Bridge communication between ROS and Gazebo Transport</description>
   <maintainer email="[email protected]">Aditya Pande</maintainer>
   <maintainer email="[email protected]">Alejandro Hernandez</maintainer>

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -64,6 +64,7 @@
         Mapping('Contacts', 'Contacts'),
         Mapping('Dataframe', 'Dataframe'),
         Mapping('Entity', 'Entity'),
+        Mapping('EntityWrench', 'EntityWrench'),
         Mapping('Float32Array', 'Float_V'),
         Mapping('GuiCamera', 'GUICamera'),
         Mapping('JointWrench', 'JointWrench'),

ros_gz_bridge/src/bridge_handle.cpp

@@ -30,6 +30,8 @@ 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,6 +101,9 @@ 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

@@ -70,7 +70,8 @@ void BridgeHandleGzToRos::StartSubscriber()
     this->gz_node_,
     this->config_.gz_topic_name,
     this->config_.subscriber_queue_size,
-    this->ros_publisher_);
+    this->ros_publisher_,
+    override_timestamps_with_wall_time_);
 
   this->gz_subscriber_ = this->gz_node_;
 }

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -139,6 +139,28 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityWrench & ros_msg,
+  gz::msgs::EntityWrench & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  convert_ros_to_gz(ros_msg.entity, (*gz_msg.mutable_entity()));
+  convert_ros_to_gz(ros_msg.wrench, (*gz_msg.mutable_wrench()));
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityWrench & gz_msg,
+  ros_gz_interfaces::msg::EntityWrench & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg.entity(), ros_msg.entity);
+  convert_gz_to_ros(gz_msg.wrench(), ros_msg.wrench);
+}
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/src/convert/sensor_msgs.cpp

@@ -166,13 +166,11 @@ convert_gz_to_ros(
 
   ros_msg.is_bigendian = false;
   ros_msg.step = ros_msg.width * num_channels * octets_per_channel;
-
-  auto count = ros_msg.step * ros_msg.height;
   ros_msg.data.resize(ros_msg.step * ros_msg.height);
-  std::copy(
-    gz_msg.data().begin(),
-    gz_msg.data().begin() + count,
-    ros_msg.data.begin());
+
+  // Prefer memcpy over std::copy for performance reasons,
+  // see https://github.com/gazebosim/ros_gz/pull/565
+  memcpy(ros_msg.data.data(), gz_msg.data().c_str(), gz_msg.data().size());
 }
 
 template<>

ros_gz_bridge/src/factory.hpp

@@ -15,9 +15,11 @@
 #ifndef FACTORY_HPP_
 #define FACTORY_HPP_
 
+#include <chrono>
 #include <functional>
 #include <memory>
 #include <string>
+#include <type_traits>
 
 #include <gz/transport/Node.hh>
 
@@ -27,6 +29,16 @@
 
 #include "factory_interface.hpp"
 
+template<class T, class = void>
+struct has_header : std::false_type
+{
+};
+
+template<class T>
+struct has_header<T, std::void_t<decltype(T::header)>>: std::true_type
+{
+};
+
 namespace ros_gz_bridge
 {
 
@@ -52,9 +64,11 @@ class Factory : public FactoryInterface
     auto options = rclcpp::PublisherOptions();
     options.qos_overriding_options = rclcpp::QosOverridingOptions {
       {
+        rclcpp::QosPolicyKind::Deadline,
         rclcpp::QosPolicyKind::Depth,
         rclcpp::QosPolicyKind::Durability,
         rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Liveliness,
         rclcpp::QosPolicyKind::Reliability
       },
     };
@@ -103,15 +117,17 @@ class Factory : public FactoryInterface
     std::shared_ptr<gz::transport::Node> node,
     const std::string & topic_name,
     size_t /*queue_size*/,
-    rclcpp::PublisherBase::SharedPtr ros_pub)
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time)
   {
     std::function<void(const GZ_T &,
       const gz::transport::MessageInfo &)> subCb =
-      [this, ros_pub](const GZ_T & _msg, const gz::transport::MessageInfo & _info)
+      [this, ros_pub, override_timestamps_with_wall_time](const GZ_T & _msg,
+      const gz::transport::MessageInfo & _info)
       {
         // Ignore messages that are published from this bridge.
         if (!_info.IntraProcess()) {
-          this->gz_callback(_msg, ros_pub);
+          this->gz_callback(_msg, ros_pub, override_timestamps_with_wall_time);
         }
       };
 
@@ -139,10 +155,20 @@ class Factory : public FactoryInterface
   static
   void gz_callback(
     const GZ_T & gz_msg,
-    rclcpp::PublisherBase::SharedPtr ros_pub)
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time)
   {
     ROS_T ros_msg;
     convert_gz_to_ros(gz_msg, ros_msg);
+    if constexpr (has_header<ROS_T>::value) {
+      if (override_timestamps_with_wall_time) {
+        auto now = std::chrono::system_clock::now().time_since_epoch();
+        auto ns =
+          std::chrono::duration_cast<std::chrono::nanoseconds>(now).count();
+        ros_msg.header.stamp.sec = ns / 1e9;
+        ros_msg.header.stamp.nanosec = ns - ros_msg.header.stamp.sec * 1e9;
+      }
+    }
     std::shared_ptr<rclcpp::Publisher<ROS_T>> pub =
       std::dynamic_pointer_cast<rclcpp::Publisher<ROS_T>>(ros_pub);
     if (pub != nullptr) {

ros_gz_bridge/src/factory_interface.hpp

@@ -60,7 +60,8 @@ class FactoryInterface
     std::shared_ptr<gz::transport::Node> node,
     const std::string & topic_name,
     size_t queue_size,
-    rclcpp::PublisherBase::SharedPtr ros_pub) = 0;
+    rclcpp::PublisherBase::SharedPtr ros_pub,
+    bool override_timestamps_with_wall_time) = 0;
 };
 
 }  // namespace ros_gz_bridge

ros_gz_bridge/src/ros_gz_bridge.cpp

@@ -33,6 +33,7 @@ RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options)
   this->declare_parameter<int>("subscription_heartbeat", 1000);
   this->declare_parameter<std::string>("config_file", "");
   this->declare_parameter<bool>("expand_gz_topic_names", false);
+  this->declare_parameter<bool>("override_timestamps_with_wall_time", false);
 
   int heartbeat;
   this->get_parameter("subscription_heartbeat", heartbeat);

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -526,6 +526,30 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Entity> & _msg)
   EXPECT_EQ(expected_msg.type(), _msg->type());
 }
 
+void createTestMsg(gz::msgs::EntityWrench & _msg)
+{
+  gz::msgs::Header header_msg;
+  gz::msgs::Entity entity_msg;
+  gz::msgs::Wrench wrench_msg;
+
+  createTestMsg(header_msg);
+  createTestMsg(entity_msg);
+  createTestMsg(wrench_msg);
+
+  _msg.mutable_header()->CopyFrom(header_msg);
+  _msg.mutable_entity()->CopyFrom(entity_msg);
+  _msg.mutable_wrench()->CopyFrom(wrench_msg);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::EntityWrench> & _msg)
+{
+  gz::msgs::EntityWrench expected_msg;
+  createTestMsg(expected_msg);
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  compareTestMsg(std::make_shared<gz::msgs::Entity>(_msg->entity()));
+  compareTestMsg(std::make_shared<gz::msgs::Wrench>(_msg->wrench()));
+}
+
 void createTestMsg(gz::msgs::Contact & _msg)
 {
   gz::msgs::Entity collision1;