Add option to change material color from ROS. (gazebosim#521)

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]>

Author: bperseghetti

ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

@@ -25,6 +25,7 @@
 #include <gz/msgs/float_v.pb.h>
 #include <gz/msgs/gui_camera.pb.h>
 #include <gz/msgs/light.pb.h>
+#include <gz/msgs/material_color.pb.h>
 #include <gz/msgs/param.pb.h>
 #include <gz/msgs/param_v.pb.h>
 #include <gz/msgs/sensor_noise.pb.h>
@@ -43,6 +44,7 @@
 #include <ros_gz_interfaces/msg/float32_array.hpp>
 #include <ros_gz_interfaces/msg/gui_camera.hpp>
 #include <ros_gz_interfaces/msg/light.hpp>
+#include <ros_gz_interfaces/msg/material_color.hpp>
 #include <ros_gz_interfaces/msg/param_vec.hpp>
 #include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
@@ -153,6 +155,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(

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -68,6 +68,7 @@
         Mapping('GuiCamera', 'GUICamera'),
         Mapping('JointWrench', 'JointWrench'),
         Mapping('Light', 'Light'),
+        Mapping('MaterialColor', 'MaterialColor'),
         Mapping('ParamVec', 'Param'),
         Mapping('ParamVec', 'Param_V'),
         Mapping('SensorNoise', 'SensorNoise'),

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -378,6 +378,64 @@ 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)
+{
+  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();
+}
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -1344,6 +1344,35 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _msg)
   EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity());
 }
 
+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());
+}
+
 void createTestMsg(gz::msgs::GUICamera & _msg)
 {
   gz::msgs::Header header_msg;

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -43,6 +43,7 @@
 #include <gz/msgs/laserscan.pb.h>
 #include <gz/msgs/light.pb.h>
 #include <gz/msgs/magnetometer.pb.h>
+#include <gz/msgs/material_color.pb.h>
 #include <gz/msgs/model.pb.h>
 #include <gz/msgs/navsat.pb.h>
 #include <gz/msgs/odometry.pb.h>
@@ -451,6 +452,14 @@ void createTestMsg(gz::msgs::Light & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _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<gz::msgs::MaterialColor> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(gz::msgs::GUICamera & _msg);

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -622,6 +622,34 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Light> & _msg)
   EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity);
 }
 
+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);
+}
+
 void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg)
 {
   createTestMsg(_msg.header);

ros_gz_bridge/test/utils/ros_test_msg.hpp

@@ -57,6 +57,7 @@
 #include <ros_gz_interfaces/msg/float32_array.hpp>
 #include <ros_gz_interfaces/msg/dataframe.hpp>
 #include <ros_gz_interfaces/msg/light.hpp>
+#include <ros_gz_interfaces/msg/material_color.hpp>
 #include <ros_gz_interfaces/msg/param_vec.hpp>
 #include <ros_gz_interfaces/msg/sensor_noise.hpp>
 #include <ros_gz_interfaces/msg/string_vec.hpp>
@@ -408,6 +409,14 @@ 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);
 
+/// \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);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ros_gz_interfaces::msg::Entity & _msg);

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"

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