From e79a68ebedb5fca30cd7556ec0e53abe2cff1cab Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 14 Jan 2024 12:00:13 -0500 Subject: [PATCH 01/19] 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 --- .../convert/ros_gz_interfaces.hpp | 16 ++++++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 3 ++ .../src/convert/ros_gz_interfaces.cpp | 37 +++++++++++++++++++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/msg/MaterialColor.msg | 9 +++++ 5 files changed, 66 insertions(+) create mode 100644 ros_gz_interfaces/msg/MaterialColor.msg 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..1a686aa4 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 @@ -1,4 +1,6 @@ // Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright (C) 2024 CogniPilot Foundation +// Copyright (C) 2024 Rudis Laboratories LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -159,6 +163,18 @@ convert_gz_to_ros( const gz::msgs::Light & gz_msg, ros_gz_interfaces::msg::Light & ros_msg); +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); + 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..54480000 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -1,4 +1,6 @@ # Copyright 2022 Open Source Robotics Foundation, Inc. +# Copyright (C) 2024 CogniPilot Foundation +# Copyright (C) 2024 Rudis Laboratories LLC # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -62,6 +64,7 @@ Mapping('GuiCamera', 'GUICamera'), Mapping('JointWrench', 'JointWrench'), Mapping('Light', 'Light'), + Mapping('MaterialColor', 'MaterialColor'), Mapping('ParamVec', 'Param'), Mapping('ParamVec', 'Param_V'), Mapping('SensorNoise', 'SensorNoise'), diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index fceb296a..ed85020b 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -1,4 +1,6 @@ // Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright (C) 2024 CogniPilot Foundation +// Copyright (C) 2024 Rudis Laboratories LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -380,6 +382,41 @@ convert_gz_to_ros( ros_msg.intensity = gz_msg.intensity(); } +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + + gz_msg.set_name(ros_msg.name); + + 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_parent_name(ros_msg.parent_name); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg) +{ + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + + ros_msg.name = gz_msg.name(); + 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.parent_name = gz_msg.parent_name(); +} + template<> void convert_ros_to_gz( 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..d793951c --- /dev/null +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -0,0 +1,9 @@ +std_msgs/Header header # Optional header data + +string name # name +string parent_name # parent name + +std_msgs/ColorRGBA ambient # ambient +std_msgs/ColorRGBA diffuse # diffuse +std_msgs/ColorRGBA specular # specular +std_msgs/ColorRGBA emissive # emissive From 7073075795489cdab16902d3fca8f553a75dacab Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 15 Jan 2024 10:49:13 -0500 Subject: [PATCH 02/19] Update ros_gz_interfaces/msg/MaterialColor.msg MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Benjamin Perseghetti --- ros_gz_interfaces/msg/MaterialColor.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg index d793951c..d3d38d32 100644 --- a/ros_gz_interfaces/msg/MaterialColor.msg +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -1,6 +1,6 @@ std_msgs/Header header # Optional header data -string name # name +string name # name string parent_name # parent name std_msgs/ColorRGBA ambient # ambient From 6c708b0362e0d1d67c87620ba48a82f476402deb Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 15 Jan 2024 22:31:51 -0500 Subject: [PATCH 03/19] Test for MaterialColor message. Test for the MaterialColor message ROS<->GZ. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/test/utils/gz_test_msg.cpp | 39 +++++++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 9 ++++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 29 +++++++++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 ++++++ 4 files changed, 86 insertions(+) diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 505ed922..d4a14de7 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1325,6 +1325,45 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity()); } +void createTestMsg(gz::msgs::MaterialColor & _msg) +{ + gz::msgs::Header header_msg; + gz::msgs::Color ambient_msg; + gz::msgs::Color diffuse_msg; + gz::msgs::Color specular_msg; + gz::msgs::Color emissive_msg; + + createTestMsg(header_msg); + createTestMsg(ambient_msg); + createTestMsg(diffuse_msg); + createTestMsg(specular_msg); + createTestMsg(emissive_msg); + + _msg.set_name("test_material_color_name"); + _msg.set_parent_name("test_material_color_parent_name"); + + _msg.mutable_header()->CopyFrom(header_msg); + _msg.mutable_specular()->CopyFrom(ambient_msg); + _msg.mutable_diffuse()->CopyFrom(diffuse_msg); + _msg.mutable_specular()->CopyFrom(specular_msg); + _msg.mutable_specular()->CopyFrom(emissive_msg); +} + +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->ambient())); + compareTestMsg(std::make_shared(_msg->diffuse())); + compareTestMsg(std::make_shared(_msg->specular())); + compareTestMsg(std::make_shared(_msg->emissive())); + + EXPECT_EQ(expected_msg.name(), _msg->name()); + EXPECT_EQ(expected_msg.parent_name(), _msg->parent_name()); +} + 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..83740504 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -455,6 +456,14 @@ void createTestMsg(gz::msgs::Light & _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::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); + /// \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 6c30e98b..14427694 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -559,6 +559,35 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity); } +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) +{ + createTestMsg(_msg.header); + + _msg.name = "test_material_color_name"; + _msg.parent_name = "test_material_color_parent_name"; + + createTestMsg(_msg.ambient); + createTestMsg(_msg.diffuse); + createTestMsg(_msg.specular); + createTestMsg(_msg.emissive); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + + EXPECT_EQ(expected_msg.name, _msg->name); + EXPECT_EQ(expected_msg.parent_name, _msg->parent_name); + + compareTestMsg(std::make_shared(_msg->ambient)); + compareTestMsg(std::make_shared(_msg->diffuse)); + compareTestMsg(std::make_shared(_msg->specular)); + compareTestMsg(std::make_shared(_msg->emissive)); +} + 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 d1e6caf2..fc5a5f97 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -56,6 +56,7 @@ #include #endif // HAVE_DATAFRAME #include +#include #include #include #include @@ -376,6 +377,14 @@ void createTestMsg(ros_gz_interfaces::msg::Light & _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(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); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); From 7b4fc7db6e215a7223bb7bbf0664823d7009f1d5 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 16 Jan 2024 00:30:07 -0500 Subject: [PATCH 04/19] Update README(s). Signed-off-by: Benjamin Perseghetti --- README.md | 2 +- ros_gz_bridge/README.md | 156 ++++++++++++++++++++-------------------- 2 files changed, 80 insertions(+), 78 deletions(-) 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/README.md b/ros_gz_bridge/README.md index e9a062ce..b88c7b18 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -7,67 +7,69 @@ 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 | +| builtin_interfaces/msg/Time | gz::msgs::Time | +| actuator_msgs/msg/Actuators | gz::msgs::Actuators | +| std_msgs/msg/Bool | gz::msgs::Boolean | +| std_msgs/msg/ColorRGBA | gz::msgs::Color | +| std_msgs/msg/Empty | gz::msgs::Empty | +| std_msgs/msg/Float32 | gz::msgs::Float | +| std_msgs/msg/Float64 | gz::msgs::Double | +| std_msgs/msg/Header | gz::msgs::Header | +| std_msgs/msg/Int32 | gz::msgs::Int32 | +| std_msgs/msg/UInt32 | gz::msgs::UInt32 | +| std_msgs/msg/String | gz::msgs::StringMsg | +| geometry_msgs/msg/Wrench | gz::msgs::Wrench | +| geometry_msgs/msg/WrenchStamped | gz::msgs::Wrench | +| geometry_msgs/msg/Quaternion | gz::msgs::Quaternion | +| geometry_msgs/msg/Vector3 | gz::msgs::Vector3d | +| geometry_msgs/msg/Point | gz::msgs::Vector3d | +| geometry_msgs/msg/Pose | gz::msgs::Pose | +| geometry_msgs/msg/PoseArray | gz::msgs::Pose_V | +| geometry_msgs/msg/PoseWithCovariance | gz::msgs::PoseWithCovariance | +| geometry_msgs/msg/PoseStamped | gz::msgs::Pose | +| geometry_msgs/msg/Transform | gz::msgs::Pose | +| geometry_msgs/msg/TransformStamped | gz::msgs::Pose | +| geometry_msgs/msg/Twist | gz::msgs::Twist | +| geometry_msgs/msg/TwistStamped | gz::msgs::Twist | +| geometry_msgs/msg/TwistWithCovariance| gz::msgs::TwistWithCovariance | +| nav_msgs/msg/Odometry | gz::msgs::Odometry | +| nav_msgs/msg/Odometry | gz::msgs::OdometryWithCovariance | +| rcl_interfaces/msg/ParameterValue | gz::msgs::Any | +| ros_gz_interfaces/msg/Altimeter | gz::msgs::Altimeter | +| ros_gz_interfaces/msg/Contact | gz::msgs::Contact | +| ros_gz_interfaces/msg/Contacts | gz::msgs::Contacts | +| ros_gz_interfaces/msg/Dataframe | gz::msgs::Dataframe | +| ros_gz_interfaces/msg/Entity | gz::msgs::Entity | +| ros_gz_interfaces/msg/Float32Array | gz::msgs::Float_V | +| ros_gz_interfaces/msg/GuiCamera | gz::msgs::GUICamera | +| ros_gz_interfaces/msg/JointWrench | gz::msgs::JointWrench | +| ros_gz_interfaces/msg/Light | gz::msgs::Light | +| ros_gz_interfaces/msg/MaterialColor | gz::msgs::MaterialColor | +| ros_gz_interfaces/msg/SensorNoise | gz::msgs::SensorNoise | +| ros_gz_interfaces/msg/StringVec | gz::msgs::StringMsg_V | +| ros_gz_interfaces/msg/TrackVisual | gz::msgs::TrackVisual | +| ros_gz_interfaces/msg/VideoRecord | gz::msgs::VideoRecord | +| ros_gz_interfaces/msg/WorldControl | gz::msgs::WorldControl | +| rosgraph_msgs/msg/Clock | gz::msgs::Clock | +| sensor_msgs/msg/BatteryState | gz::msgs::BatteryState | +| sensor_msgs/msg/CameraInfo | gz::msgs::CameraInfo | +| sensor_msgs/msg/FluidPressure | gz::msgs::FluidPressure | +| sensor_msgs/msg/Imu | gz::msgs::IMU | +| sensor_msgs/msg/Image | gz::msgs::Image | +| sensor_msgs/msg/JointState | gz::msgs::Model | +| sensor_msgs/msg/Joy | gz::msgs::Joy | +| sensor_msgs/msg/LaserScan | gz::msgs::LaserScan | +| sensor_msgs/msg/MagneticField | gz::msgs::Magnetometer | +| sensor_msgs/msg/NavSatFix | gz::msgs::NavSat | +| sensor_msgs/msg/PointCloud2 | gz::msgs::PointCloudPacked | +| tf2_msgs/msg/TFMessage | gz::msgs::Pose_V | +| trajectory_msgs/msg/JointTrajectory | gz::msgs::JointTrajectory | And the following for services: | ROS type | Gazebo request | Gazebo response | |--------------------------------------|:--------------------------:| --------------------- | -| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | +| ros_gz_interfaces/srv/ControlWorld | gz.msgs.WorldControl | gz.msgs.Boolean | Run `ros2 run ros_gz_bridge parameter_bridge -h` for instructions. @@ -78,7 +80,7 @@ Start the parameter bridge which will watch the specified topics. ``` # Shell A: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg ``` Now we start the ROS listener. @@ -93,7 +95,7 @@ Now we start the Gazebo Transport talker. ``` # Shell C: -ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"' +gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"' ``` ## Example 1b: ROS 2 talker and Gazebo Transport listener @@ -103,14 +105,14 @@ Start the parameter bridge which will watch the specified topics. ``` # Shell A: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg ``` Now we start the Gazebo Transport listener. ``` # Shell B: -ign topic -e -t /chatter +gz topic -e -t /chatter ``` Now we start the ROS talker. @@ -131,14 +133,14 @@ First we start Gazebo Sim (don't forget to hit play, or Gazebo Sim won't generat ``` # Shell A: -ign gazebo sensors_demo.sdf +gz gazebo sensors_demo.sdf ``` Let's see the topic where camera images are published. ``` # Shell B: -ign topic -l | grep image +gz topic -l | grep image /rgbd_camera/depth_image /rgbd_camera/image ``` @@ -148,7 +150,7 @@ Then we start the parameter bridge with the previous topic. ``` # Shell B: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image +ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image ``` Now we start the ROS GUI: @@ -189,15 +191,15 @@ On terminal B, we start a ROS 2 listener: And terminal C, publish an Gazebo message: -`ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'` +`gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'` At this point, you should see the ROS 2 listener echoing the message. Now let's try the other way around, ROS 2 -> Gazebo. -On terminal D, start an Igntion listener: +On terminal D, start an gz listener: -`ign topic -e -t /chatter` +`gz topic -e -t /chatter` And on terminal E, publish a ROS 2 message: @@ -215,7 +217,7 @@ On terminal A, start the service bridge: On terminal B, start Gazebo, it will be paused by default: -`ign gazebo shapes.sdf` +`gz gazebo shapes.sdf` On terminal C, make a ROS request to unpause simulation: @@ -237,35 +239,35 @@ bridge may be specified: # Set just topic name, applies to both - topic_name: "chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set just ROS topic name, applies to both - ros_topic_name: "chatter_ros" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set just GZ topic name, applies to both -- gz_topic_name: "chatter_ign" +- gz_topic_name: "chatter_gz" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Set each topic name explicitly - ros_topic_name: "chatter_both_ros" - gz_topic_name: "chatter_both_ign" + gz_topic_name: "chatter_both_gz" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" # Full set of configurations - ros_topic_name: "ros_chatter" - gz_topic_name: "ign_chatter" + gz_topic_name: "gz_chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" + gz_type_name: "gz.msgs.StringMsg" subscriber_queue: 5 # Default 10 publisher_queue: 6 # Default 10 lazy: true # Default "false" direction: BIDIRECTIONAL # Default "BIDIRECTIONAL" - Bridge both directions - # "GZ_TO_ROS" - Bridge Ignition topic to ROS - # "ROS_TO_GZ" - Bridge ROS topic to Ignition + # "GZ_TO_ROS" - Bridge gz topic to ROS + # "ROS_TO_GZ" - Bridge ROS topic to gz ``` To run the bridge node with the above configuration: From 4933ac2b98a54caff27f821b834fed18fbede2b9 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 16 Jan 2024 18:59:28 -0500 Subject: [PATCH 05/19] Update MaterialColor for use with Entity. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 11 ++++------- ros_gz_bridge/test/utils/gz_test_msg.cpp | 11 ++++++----- ros_gz_bridge/test/utils/ros_test_msg.cpp | 13 +++++-------- ros_gz_interfaces/msg/MaterialColor.msg | 12 ++++++------ 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index ed85020b..e608c05b 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -389,15 +389,13 @@ convert_ros_to_gz( gz::msgs::MaterialColor & gz_msg) { convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); - - gz_msg.set_name(ros_msg.name); - + 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_parent_name(ros_msg.parent_name); + gz_msg.set_shininess(ros_msg.shininess); } template<> @@ -407,14 +405,13 @@ convert_gz_to_ros( ros_gz_interfaces::msg::MaterialColor & ros_msg) { convert_gz_to_ros(gz_msg.header(), ros_msg.header); - - ros_msg.name = gz_msg.name(); + 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.parent_name = gz_msg.parent_name(); + ros_msg.shininess = gz_msg.shininess(); } template<> diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index d4a14de7..f7922cb7 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1328,25 +1328,26 @@ void compareTestMsg(const std::shared_ptr & _msg) void createTestMsg(gz::msgs::MaterialColor & _msg) { gz::msgs::Header header_msg; + gz::msgs::Entity entity_msg; gz::msgs::Color ambient_msg; gz::msgs::Color diffuse_msg; gz::msgs::Color specular_msg; gz::msgs::Color emissive_msg; createTestMsg(header_msg); + createTestMsg(entity_msg); createTestMsg(ambient_msg); createTestMsg(diffuse_msg); createTestMsg(specular_msg); createTestMsg(emissive_msg); - _msg.set_name("test_material_color_name"); - _msg.set_parent_name("test_material_color_parent_name"); - _msg.mutable_header()->CopyFrom(header_msg); + _msg.mutable_entity()->CopyFrom(entity_msg); _msg.mutable_specular()->CopyFrom(ambient_msg); _msg.mutable_diffuse()->CopyFrom(diffuse_msg); _msg.mutable_specular()->CopyFrom(specular_msg); _msg.mutable_specular()->CopyFrom(emissive_msg); + _msg.set_shininess(1.0); } void compareTestMsg(const std::shared_ptr & _msg) @@ -1355,13 +1356,13 @@ void compareTestMsg(const std::shared_ptr & _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.name(), _msg->name()); - EXPECT_EQ(expected_msg.parent_name(), _msg->parent_name()); + EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); } 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 14427694..4397824d 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -562,14 +562,12 @@ void compareTestMsg(const std::shared_ptr & _msg) void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) { createTestMsg(_msg.header); - - _msg.name = "test_material_color_name"; - _msg.parent_name = "test_material_color_parent_name"; - + createTestMsg(_msg.entity); createTestMsg(_msg.ambient); createTestMsg(_msg.diffuse); createTestMsg(_msg.specular); createTestMsg(_msg.emissive); + _msg.shininess = 1.0; } void compareTestMsg(const std::shared_ptr & _msg) @@ -578,14 +576,13 @@ void compareTestMsg(const std::shared_ptr createTestMsg(expected_msg); compareTestMsg(_msg->header); - - EXPECT_EQ(expected_msg.name, _msg->name); - EXPECT_EQ(expected_msg.parent_name, _msg->parent_name); - + 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); } void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg index d3d38d32..6ddc1610 100644 --- a/ros_gz_interfaces/msg/MaterialColor.msg +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -1,9 +1,9 @@ std_msgs/Header header # Optional header data -string name # name -string parent_name # parent name +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 -std_msgs/ColorRGBA ambient # ambient -std_msgs/ColorRGBA diffuse # diffuse -std_msgs/ColorRGBA specular # specular -std_msgs/ColorRGBA emissive # emissive +Float64 shininess # Specular exponent From fa785ae2b03e615a1724eb7d781c66d9a1f69213 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 16 Jan 2024 20:43:47 -0500 Subject: [PATCH 06/19] Update MaterialColor for use with Apply enum. Signed-off-by: Benjamin Perseghetti --- .../src/convert/ros_gz_interfaces.cpp | 19 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 2 ++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 2 ++ ros_gz_interfaces/msg/MaterialColor.msg | 9 ++++++--- 4 files changed, 29 insertions(+), 3 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index e608c05b..d5f7f744 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -388,6 +388,17 @@ convert_ros_to_gz( const ros_gz_interfaces::msg::MaterialColor & ros_msg, gz::msgs::MaterialColor & gz_msg) { + switch (ros_msg.apply) { + case ros_gz_interfaces::msg::MaterialColor::FIRST: + gz_msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_FIRST); + break; + case ros_gz_interfaces::msg::MaterialColor::ALL: + gz_msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL); + break; + default: + std::cerr << "Unsupported apply type [" << ros_msg.apply << "]\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()); @@ -404,6 +415,14 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { + if (gz_msg.apply() == gz::msgs::MaterialColor::Apply::MaterialColor_Apply_FIRST) { + ros_msg.apply = ros_gz_interfaces::msg::MaterialColor::FIRST; + } else if (gz_msg.apply() == gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL) { + ros_msg.apply = ros_gz_interfaces::msg::MaterialColor::ALL; + } else { + std::cerr << "Unsupported Apply [" << + gz_msg.apply() << "]" << 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); diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index f7922cb7..a68965e3 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1348,6 +1348,7 @@ void createTestMsg(gz::msgs::MaterialColor & _msg) _msg.mutable_specular()->CopyFrom(specular_msg); _msg.mutable_specular()->CopyFrom(emissive_msg); _msg.set_shininess(1.0); + _msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL); } void compareTestMsg(const std::shared_ptr & _msg) @@ -1363,6 +1364,7 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->emissive())); EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); + EXPECT_EQ(expected_msg.apply(), _msg->apply()); } 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 4397824d..d14e8d82 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -568,6 +568,7 @@ void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) createTestMsg(_msg.specular); createTestMsg(_msg.emissive); _msg.shininess = 1.0; + _msg.apply = ros_gz_interfaces::msg::MaterialColor::ALL; } void compareTestMsg(const std::shared_ptr & _msg) @@ -583,6 +584,7 @@ void compareTestMsg(const std::shared_ptr compareTestMsg(std::make_shared(_msg->emissive)); EXPECT_EQ(expected_msg.shininess, _msg->shininess); + EXPECT_EQ(expected_msg.apply, _msg->apply); } void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg index 6ddc1610..c87bcd9c 100644 --- a/ros_gz_interfaces/msg/MaterialColor.msg +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -1,9 +1,12 @@ -std_msgs/Header header # Optional header data +# Entities 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 +float64 shininess # Specular exponent +uint8 apply # Entities to apply material color From efb85918cfe0fdc99842d957a0a79142ea6e7b41 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 17 Jan 2024 15:49:36 -0500 Subject: [PATCH 07/19] Make README more specific. Signed-off-by: Benjamin Perseghetti --- ros_ign_bridge/README.md | 282 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 282 insertions(+) diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index e93d8f26..3f09cff1 100644 --- a/ros_ign_bridge/README.md +++ b/ros_ign_bridge/README.md @@ -1,2 +1,284 @@ # This is a shim package For [ros_gz_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge) + +# Bridge communication between ROS and Ignition Gazebo + +This package provides a network bridge which enables the exchange of messages +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 | + +And the following for services: + +| ROS type | Gazebo request | Gazebo response | +|--------------------------------------|:--------------------------:| --------------------- | +| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | + +Run `ros2 run ros_gz_bridge parameter_bridge -h` for instructions. + +## Example 1a: Gazebo Transport talker and ROS 2 listener + +Start the parameter bridge which will watch the specified topics. + +``` +# Shell A: +. ~/bridge_ws/install/setup.bash +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg +``` + +Now we start the ROS listener. + +``` +# Shell B: +. /opt/ros/galactic/setup.bash +ros2 topic echo /chatter +``` + +Now we start the Gazebo Transport talker. + +``` +# Shell C: +ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"' +``` + +## Example 1b: ROS 2 talker and Gazebo Transport listener + +Start the parameter bridge which will watch the specified topics. + +``` +# Shell A: +. ~/bridge_ws/install/setup.bash +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg +``` + +Now we start the Gazebo Transport listener. + +``` +# Shell B: +ign topic -e -t /chatter +``` + +Now we start the ROS talker. + +``` +# Shell C: +. /opt/ros/galactic/setup.bash +ros2 topic pub /chatter std_msgs/msg/String "data: 'Hi'" --once +``` + +## Example 2: Run the bridge and exchange images + +In this example, we're going to generate Gazebo Transport images using +Gazebo Sim, that will be converted into ROS images, and visualized with +`rqt_image_viewer`. + +First we start Gazebo Sim (don't forget to hit play, or Gazebo Sim won't generate any images). + +``` +# Shell A: +ign gazebo sensors_demo.sdf +``` + +Let's see the topic where camera images are published. + +``` +# Shell B: +ign topic -l | grep image +/rgbd_camera/depth_image +/rgbd_camera/image +``` + +Then we start the parameter bridge with the previous topic. + +``` +# Shell B: +. ~/bridge_ws/install/setup.bash +ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image +``` + +Now we start the ROS GUI: + +``` +# Shell C: +. /opt/ros/galactic/setup.bash +ros2 run rqt_image_view rqt_image_view /rgbd_camera/image +``` + +You should see the current images in `rqt_image_view` which are coming from +Gazebo (published as Gazebo Msgs over Gazebo Transport). + +The screenshot shows all the shell windows and their expected content +(it was taken using ROS 2 Galactic and Gazebo Fortress): + +![Gazebo Transport images and ROS rqt](images/bridge_image_exchange.png) + +## Example 3: Static bridge + +In this example, we're going to run an executable that starts a bidirectional +bridge for a specific topic and message type. We'll use the `static_bridge` +executable that is installed with the bridge. + +The example's code can be found under `ros_gz_bridge/src/static_bridge.cpp`. +In the code, it's possible to see how the bridge is hardcoded to bridge string +messages published on the `/chatter` topic. + +Let's give it a try, starting with Gazebo -> ROS 2. + +On terminal A, start the bridge: + +`ros2 run ros_gz_bridge static_bridge` + +On terminal B, we start a ROS 2 listener: + +`ros2 topic echo /chatter std_msgs/msg/String` + +And terminal C, publish an Gazebo message: + +`ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'` + +At this point, you should see the ROS 2 listener echoing the message. + +Now let's try the other way around, ROS 2 -> Gazebo. + +On terminal D, start an Igntion listener: + +`ign topic -e -t /chatter` + +And on terminal E, publish a ROS 2 message: + +`ros2 topic pub /chatter std_msgs/msg/String 'data: "Hello"' -1` + +You should see the Gazebo listener echoing the message. + +## Example 4: Service bridge + +It's possible to make ROS service requests into Gazebo. Let's try unpausing the simulation. + +On terminal A, start the service bridge: + +`ros2 run ros_gz_bridge parameter_bridge /world/shapes/control@ros_gz_interfaces/srv/ControlWorld` + +On terminal B, start Gazebo, it will be paused by default: + +`ign gazebo shapes.sdf` + +On terminal C, make a ROS request to unpause simulation: + +``` +ros2 service call /world//control ros_gz_interfaces/srv/ControlWorld "{world_control: {pause: false}}" +``` + +## Example 5: Configuring the Bridge via YAML + +When configuring many topics, it is easier to use a file-based configuration in a markup +language. In this case, the `ros_gz` bridge supports using a YAML file to configure the +various parameters. + +The configuration file must be a YAML array of maps. +An example configuration for 5 bridges is below, showing the various ways that a +bridge may be specified: + +```yaml + # Set just topic name, applies to both +- topic_name: "chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + +# Set just ROS topic name, applies to both +- ros_topic_name: "chatter_ros" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + +# Set just GZ topic name, applies to both +- gz_topic_name: "chatter_ign" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + +# Set each topic name explicitly +- ros_topic_name: "chatter_both_ros" + gz_topic_name: "chatter_both_ign" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + +# Full set of configurations +- ros_topic_name: "ros_chatter" + gz_topic_name: "ign_chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + subscriber_queue: 5 # Default 10 + publisher_queue: 6 # Default 10 + lazy: true # Default "false" + direction: BIDIRECTIONAL # Default "BIDIRECTIONAL" - Bridge both directions + # "GZ_TO_ROS" - Bridge Ignition topic to ROS + # "ROS_TO_GZ" - Bridge ROS topic to Ignition +``` + +To run the bridge node with the above configuration: +```bash +ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=$WORKSPACE/ros_gz/ros_gz_bridge/test/config/full.yaml +``` + +## 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 From b36864e83f6560fa8b78e002729b860ebbfb4205 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 17 Jan 2024 16:30:00 -0500 Subject: [PATCH 08/19] Updated MaterialColor for use with EnityMatch. Signed-off-by: Benjamin Perseghetti --- .../src/convert/ros_gz_interfaces.cpp | 20 +++++++++---------- ros_gz_bridge/test/utils/gz_test_msg.cpp | 4 ++-- ros_gz_bridge/test/utils/ros_test_msg.cpp | 4 ++-- ros_gz_interfaces/msg/MaterialColor.msg | 4 ++-- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index d5f7f744..14d01395 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -388,15 +388,15 @@ convert_ros_to_gz( const ros_gz_interfaces::msg::MaterialColor & ros_msg, gz::msgs::MaterialColor & gz_msg) { - switch (ros_msg.apply) { + switch (ros_msg.entity_match) { case ros_gz_interfaces::msg::MaterialColor::FIRST: - gz_msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_FIRST); + gz_msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); break; case ros_gz_interfaces::msg::MaterialColor::ALL: - gz_msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL); + gz_msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); break; default: - std::cerr << "Unsupported apply type [" << ros_msg.apply << "]\n"; + std::cerr << "Unsupported entity match type [" << ros_msg.entity_match << "]\n"; } convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); @@ -415,13 +415,13 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { - if (gz_msg.apply() == gz::msgs::MaterialColor::Apply::MaterialColor_Apply_FIRST) { - ros_msg.apply = ros_gz_interfaces::msg::MaterialColor::FIRST; - } else if (gz_msg.apply() == gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL) { - ros_msg.apply = ros_gz_interfaces::msg::MaterialColor::ALL; + if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; + } else if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { - std::cerr << "Unsupported Apply [" << - gz_msg.apply() << "]" << std::endl; + 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); diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index a68965e3..d12a9e85 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1348,7 +1348,7 @@ void createTestMsg(gz::msgs::MaterialColor & _msg) _msg.mutable_specular()->CopyFrom(specular_msg); _msg.mutable_specular()->CopyFrom(emissive_msg); _msg.set_shininess(1.0); - _msg.set_apply(gz::msgs::MaterialColor::Apply::MaterialColor_Apply_ALL); + _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); } void compareTestMsg(const std::shared_ptr & _msg) @@ -1364,7 +1364,7 @@ void compareTestMsg(const std::shared_ptr & _msg) compareTestMsg(std::make_shared(_msg->emissive())); EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); - EXPECT_EQ(expected_msg.apply(), _msg->apply()); + EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); } 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 d14e8d82..448af2db 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -568,7 +568,7 @@ void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) createTestMsg(_msg.specular); createTestMsg(_msg.emissive); _msg.shininess = 1.0; - _msg.apply = ros_gz_interfaces::msg::MaterialColor::ALL; + _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } void compareTestMsg(const std::shared_ptr & _msg) @@ -584,7 +584,7 @@ void compareTestMsg(const std::shared_ptr compareTestMsg(std::make_shared(_msg->emissive)); EXPECT_EQ(expected_msg.shininess, _msg->shininess); - EXPECT_EQ(expected_msg.apply, _msg->apply); + EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); } void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg index c87bcd9c..cda67d27 100644 --- a/ros_gz_interfaces/msg/MaterialColor.msg +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -1,4 +1,4 @@ -# Entities to apply material color: constant definition +# Entities that match to apply material color: constant definition uint8 FIRST = 0 uint8 ALL = 1 @@ -9,4 +9,4 @@ std_msgs/ColorRGBA diffuse # Diffuse color std_msgs/ColorRGBA specular # Specular color std_msgs/ColorRGBA emissive # Emissive color float64 shininess # Specular exponent -uint8 apply # Entities to apply material color +uint8 entity_match # Entities that match to apply material color From 77fef0c40b6a7c51058a6d50b11c662e085fb2d2 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sat, 27 Jan 2024 14:22:06 -0500 Subject: [PATCH 09/19] Add if/def conditionals for MaterialColor. Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- .../include/ros_gz_bridge/convert/ros_gz_interfaces.hpp | 9 +++++++-- ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp | 7 +++++++ ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 2 ++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 2 ++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 7 ++++++- ros_gz_bridge/test/utils/ros_test_msg.cpp | 2 ++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 4 ++++ 7 files changed, 30 insertions(+), 3 deletions(-) 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 1a686aa4..eb7b3af8 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 @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -44,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -60,6 +58,11 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#include +#endif // HAVE_MATERIALCOLOR + #include namespace ros_gz_bridge @@ -163,6 +166,7 @@ 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( @@ -174,6 +178,7 @@ void convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg); +#endif // HAVE_MATERIALCOLOR template<> void 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/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 14d01395..028184da 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -382,6 +382,7 @@ convert_gz_to_ros( ros_msg.intensity = gz_msg.intensity(); } +#if HAVE_MATERIALCOLOR template<> void convert_ros_to_gz( @@ -432,6 +433,7 @@ convert_gz_to_ros( ros_msg.shininess = gz_msg.shininess(); } +#endif // HAVE_MATERIALCOLOR template<> void diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 7bc47fa9..69b27b18 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1346,6 +1346,7 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity()); } +#if HAVE_MATERIALCOLOR void createTestMsg(gz::msgs::MaterialColor & _msg) { gz::msgs::Header header_msg; @@ -1387,6 +1388,7 @@ void compareTestMsg(const std::shared_ptr & _msg) 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) { diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 83740504..1ce285a6 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -74,6 +73,10 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#endif // HAVE_MATERIALCOLOR + namespace ros_gz_bridge { namespace testing @@ -456,6 +459,7 @@ 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); @@ -463,6 +467,7 @@ 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. diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 02a19edb..24a22c4e 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -577,6 +577,7 @@ 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); @@ -604,6 +605,7 @@ void compareTestMsg(const std::shared_ptr 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) { diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index 4b51ec2a..a37bdb6c 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -57,7 +57,9 @@ #include #endif // HAVE_DATAFRAME #include +#if HAVE_MATERIALCOLOR #include +#endif // HAVE_MATERIALCOLOR #include #include #include @@ -386,6 +388,7 @@ 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); @@ -393,6 +396,7 @@ 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. From ad9cfbe52dfc22757449e62f68bf823ceab830a4 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sat, 27 Jan 2024 14:45:48 -0500 Subject: [PATCH 10/19] Add bridge mapping for gz-msgs 10.1.0. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/ros_gz_bridge/__init__.py | 10 +++++++++- ros_gz_bridge/ros_gz_bridge/mappings.py | 7 ++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index b9291840..cffda658 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_8_4_0, MAPPINGS_10_1_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 9f4775b1..ee0d3ccc 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -65,7 +65,6 @@ Mapping('GuiCamera', 'GUICamera'), Mapping('JointWrench', 'JointWrench'), Mapping('Light', 'Light'), - Mapping('MaterialColor', 'MaterialColor'), Mapping('ParamVec', 'Param'), Mapping('ParamVec', 'Param_V'), Mapping('SensorNoise', 'SensorNoise'), @@ -117,3 +116,9 @@ Mapping('Dataframe', 'Dataframe'), ], } + +MAPPINGS_10_1_0 = { + 'ros_gz_interfaces': [ + Mapping('MaterialColor', 'MaterialColor'), + ], +} From 997a5d3d3734f108fb123a4a9ee95c511f649d77 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sat, 27 Jan 2024 15:12:37 -0500 Subject: [PATCH 11/19] Fix whitespace/linting. Signed-off-by: Benjamin Perseghetti --- .../ros_gz_bridge/convert/ros_gz_interfaces.hpp | 4 ++-- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 17 +++++++++++------ ros_gz_bridge/test/utils/gz_test_msg.cpp | 2 +- ros_gz_bridge/test/utils/gz_test_msg.hpp | 4 ++-- ros_gz_bridge/test/utils/ros_test_msg.cpp | 2 +- ros_gz_bridge/test/utils/ros_test_msg.hpp | 4 ++-- 6 files changed, 19 insertions(+), 14 deletions(-) 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 eb7b3af8..a375940d 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 @@ -61,7 +61,7 @@ #if HAVE_MATERIALCOLOR #include #include -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR #include @@ -178,7 +178,7 @@ void convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg); -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR template<> void diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 028184da..c33c1ec8 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -391,13 +391,16 @@ convert_ros_to_gz( { switch (ros_msg.entity_match) { case ros_gz_interfaces::msg::MaterialColor::FIRST: - gz_msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); + gz_msg.set_entity_match( + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); break; case ros_gz_interfaces::msg::MaterialColor::ALL: - gz_msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); + gz_msg.set_entity_match( + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); break; default: - std::cerr << "Unsupported entity match type [" << ros_msg.entity_match << "]\n"; + std::cerr << "Unsupported entity match type [" + << ros_msg.entity_match << "]\n"; } convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); @@ -416,9 +419,11 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { - if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { + if (gz_msg.entity_match() == + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; - } else if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { + } else if (gz_msg.entity_match() == + gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { std::cerr << "Unsupported EntityMatch [" << @@ -433,7 +438,7 @@ convert_gz_to_ros( ros_msg.shininess = gz_msg.shininess(); } -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR template<> void diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 69b27b18..c93fbf4d 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1388,7 +1388,7 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); } -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR void createTestMsg(gz::msgs::GUICamera & _msg) { diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 1ce285a6..d939a4ff 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -75,7 +75,7 @@ #if HAVE_MATERIALCOLOR #include -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR namespace ros_gz_bridge { @@ -467,7 +467,7 @@ 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 +#endif // HAVE_MATERIALCOLOR /// \brief Create a message used for testing. /// \param[out] _msg The message populated. diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 24a22c4e..f80b8553 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -605,7 +605,7 @@ void compareTestMsg(const std::shared_ptr EXPECT_EQ(expected_msg.shininess, _msg->shininess); EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); } -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) { diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index a37bdb6c..ee4426fd 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -59,7 +59,7 @@ #include #if HAVE_MATERIALCOLOR #include -#endif // HAVE_MATERIALCOLOR +#endif // HAVE_MATERIALCOLOR #include #include #include @@ -396,7 +396,7 @@ 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 +#endif // HAVE_MATERIALCOLOR /// \brief Create a message used for testing. /// \param[out] _msg The message populated. From e650ac5cc8b6fa2255cf8fcab2ba08dd0079b97e Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sat, 27 Jan 2024 16:06:32 -0500 Subject: [PATCH 12/19] Remove correct license for broken license checker. Signed-off-by: Benjamin Perseghetti --- .../include/ros_gz_bridge/convert/ros_gz_interfaces.hpp | 2 -- ros_gz_bridge/ros_gz_bridge/mappings.py | 2 -- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 2 -- 3 files changed, 6 deletions(-) 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 a375940d..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 @@ -1,6 +1,4 @@ // Copyright 2018 Open Source Robotics Foundation, Inc. -// Copyright (C) 2024 CogniPilot Foundation -// Copyright (C) 2024 Rudis Laboratories LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index ee0d3ccc..04137082 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -1,6 +1,4 @@ # Copyright 2022 Open Source Robotics Foundation, Inc. -# Copyright (C) 2024 CogniPilot Foundation -# Copyright (C) 2024 Rudis Laboratories LLC # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index c33c1ec8..91848afe 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -1,6 +1,4 @@ // Copyright 2018 Open Source Robotics Foundation, Inc. -// Copyright (C) 2024 CogniPilot Foundation -// Copyright (C) 2024 Rudis Laboratories LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 6ced4cdfbbefe45237553dbc8cb38a827ff1dfef Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 31 Jan 2024 16:31:48 -0500 Subject: [PATCH 13/19] Fix indent level of switch statement. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 91848afe..7e5e95be 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -387,7 +387,7 @@ convert_ros_to_gz( const ros_gz_interfaces::msg::MaterialColor & ros_msg, gz::msgs::MaterialColor & gz_msg) { - switch (ros_msg.entity_match) { + switch (ros_msg.entity_match) { case ros_gz_interfaces::msg::MaterialColor::FIRST: gz_msg.set_entity_match( gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); From 2c0af24212ff462ccf6725687c7279a337323687 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 31 Jan 2024 17:36:42 -0500 Subject: [PATCH 14/19] Fix duplicates, remove readme, simplify tests. Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/README.md | 161 +++++++------ ros_gz_bridge/test/utils/gz_test_msg.cpp | 25 +- ros_ign_bridge/README.md | 282 ----------------------- 3 files changed, 86 insertions(+), 382 deletions(-) diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index b88c7b18..aff3b379 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,71 +5,70 @@ between ROS and Gazebo Transport. The following message types can be bridged for topics: -| ROS type | Gazebo type | -|--------------------------------------|:--------------------------------------:| -| builtin_interfaces/msg/Time | gz::msgs::Time | -| actuator_msgs/msg/Actuators | gz::msgs::Actuators | -| std_msgs/msg/Bool | gz::msgs::Boolean | -| std_msgs/msg/ColorRGBA | gz::msgs::Color | -| std_msgs/msg/Empty | gz::msgs::Empty | -| std_msgs/msg/Float32 | gz::msgs::Float | -| std_msgs/msg/Float64 | gz::msgs::Double | -| std_msgs/msg/Header | gz::msgs::Header | -| std_msgs/msg/Int32 | gz::msgs::Int32 | -| std_msgs/msg/UInt32 | gz::msgs::UInt32 | -| std_msgs/msg/String | gz::msgs::StringMsg | -| geometry_msgs/msg/Wrench | gz::msgs::Wrench | -| geometry_msgs/msg/WrenchStamped | gz::msgs::Wrench | -| geometry_msgs/msg/Quaternion | gz::msgs::Quaternion | -| geometry_msgs/msg/Vector3 | gz::msgs::Vector3d | -| geometry_msgs/msg/Point | gz::msgs::Vector3d | -| geometry_msgs/msg/Pose | gz::msgs::Pose | -| geometry_msgs/msg/PoseArray | gz::msgs::Pose_V | -| geometry_msgs/msg/PoseWithCovariance | gz::msgs::PoseWithCovariance | -| geometry_msgs/msg/PoseStamped | gz::msgs::Pose | -| geometry_msgs/msg/Transform | gz::msgs::Pose | -| geometry_msgs/msg/TransformStamped | gz::msgs::Pose | -| geometry_msgs/msg/Twist | gz::msgs::Twist | -| geometry_msgs/msg/TwistStamped | gz::msgs::Twist | -| geometry_msgs/msg/TwistWithCovariance| gz::msgs::TwistWithCovariance | -| nav_msgs/msg/Odometry | gz::msgs::Odometry | -| nav_msgs/msg/Odometry | gz::msgs::OdometryWithCovariance | -| rcl_interfaces/msg/ParameterValue | gz::msgs::Any | -| ros_gz_interfaces/msg/Altimeter | gz::msgs::Altimeter | -| ros_gz_interfaces/msg/Contact | gz::msgs::Contact | -| ros_gz_interfaces/msg/Contacts | gz::msgs::Contacts | -| ros_gz_interfaces/msg/Dataframe | gz::msgs::Dataframe | -| ros_gz_interfaces/msg/Entity | gz::msgs::Entity | -| ros_gz_interfaces/msg/Float32Array | gz::msgs::Float_V | -| ros_gz_interfaces/msg/GuiCamera | gz::msgs::GUICamera | -| ros_gz_interfaces/msg/JointWrench | gz::msgs::JointWrench | -| ros_gz_interfaces/msg/Light | gz::msgs::Light | -| ros_gz_interfaces/msg/MaterialColor | gz::msgs::MaterialColor | -| ros_gz_interfaces/msg/SensorNoise | gz::msgs::SensorNoise | -| ros_gz_interfaces/msg/StringVec | gz::msgs::StringMsg_V | -| ros_gz_interfaces/msg/TrackVisual | gz::msgs::TrackVisual | -| ros_gz_interfaces/msg/VideoRecord | gz::msgs::VideoRecord | -| ros_gz_interfaces/msg/WorldControl | gz::msgs::WorldControl | -| rosgraph_msgs/msg/Clock | gz::msgs::Clock | -| sensor_msgs/msg/BatteryState | gz::msgs::BatteryState | -| sensor_msgs/msg/CameraInfo | gz::msgs::CameraInfo | -| sensor_msgs/msg/FluidPressure | gz::msgs::FluidPressure | -| sensor_msgs/msg/Imu | gz::msgs::IMU | -| sensor_msgs/msg/Image | gz::msgs::Image | -| sensor_msgs/msg/JointState | gz::msgs::Model | -| sensor_msgs/msg/Joy | gz::msgs::Joy | -| sensor_msgs/msg/LaserScan | gz::msgs::LaserScan | -| sensor_msgs/msg/MagneticField | gz::msgs::Magnetometer | -| sensor_msgs/msg/NavSatFix | gz::msgs::NavSat | -| sensor_msgs/msg/PointCloud2 | gz::msgs::PointCloudPacked | -| tf2_msgs/msg/TFMessage | gz::msgs::Pose_V | -| trajectory_msgs/msg/JointTrajectory | gz::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: | ROS type | Gazebo request | Gazebo response | |--------------------------------------|:--------------------------:| --------------------- | -| ros_gz_interfaces/srv/ControlWorld | gz.msgs.WorldControl | gz.msgs.Boolean | +| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | Run `ros2 run ros_gz_bridge parameter_bridge -h` for instructions. @@ -80,7 +79,7 @@ Start the parameter bridge which will watch the specified topics. ``` # Shell A: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg ``` Now we start the ROS listener. @@ -95,7 +94,7 @@ Now we start the Gazebo Transport talker. ``` # Shell C: -gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"' +ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"' ``` ## Example 1b: ROS 2 talker and Gazebo Transport listener @@ -105,14 +104,14 @@ Start the parameter bridge which will watch the specified topics. ``` # Shell A: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@gz.msgs.StringMsg +ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg ``` Now we start the Gazebo Transport listener. ``` # Shell B: -gz topic -e -t /chatter +ign topic -e -t /chatter ``` Now we start the ROS talker. @@ -133,14 +132,14 @@ First we start Gazebo Sim (don't forget to hit play, or Gazebo Sim won't generat ``` # Shell A: -gz gazebo sensors_demo.sdf +ign gazebo sensors_demo.sdf ``` Let's see the topic where camera images are published. ``` # Shell B: -gz topic -l | grep image +ign topic -l | grep image /rgbd_camera/depth_image /rgbd_camera/image ``` @@ -150,7 +149,7 @@ Then we start the parameter bridge with the previous topic. ``` # Shell B: . ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image +ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image ``` Now we start the ROS GUI: @@ -191,15 +190,15 @@ On terminal B, we start a ROS 2 listener: And terminal C, publish an Gazebo message: -`gz topic -t /chatter -m gz.msgs.StringMsg -p 'data:"Hello"'` +`ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'` At this point, you should see the ROS 2 listener echoing the message. Now let's try the other way around, ROS 2 -> Gazebo. -On terminal D, start an gz listener: +On terminal D, start an Igntion listener: -`gz topic -e -t /chatter` +`ign topic -e -t /chatter` And on terminal E, publish a ROS 2 message: @@ -217,7 +216,7 @@ On terminal A, start the service bridge: On terminal B, start Gazebo, it will be paused by default: -`gz gazebo shapes.sdf` +`ign gazebo shapes.sdf` On terminal C, make a ROS request to unpause simulation: @@ -239,35 +238,35 @@ bridge may be specified: # Set just topic name, applies to both - topic_name: "chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" + gz_type_name: "ignition.msgs.StringMsg" # Set just ROS topic name, applies to both - ros_topic_name: "chatter_ros" ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" + gz_type_name: "ignition.msgs.StringMsg" # Set just GZ topic name, applies to both -- gz_topic_name: "chatter_gz" +- gz_topic_name: "chatter_ign" ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" + gz_type_name: "ignition.msgs.StringMsg" # Set each topic name explicitly - ros_topic_name: "chatter_both_ros" - gz_topic_name: "chatter_both_gz" + gz_topic_name: "chatter_both_ign" ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" + gz_type_name: "ignition.msgs.StringMsg" # Full set of configurations - ros_topic_name: "ros_chatter" - gz_topic_name: "gz_chatter" + gz_topic_name: "ign_chatter" ros_type_name: "std_msgs/msg/String" - gz_type_name: "gz.msgs.StringMsg" + gz_type_name: "ignition.msgs.StringMsg" subscriber_queue: 5 # Default 10 publisher_queue: 6 # Default 10 lazy: true # Default "false" direction: BIDIRECTIONAL # Default "BIDIRECTIONAL" - Bridge both directions - # "GZ_TO_ROS" - Bridge gz topic to ROS - # "ROS_TO_GZ" - Bridge ROS topic to gz + # "GZ_TO_ROS" - Bridge Ignition topic to ROS + # "ROS_TO_GZ" - Bridge ROS topic to Ignition ``` To run the bridge node with the above configuration: diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index c93fbf4d..b64c0f40 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1349,26 +1349,13 @@ void compareTestMsg(const std::shared_ptr & _msg) #if HAVE_MATERIALCOLOR void createTestMsg(gz::msgs::MaterialColor & _msg) { - gz::msgs::Header header_msg; - gz::msgs::Entity entity_msg; - gz::msgs::Color ambient_msg; - gz::msgs::Color diffuse_msg; - gz::msgs::Color specular_msg; - gz::msgs::Color emissive_msg; - - createTestMsg(header_msg); - createTestMsg(entity_msg); - createTestMsg(ambient_msg); - createTestMsg(diffuse_msg); - createTestMsg(specular_msg); - createTestMsg(emissive_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.mutable_header()->CopyFrom(header_msg); - _msg.mutable_entity()->CopyFrom(entity_msg); - _msg.mutable_specular()->CopyFrom(ambient_msg); - _msg.mutable_diffuse()->CopyFrom(diffuse_msg); - _msg.mutable_specular()->CopyFrom(specular_msg); - _msg.mutable_specular()->CopyFrom(emissive_msg); _msg.set_shininess(1.0); _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); } diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md index 3f09cff1..e93d8f26 100644 --- a/ros_ign_bridge/README.md +++ b/ros_ign_bridge/README.md @@ -1,284 +1,2 @@ # This is a shim package For [ros_gz_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge) - -# Bridge communication between ROS and Ignition Gazebo - -This package provides a network bridge which enables the exchange of messages -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 | - -And the following for services: - -| ROS type | Gazebo request | Gazebo response | -|--------------------------------------|:--------------------------:| --------------------- | -| ros_gz_interfaces/srv/ControlWorld | ignition.msgs.WorldControl | ignition.msgs.Boolean | - -Run `ros2 run ros_gz_bridge parameter_bridge -h` for instructions. - -## Example 1a: Gazebo Transport talker and ROS 2 listener - -Start the parameter bridge which will watch the specified topics. - -``` -# Shell A: -. ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg -``` - -Now we start the ROS listener. - -``` -# Shell B: -. /opt/ros/galactic/setup.bash -ros2 topic echo /chatter -``` - -Now we start the Gazebo Transport talker. - -``` -# Shell C: -ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"' -``` - -## Example 1b: ROS 2 talker and Gazebo Transport listener - -Start the parameter bridge which will watch the specified topics. - -``` -# Shell A: -. ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /chatter@std_msgs/msg/String@ignition.msgs.StringMsg -``` - -Now we start the Gazebo Transport listener. - -``` -# Shell B: -ign topic -e -t /chatter -``` - -Now we start the ROS talker. - -``` -# Shell C: -. /opt/ros/galactic/setup.bash -ros2 topic pub /chatter std_msgs/msg/String "data: 'Hi'" --once -``` - -## Example 2: Run the bridge and exchange images - -In this example, we're going to generate Gazebo Transport images using -Gazebo Sim, that will be converted into ROS images, and visualized with -`rqt_image_viewer`. - -First we start Gazebo Sim (don't forget to hit play, or Gazebo Sim won't generate any images). - -``` -# Shell A: -ign gazebo sensors_demo.sdf -``` - -Let's see the topic where camera images are published. - -``` -# Shell B: -ign topic -l | grep image -/rgbd_camera/depth_image -/rgbd_camera/image -``` - -Then we start the parameter bridge with the previous topic. - -``` -# Shell B: -. ~/bridge_ws/install/setup.bash -ros2 run ros_gz_bridge parameter_bridge /rgbd_camera/image@sensor_msgs/msg/Image@ignition.msgs.Image -``` - -Now we start the ROS GUI: - -``` -# Shell C: -. /opt/ros/galactic/setup.bash -ros2 run rqt_image_view rqt_image_view /rgbd_camera/image -``` - -You should see the current images in `rqt_image_view` which are coming from -Gazebo (published as Gazebo Msgs over Gazebo Transport). - -The screenshot shows all the shell windows and their expected content -(it was taken using ROS 2 Galactic and Gazebo Fortress): - -![Gazebo Transport images and ROS rqt](images/bridge_image_exchange.png) - -## Example 3: Static bridge - -In this example, we're going to run an executable that starts a bidirectional -bridge for a specific topic and message type. We'll use the `static_bridge` -executable that is installed with the bridge. - -The example's code can be found under `ros_gz_bridge/src/static_bridge.cpp`. -In the code, it's possible to see how the bridge is hardcoded to bridge string -messages published on the `/chatter` topic. - -Let's give it a try, starting with Gazebo -> ROS 2. - -On terminal A, start the bridge: - -`ros2 run ros_gz_bridge static_bridge` - -On terminal B, we start a ROS 2 listener: - -`ros2 topic echo /chatter std_msgs/msg/String` - -And terminal C, publish an Gazebo message: - -`ign topic -t /chatter -m ignition.msgs.StringMsg -p 'data:"Hello"'` - -At this point, you should see the ROS 2 listener echoing the message. - -Now let's try the other way around, ROS 2 -> Gazebo. - -On terminal D, start an Igntion listener: - -`ign topic -e -t /chatter` - -And on terminal E, publish a ROS 2 message: - -`ros2 topic pub /chatter std_msgs/msg/String 'data: "Hello"' -1` - -You should see the Gazebo listener echoing the message. - -## Example 4: Service bridge - -It's possible to make ROS service requests into Gazebo. Let's try unpausing the simulation. - -On terminal A, start the service bridge: - -`ros2 run ros_gz_bridge parameter_bridge /world/shapes/control@ros_gz_interfaces/srv/ControlWorld` - -On terminal B, start Gazebo, it will be paused by default: - -`ign gazebo shapes.sdf` - -On terminal C, make a ROS request to unpause simulation: - -``` -ros2 service call /world//control ros_gz_interfaces/srv/ControlWorld "{world_control: {pause: false}}" -``` - -## Example 5: Configuring the Bridge via YAML - -When configuring many topics, it is easier to use a file-based configuration in a markup -language. In this case, the `ros_gz` bridge supports using a YAML file to configure the -various parameters. - -The configuration file must be a YAML array of maps. -An example configuration for 5 bridges is below, showing the various ways that a -bridge may be specified: - -```yaml - # Set just topic name, applies to both -- topic_name: "chatter" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" - -# Set just ROS topic name, applies to both -- ros_topic_name: "chatter_ros" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" - -# Set just GZ topic name, applies to both -- gz_topic_name: "chatter_ign" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" - -# Set each topic name explicitly -- ros_topic_name: "chatter_both_ros" - gz_topic_name: "chatter_both_ign" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" - -# Full set of configurations -- ros_topic_name: "ros_chatter" - gz_topic_name: "ign_chatter" - ros_type_name: "std_msgs/msg/String" - gz_type_name: "ignition.msgs.StringMsg" - subscriber_queue: 5 # Default 10 - publisher_queue: 6 # Default 10 - lazy: true # Default "false" - direction: BIDIRECTIONAL # Default "BIDIRECTIONAL" - Bridge both directions - # "GZ_TO_ROS" - Bridge Ignition topic to ROS - # "ROS_TO_GZ" - Bridge ROS topic to Ignition -``` - -To run the bridge node with the above configuration: -```bash -ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=$WORKSPACE/ros_gz/ros_gz_bridge/test/config/full.yaml -``` - -## 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 From 0d1108dbc1b204600deeae00f3694f894cec8b61 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 31 Jan 2024 18:28:31 -0500 Subject: [PATCH 15/19] fix uncrustify but maybe break codecheck. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 7e5e95be..c4d06e49 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -417,11 +417,9 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { - if (gz_msg.entity_match() == - gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { + if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; - } else if (gz_msg.entity_match() == - gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { + } else if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { std::cerr << "Unsupported EntityMatch [" << From e27a19c352ca4f895699798e63ab336ff7c3b8b7 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 31 Jan 2024 20:11:44 -0500 Subject: [PATCH 16/19] Using MCEntityMatch. Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index c4d06e49..428a400c 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -14,6 +14,10 @@ #include "ros_gz_bridge/convert/ros_gz_interfaces.hpp" +#if HAVE_MATERIALCOLOR +using MCEntityMatch = gz::msgs::MaterialColor::EntityMatch; +#endif // HAVE_MATERIALCOLOR + namespace ros_gz_bridge { @@ -389,12 +393,10 @@ convert_ros_to_gz( { switch (ros_msg.entity_match) { case ros_gz_interfaces::msg::MaterialColor::FIRST: - gz_msg.set_entity_match( - gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST); + gz_msg.set_entity_match(MCEntityMatch::MaterialColor_EntityMatch_FIRST); break; case ros_gz_interfaces::msg::MaterialColor::ALL: - gz_msg.set_entity_match( - gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); + gz_msg.set_entity_match(MCEntityMatch::MaterialColor_EntityMatch_ALL); break; default: std::cerr << "Unsupported entity match type [" @@ -417,9 +419,9 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { - if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST) { + if (gz_msg.entity_match() == MCEntityMatch::MaterialColor_EntityMatch_FIRST) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; - } else if (gz_msg.entity_match() == gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL) { + } else if (gz_msg.entity_match() == MCEntityMatch::MaterialColor_EntityMatch_ALL) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { std::cerr << "Unsupported EntityMatch [" << From 1b8722cf27b7b1f27d3b33695a7cb7a2747c0846 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 31 Jan 2024 20:23:15 -0600 Subject: [PATCH 17/19] Fix linters Signed-off-by: Addisu Z. Taddese --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 428a400c..e48aaad2 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -14,10 +14,6 @@ #include "ros_gz_bridge/convert/ros_gz_interfaces.hpp" -#if HAVE_MATERIALCOLOR -using MCEntityMatch = gz::msgs::MaterialColor::EntityMatch; -#endif // HAVE_MATERIALCOLOR - namespace ros_gz_bridge { @@ -391,12 +387,14 @@ 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(MCEntityMatch::MaterialColor_EntityMatch_FIRST); + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST); break; case ros_gz_interfaces::msg::MaterialColor::ALL: - gz_msg.set_entity_match(MCEntityMatch::MaterialColor_EntityMatch_ALL); + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL); break; default: std::cerr << "Unsupported entity match type [" @@ -419,9 +417,11 @@ convert_gz_to_ros( const gz::msgs::MaterialColor & gz_msg, ros_gz_interfaces::msg::MaterialColor & ros_msg) { - if (gz_msg.entity_match() == MCEntityMatch::MaterialColor_EntityMatch_FIRST) { + 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; - } else if (gz_msg.entity_match() == MCEntityMatch::MaterialColor_EntityMatch_ALL) { + } else if (gz_msg.entity_match() == + EntityMatch::MaterialColor_EntityMatch_ALL) { ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { std::cerr << "Unsupported EntityMatch [" << From c880593d0c00a3df4f6b47b9adf54fbd611801e4 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Wed, 31 Jan 2024 22:32:22 -0500 Subject: [PATCH 18/19] Bad linter! Bad! Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index e48aaad2..0d619955 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -420,8 +420,10 @@ convert_gz_to_ros( 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) { + EntityMatch::MaterialColor_EntityMatch_ALL) { +/* *INDENT-ON* */ ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; } else { std::cerr << "Unsupported EntityMatch [" << From 3f3f3bbcbef103065104f6131ac65a64a96af31d Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 1 Feb 2024 09:07:20 -0500 Subject: [PATCH 19/19] Linters. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Addisu Z. Taddese Signed-off-by: Benjamin Perseghetti --- ros_gz_bridge/ros_gz_bridge/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index cffda658..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, MAPPINGS_10_1_0 +from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_10_1_0, MAPPINGS_8_4_0 from rosidl_cmake import expand_template