gazebosim · azeey · Jun 24, 2024 · Dec 12, 2023 · Dec 13, 2023 · Dec 13, 2023
diff --git a/README.md b/README.md
@@ -8,7 +8,7 @@ Galactic | Edifice | [galactic](https://github.com/gazebosim/ros_gz/tree/galacti
 Galactic | Fortress | [galactic](https://github.com/gazebosim/ros_gz/tree/galactic) | only from source
 Humble | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | https://packages.ros.org
 Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/latest/ros_installation#gazebo-garden-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1]
-Humble | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | only from source
+Humble | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/harmonic/ros_installation#-gazebo-harmonic-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1]
 Iron | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | https://packages.ros.org
 Iron | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source
 Iron | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source

diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
@@ -5,68 +5,67 @@ between ROS and Gazebo Transport.
 
 The following message types can be bridged for topics:
 
-| ROS type                             | Gazebo type                                 |
-|--------------------------------------|:-------------------------------------------:|
-| builtin_interfaces/msg/Time          | ignition::msgs::Time                        |
-| std_msgs/msg/Bool                    | ignition::msgs::Boolean                     |
-| std_msgs/msg/ColorRGBA               | ignition::msgs::Color                       |
-| std_msgs/msg/Empty                   | ignition::msgs::Empty                       |
-| std_msgs/msg/Float32                 | ignition::msgs::Float                       |
-| std_msgs/msg/Float64                 | ignition::msgs::Double                      |
-| std_msgs/msg/Header                  | ignition::msgs::Header                      |
-| std_msgs/msg/Int32                   | ignition::msgs::Int32                       |
-| std_msgs/msg/UInt32                  | ignition::msgs::UInt32                      |
-| std_msgs/msg/String                  | ignition::msgs::StringMsg                   |
-| geometry_msgs/msg/Wrench             | ignition::msgs::Wrench                      |
-| geometry_msgs/msg/WrenchStamped      | ignition::msgs::Wrench                      |
-| geometry_msgs/msg/Quaternion         | ignition::msgs::Quaternion                  |
-| geometry_msgs/msg/Vector3            | ignition::msgs::Vector3d                    |
-| geometry_msgs/msg/Point              | ignition::msgs::Vector3d                    |
-| geometry_msgs/msg/Pose               | ignition::msgs::Pose                        |
-| geometry_msgs/msg/PoseArray          | ignition::msgs::Pose_V                      |
-| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance          |
-| geometry_msgs/msg/PoseStamped        | ignition::msgs::Pose                        |
-| geometry_msgs/msg/Transform          | ignition::msgs::Pose                        |
-| geometry_msgs/msg/TransformStamped   | ignition::msgs::Pose                        |
-| geometry_msgs/msg/Twist              | ignition::msgs::Twist                       |
-| geometry_msgs/msg/TwistStamped       | ignition::msgs::Twist                       |
-| geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance         |
-| gps_msgs/GPSFix                      | ignition::msgs::NavSat                      |
-| nav_msgs/msg/Odometry                | ignition::msgs::Odometry                    |
-| nav_msgs/msg/Odometry                | ignition::msgs::OdometryWithCovariance      |
-| rcl_interfaces/msg/ParameterValue    | ignition::msgs::Any                         |
-| ros_gz_interfaces/msg/Altimeter      | ignition::msgs::Altimeter                   |
-| ros_gz_interfaces/msg/Contact        | ignition::msgs::Contact                     |
-| ros_gz_interfaces/msg/Contacts       | ignition::msgs::Contacts                    |
-| ros_gz_interfaces/msg/Dataframe      | ignition::msgs::Dataframe                   |
-| ros_gz_interfaces/msg/Entity         | ignition::msgs::Entity                      |
-| ros_gz_interfaces/msg/Float32Array   | ignition::msgs::Float_V                     |
-| ros_gz_interfaces/msg/GuiCamera      | ignition::msgs::GUICamera                   |
-| ros_gz_interfaces/msg/JointWrench    | ignition::msgs::JointWrench                 |
-| ros_gz_interfaces/msg/Light          | ignition::msgs::Light                       |
-| ros_gz_interfaces/msg/SensorNoise    | ignition::msgs::SensorNoise                 |
-| ros_gz_interfaces/msg/StringVec      | ignition::msgs::StringMsg_V                 |
-| ros_gz_interfaces/msg/TrackVisual    | ignition::msgs::TrackVisual                 |
-| ros_gz_interfaces/msg/VideoRecord    | ignition::msgs::VideoRecord                 |
-| ros_gz_interfaces/msg/WorldControl   | ignition::msgs::WorldControl                |
-| rosgraph_msgs/msg/Clock              | ignition::msgs::Clock                       |
-| sensor_msgs/msg/BatteryState         | ignition::msgs::BatteryState                |
-| sensor_msgs/msg/CameraInfo           | ignition::msgs::CameraInfo                  |
-| sensor_msgs/msg/FluidPressure        | ignition::msgs::FluidPressure               |
-| sensor_msgs/msg/Imu                  | ignition::msgs::IMU                         |
-| sensor_msgs/msg/Image                | ignition::msgs::Image                       |
-| sensor_msgs/msg/JointState           | ignition::msgs::Model                       |
-| sensor_msgs/msg/Joy                  | ignition::msgs::Joy                         |
-| sensor_msgs/msg/LaserScan            | ignition::msgs::LaserScan                   |
-| sensor_msgs/msg/MagneticField        | ignition::msgs::Magnetometer                |
-| sensor_msgs/msg/NavSatFix            | ignition::msgs::NavSat                      |
-| sensor_msgs/msg/PointCloud2          | ignition::msgs::PointCloudPacked            |
-| tf2_msgs/msg/TFMessage               | ignition::msgs::Pose_V                      |
-| trajectory_msgs/msg/JointTrajectory  | ignition::msgs::JointTrajectory             |
-| vision_msgs/msg/Detection2D          | ignition::msgs::AnnotatedAxisAligned2DBox   |
-| vision_msgs/msg/Detection2DArray     | ignition::msgs::AnnotatedAxisAligned2DBox_V |
-| vision_msgs/msg/Detection3D          | ignition::msgs::AnnotatedOriented3DBox      |
-| vision_msgs/msg/Detection3DArray     | ignition::msgs::AnnotatedOriented3DBox_V    |
+| ROS type                                    | Gazebo type                                 |
+|---------------------------------------------|:-------------------------------------------:|
+| builtin_interfaces/msg/Time                 | ignition::msgs::Time                        |
+| std_msgs/msg/Bool                           | ignition::msgs::Boolean                     |
+| std_msgs/msg/ColorRGBA                      | ignition::msgs::Color                       |
+| std_msgs/msg/Empty                          | ignition::msgs::Empty                       |
+| std_msgs/msg/Float32                        | ignition::msgs::Float                       |
+| std_msgs/msg/Float64                        | ignition::msgs::Double                      |
+| std_msgs/msg/Header                         | ignition::msgs::Header                      |
+| std_msgs/msg/Int32                          | ignition::msgs::Int32                       |
+| std_msgs/msg/UInt32                         | ignition::msgs::UInt32                      |
+| std_msgs/msg/String                         | ignition::msgs::StringMsg                   |
+| geometry_msgs/msg/Wrench                    | ignition::msgs::Wrench                      |
+| geometry_msgs/msg/WrenchStamped             | ignition::msgs::Wrench                      |
+| geometry_msgs/msg/Quaternion                | ignition::msgs::Quaternion                  |
+| geometry_msgs/msg/Vector3                   | ignition::msgs::Vector3d                    |
+| geometry_msgs/msg/Point                     | ignition::msgs::Vector3d                    |
+| geometry_msgs/msg/Pose                      | ignition::msgs::Pose                        |
+| geometry_msgs/msg/PoseArray                 | ignition::msgs::Pose_V                      |
+| geometry_msgs/msg/PoseWithCovariance        | ignition::msgs::PoseWithCovariance          |
+| geometry_msgs/msg/PoseStamped               | ignition::msgs::Pose                        |
+| geometry_msgs/msg/Transform                 | ignition::msgs::Pose                        |
+| geometry_msgs/msg/TransformStamped          | ignition::msgs::Pose                        |
+| geometry_msgs/msg/Twist                     | ignition::msgs::Twist                       |
+| geometry_msgs/msg/TwistStamped              | ignition::msgs::Twist                       |
+| geometry_msgs/msg/TwistWithCovariance       | ignition::msgs::TwistWithCovariance         |
+| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance         |
+| gps_msgs/GPSFix                             | ignition::msgs::NavSat                      |
+| nav_msgs/msg/Odometry                       | ignition::msgs::Odometry                    |
+| nav_msgs/msg/Odometry                       | ignition::msgs::OdometryWithCovariance      |
+| rcl_interfaces/msg/ParameterValue           | ignition::msgs::Any                         |
+| ros_gz_interfaces/msg/Altimeter             | ignition::msgs::Altimeter                   |
+| ros_gz_interfaces/msg/Contact               | ignition::msgs::Contact                     |
+| ros_gz_interfaces/msg/Contacts              | ignition::msgs::Contacts                    |
+| ros_gz_interfaces/msg/Dataframe             | ignition::msgs::Dataframe                   |
+| ros_gz_interfaces/msg/Entity                | ignition::msgs::Entity                      |
+| ros_gz_interfaces/msg/Float32Array          | ignition::msgs::Float_V                     |
+| ros_gz_interfaces/msg/GuiCamera             | ignition::msgs::GUICamera                   |
+| ros_gz_interfaces/msg/JointWrench           | ignition::msgs::JointWrench                 |
+| ros_gz_interfaces/msg/Light                 | ignition::msgs::Light                       |
+| ros_gz_interfaces/msg/SensorNoise           | ignition::msgs::SensorNoise                 |
+| ros_gz_interfaces/msg/StringVec             | ignition::msgs::StringMsg_V                 |
+| ros_gz_interfaces/msg/TrackVisual           | ignition::msgs::TrackVisual                 |
+| ros_gz_interfaces/msg/VideoRecord           | ignition::msgs::VideoRecord                 |
+| ros_gz_interfaces/msg/WorldControl          | ignition::msgs::WorldControl                |
+| rosgraph_msgs/msg/Clock                     | ignition::msgs::Clock                       |
+| sensor_msgs/msg/BatteryState                | ignition::msgs::BatteryState                |
+| sensor_msgs/msg/CameraInfo                  | ignition::msgs::CameraInfo                  |
+| sensor_msgs/msg/FluidPressure               | ignition::msgs::FluidPressure               |
+| sensor_msgs/msg/Imu                         | ignition::msgs::IMU                         |
+| sensor_msgs/msg/Image                       | ignition::msgs::Image                       |
+| sensor_msgs/msg/JointState                  | ignition::msgs::Model                       |
+| sensor_msgs/msg/Joy                         | ignition::msgs::Joy                         |
+| sensor_msgs/msg/LaserScan                   | ignition::msgs::LaserScan                   |
+| sensor_msgs/msg/MagneticField               | ignition::msgs::Magnetometer                |
+| sensor_msgs/msg/NavSatFix                   | ignition::msgs::NavSat                      |
+| sensor_msgs/msg/PointCloud2                 | ignition::msgs::PointCloudPacked            |
+| tf2_msgs/msg/TFMessage                      | ignition::msgs::Pose_V                      |
+| trajectory_msgs/msg/JointTrajectory         | ignition::msgs::JointTrajectory             |
+| vision_msgs/msg/Detection3D                 | ignition::msgs::AnnotatedOriented3DBox      |
+| vision_msgs/msg/Detection3DArray            | ignition::msgs::AnnotatedOriented3DBox_V    |
 
 And the following for services:
 

diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp
@@ -36,6 +36,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 
@@ -189,6 +190,18 @@ convert_gz_to_ros(
   const gz::msgs::TwistWithCovariance & gz_msg,
   geometry_msgs::msg::TwistWithCovariance & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg,
+  gz::msgs::TwistWithCovariance & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::TwistWithCovariance & gz_msg,
+  geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py
@@ -42,6 +42,7 @@
         Mapping('Twist', 'Twist'),
         Mapping('TwistStamped', 'Twist'),
         Mapping('TwistWithCovariance', 'TwistWithCovariance'),
+        Mapping('TwistWithCovarianceStamped', 'TwistWithCovariance'),
         Mapping('Wrench', 'Wrench'),
         Mapping('WrenchStamped', 'Wrench'),
         Mapping('Vector3', 'Vector3d'),

diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp
@@ -306,6 +306,26 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg,
+  gz::msgs::TwistWithCovariance & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_twist()->mutable_header()));
+  convert_ros_to_gz(ros_msg.twist, gz_msg);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::TwistWithCovariance & gz_msg,
+  geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.twist().header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg, ros_msg.twist);
+}
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp
@@ -433,12 +433,15 @@ void createTestMsg(gz::msgs::TwistWithCovariance & _msg)
 {
   gz::msgs::Vector3d linear_msg;
   gz::msgs::Vector3d angular_msg;
+  gz::msgs::Header header_msg;
 
   createTestMsg(linear_msg);
   createTestMsg(angular_msg);
+  createTestMsg(header_msg);
 
   _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg);
   _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg);
+  _msg.mutable_twist()->mutable_header()->CopyFrom(header_msg);
   for (int i = 0; i < 36; i++) {
     _msg.mutable_covariance()->add_data(i);
   }

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -484,6 +484,18 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::TwistWithCovarianc
   }
 }
 
+void createTestMsg(geometry_msgs::msg::TwistWithCovarianceStamped & _msg)
+{
+  createTestMsg(_msg.header);
+  createTestMsg(_msg.twist);
+}
+
+void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::TwistWithCovarianceStamped> & _msg)
+{
+  compareTestMsg(std::make_shared<geometry_msgs::msg::TwistWithCovariance>(_msg->twist));
+  compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
+}
+
 void createTestMsg(geometry_msgs::msg::Wrench & _msg)
 {
   createTestMsg(_msg.force);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -40,6 +40,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <geometry_msgs/msg/wrench.hpp>
@@ -319,6 +320,14 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::TwistWithCovariance> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(geometry_msgs::msg::TwistWithCovarianceStamped & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare
+void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::TwistWithCovarianceStamped> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(geometry_msgs::msg::Wrench & _msg);

diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md
@@ -37,3 +37,77 @@ See more options with:
 ```
 ros2 run ros_gz_sim create --helpshort
 ```
+
+### Using `<gazebo_ros>` to export model paths in `package.xml`
+
+The `<gazebo_ros>` tag inside the `<export>` tag of a `package.xml` file can be
+used to add paths to `GZ_SIM_RESOURCE_PATH` and `GZ_SIM_SYSTEM_PLUGIN_PATH`,
+which are environment variables used to configure Gazebo search paths for
+resources (e.g. SDFormat files, meshes, etc) and plugins respectively.
+
+The values in the attributes `gazebo_model_path` and `gazebo_media_path` are
+appended to `GZ_SIM_RESOURCE_PATH`. The value of `plugin_path` is appended to
+`GZ_SIM_SYSTEM_PLUGIN_PATH`. See the
+[Finding resources](https://gazebosim.org/api/sim/8/resources.html) tutorial to
+learn more about these environment variables.
+
+The keyword `${prefix}` can be used when setting these values and it will be
+expanded to the package's share path (i.e., the value of
+`ros2 pkg prefix --share <package name>`)
+
+```xml
+<export>
+   <gazebo_ros gazebo_model_path="${prefix}/models"/>
+   <gazebo_ros gazebo_media_path="${prefix}/media"/>
+   <gazebo_ros plugin_path="${prefix}/plugins"/>
+</export>
+
+```
+
+Thus the required directory needs to be installed from `CMakeLists.txt`
+
+```cmake
+install(DIRECTORY models
+    DESTINATION share/${PROJECT_NAME})
+```
+
+In order to reference the models in a ROS package unambiguously, it is
+recommended to set the value of `gazebo_model_path` to be the parent
+of the `prefix`.
+
+```xml
+<export>
+   <gazebo_ros gazebo_model_path="${prefix}/../"/>
+</export>
+
+```
+
+Consider an example where we have a ROS package called `my_awesome_pkg`
+and it contains an SDFormat model cool `cool_robot`:
+
+```bash
+my_awesome_pkg
+├── models
+│   └── cool_robot
+│       ├── model.config
+│       └── model.sdf
+└── package.xml
+```
+
+With `gazebo_model_path="${prefix}/../` set up, we can
+reference the `cool_robot` model in a world file using the package name
+in the `uri`:
+
+```xml
+<sdf version="1.6">
+  <world name="default">
+    <include>
+      <uri>package://my_awesome_pkg/models/cool_robot</uri>
+    </include>
+  </world>
+</sdf>
+```
+
+However, if we set `gazebo_model_path=${prefix}/models`, we would
+need to reference `cool_robot` as `package://cool_robot`, which
+might have a name conflict with other models in the system.