Add option to change material color from ROS.

Forward port of gazebosim#486 * Message and bridge for MaterialColor. This allows bridging MaterialColor from ROS to GZ and is important for allowing simulation users to create status lights. Signed-off-by: Benjamin Perseghetti <[email protected]>
rudislabs · Mar 27, 2024 · 7fea0ca · 7fea0ca
1 parent b986388
commit 7fea0ca
Show file tree

Hide file tree

Showing 11 changed files with 206 additions and 1 deletion.
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
@@ -56,6 +56,11 @@
 #include <ros_gz_interfaces/msg/dataframe.hpp>
 #endif  // HAVE_DATAFRAME
 
+#if HAVE_MATERIALCOLOR
+#include <gz/msgs/material_color.pb.h>
+#include <ros_gz_interfaces/msg/material_color.hpp>
+#endif  // HAVE_MATERIALCOLOR
+
 #include <ros_gz_bridge/convert_decl.hpp>
 
 namespace ros_gz_bridge
@@ -159,6 +164,20 @@ convert_gz_to_ros(
   const gz::msgs::Light & gz_msg,
   ros_gz_interfaces::msg::Light & ros_msg);
 
+#if HAVE_MATERIALCOLOR
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::MaterialColor & ros_msg,
+  gz::msgs::MaterialColor & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::MaterialColor & gz_msg,
+  ros_gz_interfaces::msg::MaterialColor & ros_msg);
+#endif  // HAVE_MATERIALCOLOR
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp
@@ -36,6 +36,13 @@
 #define HAVE_DATAFRAME true
 #endif
 
+// MaterialColor is available from versions 10.1.0 (Harmonic) forward
+// This can be removed when the minimum supported version passes 10.1.0
+#if (GZ_MSGS_MAJOR_VERSION > 10) || \
+  ((GZ_MSGS_MAJOR_VERSION == 10) && (GZ_MSGS_MINOR_VERSION >= 1))
+#define HAVE_MATERIALCOLOR true
+#endif
+
 namespace ros_gz_bridge
 {
 /// Forward declarations

diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py
@@ -16,7 +16,7 @@
 
 import os
 
-from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_8_4_0
+from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_10_1_0, MAPPINGS_8_4_0
 
 from rosidl_pycommon 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
@@ -113,3 +113,9 @@
         Mapping('Dataframe', 'Dataframe'),
     ],
 }
+
+MAPPINGS_10_1_0 = {
+    'ros_gz_interfaces': [
+        Mapping('MaterialColor', 'MaterialColor'),
+    ],
+}
diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp
@@ -380,6 +380,66 @@ convert_gz_to_ros(
   ros_msg.intensity = gz_msg.intensity();
 }
 
+#if HAVE_MATERIALCOLOR
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::MaterialColor & ros_msg,
+  gz::msgs::MaterialColor & gz_msg)
+{
+  using EntityMatch = gz::msgs::MaterialColor::EntityMatch;
+
+  switch (ros_msg.entity_match) {
+    case ros_gz_interfaces::msg::MaterialColor::FIRST:
+      gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST);
+      break;
+    case ros_gz_interfaces::msg::MaterialColor::ALL:
+      gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL);
+      break;
+    default:
+      std::cerr << "Unsupported entity match type ["
+                << ros_msg.entity_match << "]\n";
+  }
+
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+  convert_ros_to_gz(ros_msg.entity, *gz_msg.mutable_entity());
+  convert_ros_to_gz(ros_msg.ambient, *gz_msg.mutable_ambient());
+  convert_ros_to_gz(ros_msg.diffuse, *gz_msg.mutable_diffuse());
+  convert_ros_to_gz(ros_msg.specular, *gz_msg.mutable_specular());
+  convert_ros_to_gz(ros_msg.emissive, *gz_msg.mutable_emissive());
+
+  gz_msg.set_shininess(ros_msg.shininess);
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::MaterialColor & gz_msg,
+  ros_gz_interfaces::msg::MaterialColor & ros_msg)
+{
+  using EntityMatch = gz::msgs::MaterialColor::EntityMatch;
+  if (gz_msg.entity_match() == EntityMatch::MaterialColor_EntityMatch_FIRST) {
+    ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST;
+/* *INDENT-OFF* */
+  } else if (gz_msg.entity_match() ==
+    EntityMatch::MaterialColor_EntityMatch_ALL) {
+/* *INDENT-ON* */
+    ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL;
+  } else {
+    std::cerr << "Unsupported EntityMatch [" <<
+      gz_msg.entity_match() << "]" << std::endl;
+  }
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+  convert_gz_to_ros(gz_msg.entity(), ros_msg.entity);
+  convert_gz_to_ros(gz_msg.ambient(), ros_msg.ambient);
+  convert_gz_to_ros(gz_msg.diffuse(), ros_msg.diffuse);
+  convert_gz_to_ros(gz_msg.specular(), ros_msg.specular);
+  convert_gz_to_ros(gz_msg.emissive(), ros_msg.emissive);
+
+  ros_msg.shininess = gz_msg.shininess();
+}
+#endif  // HAVE_MATERIALCOLOR
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp
@@ -19,6 +19,10 @@
 #include <memory>
 #include <string>
 
+#if GZ_MSGS_MAJOR_VERSION >= 10
+#define GZ_MSGS_IMU_HAS_COVARIANCE
+#endif
+
 namespace ros_gz_bridge
 {
 namespace testing
@@ -1325,6 +1329,37 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _msg)
   EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity());
 }
 
+#if HAVE_MATERIALCOLOR
+void createTestMsg(gz::msgs::MaterialColor & _msg)
+{
+  createTestMsg(*_msg.mutable_header());
+  createTestMsg(*_msg.mutable_entity());
+  createTestMsg(*_msg.mutable_ambient());
+  createTestMsg(*_msg.mutable_diffuse());
+  createTestMsg(*_msg.mutable_specular());
+  createTestMsg(*_msg.mutable_emissive());
+
+  _msg.set_shininess(1.0);
+  _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL);
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::MaterialColor> & _msg)
+{
+  gz::msgs::MaterialColor expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
+  compareTestMsg(std::make_shared<gz::msgs::Entity>(_msg->entity()));
+  compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->ambient()));
+  compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->diffuse()));
+  compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->specular()));
+  compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->emissive()));
+
+  EXPECT_EQ(expected_msg.shininess(), _msg->shininess());
+  EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match());
+}
+#endif  // HAVE_MATERIALCOLOR
+
 void createTestMsg(gz::msgs::GUICamera & _msg)
 {
   gz::msgs::Header header_msg;

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp
@@ -73,6 +73,10 @@
 #include <gz/msgs/dataframe.pb.h>
 #endif  // HAVE_DATAFRAME
 
+#if HAVE_MATERIALCOLOR
+#include <gz/msgs/material_color.pb.h>
+#endif  // HAVE_MATERIALCOLOR
+
 namespace ros_gz_bridge
 {
 namespace testing
@@ -455,6 +459,16 @@ void createTestMsg(gz::msgs::Light & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _msg);
 
+#if HAVE_MATERIALCOLOR
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::MaterialColor & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::MaterialColor> & _msg);
+#endif  // HAVE_MATERIALCOLOR
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(gz::msgs::GUICamera & _msg);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -559,6 +559,36 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Light> & _msg)
   EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity);
 }
 
+#if HAVE_MATERIALCOLOR
+void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg)
+{
+  createTestMsg(_msg.header);
+  createTestMsg(_msg.entity);
+  createTestMsg(_msg.ambient);
+  createTestMsg(_msg.diffuse);
+  createTestMsg(_msg.specular);
+  createTestMsg(_msg.emissive);
+  _msg.shininess = 1.0;
+  _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL;
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::MaterialColor> & _msg)
+{
+  ros_gz_interfaces::msg::MaterialColor expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+  compareTestMsg(std::make_shared<ros_gz_interfaces::msg::Entity>(_msg->entity));
+  compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->ambient));
+  compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->diffuse));
+  compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->specular));
+  compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->emissive));
+
+  EXPECT_EQ(expected_msg.shininess, _msg->shininess);
+  EXPECT_EQ(expected_msg.entity_match, _msg->entity_match);
+}
+#endif  // HAVE_MATERIALCOLOR
+
 void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg)
 {
   createTestMsg(_msg.header);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -56,6 +56,9 @@
 #include <ros_gz_interfaces/msg/dataframe.hpp>
 #endif  // HAVE_DATAFRAME
 #include <ros_gz_interfaces/msg/light.hpp>
+#if HAVE_MATERIALCOLOR
+#include <ros_gz_interfaces/msg/material_color.hpp>
+#endif  // HAVE_MATERIALCOLOR
 #include <ros_gz_interfaces/msg/param_vec.hpp>
 #include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
@@ -376,6 +379,16 @@ void createTestMsg(ros_gz_interfaces::msg::Light & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Light> & _msg);
 
+#if HAVE_MATERIALCOLOR
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::MaterialColor> & _msg);
+#endif  // HAVE_MATERIALCOLOR
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ros_gz_interfaces::msg::Entity & _msg);

diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt
@@ -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
@@ -0,0 +1,12 @@
+# Entities that match to apply material color: constant definition
+uint8 FIRST     = 0
+uint8 ALL       = 1
+
+std_msgs/Header header            # Optional header data
+ros_gz_interfaces/Entity entity   # Entity to change material color
+std_msgs/ColorRGBA ambient        # Ambient color
+std_msgs/ColorRGBA diffuse        # Diffuse color
+std_msgs/ColorRGBA specular       # Specular color
+std_msgs/ColorRGBA emissive       # Emissive color
+float64 shininess                 # Specular exponent
+uint8 entity_match                # Entities that match to apply material color