From 4b3ee9e7b5d57ab468f9e9a967ed50a19f7da0cc Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 12 Dec 2023 15:51:24 -0600 Subject: [PATCH 01/19] Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (#468) (#470) Signed-off-by: Addisu Z. Taddese --- ros_gz_bridge/README.md | 1 + .../bin/ros_gz_bridge_markdown_table | 2 +- .../ros_gz_bridge/convert/geometry_msgs.hpp | 13 ++++++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + ros_gz_bridge/src/convert/geometry_msgs.cpp | 20 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 3 +++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 12 +++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 +++++++++ 8 files changed, 60 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 11135cdb..e9a062ce 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -29,6 +29,7 @@ The following message types can be bridged for topics: | 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 | | nav_msgs/msg/Odometry | ignition::msgs::Odometry | | nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | diff --git a/ros_gz_bridge/bin/ros_gz_bridge_markdown_table b/ros_gz_bridge/bin/ros_gz_bridge_markdown_table index 65c33e8b..05c11b72 100755 --- a/ros_gz_bridge/bin/ros_gz_bridge_markdown_table +++ b/ros_gz_bridge/bin/ros_gz_bridge_markdown_table @@ -42,7 +42,7 @@ def main(argv=sys.argv[1:]): for mapping in mappings(msgs_ver): rows.append('| {:32}| {:32}|'.format( - mapping.ros2_package_name + '/' + mapping.ros2_message_name, + mapping.ros2_package_name + '/msg/' + mapping.ros2_message_name, mapping.gz_string())) print('\n'.join(rows)) 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 index 3256f68e..43860e69 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,18 @@ convert_gz_to_ros( const gz::msgs::Twist & gz_msg, geometry_msgs::msg::Twist & ros_msg); +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistStamped & ros_msg, + gz::msgs::Twist & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::Twist & gz_msg, + geometry_msgs::msg::TwistStamped & 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 index 33edc5c5..c8788c89 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -40,6 +40,7 @@ Mapping('Transform', 'Pose'), Mapping('TransformStamped', 'Pose'), Mapping('Twist', 'Twist'), + Mapping('TwistStamped', 'Twist'), Mapping('TwistWithCovariance', 'TwistWithCovariance'), Mapping('Wrench', 'Wrench'), Mapping('WrenchStamped', 'Wrench'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index be5f755c..60ae1413 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -256,6 +256,26 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.angular(), ros_msg.angular); } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistStamped & ros_msg, + gz::msgs::Twist & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.twist, gz_msg); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::Twist & gz_msg, + geometry_msgs::msg::TwistStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.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 index 52d076ab..505ed922 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -417,6 +417,9 @@ void createTestMsg(gz::msgs::Twist & _msg) void compareTestMsg(const std::shared_ptr & _msg) { + if (_msg->has_header()) { + compareTestMsg(std::make_shared(_msg->header())); + } compareTestMsg(std::make_shared(_msg->linear())); compareTestMsg(std::make_shared(_msg->angular())); } diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index df757852..6c30e98b 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -447,6 +447,18 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->angular)); } +void createTestMsg(geometry_msgs::msg::TwistStamped & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.twist); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist)); + compareTestMsg(std::make_shared(_msg->header)); +} + void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg) { createTestMsg(_msg.twist.linear); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index e1ec8338..d1e6caf2 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -297,6 +298,14 @@ void createTestMsg(geometry_msgs::msg::Twist & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(geometry_msgs::msg::TwistStamped & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg); From d598e13ce2e66061e38dd8dbcbd8854e6a030c02 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 13 Dec 2023 15:04:51 -0600 Subject: [PATCH 02/19] Changelog Signed-off-by: Addisu Z. Taddese --- ros_gz/CHANGELOG.rst | 3 +++ ros_gz_bridge/CHANGELOG.rst | 12 ++++++++++++ ros_gz_image/CHANGELOG.rst | 6 ++++++ ros_gz_interfaces/CHANGELOG.rst | 6 ++++++ ros_gz_sim/CHANGELOG.rst | 6 ++++++ ros_gz_sim_demos/CHANGELOG.rst | 7 +++++++ ros_ign/CHANGELOG.rst | 3 +++ ros_ign_bridge/CHANGELOG.rst | 3 +++ ros_ign_gazebo/CHANGELOG.rst | 5 +++++ ros_ign_gazebo_demos/CHANGELOG.rst | 3 +++ ros_ign_image/CHANGELOG.rst | 3 +++ ros_ign_interfaces/CHANGELOG.rst | 3 +++ 12 files changed, 60 insertions(+) diff --git a/ros_gz/CHANGELOG.rst b/ros_gz/CHANGELOG.rst index 16fa268a..207f73ac 100644 --- a/ros_gz/CHANGELOG.rst +++ b/ros_gz/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_gz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_gz_bridge/CHANGELOG.rst b/ros_gz_bridge/CHANGELOG.rst index 20d90de8..d0b66110 100644 --- a/ros_gz_bridge/CHANGELOG.rst +++ b/ros_gz_bridge/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (`#468 `_) (`#470 `_) +* Add support for Harmonic/Humble pairing (`#462 `_) +* Added messages for 2D Bounding Boxes to ros_gz_bridge (`#458 `_) +* Fix double wait in ros_gz_bridge (`#347 `_) (`#450 `_) +* [backport humble] SensorNoise msg bridging (`#417 `_) +* [backport humble] Added Altimeter msg bridging (`#413 `_) (`#414 `_) (`#426 `_) +* [backport humble] Update README.md (`#411 `_) + The ROS type for gz.msgs.NavSat messages should be **sensor_msgs/msg/NavSatFix** instead of **sensor_msgs/msg/NavSatFixed** +* Contributors: Addisu Z. Taddese, Alejandro Hernández Cordero, Arjun K Haridas, wittenator + 0.244.11 (2023-05-23) --------------------- * Add actuator_msgs to humble bridge. (`#394 `_) diff --git a/ros_gz_image/CHANGELOG.rst b/ros_gz_image/CHANGELOG.rst index f2479e5b..19e54312 100644 --- a/ros_gz_image/CHANGELOG.rst +++ b/ros_gz_image/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros1_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for Harmonic/Humble pairing (`#462 `_) +* Fix double wait in ros_gz_bridge (`#347 `_) (`#450 `_) +* Contributors: Addisu Z. Taddese, Alejandro Hernández Cordero + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_gz_interfaces/CHANGELOG.rst b/ros_gz_interfaces/CHANGELOG.rst index b6cfa77a..e562edd7 100644 --- a/ros_gz_interfaces/CHANGELOG.rst +++ b/ros_gz_interfaces/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros_gz_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [backport humble] SensorNoise msg bridging (`#417 `_) +* [backport humble] Added Altimeter msg bridging (`#413 `_) (`#414 `_) (`#426 `_) +* Contributors: Alejandro Hernández Cordero + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_gz_sim/CHANGELOG.rst b/ros_gz_sim/CHANGELOG.rst index 6d3e9805..95519a78 100644 --- a/ros_gz_sim/CHANGELOG.rst +++ b/ros_gz_sim/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros_gz_sim ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for Harmonic/Humble pairing (`#462 `_) +* Set on_exit_shutdown argument for gz-sim ExecuteProcess (`#355 `_) (`#451 `_) +* Contributors: Addisu Z. Taddese, Michael Carroll + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_gz_sim_demos/CHANGELOG.rst b/ros_gz_sim_demos/CHANGELOG.rst index 06ae22e2..5775aeab 100644 --- a/ros_gz_sim_demos/CHANGELOG.rst +++ b/ros_gz_sim_demos/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ros1_gz_sim_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [backport Humble] Added more topic to the bridge (`#422 `_) +* Added more topic to the bridge (`#422 `_) +* Fix incorrect subscription on demo (`#405 `_) +* Contributors: Alejandro Hernández Cordero, Arjo Chakravarty + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign/CHANGELOG.rst b/ros_ign/CHANGELOG.rst index aed0e23d..cb401ced 100644 --- a/ros_ign/CHANGELOG.rst +++ b/ros_ign/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_bridge/CHANGELOG.rst b/ros_ign_bridge/CHANGELOG.rst index 14dfce11..9b29e9fb 100644 --- a/ros_ign_bridge/CHANGELOG.rst +++ b/ros_ign_bridge/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_gazebo/CHANGELOG.rst b/ros_ign_gazebo/CHANGELOG.rst index e55afec9..0b75dce7 100644 --- a/ros_ign_gazebo/CHANGELOG.rst +++ b/ros_ign_gazebo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros_ign_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for Harmonic/Humble pairing (`#462 `_) +* Contributors: Addisu Z. Taddese + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_gazebo_demos/CHANGELOG.rst b/ros_ign_gazebo_demos/CHANGELOG.rst index e6a99bd5..1482e1d3 100644 --- a/ros_ign_gazebo_demos/CHANGELOG.rst +++ b/ros_ign_gazebo_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_gazebo_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_image/CHANGELOG.rst b/ros_ign_image/CHANGELOG.rst index 3261e04b..32044f28 100644 --- a/ros_ign_image/CHANGELOG.rst +++ b/ros_ign_image/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_interfaces/CHANGELOG.rst b/ros_ign_interfaces/CHANGELOG.rst index 8953b611..ed54a27b 100644 --- a/ros_ign_interfaces/CHANGELOG.rst +++ b/ros_ign_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.244.11 (2023-05-23) --------------------- From 78d3398d5817c06d2bc0ecdaee77f3ab6735e4bf Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 13 Dec 2023 15:06:02 -0600 Subject: [PATCH 03/19] 0.244.12 --- ros_gz/CHANGELOG.rst | 4 ++-- ros_gz/package.xml | 2 +- ros_gz_bridge/CHANGELOG.rst | 4 ++-- ros_gz_bridge/package.xml | 2 +- ros_gz_image/CHANGELOG.rst | 4 ++-- ros_gz_image/package.xml | 2 +- ros_gz_interfaces/CHANGELOG.rst | 4 ++-- ros_gz_interfaces/package.xml | 2 +- ros_gz_sim/CHANGELOG.rst | 4 ++-- ros_gz_sim/package.xml | 2 +- ros_gz_sim_demos/CHANGELOG.rst | 4 ++-- ros_gz_sim_demos/package.xml | 2 +- ros_ign/CHANGELOG.rst | 4 ++-- ros_ign/package.xml | 2 +- ros_ign_bridge/CHANGELOG.rst | 4 ++-- ros_ign_bridge/package.xml | 2 +- ros_ign_gazebo/CHANGELOG.rst | 4 ++-- ros_ign_gazebo/package.xml | 2 +- ros_ign_gazebo_demos/CHANGELOG.rst | 4 ++-- ros_ign_gazebo_demos/package.xml | 2 +- ros_ign_image/CHANGELOG.rst | 4 ++-- ros_ign_image/package.xml | 2 +- ros_ign_interfaces/CHANGELOG.rst | 4 ++-- ros_ign_interfaces/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/ros_gz/CHANGELOG.rst b/ros_gz/CHANGELOG.rst index 207f73ac..61e5fd4f 100644 --- a/ros_gz/CHANGELOG.rst +++ b/ros_gz/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_gz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_gz/package.xml b/ros_gz/package.xml index 16dc9b36..9981ecb4 100644 --- a/ros_gz/package.xml +++ b/ros_gz/package.xml @@ -4,7 +4,7 @@ ros_gz - 0.244.11 + 0.244.12 Meta-package containing interfaces for using ROS 2 with Gazebo simulation. Louise Poubel Apache 2.0 diff --git a/ros_gz_bridge/CHANGELOG.rst b/ros_gz_bridge/CHANGELOG.rst index d0b66110..74b8603b 100644 --- a/ros_gz_bridge/CHANGELOG.rst +++ b/ros_gz_bridge/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (`#468 `_) (`#470 `_) * Add support for Harmonic/Humble pairing (`#462 `_) * Added messages for 2D Bounding Boxes to ros_gz_bridge (`#458 `_) diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index adf330fe..869f176f 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ ros_gz_bridge - 0.244.11 + 0.244.12 Bridge communication between ROS and Gazebo Transport Louise Poubel diff --git a/ros_gz_image/CHANGELOG.rst b/ros_gz_image/CHANGELOG.rst index 19e54312..3a3b593c 100644 --- a/ros_gz_image/CHANGELOG.rst +++ b/ros_gz_image/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros1_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * Add support for Harmonic/Humble pairing (`#462 `_) * Fix double wait in ros_gz_bridge (`#347 `_) (`#450 `_) * Contributors: Addisu Z. Taddese, Alejandro Hernández Cordero diff --git a/ros_gz_image/package.xml b/ros_gz_image/package.xml index e7dba5e3..12bd3f94 100644 --- a/ros_gz_image/package.xml +++ b/ros_gz_image/package.xml @@ -1,6 +1,6 @@ ros_gz_image - 0.244.11 + 0.244.12 Image utilities for Gazebo simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_gz_interfaces/CHANGELOG.rst b/ros_gz_interfaces/CHANGELOG.rst index e562edd7..a58b27cf 100644 --- a/ros_gz_interfaces/CHANGELOG.rst +++ b/ros_gz_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_gz_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * [backport humble] SensorNoise msg bridging (`#417 `_) * [backport humble] Added Altimeter msg bridging (`#413 `_) (`#414 `_) (`#426 `_) * Contributors: Alejandro Hernández Cordero diff --git a/ros_gz_interfaces/package.xml b/ros_gz_interfaces/package.xml index d686b05f..ba558aa2 100644 --- a/ros_gz_interfaces/package.xml +++ b/ros_gz_interfaces/package.xml @@ -1,6 +1,6 @@ ros_gz_interfaces - 0.244.11 + 0.244.12 Message and service data structures for interacting with Gazebo from ROS2. Apache 2.0 Louise Poubel diff --git a/ros_gz_sim/CHANGELOG.rst b/ros_gz_sim/CHANGELOG.rst index 95519a78..0351b1f8 100644 --- a/ros_gz_sim/CHANGELOG.rst +++ b/ros_gz_sim/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_gz_sim ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * Add support for Harmonic/Humble pairing (`#462 `_) * Set on_exit_shutdown argument for gz-sim ExecuteProcess (`#355 `_) (`#451 `_) * Contributors: Addisu Z. Taddese, Michael Carroll diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 7609b55d..9774b03a 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -2,7 +2,7 @@ ros_gz_sim - 0.244.11 + 0.244.12 Tools for using Gazebo Sim simulation with ROS. Alejandro Hernandez diff --git a/ros_gz_sim_demos/CHANGELOG.rst b/ros_gz_sim_demos/CHANGELOG.rst index 5775aeab..8763d55e 100644 --- a/ros_gz_sim_demos/CHANGELOG.rst +++ b/ros_gz_sim_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros1_gz_sim_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * [backport Humble] Added more topic to the bridge (`#422 `_) * Added more topic to the bridge (`#422 `_) * Fix incorrect subscription on demo (`#405 `_) diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index 0789aaea..4caa54fe 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -1,6 +1,6 @@ ros_gz_sim_demos - 0.244.11 + 0.244.12 Demos using Gazebo Sim simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_ign/CHANGELOG.rst b/ros_ign/CHANGELOG.rst index cb401ced..ca3124cd 100644 --- a/ros_ign/CHANGELOG.rst +++ b/ros_ign/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign/package.xml b/ros_ign/package.xml index 3f340d63..6217f10a 100644 --- a/ros_ign/package.xml +++ b/ros_ign/package.xml @@ -2,7 +2,7 @@ ros_ign - 0.244.11 + 0.244.12 Shim meta-package to redirect to ros_gz. Brandon Ong Apache 2.0 diff --git a/ros_ign_bridge/CHANGELOG.rst b/ros_ign_bridge/CHANGELOG.rst index 9b29e9fb..0501476c 100644 --- a/ros_ign_bridge/CHANGELOG.rst +++ b/ros_ign_bridge/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_bridge/package.xml b/ros_ign_bridge/package.xml index 5402c3ad..9d6b3051 100644 --- a/ros_ign_bridge/package.xml +++ b/ros_ign_bridge/package.xml @@ -2,7 +2,7 @@ ros_ign_bridge - 0.244.11 + 0.244.12 Shim package to redirect to ros_gz_bridge. Brandon Ong diff --git a/ros_ign_gazebo/CHANGELOG.rst b/ros_ign_gazebo/CHANGELOG.rst index 0b75dce7..90c00480 100644 --- a/ros_ign_gazebo/CHANGELOG.rst +++ b/ros_ign_gazebo/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- * Add support for Harmonic/Humble pairing (`#462 `_) * Contributors: Addisu Z. Taddese diff --git a/ros_ign_gazebo/package.xml b/ros_ign_gazebo/package.xml index 1163cc31..12d7813c 100644 --- a/ros_ign_gazebo/package.xml +++ b/ros_ign_gazebo/package.xml @@ -2,7 +2,7 @@ ros_ign_gazebo - 0.244.11 + 0.244.12 Shim package to redirect to ros_gz_sim. Brandon Ong diff --git a/ros_ign_gazebo_demos/CHANGELOG.rst b/ros_ign_gazebo_demos/CHANGELOG.rst index 1482e1d3..1e8aeaab 100644 --- a/ros_ign_gazebo_demos/CHANGELOG.rst +++ b/ros_ign_gazebo_demos/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign_gazebo_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_gazebo_demos/package.xml b/ros_ign_gazebo_demos/package.xml index 0be9866c..e72072b1 100644 --- a/ros_ign_gazebo_demos/package.xml +++ b/ros_ign_gazebo_demos/package.xml @@ -1,6 +1,6 @@ ros_ign_gazebo_demos - 0.244.11 + 0.244.12 Shim package to redirect to ros_gz_sim_demos. Apache 2.0 Brandon Ong diff --git a/ros_ign_image/CHANGELOG.rst b/ros_ign_image/CHANGELOG.rst index 32044f28..216a751d 100644 --- a/ros_ign_image/CHANGELOG.rst +++ b/ros_ign_image/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_image/package.xml b/ros_ign_image/package.xml index fbaac431..2ed549db 100644 --- a/ros_ign_image/package.xml +++ b/ros_ign_image/package.xml @@ -2,7 +2,7 @@ ros_ign_image - 0.244.11 + 0.244.12 Shim package to redirect to ros_gz_image. Brandon Ong diff --git a/ros_ign_interfaces/CHANGELOG.rst b/ros_ign_interfaces/CHANGELOG.rst index ed54a27b..663c713b 100644 --- a/ros_ign_interfaces/CHANGELOG.rst +++ b/ros_ign_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros_ign_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.244.12 (2023-12-13) +--------------------- 0.244.11 (2023-05-23) --------------------- diff --git a/ros_ign_interfaces/package.xml b/ros_ign_interfaces/package.xml index d40a8e99..d584960a 100644 --- a/ros_ign_interfaces/package.xml +++ b/ros_ign_interfaces/package.xml @@ -1,6 +1,6 @@ ros_ign_interfaces - 0.244.11 + 0.244.12 Shim package to redirect to ros_gz_interfaces. Apache 2.0 Brandon Ong From bdf138eeec5c2afb90a0ad667e1ccdeec1ecbab6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 3 Jan 2024 16:06:34 +0100 Subject: [PATCH 04/19] Add tip about non-parallel builds to README (#477) (#479) Signed-off-by: Addisu Z. Taddese --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 67569364..0d969016 100644 --- a/README.md +++ b/README.md @@ -125,6 +125,9 @@ The following steps are for Linux and OSX. cd ~/ws colcon build ``` + > [!TIP] + > The `ros_gz` library makes heavy use of templates which causes compilers to consume a lot of memory. If your build fails with `c++: fatal error: Killed signal terminated program cc1plus` + > try building with `colcon build --parallel-workers=1 --executor sequential`. If `colcon build` fails with [this issue](https://github.com/gazebosim/ros_gz/issues/401) From 84d5d27e6f730db506e6b229c40ef3fdc7042cf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Jan 2024 10:20:47 +0100 Subject: [PATCH 05/19] Additional tip for limiting number of cpus (#480) (#481) Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0d969016..53755a5d 100644 --- a/README.md +++ b/README.md @@ -127,7 +127,8 @@ The following steps are for Linux and OSX. ``` > [!TIP] > The `ros_gz` library makes heavy use of templates which causes compilers to consume a lot of memory. If your build fails with `c++: fatal error: Killed signal terminated program cc1plus` - > try building with `colcon build --parallel-workers=1 --executor sequential`. + > try building with `colcon build --parallel-workers=1 --executor sequential`. You might also have to set `export MAKEFLAGS="-j 1"` before running `colcon build` to limit + > the number of processors used to build a single package. If `colcon build` fails with [this issue](https://github.com/gazebosim/ros_gz/issues/401) From d36e3717429f71d72cccaaae041fe074bbc33440 Mon Sep 17 00:00:00 2001 From: El Jawad Alaa Date: Tue, 23 Jan 2024 14:39:14 +0100 Subject: [PATCH 06/19] populate imu covariances when converting (#488) Signed-off-by: Alaa El Jawad --- ros_gz_bridge/src/convert/sensor_msgs.cpp | 42 ++++++++++++++++++++++- ros_gz_bridge/test/utils/gz_test_msg.cpp | 18 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 19 ++++++++++ 3 files changed, 78 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp index cddc4f00..887f16d0 100644 --- a/ros_gz_bridge/src/convert/sensor_msgs.cpp +++ b/ros_gz_bridge/src/convert/sensor_msgs.cpp @@ -17,6 +17,12 @@ #include "convert/utils.hpp" #include "ros_gz_bridge/convert/sensor_msgs.hpp" +#include "gz/msgs/config.hh" + +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { @@ -274,6 +280,18 @@ convert_ros_to_gz( convert_ros_to_gz(ros_msg.orientation, (*gz_msg.mutable_orientation())); convert_ros_to_gz(ros_msg.angular_velocity, (*gz_msg.mutable_angular_velocity())); convert_ros_to_gz(ros_msg.linear_acceleration, (*gz_msg.mutable_linear_acceleration())); + +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (const auto & elem : ros_msg.linear_acceleration_covariance) { + gz_msg.mutable_linear_acceleration_covariance()->add_data(elem); + } + for (const auto & elem : ros_msg.orientation_covariance) { + gz_msg.mutable_orientation_covariance()->add_data(elem); + } + for (const auto & elem : ros_msg.angular_velocity_covariance) { + gz_msg.mutable_angular_velocity_covariance()->add_data(elem); + } +#endif } template<> @@ -287,7 +305,29 @@ convert_gz_to_ros( convert_gz_to_ros(gz_msg.angular_velocity(), ros_msg.angular_velocity); convert_gz_to_ros(gz_msg.linear_acceleration(), ros_msg.linear_acceleration); - // Covariances not supported in gz::msgs::IMU +#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; + } + } + 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; + } + } + 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; + } + } +#endif } template<> diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 505ed922..60cc9266 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -19,6 +19,10 @@ #include #include +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { namespace testing @@ -810,6 +814,13 @@ void createTestMsg(gz::msgs::IMU & _msg) _msg.mutable_orientation()->CopyFrom(quaternion_msg); _msg.mutable_angular_velocity()->CopyFrom(vector3_msg); _msg.mutable_linear_acceleration()->CopyFrom(vector3_msg); +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; i++) { + _msg.mutable_orientation_covariance()->add_data(i + 1); + _msg.mutable_angular_velocity_covariance()->add_data(i + 1); + _msg.mutable_linear_acceleration_covariance()->add_data(i + 1); + } +#endif } void compareTestMsg(const std::shared_ptr & _msg) @@ -818,6 +829,13 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->orientation())); compareTestMsg(std::make_shared(_msg->angular_velocity())); compareTestMsg(std::make_shared(_msg->linear_acceleration())); +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; i++) { + EXPECT_EQ(_msg->orientation_covariance().data(i), i + 1); + EXPECT_EQ(_msg->angular_velocity_covariance().data(i), i + 1); + EXPECT_EQ(_msg->linear_acceleration_covariance().data(i), i + 1); + } +#endif } void createTestMsg(gz::msgs::Axis & _msg) diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 6c30e98b..f5c490d2 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -19,6 +19,12 @@ #include #include +#include "gz/msgs/config.hh" + +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { namespace testing @@ -1022,6 +1028,11 @@ void createTestMsg(sensor_msgs::msg::Imu & _msg) _msg.orientation = quaternion_msg; _msg.angular_velocity = vector3_msg; _msg.linear_acceleration = vector3_msg; +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + _msg.orientation_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + _msg.angular_velocity_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + _msg.linear_acceleration_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; +#endif } void compareTestMsg(const std::shared_ptr & _msg) @@ -1030,6 +1041,14 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(_msg->orientation); compareTestMsg(std::make_shared(_msg->angular_velocity)); compareTestMsg(std::make_shared(_msg->linear_acceleration)); + +#ifdef GZ_MSGS_IMU_HAS_COVARIANCE + for (int i = 0; i < 9; ++i) { + EXPECT_EQ(_msg->orientation_covariance[i], i + 1); + EXPECT_EQ(_msg->angular_velocity_covariance[i], i + 1); + EXPECT_EQ(_msg->linear_acceleration_covariance[i], i + 1); + } +#endif } void createTestMsg(sensor_msgs::msg::JointState & _msg) From 3abc34f62fc81a099891a70ab2ff89924bd0ba28 Mon Sep 17 00:00:00 2001 From: El Jawad Alaa Date: Tue, 23 Jan 2024 14:40:56 +0100 Subject: [PATCH 07/19] backport pr 374 (#489) Signed-off-by: Alaa El Jawad --- ros_gz_bridge/README.md | 115 +++++++++--------- .../ros_gz_bridge/convert/geometry_msgs.hpp | 13 ++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + ros_gz_bridge/src/convert/geometry_msgs.cpp | 20 +++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 3 + ros_gz_bridge/test/utils/ros_test_msg.cpp | 12 ++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 ++ 7 files changed, 116 insertions(+), 57 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index e9a062ce..aff3b379 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,63 +5,64 @@ 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 | -| 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 | +| 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 | +| 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 | 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 index 43860e69..70537def 100644 --- 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 #include #include +#include #include #include @@ -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 index c8788c89..649fd6e3 100644 --- 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 index 60ae1413..ad83ba6d 100644 --- 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 index 60cc9266..0bfdea1d 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -432,12 +432,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 index f5c490d2..efca25ea 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -483,6 +483,18 @@ void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist)); + compareTestMsg(std::make_shared(_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 index d1e6caf2..589f4de1 100644 --- 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 #include #include +#include #include #include #include @@ -314,6 +315,14 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::Wrench & _msg); From 60ac0787e88afcfda6360f250b6de351c3791fee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 23 Jan 2024 21:51:21 +0100 Subject: [PATCH 08/19] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- ros_gz/CHANGELOG.rst | 3 +++ ros_gz_bridge/CHANGELOG.rst | 6 ++++++ ros_gz_image/CHANGELOG.rst | 3 +++ ros_gz_interfaces/CHANGELOG.rst | 3 +++ ros_gz_sim/CHANGELOG.rst | 3 +++ ros_gz_sim_demos/CHANGELOG.rst | 3 +++ ros_ign/CHANGELOG.rst | 3 +++ ros_ign_bridge/CHANGELOG.rst | 3 +++ ros_ign_gazebo/CHANGELOG.rst | 3 +++ ros_ign_gazebo_demos/CHANGELOG.rst | 3 +++ ros_ign_image/CHANGELOG.rst | 3 +++ ros_ign_interfaces/CHANGELOG.rst | 3 +++ 12 files changed, 39 insertions(+) diff --git a/ros_gz/CHANGELOG.rst b/ros_gz/CHANGELOG.rst index 61e5fd4f..b305581d 100644 --- a/ros_gz/CHANGELOG.rst +++ b/ros_gz/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_gz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- diff --git a/ros_gz_bridge/CHANGELOG.rst b/ros_gz_bridge/CHANGELOG.rst index 74b8603b..afd5bb04 100644 --- a/ros_gz_bridge/CHANGELOG.rst +++ b/ros_gz_bridge/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- +* backport pr 374 (`#489 `_) +* populate imu covariances when converting (`#488 `_) +* Contributors: El Jawad Alaa + 0.244.12 (2023-12-13) --------------------- * Backport: Add conversion for geometry_msgs/msg/TwistStamped <-> gz.msgs.Twist (`#468 `_) (`#470 `_) diff --git a/ros_gz_image/CHANGELOG.rst b/ros_gz_image/CHANGELOG.rst index 3a3b593c..328917c9 100644 --- a/ros_gz_image/CHANGELOG.rst +++ b/ros_gz_image/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- * Add support for Harmonic/Humble pairing (`#462 `_) diff --git a/ros_gz_interfaces/CHANGELOG.rst b/ros_gz_interfaces/CHANGELOG.rst index a58b27cf..abe9c879 100644 --- a/ros_gz_interfaces/CHANGELOG.rst +++ b/ros_gz_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_gz_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- * [backport humble] SensorNoise msg bridging (`#417 `_) diff --git a/ros_gz_sim/CHANGELOG.rst b/ros_gz_sim/CHANGELOG.rst index 0351b1f8..4a13427e 100644 --- a/ros_gz_sim/CHANGELOG.rst +++ b/ros_gz_sim/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_gz_sim ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- * Add support for Harmonic/Humble pairing (`#462 `_) diff --git a/ros_gz_sim_demos/CHANGELOG.rst b/ros_gz_sim_demos/CHANGELOG.rst index 8763d55e..377c19d5 100644 --- a/ros_gz_sim_demos/CHANGELOG.rst +++ b/ros_gz_sim_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_gz_sim_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- * [backport Humble] Added more topic to the bridge (`#422 `_) diff --git a/ros_ign/CHANGELOG.rst b/ros_ign/CHANGELOG.rst index ca3124cd..ce4c4646 100644 --- a/ros_ign/CHANGELOG.rst +++ b/ros_ign/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- diff --git a/ros_ign_bridge/CHANGELOG.rst b/ros_ign_bridge/CHANGELOG.rst index 0501476c..17b74fd3 100644 --- a/ros_ign_bridge/CHANGELOG.rst +++ b/ros_ign_bridge/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- diff --git a/ros_ign_gazebo/CHANGELOG.rst b/ros_ign_gazebo/CHANGELOG.rst index 90c00480..2f10998c 100644 --- a/ros_ign_gazebo/CHANGELOG.rst +++ b/ros_ign_gazebo/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- * Add support for Harmonic/Humble pairing (`#462 `_) diff --git a/ros_ign_gazebo_demos/CHANGELOG.rst b/ros_ign_gazebo_demos/CHANGELOG.rst index 1e8aeaab..7edbe335 100644 --- a/ros_ign_gazebo_demos/CHANGELOG.rst +++ b/ros_ign_gazebo_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_gazebo_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- diff --git a/ros_ign_image/CHANGELOG.rst b/ros_ign_image/CHANGELOG.rst index 216a751d..ebc65c75 100644 --- a/ros_ign_image/CHANGELOG.rst +++ b/ros_ign_image/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- diff --git a/ros_ign_interfaces/CHANGELOG.rst b/ros_ign_interfaces/CHANGELOG.rst index 663c713b..4a008fd4 100644 --- a/ros_ign_interfaces/CHANGELOG.rst +++ b/ros_ign_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.13 (2024-01-23) +--------------------- + 0.244.12 (2023-12-13) --------------------- From 6af4c0a4f90bd50a95cc314d6d4290e5701fd2f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 23 Jan 2024 21:51:31 +0100 Subject: [PATCH 09/19] 0.244.13 --- ros_gz/package.xml | 2 +- ros_gz_bridge/package.xml | 2 +- ros_gz_image/package.xml | 2 +- ros_gz_interfaces/package.xml | 2 +- ros_gz_sim/package.xml | 2 +- ros_gz_sim_demos/package.xml | 2 +- ros_ign/package.xml | 2 +- ros_ign_bridge/package.xml | 2 +- ros_ign_gazebo/package.xml | 2 +- ros_ign_gazebo_demos/package.xml | 2 +- ros_ign_image/package.xml | 2 +- ros_ign_interfaces/package.xml | 2 +- 12 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ros_gz/package.xml b/ros_gz/package.xml index 9981ecb4..1d2a50af 100644 --- a/ros_gz/package.xml +++ b/ros_gz/package.xml @@ -4,7 +4,7 @@ ros_gz - 0.244.12 + 0.244.13 Meta-package containing interfaces for using ROS 2 with Gazebo simulation. Louise Poubel Apache 2.0 diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index 869f176f..704b7fd4 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ ros_gz_bridge - 0.244.12 + 0.244.13 Bridge communication between ROS and Gazebo Transport Louise Poubel diff --git a/ros_gz_image/package.xml b/ros_gz_image/package.xml index 12bd3f94..e524cafd 100644 --- a/ros_gz_image/package.xml +++ b/ros_gz_image/package.xml @@ -1,6 +1,6 @@ ros_gz_image - 0.244.12 + 0.244.13 Image utilities for Gazebo simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_gz_interfaces/package.xml b/ros_gz_interfaces/package.xml index ba558aa2..c6cdb2a5 100644 --- a/ros_gz_interfaces/package.xml +++ b/ros_gz_interfaces/package.xml @@ -1,6 +1,6 @@ ros_gz_interfaces - 0.244.12 + 0.244.13 Message and service data structures for interacting with Gazebo from ROS2. Apache 2.0 Louise Poubel diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 9774b03a..181c3199 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -2,7 +2,7 @@ ros_gz_sim - 0.244.12 + 0.244.13 Tools for using Gazebo Sim simulation with ROS. Alejandro Hernandez diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index 4caa54fe..15bf28dc 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -1,6 +1,6 @@ ros_gz_sim_demos - 0.244.12 + 0.244.13 Demos using Gazebo Sim simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_ign/package.xml b/ros_ign/package.xml index 6217f10a..2e293389 100644 --- a/ros_ign/package.xml +++ b/ros_ign/package.xml @@ -2,7 +2,7 @@ ros_ign - 0.244.12 + 0.244.13 Shim meta-package to redirect to ros_gz. Brandon Ong Apache 2.0 diff --git a/ros_ign_bridge/package.xml b/ros_ign_bridge/package.xml index 9d6b3051..ccf61c4d 100644 --- a/ros_ign_bridge/package.xml +++ b/ros_ign_bridge/package.xml @@ -2,7 +2,7 @@ ros_ign_bridge - 0.244.12 + 0.244.13 Shim package to redirect to ros_gz_bridge. Brandon Ong diff --git a/ros_ign_gazebo/package.xml b/ros_ign_gazebo/package.xml index 12d7813c..9568dec5 100644 --- a/ros_ign_gazebo/package.xml +++ b/ros_ign_gazebo/package.xml @@ -2,7 +2,7 @@ ros_ign_gazebo - 0.244.12 + 0.244.13 Shim package to redirect to ros_gz_sim. Brandon Ong diff --git a/ros_ign_gazebo_demos/package.xml b/ros_ign_gazebo_demos/package.xml index e72072b1..77bb5ad7 100644 --- a/ros_ign_gazebo_demos/package.xml +++ b/ros_ign_gazebo_demos/package.xml @@ -1,6 +1,6 @@ ros_ign_gazebo_demos - 0.244.12 + 0.244.13 Shim package to redirect to ros_gz_sim_demos. Apache 2.0 Brandon Ong diff --git a/ros_ign_image/package.xml b/ros_ign_image/package.xml index 2ed549db..2b69c8d5 100644 --- a/ros_ign_image/package.xml +++ b/ros_ign_image/package.xml @@ -2,7 +2,7 @@ ros_ign_image - 0.244.12 + 0.244.13 Shim package to redirect to ros_gz_image. Brandon Ong diff --git a/ros_ign_interfaces/package.xml b/ros_ign_interfaces/package.xml index d584960a..61d040a1 100644 --- a/ros_ign_interfaces/package.xml +++ b/ros_ign_interfaces/package.xml @@ -1,6 +1,6 @@ ros_ign_interfaces - 0.244.12 + 0.244.13 Shim package to redirect to ros_gz_interfaces. Apache 2.0 Brandon Ong From b5dffdeb9ce8d99fdb502f3b1c73e20325d04c3f Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 1 Feb 2024 10:43:39 -0500 Subject: [PATCH 10/19] Add option to change material color from ROS. (#486) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Message and bridge for MaterialColor. This allows bridging MaterialColor from ROS to GZ and is important for allowing simulation users to create status lights. --------- Signed-off-by: Benjamin Perseghetti Signed-off-by: Addisu Z. Taddese Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- README.md | 2 +- .../convert/ros_gz_interfaces.hpp | 19 ++++++ .../include/ros_gz_bridge/ros_gz_bridge.hpp | 7 +++ ros_gz_bridge/ros_gz_bridge/__init__.py | 10 +++- ros_gz_bridge/ros_gz_bridge/mappings.py | 6 ++ .../src/convert/ros_gz_interfaces.cpp | 60 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 31 ++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 14 +++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 30 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 13 ++++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/msg/MaterialColor.msg | 12 ++++ 12 files changed, 203 insertions(+), 2 deletions(-) create mode 100644 ros_gz_interfaces/msg/MaterialColor.msg diff --git a/README.md b/README.md index 53755a5d..8e1d74d0 100644 --- 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] Rolling | Edifice | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source Rolling | Fortress | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | https://packages.ros.org Rolling | Garden | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp index c7e671d4..136c8d9b 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp @@ -56,6 +56,11 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#include +#endif // HAVE_MATERIALCOLOR + #include namespace ros_gz_bridge @@ -159,6 +164,20 @@ convert_gz_to_ros( const gz::msgs::Light & gz_msg, ros_gz_interfaces::msg::Light & ros_msg); +#if HAVE_MATERIALCOLOR +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg); +#endif // HAVE_MATERIALCOLOR + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp index 8d9ff631..71abf1d7 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp @@ -36,6 +36,13 @@ #define HAVE_DATAFRAME true #endif +// MaterialColor is available from versions 10.1.0 (Harmonic) forward +// This can be removed when the minimum supported version passes 10.1.0 +#if (GZ_MSGS_MAJOR_VERSION > 10) || \ + ((GZ_MSGS_MAJOR_VERSION == 10) && (GZ_MSGS_MINOR_VERSION >= 1)) +#define HAVE_MATERIALCOLOR true +#endif + namespace ros_gz_bridge { /// Forward declarations diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index b9291840..ef021faf 100644 --- a/ros_gz_bridge/ros_gz_bridge/__init__.py +++ b/ros_gz_bridge/ros_gz_bridge/__init__.py @@ -16,7 +16,7 @@ import os -from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_8_4_0 +from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_10_1_0, MAPPINGS_8_4_0 from rosidl_cmake import expand_template @@ -75,6 +75,14 @@ def mappings(gz_msgs_ver): ros2_message_name=mapping.ros_type, gz_message_name=mapping.gz_type )) + if gz_msgs_ver >= (10, 1, 0): + for (ros2_package_name, mappings) in MAPPINGS_10_1_0.items(): + for mapping in sorted(mappings): + data.append(MessageMapping( + ros2_package_name=ros2_package_name, + ros2_message_name=mapping.ros_type, + gz_message_name=mapping.gz_type + )) return sorted(data, key=lambda mm: mm.ros2_string()) diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 649fd6e3..04137082 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -114,3 +114,9 @@ Mapping('Dataframe', 'Dataframe'), ], } + +MAPPINGS_10_1_0 = { + 'ros_gz_interfaces': [ + Mapping('MaterialColor', 'MaterialColor'), + ], +} diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index fceb296a..0d619955 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -380,6 +380,66 @@ convert_gz_to_ros( ros_msg.intensity = gz_msg.intensity(); } +#if HAVE_MATERIALCOLOR +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + + switch (ros_msg.entity_match) { + case ros_gz_interfaces::msg::MaterialColor::FIRST: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST); + break; + case ros_gz_interfaces::msg::MaterialColor::ALL: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL); + break; + default: + std::cerr << "Unsupported entity match type [" + << ros_msg.entity_match << "]\n"; + } + + 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.ambient, *gz_msg.mutable_ambient()); + convert_ros_to_gz(ros_msg.diffuse, *gz_msg.mutable_diffuse()); + convert_ros_to_gz(ros_msg.specular, *gz_msg.mutable_specular()); + convert_ros_to_gz(ros_msg.emissive, *gz_msg.mutable_emissive()); + + gz_msg.set_shininess(ros_msg.shininess); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + if (gz_msg.entity_match() == EntityMatch::MaterialColor_EntityMatch_FIRST) { + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; +/* *INDENT-OFF* */ + } else if (gz_msg.entity_match() == + EntityMatch::MaterialColor_EntityMatch_ALL) { +/* *INDENT-ON* */ + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; + } else { + std::cerr << "Unsupported EntityMatch [" << + gz_msg.entity_match() << "]" << std::endl; + } + 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.ambient(), ros_msg.ambient); + convert_gz_to_ros(gz_msg.diffuse(), ros_msg.diffuse); + convert_gz_to_ros(gz_msg.specular(), ros_msg.specular); + convert_gz_to_ros(gz_msg.emissive(), ros_msg.emissive); + + ros_msg.shininess = gz_msg.shininess(); +} +#endif // HAVE_MATERIALCOLOR + 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 index 0bfdea1d..b64c0f40 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1346,6 +1346,37 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity()); } +#if HAVE_MATERIALCOLOR +void createTestMsg(gz::msgs::MaterialColor & _msg) +{ + createTestMsg(*_msg.mutable_header()); + createTestMsg(*_msg.mutable_entity()); + createTestMsg(*_msg.mutable_ambient()); + createTestMsg(*_msg.mutable_diffuse()); + createTestMsg(*_msg.mutable_specular()); + createTestMsg(*_msg.mutable_emissive()); + + _msg.set_shininess(1.0); + _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_msg->ambient())); + compareTestMsg(std::make_shared(_msg->diffuse())); + compareTestMsg(std::make_shared(_msg->specular())); + compareTestMsg(std::make_shared(_msg->emissive())); + + EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); + EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); +} +#endif // HAVE_MATERIALCOLOR + void createTestMsg(gz::msgs::GUICamera & _msg) { gz::msgs::Header header_msg; diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 7ec6984a..d939a4ff 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -73,6 +73,10 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#endif // HAVE_MATERIALCOLOR + namespace ros_gz_bridge { namespace testing @@ -455,6 +459,16 @@ void createTestMsg(gz::msgs::Light & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +#if HAVE_MATERIALCOLOR +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); +#endif // HAVE_MATERIALCOLOR + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::GUICamera & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index efca25ea..f80b8553 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -577,6 +577,36 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity); } +#if HAVE_MATERIALCOLOR +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.entity); + createTestMsg(_msg.ambient); + createTestMsg(_msg.diffuse); + createTestMsg(_msg.specular); + createTestMsg(_msg.emissive); + _msg.shininess = 1.0; + _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_msg->ambient)); + compareTestMsg(std::make_shared(_msg->diffuse)); + compareTestMsg(std::make_shared(_msg->specular)); + compareTestMsg(std::make_shared(_msg->emissive)); + + EXPECT_EQ(expected_msg.shininess, _msg->shininess); + EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); +} +#endif // HAVE_MATERIALCOLOR + void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) { createTestMsg(_msg.header); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 589f4de1..ee4426fd 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -57,6 +57,9 @@ #include #endif // HAVE_DATAFRAME #include +#if HAVE_MATERIALCOLOR +#include +#endif // HAVE_MATERIALCOLOR #include #include #include @@ -385,6 +388,16 @@ void createTestMsg(ros_gz_interfaces::msg::Light & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +#if HAVE_MATERIALCOLOR +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); +#endif // HAVE_MATERIALCOLOR + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index 49c49126..5f3b312e 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/ros_gz_interfaces/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/GuiCamera.msg" "msg/JointWrench.msg" "msg/Light.msg" + "msg/MaterialColor.msg" "msg/ParamVec.msg" "msg/SensorNoise.msg" "msg/StringVec.msg" diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg new file mode 100644 index 00000000..cda67d27 --- /dev/null +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -0,0 +1,12 @@ +# Entities that match to apply material color: constant definition +uint8 FIRST = 0 +uint8 ALL = 1 + +std_msgs/Header header # Optional header data +ros_gz_interfaces/Entity entity # Entity to change material color +std_msgs/ColorRGBA ambient # Ambient color +std_msgs/ColorRGBA diffuse # Diffuse color +std_msgs/ColorRGBA specular # Specular color +std_msgs/ColorRGBA emissive # Emissive color +float64 shininess # Specular exponent +uint8 entity_match # Entities that match to apply material color From 3a3d22ae2378d4d5e47c5d65493ecba825ab466d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 23 Feb 2024 16:23:39 +0100 Subject: [PATCH 11/19] Add a virtual destructor to suppress compiler warning (#502) (#505) Signed-off-by: Michael Carroll Co-authored-by: Michael Carroll --- ros_gz_bridge/CMakeLists.txt | 1 + .../src/service_factory_interface.cpp | 24 +++++++++++++++++++ .../src/service_factory_interface.hpp | 2 ++ 3 files changed, 27 insertions(+) create mode 100644 ros_gz_bridge/src/service_factory_interface.cpp diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 4d0e4057..596dfd80 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -129,6 +129,7 @@ add_library(${bridge_lib} src/bridge_handle_ros_to_gz.cpp src/bridge_handle_gz_to_ros.cpp src/factory_interface.cpp + src/service_factory_interface.cpp src/service_factories/ros_gz_interfaces.cpp src/ros_gz_bridge.cpp ${convert_files} diff --git a/ros_gz_bridge/src/service_factory_interface.cpp b/ros_gz_bridge/src/service_factory_interface.cpp new file mode 100644 index 00000000..4789e066 --- /dev/null +++ b/ros_gz_bridge/src/service_factory_interface.cpp @@ -0,0 +1,24 @@ +// Copyright 2024 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. + +#include "service_factory_interface.hpp" + +namespace ros_gz_bridge +{ + +ServiceFactoryInterface::~ServiceFactoryInterface() +{ +} + +} // namespace ros_gz_bridge diff --git a/ros_gz_bridge/src/service_factory_interface.hpp b/ros_gz_bridge/src/service_factory_interface.hpp index 9d4f7334..766ce515 100644 --- a/ros_gz_bridge/src/service_factory_interface.hpp +++ b/ros_gz_bridge/src/service_factory_interface.hpp @@ -29,6 +29,8 @@ namespace ros_gz_bridge class ServiceFactoryInterface { public: + virtual ~ServiceFactoryInterface() = 0; + virtual rclcpp::ServiceBase::SharedPtr create_ros_service( From a6f58b69005654e759174ef549b4d7c37e8f0e13 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 23 Feb 2024 10:11:39 -0600 Subject: [PATCH 12/19] Correctly export ros_gz_bridge for downstream targets (#503) (#506) Signed-off-by: Michael Carroll --- ros_gz_bridge/CMakeLists.txt | 37 ++++++++++++------- test_ros_gz_bridge/CMakeLists.txt | 29 +++++++++++++++ test_ros_gz_bridge/package.xml | 28 ++++++++++++++ test_ros_gz_bridge/src/test_ros_gz_bridge.cpp | 36 ++++++++++++++++++ 4 files changed, 117 insertions(+), 13 deletions(-) create mode 100644 test_ros_gz_bridge/CMakeLists.txt create mode 100644 test_ros_gz_bridge/package.xml create mode 100644 test_ros_gz_bridge/src/test_ros_gz_bridge.cpp diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 596dfd80..7df2399b 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -119,7 +119,7 @@ add_custom_command( COMMENT "Generating factories for interface types") set(bridge_lib - ros_gz_bridge_lib + ros_gz_bridge ) add_library(${bridge_lib} @@ -149,8 +149,12 @@ ament_target_dependencies(${bridge_lib} ) target_include_directories(${bridge_lib} - PUBLIC include - PRIVATE src ${generated_path} + PUBLIC + "$" + "$" + PRIVATE + "$" + "$" ) target_link_libraries(${bridge_lib} @@ -163,19 +167,16 @@ rclcpp_components_register_node( PLUGIN ros_gz_bridge::RosGzBridge EXECUTABLE bridge_node) -install(TARGETS ${bridge_lib} +install(TARGETS ${bridge_lib} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) -ament_export_include_directories(include) -ament_export_libraries(${bridge_lib}) - set(bridge_executables parameter_bridge static_bridge @@ -194,7 +195,7 @@ foreach(bridge ${bridge_executables}) ${BRIDGE_MESSAGE_TYPES} ) install(TARGETS ${bridge} - DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) endforeach() @@ -349,9 +350,19 @@ if(BUILD_TESTING) endif() -ament_export_dependencies( - rclcpp - ${BRIDGE_MESSAGE_TYPES} -) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${bridge_lib}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +# specific order: dependents before dependencies +ament_export_dependencies(rclcpp) +ament_export_dependencies(rclcpp_components) +ament_export_dependencies(${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}) +ament_export_dependencies(${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}) +ament_export_dependencies(yaml_cpp_vendor) +ament_export_dependencies(${BRIDGE_MESSAGE_TYPES}) ament_package() diff --git a/test_ros_gz_bridge/CMakeLists.txt b/test_ros_gz_bridge/CMakeLists.txt new file mode 100644 index 00000000..9a3bb8a8 --- /dev/null +++ b/test_ros_gz_bridge/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) + +project(test_ros_gz_bridge) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(ros_gz_bridge REQUIRED) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + ament_find_gtest() + + ament_add_gtest(test_ros_gz_bridge src/test_ros_gz_bridge.cpp) + target_link_libraries(test_ros_gz_bridge ros_gz_bridge::ros_gz_bridge) +endif() + +ament_package() diff --git a/test_ros_gz_bridge/package.xml b/test_ros_gz_bridge/package.xml new file mode 100644 index 00000000..124b8044 --- /dev/null +++ b/test_ros_gz_bridge/package.xml @@ -0,0 +1,28 @@ + + + + test_ros_gz_bridge + 0.246.0 + Bridge communication between ROS and Gazebo Transport + Aditya Pande + Alejandro Hernandez + + Apache 2.0 + + Michael Carroll + + ament_cmake + + ros_gz_bridge + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing_ament_cmake + launch_ros + launch_testing + + + ament_cmake + + diff --git a/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp b/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp new file mode 100644 index 00000000..a32ca8e5 --- /dev/null +++ b/test_ros_gz_bridge/src/test_ros_gz_bridge.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 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. + +#include + +#include + +class test_ros_gz_bridge : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(test_ros_gz_bridge, SpawnNode) +{ + ros_gz_bridge::RosGzBridge node; +} From 13e5592c3c05cd160903b52c23fd45c10ecaccd7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 11 Mar 2024 12:10:38 -0500 Subject: [PATCH 13/19] Support `` in `package.xml` exports (#492) This copies the implementation from `gazebo_ros_paths.py` to provide a way for packages to set resource paths from `package.xml`. ``` e.g. ``` The value of `gazebo_model_path` and `gazebo_media_path` is appended to `GZ_SIM_RESOURCE_PATH` The value of `plugin_path` appended to `GZ_SIM_SYSTEM_PLUGIN_PATH` --------- Signed-off-by: Addisu Z. Taddese --- ros_gz_sim/README.md | 74 ++++++++++++++++++++++ ros_gz_sim/launch/gz_sim.launch.py.in | 91 +++++++++++++++++++++++++-- 2 files changed, 159 insertions(+), 6 deletions(-) diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md index c4561a5d..e0b05f62 100644 --- 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 `` to export model paths in `package.xml` + +The `` tag inside the `` 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 `) + +```xml + + + + + + +``` + +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 + + + + +``` + +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 + + + + package://my_awesome_pkg/models/cool_robot + + + +``` + +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. diff --git a/ros_gz_sim/launch/gz_sim.launch.py.in b/ros_gz_sim/launch/gz_sim.launch.py.in index 7bed5243..8f87fb7d 100644 --- a/ros_gz_sim/launch/gz_sim.launch.py.in +++ b/ros_gz_sim/launch/gz_sim.launch.py.in @@ -14,20 +14,99 @@ """Launch Gazebo Sim with command line arguments.""" +import os from os import environ +from ament_index_python.packages import get_package_share_directory +from catkin_pkg.package import InvalidPackage, PACKAGE_MANIFEST_FILENAME, parse_package +from ros2pkg.api import get_package_names from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.actions import ExecuteProcess, Shutdown from launch.substitutions import LaunchConfiguration +# Copied from https://github.com/ros-simulation/gazebo_ros_pkgs/blob/79fd94c6da76781a91499bc0f54b70560b90a9d2/gazebo_ros/scripts/gazebo_ros_paths.py +""" +Search for model, plugin and media paths exported by packages. + +e.g. + + + +${prefix} is replaced by package's share directory in install. + +Thus the required directory needs to be installed from CMakeLists.txt +e.g. install(DIRECTORY models + DESTINATION share/${PROJECT_NAME}) +""" + +class GazeboRosPaths: + + @staticmethod + def get_paths(): + gazebo_model_path = [] + gazebo_plugin_path = [] + gazebo_media_path = [] + + for package_name in get_package_names(): + package_share_path = get_package_share_directory(package_name) + package_file_path = os.path.join(package_share_path, PACKAGE_MANIFEST_FILENAME) + if os.path.isfile(package_file_path): + try: + package = parse_package(package_file_path) + except InvalidPackage: + continue + for export in package.exports: + if export.tagname == 'gazebo_ros': + if 'gazebo_model_path' in export.attributes: + xml_path = export.attributes['gazebo_model_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_model_path.append(xml_path) + if 'plugin_path' in export.attributes: + xml_path = export.attributes['plugin_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_plugin_path.append(xml_path) + if 'gazebo_media_path' in export.attributes: + xml_path = export.attributes['gazebo_media_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_media_path.append(xml_path) + + gazebo_model_path = os.pathsep.join(gazebo_model_path + gazebo_media_path) + gazebo_plugin_path = os.pathsep.join(gazebo_plugin_path) + + return gazebo_model_path, gazebo_plugin_path + def launch_gz(context, *args, **kwargs): - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': # TODO(CH3): To support pre-garden. Deprecated. - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')])} + model_paths, plugin_paths = GazeboRosPaths.get_paths() + + env = { + "GZ_SIM_SYSTEM_PLUGIN_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", default=""), + environ.get("LD_LIBRARY_PATH", default=""), + plugin_paths, + ] + ), + "IGN_GAZEBO_SYSTEM_PLUGIN_PATH": os.pathsep.join( # TODO(azeey): To support pre-garden. Deprecated. + [ + environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", default=""), + environ.get("LD_LIBRARY_PATH", default=""), + plugin_paths, + ] + ), + "GZ_SIM_RESOURCE_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_RESOURCE_PATH", default=""), + model_paths, + ] + ), + "IGN_GAZEBO_RESOURCE_PATH": os.pathsep.join( # TODO(azeey): To support pre-garden. Deprecated. + [ + environ.get("IGN_GAZEBO_RESOURCE_PATH", default=""), + model_paths, + ] + ), + } gz_args = LaunchConfiguration('gz_args').perform(context) gz_version = LaunchConfiguration('gz_version').perform(context) From 0b7c5e942e2b341be0bc50936e1b4fb4d0855cee Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Wed, 20 Mar 2024 18:27:29 +0100 Subject: [PATCH 14/19] Add ROS namespaces to GZ topics (#512) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kotochleb Signed-off-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Co-authored-by: Alejandro Hernández Cordero --- ros_gz_bridge/README.md | 40 ++++++++++++++++++++++++++--- ros_gz_bridge/src/ros_gz_bridge.cpp | 14 ++++++++-- 2 files changed, 49 insertions(+), 5 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index aff3b379..19b8a77d 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -86,7 +86,7 @@ Now we start the ROS listener. ``` # Shell B: -. /opt/ros/galactic/setup.bash +. /opt/ros/humble/setup.bash ros2 topic echo /chatter ``` @@ -118,7 +118,7 @@ Now we start the ROS talker. ``` # Shell C: -. /opt/ros/galactic/setup.bash +. /opt/ros/humble/setup.bash ros2 topic pub /chatter std_msgs/msg/String "data: 'Hi'" --once ``` @@ -156,7 +156,7 @@ Now we start the ROS GUI: ``` # Shell C: -. /opt/ros/galactic/setup.bash +. /opt/ros/humble/setup.bash ros2 run rqt_image_view rqt_image_view /rgbd_camera/image ``` @@ -274,9 +274,43 @@ To run the bridge node with the above configuration: ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=$WORKSPACE/ros_gz/ros_gz_bridge/test/config/full.yaml ``` +## Example 6: Using ROS namespace with the Bridge + +When spawning multiple robots inside the same ROS environment, it is convenient to use namespaces to avoid overlapping topic names. +There are three main types of namespaces: relative, global (`/`) and private (`~/`). For more information, refer to ROS documentation. +Namespaces are applied to Gazebo topic both when specified as `topic_name` as well as `gz_topic_name`. + +By default, the Bridge will not apply ROS namespace on the Gazebo topics. To enable this feature, use parameter `expand_gz_topic_names`. +Let's test our topic with namespace: + +```bash +# Shell A: +. ~/bridge_ws/install/setup.bash +ros2 run ros_gz_bridge parameter_bridge chatter@std_msgs/msg/String@ignition.msgs.StringMsg \ + --ros-args -p expand_gz_topic_names:=true -r __ns:=/demo +``` + +Now we start the Gazebo Transport listener. + +```bash +# Shell B: +ign topic -e -t /demo/chatter +``` + +Now we start the ROS talker. + +```bash +# Shell C: +. /opt/ros/humble/setup.bash +ros2 topic pub /demo/chatter std_msgs/msg/String "data: 'Hi from inside of a namespace'" --once +``` + +By changing `chatter` to `/chatter` or `~/chatter` you can obtain different results. + ## API 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. diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index f542ee90..46a86d98 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -20,6 +20,8 @@ #include "bridge_handle_ros_to_gz.hpp" #include "bridge_handle_gz_to_ros.hpp" +#include + namespace ros_gz_bridge { @@ -30,6 +32,7 @@ RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options) this->declare_parameter("subscription_heartbeat", 1000); this->declare_parameter("config_file", ""); + this->declare_parameter("expand_gz_topic_names", false); int heartbeat; this->get_parameter("subscription_heartbeat", heartbeat); @@ -43,14 +46,21 @@ void RosGzBridge::spin() if (handles_.empty()) { std::string config_file; this->get_parameter("config_file", config_file); + bool expand_names; + this->get_parameter("expand_gz_topic_names", expand_names); + const std::string ros_ns = this->get_namespace(); + const std::string ros_node_name = this->get_name(); if (!config_file.empty()) { auto entries = readFromYamlFile(config_file); - for (const auto & entry : entries) { + for (auto & entry : entries) { + if (expand_names) { + entry.gz_topic_name = rclcpp::expand_topic_or_service_name( + entry.gz_topic_name, ros_node_name, ros_ns, false); + } this->add_bridge(entry); } } } - for (auto & bridge : handles_) { bridge->Spin(); } From 409a94107912ba9f3808c791c1c7c271ae76877e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 2 Apr 2024 09:48:58 +0200 Subject: [PATCH 15/19] Added conversion for Detection3D and Detection3DArray (#523) (#526) Signed-off-by: wittenator <9154515+wittenator@users.noreply.github.com> Co-authored-by: wittenator <9154515+wittenator@users.noreply.github.com> --- ros_gz_bridge/README.md | 118 +++++++++--------- .../ros_gz_bridge/convert/vision_msgs.hpp | 26 ++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 2 + ros_gz_bridge/src/convert/vision_msgs.cpp | 85 +++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 88 +++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 17 +++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 72 +++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 17 +++ 8 files changed, 367 insertions(+), 58 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 19b8a77d..242d92b2 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,64 +5,66 @@ 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 | -| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance | -| 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 | +| 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 | +| 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/vision_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp index f370144f..b1556f65 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/vision_msgs.hpp @@ -17,9 +17,11 @@ // Gazebo Msgs #include +#include // ROS 2 messages #include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" #include namespace ros_gz_bridge @@ -47,6 +49,30 @@ void convert_gz_to_ros( const gz::msgs::AnnotatedAxisAligned2DBox_V & gz_msg, vision_msgs::msg::Detection2DArray & ros_msg); + +template<> +void +convert_ros_to_gz( + const vision_msgs::msg::Detection3D & ros_msg, + gz::msgs::AnnotatedOriented3DBox & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::AnnotatedOriented3DBox & gz_msg, + vision_msgs::msg::Detection3D & ros_msg); + +template<> +void +convert_ros_to_gz( + const vision_msgs::msg::Detection3DArray & ros_msg, + gz::msgs::AnnotatedOriented3DBox_V & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::AnnotatedOriented3DBox_V & gz_msg, + vision_msgs::msg::Detection3DArray & ros_msg); } // namespace ros_gz_bridge #endif // ROS_GZ_BRIDGE__CONVERT__VISION_MSGS_HPP_ diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 04137082..ddec0f68 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -106,6 +106,8 @@ 'vision_msgs': [ Mapping('Detection2DArray', 'AnnotatedAxisAligned2DBox_V'), Mapping('Detection2D', 'AnnotatedAxisAligned2DBox'), + Mapping('Detection3DArray', 'AnnotatedOriented3DBox_V'), + Mapping('Detection3D', 'AnnotatedOriented3DBox'), ], } diff --git a/ros_gz_bridge/src/convert/vision_msgs.cpp b/ros_gz_bridge/src/convert/vision_msgs.cpp index 057426c7..61924af5 100644 --- a/ros_gz_bridge/src/convert/vision_msgs.cpp +++ b/ros_gz_bridge/src/convert/vision_msgs.cpp @@ -94,4 +94,89 @@ convert_gz_to_ros( } } +template<> +void +convert_ros_to_gz( + const vision_msgs::msg::Detection3D & ros_msg, + gz::msgs::AnnotatedOriented3DBox & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + + gz::msgs::Oriented3DBox * box = new gz::msgs::Oriented3DBox(); + gz::msgs::Vector3d * center = new gz::msgs::Vector3d(); + gz::msgs::Vector3d * box_size = new gz::msgs::Vector3d(); + gz::msgs::Quaternion * orientation = new gz::msgs::Quaternion(); + + if (ros_msg.results.size() != 0) { + auto id = ros_msg.results.at(0).hypothesis.class_id; + gz_msg.set_label(std::stoi(id)); + } + + center->set_x(ros_msg.bbox.center.position.x); + center->set_y(ros_msg.bbox.center.position.y); + center->set_z(ros_msg.bbox.center.position.z); + box_size->set_x(ros_msg.bbox.size.x); + box_size->set_y(ros_msg.bbox.size.y); + box_size->set_z(ros_msg.bbox.size.z); + orientation->set_x(ros_msg.bbox.center.orientation.x); + orientation->set_y(ros_msg.bbox.center.orientation.y); + orientation->set_z(ros_msg.bbox.center.orientation.z); + orientation->set_w(ros_msg.bbox.center.orientation.w); + box->set_allocated_center(center); + box->set_allocated_boxsize(box_size); + box->set_allocated_orientation(orientation); + gz_msg.set_allocated_box(box); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::AnnotatedOriented3DBox & gz_msg, + vision_msgs::msg::Detection3D & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + + ros_msg.results.resize(1); + ros_msg.results.at(0).hypothesis.class_id = std::to_string(gz_msg.label()); + ros_msg.results.at(0).hypothesis.score = 1.0; + + ros_msg.bbox.center.position.x = gz_msg.box().center().x(); + ros_msg.bbox.center.position.y = gz_msg.box().center().y(); + ros_msg.bbox.center.position.z = gz_msg.box().center().z(); + ros_msg.bbox.size.x = gz_msg.box().boxsize().x(); + ros_msg.bbox.size.y = gz_msg.box().boxsize().y(); + ros_msg.bbox.size.z = gz_msg.box().boxsize().z(); + ros_msg.bbox.center.orientation.x = gz_msg.box().orientation().x(); + ros_msg.bbox.center.orientation.y = gz_msg.box().orientation().y(); + ros_msg.bbox.center.orientation.z = gz_msg.box().orientation().z(); + ros_msg.bbox.center.orientation.w = gz_msg.box().orientation().w(); +} + +template<> +void +convert_ros_to_gz( + const vision_msgs::msg::Detection3DArray & ros_msg, + gz::msgs::AnnotatedOriented3DBox_V & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + for (const auto & ros_box : ros_msg.detections) { + gz::msgs::AnnotatedOriented3DBox * gz_box = gz_msg.add_annotated_box(); + convert_ros_to_gz(ros_box, *gz_box); + } +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::AnnotatedOriented3DBox_V & gz_msg, + vision_msgs::msg::Detection3DArray & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + for (const auto & gz_box : gz_msg.annotated_box()) { + vision_msgs::msg::Detection3D ros_box; + convert_gz_to_ros(gz_box, ros_box); + ros_msg.detections.push_back(ros_box); + } +} + } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index b64c0f40..71a651b3 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -18,6 +18,7 @@ #include #include +#include #if GZ_MSGS_MAJOR_VERSION >= 10 #define GZ_MSGS_IMU_HAS_COVARIANCE @@ -1591,5 +1592,92 @@ void compareTestMsg(const std::shared_ptr } } +void createTestMsg(gz::msgs::AnnotatedOriented3DBox & _msg) +{ + gz::msgs::Header header_msg; + + createTestMsg(header_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + + _msg.set_label(1); + + gz::msgs::Oriented3DBox * box = new gz::msgs::Oriented3DBox(); + gz::msgs::Vector3d * center = new gz::msgs::Vector3d(); + gz::msgs::Vector3d * size = new gz::msgs::Vector3d(); + gz::msgs::Quaternion * orientation = new gz::msgs::Quaternion(); + + center->set_x(2.0); + center->set_y(4.0); + center->set_z(6.0); + size->set_x(3.0); + size->set_y(5.0); + size->set_z(7.0); + orientation->set_x(0.733); + orientation->set_y(0.462); + orientation->set_z(0.191); + orientation->set_w(0.462); + + box->set_allocated_center(center); + box->set_allocated_boxsize(size); + box->set_allocated_orientation(orientation); + _msg.set_allocated_box(box); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::AnnotatedOriented3DBox expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + + EXPECT_EQ(expected_msg.label(), _msg->label()); + + gz::msgs::Oriented3DBox box = _msg->box(); + gz::msgs::Vector3d center = box.center(); + gz::msgs::Vector3d size = box.boxsize(); + gz::msgs::Quaternion orientation = box.orientation(); + + EXPECT_EQ(expected_msg.box().center().x(), center.x()); + EXPECT_EQ(expected_msg.box().center().y(), center.y()); + EXPECT_EQ(expected_msg.box().center().z(), center.z()); + EXPECT_EQ(expected_msg.box().boxsize().x(), size.x()); + EXPECT_EQ(expected_msg.box().boxsize().y(), size.y()); + EXPECT_EQ(expected_msg.box().boxsize().z(), size.z()); + EXPECT_EQ(expected_msg.box().orientation().w(), orientation.w()); + EXPECT_EQ(expected_msg.box().orientation().x(), orientation.x()); + EXPECT_EQ(expected_msg.box().orientation().y(), orientation.y()); + EXPECT_EQ(expected_msg.box().orientation().z(), orientation.z()); +} + +void createTestMsg(gz::msgs::AnnotatedOriented3DBox_V & _msg) +{ + gz::msgs::Header header_msg; + + createTestMsg(header_msg); + + _msg.mutable_header()->CopyFrom(header_msg); + + for (size_t i = 0; i < 4; i++) { + gz::msgs::AnnotatedOriented3DBox * box = _msg.add_annotated_box(); + createTestMsg(*box); + } +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::AnnotatedOriented3DBox_V expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + + for (size_t i = 0; i < 4; i++) { + compareTestMsg( + std::make_shared( + _msg->annotated_box( + i))); + } +} + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index d939a4ff..ad063b62 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -65,6 +65,7 @@ #include #include #include +#include #include @@ -525,6 +526,22 @@ void createTestMsg(gz::msgs::AnnotatedAxisAligned2DBox_V & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::AnnotatedOriented3DBox & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::AnnotatedOriented3DBox_V & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index f80b8553..c6f085e2 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -18,6 +18,7 @@ #include #include +#include #include "gz/msgs/config.hh" @@ -1476,5 +1477,76 @@ void compareTestMsg(const std::shared_ptr & } } +void createTestMsg(vision_msgs::msg::Detection3D & _msg) +{ + std_msgs::msg::Header header_msg; + createTestMsg(header_msg); + _msg.header = header_msg; + + vision_msgs::msg::ObjectHypothesisWithPose class_prob; + class_prob.hypothesis.class_id = "1"; + class_prob.hypothesis.score = 1.0; + _msg.results.push_back(class_prob); + + _msg.bbox.size.x = 3.0; + _msg.bbox.size.y = 5.0; + _msg.bbox.size.z = 7.0; + _msg.bbox.center.position.x = 2.0; + _msg.bbox.center.position.y = 4.0; + _msg.bbox.center.position.z = 6.0; + _msg.bbox.center.orientation.x = 0.733; + _msg.bbox.center.orientation.y = 0.462; + _msg.bbox.center.orientation.z = 0.191; + _msg.bbox.center.orientation.w = 0.462; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + vision_msgs::msg::Detection3D expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + EXPECT_EQ(expected_msg.results.size(), _msg->results.size()); + for (size_t i = 0; i < _msg->results.size(); i++) { + EXPECT_EQ(expected_msg.results[i].hypothesis.class_id, _msg->results[i].hypothesis.class_id); + EXPECT_EQ(expected_msg.results[i].hypothesis.score, _msg->results[i].hypothesis.score); + } + EXPECT_EQ(expected_msg.bbox.size.x, _msg->bbox.size.x); + EXPECT_EQ(expected_msg.bbox.size.y, _msg->bbox.size.y); + EXPECT_EQ(expected_msg.bbox.size.z, _msg->bbox.size.z); + EXPECT_EQ(expected_msg.bbox.center.position.x, _msg->bbox.center.position.x); + EXPECT_EQ(expected_msg.bbox.center.position.y, _msg->bbox.center.position.y); + EXPECT_EQ(expected_msg.bbox.center.position.z, _msg->bbox.center.position.z); + EXPECT_EQ(expected_msg.bbox.center.orientation.x, _msg->bbox.center.orientation.x); + EXPECT_EQ(expected_msg.bbox.center.orientation.y, _msg->bbox.center.orientation.y); + EXPECT_EQ(expected_msg.bbox.center.orientation.z, _msg->bbox.center.orientation.z); + EXPECT_EQ(expected_msg.bbox.center.orientation.w, _msg->bbox.center.orientation.w); +} + +void createTestMsg(vision_msgs::msg::Detection3DArray & _msg) +{ + std_msgs::msg::Header header_msg; + createTestMsg(header_msg); + _msg.header = header_msg; + + for (size_t i = 0; i < 4; i++) { + vision_msgs::msg::Detection3D detection; + createTestMsg(detection); + _msg.detections.push_back(detection); + } +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + vision_msgs::msg::Detection3DArray expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + EXPECT_EQ(expected_msg.detections.size(), _msg->detections.size()); + for (size_t i = 0; i < _msg->detections.size(); i++) { + compareTestMsg(std::make_shared(_msg->detections[i])); + } +} + } // namespace testing } // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index ee4426fd..5a39c4ab 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -82,6 +82,7 @@ #include #include #include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" namespace ros_gz_bridge { @@ -620,6 +621,22 @@ void createTestMsg(vision_msgs::msg::Detection2D & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(vision_msgs::msg::Detection3DArray & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(vision_msgs::msg::Detection3D & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + } // namespace testing } // namespace ros_gz_bridge From c9a5a736bde407f9911822097972fc0a8a0e9ad4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 2 Apr 2024 15:59:29 +0200 Subject: [PATCH 16/19] Clean humble Ci (#527) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .github/workflows/ros2-ci.yml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/.github/workflows/ros2-ci.yml b/.github/workflows/ros2-ci.yml index fb806edb..1977346d 100644 --- a/.github/workflows/ros2-ci.yml +++ b/.github/workflows/ros2-ci.yml @@ -19,15 +19,6 @@ jobs: - docker-image: "ubuntu:22.04" gz-version: "harmonic" ros-distro: "humble" - - docker-image: "ubuntu:22.04" - gz-version: "fortress" - ros-distro: "rolling" - - docker-image: "ubuntu:22.04" - gz-version: "garden" - ros-distro: "rolling" - - docker-image: "ubuntu:22.04" - gz-version: "harmonic" - ros-distro: "rolling" container: image: ${{ matrix.docker-image }} steps: From 58b6cbfa04e3afc98b42d3e21ade9140cb4a01bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 8 Apr 2024 20:30:40 +0200 Subject: [PATCH 17/19] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- ros_gz/CHANGELOG.rst | 3 +++ ros_gz_bridge/CHANGELOG.rst | 19 +++++++++++++++++++ ros_gz_image/CHANGELOG.rst | 3 +++ ros_gz_interfaces/CHANGELOG.rst | 12 ++++++++++++ ros_gz_sim/CHANGELOG.rst | 16 ++++++++++++++++ ros_gz_sim_demos/CHANGELOG.rst | 3 +++ ros_ign/CHANGELOG.rst | 3 +++ ros_ign_bridge/CHANGELOG.rst | 3 +++ ros_ign_gazebo/CHANGELOG.rst | 3 +++ ros_ign_gazebo_demos/CHANGELOG.rst | 3 +++ ros_ign_image/CHANGELOG.rst | 3 +++ ros_ign_interfaces/CHANGELOG.rst | 3 +++ test_ros_gz_bridge/package.xml | 2 +- 13 files changed, 75 insertions(+), 1 deletion(-) diff --git a/ros_gz/CHANGELOG.rst b/ros_gz/CHANGELOG.rst index b305581d..fb245cca 100644 --- a/ros_gz/CHANGELOG.rst +++ b/ros_gz/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_gz ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_gz_bridge/CHANGELOG.rst b/ros_gz_bridge/CHANGELOG.rst index afd5bb04..412842e4 100644 --- a/ros_gz_bridge/CHANGELOG.rst +++ b/ros_gz_bridge/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package ros_gz_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- +* Added conversion for Detection3D and Detection3DArray (`#523 `_) (`#526 `_) + Co-authored-by: wittenator <9154515+wittenator@users.noreply.github.com> +* Add ROS namespaces to GZ topics (`#512 `_) + Co-authored-by: Alejandro Hernández Cordero +* Correctly export ros_gz_bridge for downstream targets (`#503 `_) (`#506 `_) +* Add a virtual destructor to suppress compiler warning (`#502 `_) (`#505 `_) + Co-authored-by: Michael Carroll +* Add option to change material color from ROS. (`#486 `_) + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + --------- + Co-authored-by: Alejandro Hernández Cordero + Co-authored-by: Addisu Z. Taddese + Co-authored-by: Addisu Z. Taddese +* Contributors: Alejandro Hernández Cordero, Benjamin Perseghetti, Krzysztof Wojciechowski, Michael Carroll + 0.244.13 (2024-01-23) --------------------- * backport pr 374 (`#489 `_) diff --git a/ros_gz_image/CHANGELOG.rst b/ros_gz_image/CHANGELOG.rst index 328917c9..1e68461e 100644 --- a/ros_gz_image/CHANGELOG.rst +++ b/ros_gz_image/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_gz_interfaces/CHANGELOG.rst b/ros_gz_interfaces/CHANGELOG.rst index abe9c879..dd1169aa 100644 --- a/ros_gz_interfaces/CHANGELOG.rst +++ b/ros_gz_interfaces/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ros_gz_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- +* Add option to change material color from ROS. (`#486 `_) + * Message and bridge for MaterialColor. + This allows bridging MaterialColor from ROS to GZ and is + important for allowing simulation users to create status lights. + --------- + Co-authored-by: Alejandro Hernández Cordero + Co-authored-by: Addisu Z. Taddese + Co-authored-by: Addisu Z. Taddese +* Contributors: Benjamin Perseghetti + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_gz_sim/CHANGELOG.rst b/ros_gz_sim/CHANGELOG.rst index 4a13427e..3eb837b3 100644 --- a/ros_gz_sim/CHANGELOG.rst +++ b/ros_gz_sim/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package ros_gz_sim ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- +* Support `` in `package.xml` exports (`#492 `_) + This copies the implementation from `gazebo_ros_paths.py` to provide a + way for packages to set resource paths from `package.xml`. + ``` + e.g. + + + + ``` + The value of `gazebo_model_path` and `gazebo_media_path` is appended to `GZ_SIM_RESOURCE_PATH` + The value of `plugin_path` appended to `GZ_SIM_SYSTEM_PLUGIN_PATH` + --------- +* Contributors: Addisu Z. Taddese + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_gz_sim_demos/CHANGELOG.rst b/ros_gz_sim_demos/CHANGELOG.rst index 377c19d5..47ef9ea9 100644 --- a/ros_gz_sim_demos/CHANGELOG.rst +++ b/ros_gz_sim_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros1_gz_sim_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign/CHANGELOG.rst b/ros_ign/CHANGELOG.rst index ce4c4646..d2bbe37c 100644 --- a/ros_ign/CHANGELOG.rst +++ b/ros_ign/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign_bridge/CHANGELOG.rst b/ros_ign_bridge/CHANGELOG.rst index 17b74fd3..d685f19f 100644 --- a/ros_ign_bridge/CHANGELOG.rst +++ b/ros_ign_bridge/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign_gazebo/CHANGELOG.rst b/ros_ign_gazebo/CHANGELOG.rst index 2f10998c..b01effba 100644 --- a/ros_ign_gazebo/CHANGELOG.rst +++ b/ros_ign_gazebo/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign_gazebo_demos/CHANGELOG.rst b/ros_ign_gazebo_demos/CHANGELOG.rst index 7edbe335..c2e740eb 100644 --- a/ros_ign_gazebo_demos/CHANGELOG.rst +++ b/ros_ign_gazebo_demos/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_gazebo_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign_image/CHANGELOG.rst b/ros_ign_image/CHANGELOG.rst index ebc65c75..2144f039 100644 --- a/ros_ign_image/CHANGELOG.rst +++ b/ros_ign_image/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_image ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/ros_ign_interfaces/CHANGELOG.rst b/ros_ign_interfaces/CHANGELOG.rst index 4a008fd4..281974d5 100644 --- a/ros_ign_interfaces/CHANGELOG.rst +++ b/ros_ign_interfaces/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros_ign_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.244.14 (2024-04-08) +--------------------- + 0.244.13 (2024-01-23) --------------------- diff --git a/test_ros_gz_bridge/package.xml b/test_ros_gz_bridge/package.xml index 124b8044..7e9eba76 100644 --- a/test_ros_gz_bridge/package.xml +++ b/test_ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ test_ros_gz_bridge - 0.246.0 + 0.244.13 Bridge communication between ROS and Gazebo Transport Aditya Pande Alejandro Hernandez From 2699e3fa574c21271993912bab70aafa73247f99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 8 Apr 2024 20:31:04 +0200 Subject: [PATCH 18/19] 0.244.14 --- ros_gz/package.xml | 2 +- ros_gz_bridge/package.xml | 2 +- ros_gz_image/package.xml | 2 +- ros_gz_interfaces/package.xml | 2 +- ros_gz_sim/package.xml | 2 +- ros_gz_sim_demos/package.xml | 2 +- ros_ign/package.xml | 2 +- ros_ign_bridge/package.xml | 2 +- ros_ign_gazebo/package.xml | 2 +- ros_ign_gazebo_demos/package.xml | 2 +- ros_ign_image/package.xml | 2 +- ros_ign_interfaces/package.xml | 2 +- test_ros_gz_bridge/package.xml | 2 +- 13 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ros_gz/package.xml b/ros_gz/package.xml index 1d2a50af..91c75be0 100644 --- a/ros_gz/package.xml +++ b/ros_gz/package.xml @@ -4,7 +4,7 @@ ros_gz - 0.244.13 + 0.244.14 Meta-package containing interfaces for using ROS 2 with Gazebo simulation. Louise Poubel Apache 2.0 diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index 704b7fd4..811955d2 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ ros_gz_bridge - 0.244.13 + 0.244.14 Bridge communication between ROS and Gazebo Transport Louise Poubel diff --git a/ros_gz_image/package.xml b/ros_gz_image/package.xml index e524cafd..66e761ea 100644 --- a/ros_gz_image/package.xml +++ b/ros_gz_image/package.xml @@ -1,6 +1,6 @@ ros_gz_image - 0.244.13 + 0.244.14 Image utilities for Gazebo simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_gz_interfaces/package.xml b/ros_gz_interfaces/package.xml index c6cdb2a5..1cfe9c27 100644 --- a/ros_gz_interfaces/package.xml +++ b/ros_gz_interfaces/package.xml @@ -1,6 +1,6 @@ ros_gz_interfaces - 0.244.13 + 0.244.14 Message and service data structures for interacting with Gazebo from ROS2. Apache 2.0 Louise Poubel diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 181c3199..2259a64b 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -2,7 +2,7 @@ ros_gz_sim - 0.244.13 + 0.244.14 Tools for using Gazebo Sim simulation with ROS. Alejandro Hernandez diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index 15bf28dc..d25ec95b 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -1,6 +1,6 @@ ros_gz_sim_demos - 0.244.13 + 0.244.14 Demos using Gazebo Sim simulation with ROS. Apache 2.0 Louise Poubel diff --git a/ros_ign/package.xml b/ros_ign/package.xml index 2e293389..48ad0ec1 100644 --- a/ros_ign/package.xml +++ b/ros_ign/package.xml @@ -2,7 +2,7 @@ ros_ign - 0.244.13 + 0.244.14 Shim meta-package to redirect to ros_gz. Brandon Ong Apache 2.0 diff --git a/ros_ign_bridge/package.xml b/ros_ign_bridge/package.xml index ccf61c4d..49db5fd9 100644 --- a/ros_ign_bridge/package.xml +++ b/ros_ign_bridge/package.xml @@ -2,7 +2,7 @@ ros_ign_bridge - 0.244.13 + 0.244.14 Shim package to redirect to ros_gz_bridge. Brandon Ong diff --git a/ros_ign_gazebo/package.xml b/ros_ign_gazebo/package.xml index 9568dec5..ff61e96a 100644 --- a/ros_ign_gazebo/package.xml +++ b/ros_ign_gazebo/package.xml @@ -2,7 +2,7 @@ ros_ign_gazebo - 0.244.13 + 0.244.14 Shim package to redirect to ros_gz_sim. Brandon Ong diff --git a/ros_ign_gazebo_demos/package.xml b/ros_ign_gazebo_demos/package.xml index 77bb5ad7..70951d35 100644 --- a/ros_ign_gazebo_demos/package.xml +++ b/ros_ign_gazebo_demos/package.xml @@ -1,6 +1,6 @@ ros_ign_gazebo_demos - 0.244.13 + 0.244.14 Shim package to redirect to ros_gz_sim_demos. Apache 2.0 Brandon Ong diff --git a/ros_ign_image/package.xml b/ros_ign_image/package.xml index 2b69c8d5..236a2a6a 100644 --- a/ros_ign_image/package.xml +++ b/ros_ign_image/package.xml @@ -2,7 +2,7 @@ ros_ign_image - 0.244.13 + 0.244.14 Shim package to redirect to ros_gz_image. Brandon Ong diff --git a/ros_ign_interfaces/package.xml b/ros_ign_interfaces/package.xml index 61d040a1..dadfc9ba 100644 --- a/ros_ign_interfaces/package.xml +++ b/ros_ign_interfaces/package.xml @@ -1,6 +1,6 @@ ros_ign_interfaces - 0.244.13 + 0.244.14 Shim package to redirect to ros_gz_interfaces. Apache 2.0 Brandon Ong diff --git a/test_ros_gz_bridge/package.xml b/test_ros_gz_bridge/package.xml index 7e9eba76..3fb8361f 100644 --- a/test_ros_gz_bridge/package.xml +++ b/test_ros_gz_bridge/package.xml @@ -2,7 +2,7 @@ test_ros_gz_bridge - 0.244.13 + 0.244.14 Bridge communication between ROS and Gazebo Transport Aditya Pande Alejandro Hernandez From a348c07527c54f387a576413a3e3d6d7431ff45b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 16 Apr 2024 18:35:29 +0200 Subject: [PATCH 19/19] [backport Humble] Create bridge for GPSFix msg (#316) (#538) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Vincent Rousseau Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Rousseau Vincent --- ros_gz_bridge/CMakeLists.txt | 1 + ros_gz_bridge/README.md | 1 + .../include/ros_gz_bridge/convert.hpp | 1 + .../ros_gz_bridge/convert/gps_msgs.hpp | 41 ++++++++++++ ros_gz_bridge/package.xml | 1 + ros_gz_bridge/ros_gz_bridge/mappings.py | 3 + ros_gz_bridge/src/convert/gps_msgs.cpp | 63 +++++++++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 32 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 11 ++++ .../launch/navsat_gpsfix.launch.py | 63 +++++++++++++++++++ 10 files changed, 217 insertions(+) create mode 100644 ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp create mode 100644 ros_gz_bridge/src/convert/gps_msgs.cpp create mode 100644 ros_gz_sim_demos/launch/navsat_gpsfix.launch.py diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 7df2399b..8f4cc27d 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -71,6 +71,7 @@ set(BRIDGE_MESSAGE_TYPES builtin_interfaces actuator_msgs geometry_msgs + gps_msgs nav_msgs rcl_interfaces ros_gz_interfaces diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 242d92b2..f89837c2 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -32,6 +32,7 @@ The following message types can be bridged for topics: | 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 | diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert.hpp index 98ead1eb..e25a82c8 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp new file mode 100644 index 00000000..d6cf194e --- /dev/null +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/gps_msgs.hpp @@ -0,0 +1,41 @@ +// 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. +// 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 ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ +#define ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ + +// Gazebo Msgs +#include + +// ROS 2 messages +#include + +#include + +namespace ros_gz_bridge +{ +template<> +void +convert_ros_to_gz( + const gps_msgs::msg::GPSFix & ros_msg, + gz::msgs::NavSat & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::NavSat & gz_msg, + gps_msgs::msg::GPSFix & ros_msg); +} // namespace ros_gz_bridge + +#endif // ROS_GZ_BRIDGE__CONVERT__GPS_MSGS_HPP_ diff --git a/ros_gz_bridge/package.xml b/ros_gz_bridge/package.xml index 811955d2..440d7c87 100644 --- a/ros_gz_bridge/package.xml +++ b/ros_gz_bridge/package.xml @@ -15,6 +15,7 @@ actuator_msgs geometry_msgs + gps_msgs nav_msgs rclcpp rclcpp_components diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index ddec0f68..d17fb56d 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -47,6 +47,9 @@ Mapping('WrenchStamped', 'Wrench'), Mapping('Vector3', 'Vector3d'), ], + 'gps_msgs': [ + Mapping('GPSFix', 'NavSat'), + ], 'nav_msgs': [ Mapping('Odometry', 'Odometry'), Mapping('Odometry', 'OdometryWithCovariance'), diff --git a/ros_gz_bridge/src/convert/gps_msgs.cpp b/ros_gz_bridge/src/convert/gps_msgs.cpp new file mode 100644 index 00000000..de806239 --- /dev/null +++ b/ros_gz_bridge/src/convert/gps_msgs.cpp @@ -0,0 +1,63 @@ +// 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. +// 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. + +#include + +#include "convert/utils.hpp" +#include "ros_gz_bridge/convert/gps_msgs.hpp" + +namespace ros_gz_bridge +{ + +template<> +void +convert_ros_to_gz( + const gps_msgs::msg::GPSFix & ros_msg, + gz::msgs::NavSat & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + gz_msg.set_latitude_deg(ros_msg.latitude); + gz_msg.set_longitude_deg(ros_msg.longitude); + gz_msg.set_altitude(ros_msg.altitude); + gz_msg.set_frame_id(ros_msg.header.frame_id); + + gz_msg.set_velocity_east(cos(ros_msg.track) * ros_msg.speed); + gz_msg.set_velocity_north(sin(ros_msg.track) * ros_msg.speed); + gz_msg.set_velocity_up(ros_msg.climb); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::NavSat & gz_msg, + gps_msgs::msg::GPSFix & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + ros_msg.header.frame_id = frame_id_gz_to_ros(gz_msg.frame_id()); + ros_msg.latitude = gz_msg.latitude_deg(); + ros_msg.longitude = gz_msg.longitude_deg(); + ros_msg.altitude = gz_msg.altitude(); + + ros_msg.speed = sqrt( + gz_msg.velocity_east() * gz_msg.velocity_east() + + gz_msg.velocity_north() * gz_msg.velocity_north()); + ros_msg.track = atan2(gz_msg.velocity_north(), gz_msg.velocity_east()); + ros_msg.climb = gz_msg.velocity_up(); + + // position_covariance is not supported in Ignition::Msgs::NavSat. + ros_msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN; + ros_msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; +} + +} // namespace ros_gz_bridge diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index c6f085e2..76e80d29 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -521,6 +521,38 @@ void compareTestMsg(const std::shared_ptr & _ compareTestMsg(std::make_shared(_msg->wrench.force)); compareTestMsg(std::make_shared(_msg->wrench.torque)); } + +void createTestMsg(gps_msgs::msg::GPSFix & _msg) +{ + std_msgs::msg::Header header_msg; + createTestMsg(header_msg); + + _msg.header = header_msg; + _msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; + _msg.latitude = 0.00; + _msg.longitude = 0.00; + _msg.altitude = 0.00; + _msg.position_covariance = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + _msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gps_msgs::msg::GPSFix expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + EXPECT_EQ(expected_msg.status, _msg->status); + EXPECT_FLOAT_EQ(expected_msg.latitude, _msg->latitude); + EXPECT_FLOAT_EQ(expected_msg.longitude, _msg->longitude); + EXPECT_FLOAT_EQ(expected_msg.altitude, _msg->altitude); + EXPECT_EQ(expected_msg.position_covariance_type, _msg->position_covariance_type); + + for (auto i = 0u; i < 9; ++i) { + EXPECT_FLOAT_EQ(0, _msg->position_covariance[i]); + } +} + void createTestMsg(ros_gz_interfaces::msg::Light & _msg) { createTestMsg(_msg.header); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 5a39c4ab..6bc395cb 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -343,6 +344,16 @@ void createTestMsg(geometry_msgs::msg::WrenchStamped & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// gps_msgs + +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gps_msgs::msg::GPSFix & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// tf2_msgs /// \brief Create a message used for testing. diff --git a/ros_gz_sim_demos/launch/navsat_gpsfix.launch.py b/ros_gz_sim_demos/launch/navsat_gpsfix.launch.py new file mode 100644 index 00000000..83adcdcc --- /dev/null +++ b/ros_gz_sim_demos/launch/navsat_gpsfix.launch.py @@ -0,0 +1,63 @@ +# 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. +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), + launch_arguments={ + 'gz_args': '-v 4 -r spherical_coordinates.sdf' + }.items(), + ) + + # RQt + rqt = Node( + package='rqt_topic', + executable='rqt_topic', + arguments=['-t'], + condition=IfCondition(LaunchConfiguration('rqt')) + ) + + # Bridge + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/navsat@gps_msgs/msg/GPSFix@gz.msgs.NavSat'], + output='screen' + ) + + return LaunchDescription([ + gz_sim, + DeclareLaunchArgument('rqt', default_value='true', + description='Open RQt.'), + bridge, + rqt + ])