From 7fea0cac47edc824009606d538ea3e8cf82c9edc Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 26 Mar 2024 22:12:05 -0400 Subject: [PATCH] Add option to change material color from ROS. 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 --- .../convert/ros_gz_interfaces.hpp | 19 ++++++ .../include/ros_gz_bridge/ros_gz_bridge.hpp | 7 +++ ros_gz_bridge/ros_gz_bridge/__init__.py | 10 +++- ros_gz_bridge/ros_gz_bridge/mappings.py | 6 ++ .../src/convert/ros_gz_interfaces.cpp | 60 +++++++++++++++++++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 35 +++++++++++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 14 +++++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 30 ++++++++++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 13 ++++ ros_gz_interfaces/CMakeLists.txt | 1 + ros_gz_interfaces/msg/MaterialColor.msg | 12 ++++ 11 files changed, 206 insertions(+), 1 deletion(-) 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..136c8d9b 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp @@ -56,6 +56,11 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#include +#endif // HAVE_MATERIALCOLOR + #include namespace ros_gz_bridge @@ -159,6 +164,20 @@ convert_gz_to_ros( const gz::msgs::Light & gz_msg, ros_gz_interfaces::msg::Light & ros_msg); +#if HAVE_MATERIALCOLOR +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg); +#endif // HAVE_MATERIALCOLOR + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp index 8d9ff631..71abf1d7 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp @@ -36,6 +36,13 @@ #define HAVE_DATAFRAME true #endif +// MaterialColor is available from versions 10.1.0 (Harmonic) forward +// This can be removed when the minimum supported version passes 10.1.0 +#if (GZ_MSGS_MAJOR_VERSION > 10) || \ + ((GZ_MSGS_MAJOR_VERSION == 10) && (GZ_MSGS_MINOR_VERSION >= 1)) +#define HAVE_MATERIALCOLOR true +#endif + namespace ros_gz_bridge { /// Forward declarations diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index c0aaa484..a438ad4f 100644 --- a/ros_gz_bridge/ros_gz_bridge/__init__.py +++ b/ros_gz_bridge/ros_gz_bridge/__init__.py @@ -16,7 +16,7 @@ import os -from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_8_4_0 +from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_10_1_0, MAPPINGS_8_4_0 from rosidl_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 index c8788c89..64d29f58 100644 --- 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 index fceb296a..0d619955 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -380,6 +380,66 @@ convert_gz_to_ros( ros_msg.intensity = gz_msg.intensity(); } +#if HAVE_MATERIALCOLOR +template<> +void +convert_ros_to_gz( + const ros_gz_interfaces::msg::MaterialColor & ros_msg, + gz::msgs::MaterialColor & gz_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + + switch (ros_msg.entity_match) { + case ros_gz_interfaces::msg::MaterialColor::FIRST: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST); + break; + case ros_gz_interfaces::msg::MaterialColor::ALL: + gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL); + break; + default: + std::cerr << "Unsupported entity match type [" + << ros_msg.entity_match << "]\n"; + } + + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header())); + convert_ros_to_gz(ros_msg.entity, *gz_msg.mutable_entity()); + convert_ros_to_gz(ros_msg.ambient, *gz_msg.mutable_ambient()); + convert_ros_to_gz(ros_msg.diffuse, *gz_msg.mutable_diffuse()); + convert_ros_to_gz(ros_msg.specular, *gz_msg.mutable_specular()); + convert_ros_to_gz(ros_msg.emissive, *gz_msg.mutable_emissive()); + + gz_msg.set_shininess(ros_msg.shininess); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::MaterialColor & gz_msg, + ros_gz_interfaces::msg::MaterialColor & ros_msg) +{ + using EntityMatch = gz::msgs::MaterialColor::EntityMatch; + if (gz_msg.entity_match() == EntityMatch::MaterialColor_EntityMatch_FIRST) { + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST; +/* *INDENT-OFF* */ + } else if (gz_msg.entity_match() == + EntityMatch::MaterialColor_EntityMatch_ALL) { +/* *INDENT-ON* */ + ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; + } else { + std::cerr << "Unsupported EntityMatch [" << + gz_msg.entity_match() << "]" << std::endl; + } + convert_gz_to_ros(gz_msg.header(), ros_msg.header); + convert_gz_to_ros(gz_msg.entity(), ros_msg.entity); + convert_gz_to_ros(gz_msg.ambient(), ros_msg.ambient); + convert_gz_to_ros(gz_msg.diffuse(), ros_msg.diffuse); + convert_gz_to_ros(gz_msg.specular(), ros_msg.specular); + convert_gz_to_ros(gz_msg.emissive(), ros_msg.emissive); + + ros_msg.shininess = gz_msg.shininess(); +} +#endif // HAVE_MATERIALCOLOR + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 505ed922..bf9bd7c1 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -19,6 +19,10 @@ #include #include +#if GZ_MSGS_MAJOR_VERSION >= 10 +#define GZ_MSGS_IMU_HAS_COVARIANCE +#endif + namespace ros_gz_bridge { namespace testing @@ -1325,6 +1329,37 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity()); } +#if HAVE_MATERIALCOLOR +void createTestMsg(gz::msgs::MaterialColor & _msg) +{ + createTestMsg(*_msg.mutable_header()); + createTestMsg(*_msg.mutable_entity()); + createTestMsg(*_msg.mutable_ambient()); + createTestMsg(*_msg.mutable_diffuse()); + createTestMsg(*_msg.mutable_specular()); + createTestMsg(*_msg.mutable_emissive()); + + _msg.set_shininess(1.0); + _msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL); +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + gz::msgs::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(std::make_shared(_msg->header())); + compareTestMsg(std::make_shared(_msg->entity())); + compareTestMsg(std::make_shared(_msg->ambient())); + compareTestMsg(std::make_shared(_msg->diffuse())); + compareTestMsg(std::make_shared(_msg->specular())); + compareTestMsg(std::make_shared(_msg->emissive())); + + EXPECT_EQ(expected_msg.shininess(), _msg->shininess()); + EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match()); +} +#endif // HAVE_MATERIALCOLOR + void createTestMsg(gz::msgs::GUICamera & _msg) { gz::msgs::Header header_msg; diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 7ec6984a..d939a4ff 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -73,6 +73,10 @@ #include #endif // HAVE_DATAFRAME +#if HAVE_MATERIALCOLOR +#include +#endif // HAVE_MATERIALCOLOR + namespace ros_gz_bridge { namespace testing @@ -455,6 +459,16 @@ void createTestMsg(gz::msgs::Light & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +#if HAVE_MATERIALCOLOR +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(gz::msgs::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); +#endif // HAVE_MATERIALCOLOR + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::GUICamera & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 6c30e98b..5942058a 100644 --- 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 & _msg) EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity); } +#if HAVE_MATERIALCOLOR +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg) +{ + createTestMsg(_msg.header); + createTestMsg(_msg.entity); + createTestMsg(_msg.ambient); + createTestMsg(_msg.diffuse); + createTestMsg(_msg.specular); + createTestMsg(_msg.emissive); + _msg.shininess = 1.0; + _msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL; +} + +void compareTestMsg(const std::shared_ptr & _msg) +{ + ros_gz_interfaces::msg::MaterialColor expected_msg; + createTestMsg(expected_msg); + + compareTestMsg(_msg->header); + compareTestMsg(std::make_shared(_msg->entity)); + compareTestMsg(std::make_shared(_msg->ambient)); + compareTestMsg(std::make_shared(_msg->diffuse)); + compareTestMsg(std::make_shared(_msg->specular)); + compareTestMsg(std::make_shared(_msg->emissive)); + + EXPECT_EQ(expected_msg.shininess, _msg->shininess); + EXPECT_EQ(expected_msg.entity_match, _msg->entity_match); +} +#endif // HAVE_MATERIALCOLOR + void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg) { createTestMsg(_msg.header); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index d1e6caf2..9c2b9cd8 100644 --- 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 #endif // HAVE_DATAFRAME #include +#if HAVE_MATERIALCOLOR +#include +#endif // HAVE_MATERIALCOLOR #include #include #include @@ -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 & _msg); +#if HAVE_MATERIALCOLOR +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); +#endif // HAVE_MATERIALCOLOR + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); diff --git a/ros_gz_interfaces/CMakeLists.txt b/ros_gz_interfaces/CMakeLists.txt index 49c49126..5f3b312e 100644 --- a/ros_gz_interfaces/CMakeLists.txt +++ b/ros_gz_interfaces/CMakeLists.txt @@ -27,6 +27,7 @@ set(msg_files "msg/GuiCamera.msg" "msg/JointWrench.msg" "msg/Light.msg" + "msg/MaterialColor.msg" "msg/ParamVec.msg" "msg/SensorNoise.msg" "msg/StringVec.msg" diff --git a/ros_gz_interfaces/msg/MaterialColor.msg b/ros_gz_interfaces/msg/MaterialColor.msg new file mode 100644 index 00000000..cda67d27 --- /dev/null +++ b/ros_gz_interfaces/msg/MaterialColor.msg @@ -0,0 +1,12 @@ +# Entities that match to apply material color: constant definition +uint8 FIRST = 0 +uint8 ALL = 1 + +std_msgs/Header header # Optional header data +ros_gz_interfaces/Entity entity # Entity to change material color +std_msgs/ColorRGBA ambient # Ambient color +std_msgs/ColorRGBA diffuse # Diffuse color +std_msgs/ColorRGBA specular # Specular color +std_msgs/ColorRGBA emissive # Emissive color +float64 shininess # Specular exponent +uint8 entity_match # Entities that match to apply material color