From d31ab552f1ef618de20bb7eae72f1d3df7082878 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 6 May 2024 13:59:36 -0400 Subject: [PATCH] Add option to change material color from ROS. (#521) Forward port of #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 (cherry picked from commit 78dc4823121f085594e6028a93f1e571eb04f58b) --- .../convert/ros_gz_interfaces.hpp | 14 +++++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + .../src/convert/ros_gz_interfaces.cpp | 58 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 29 ++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 9 +++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 28 +++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 +++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/msg/MaterialColor.msg | 12 ++++ 9 files changed, 161 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 426894a0..9b17d0cf 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 @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -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( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 808ee5c6..2414586f 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/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'), diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 8da4225c..ae4665e5 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/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( diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index f1a0b0ff..6308a103 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -1344,6 +1344,35 @@ void compareTestMsg(const std::shared_ptr & _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 & _msg) +{ + gz::msgs::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_msg->ambient())); + compareTestMsg(std::make_shared(_msg->diffuse())); + compareTestMsg(std::make_shared(_msg->specular())); + compareTestMsg(std::make_shared(_msg->emissive())); + + EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); + EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); +} + 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 3fd7d623..0f9b712a 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 @@ -451,6 +452,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 90b90bdd..42afcb25 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -622,6 +622,34 @@ 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); + createTestMsg(_msg.entity); + createTestMsg(_msg.ambient); + createTestMsg(_msg.diffuse); + createTestMsg(_msg.specular); + createTestMsg(_msg.emissive); + _msg.shininess = 1.0; + _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_msg->ambient)); + compareTestMsg(std::make_shared(_msg->diffuse)); + compareTestMsg(std::make_shared(_msg->specular)); + compareTestMsg(std::make_shared(_msg->emissive)); + + EXPECT_EQ(expected_msg.shininess, _msg->shininess); + EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); +} + 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 eb42d2fb..43c127e9 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -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 & _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); diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index 49c49126..5f3b312e 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/ros_gz_interfaces/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/GuiCamera.msg" "msg/JointWrench.msg" "msg/Light.msg" + "msg/MaterialColor.msg" "msg/ParamVec.msg" "msg/SensorNoise.msg" "msg/StringVec.msg" diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg new file mode 100644 index 00000000..cda67d27 --- /dev/null +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -0,0 +1,12 @@ +# Entities that match to apply material color: constant definition +uint8 FIRST = 0 +uint8 ALL = 1 + +std_msgs/Header header # Optional header data +ros_gz_interfaces/Entity entity # Entity to change material color +std_msgs/ColorRGBA ambient # Ambient color +std_msgs/ColorRGBA diffuse # Diffuse color +std_msgs/ColorRGBA specular # Specular color +std_msgs/ColorRGBA emissive # Emissive color +float64 shininess # Specular exponent +uint8 entity_match # Entities that match to apply material color