Skip to content

Commit 7fea0ca

Browse files
bperseghettibperseghetti
committed
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]>
1 parent b986388 commit 7fea0ca

File tree

11 files changed

+206
-1
lines changed

11 files changed

+206
-1
lines changed

‎ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

+19
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,11 @@
5656
#include <ros_gz_interfaces/msg/dataframe.hpp>
5757
#endif // HAVE_DATAFRAME
5858

59+
#if HAVE_MATERIALCOLOR
60+
#include <gz/msgs/material_color.pb.h>
61+
#include <ros_gz_interfaces/msg/material_color.hpp>
62+
#endif // HAVE_MATERIALCOLOR
63+
5964
#include <ros_gz_bridge/convert_decl.hpp>
6065

6166
namespace ros_gz_bridge
@@ -159,6 +164,20 @@ convert_gz_to_ros(
159164
const gz::msgs::Light & gz_msg,
160165
ros_gz_interfaces::msg::Light & ros_msg);
161166

167+
#if HAVE_MATERIALCOLOR
168+
template<>
169+
void
170+
convert_ros_to_gz(
171+
const ros_gz_interfaces::msg::MaterialColor & ros_msg,
172+
gz::msgs::MaterialColor & gz_msg);
173+
174+
template<>
175+
void
176+
convert_gz_to_ros(
177+
const gz::msgs::MaterialColor & gz_msg,
178+
ros_gz_interfaces::msg::MaterialColor & ros_msg);
179+
#endif // HAVE_MATERIALCOLOR
180+
162181
template<>
163182
void
164183
convert_ros_to_gz(

‎ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp

+7
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,13 @@
3636
#define HAVE_DATAFRAME true
3737
#endif
3838

39+
// MaterialColor is available from versions 10.1.0 (Harmonic) forward
40+
// This can be removed when the minimum supported version passes 10.1.0
41+
#if (GZ_MSGS_MAJOR_VERSION > 10) || \
42+
((GZ_MSGS_MAJOR_VERSION == 10) && (GZ_MSGS_MINOR_VERSION >= 1))
43+
#define HAVE_MATERIALCOLOR true
44+
#endif
45+
3946
namespace ros_gz_bridge
4047
{
4148
/// Forward declarations

‎ros_gz_bridge/ros_gz_bridge/__init__.py

+9-1
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616

1717
import os
1818

19-
from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_8_4_0
19+
from ros_gz_bridge.mappings import MAPPINGS, MAPPINGS_10_1_0, MAPPINGS_8_4_0
2020

2121
from rosidl_pycommon import expand_template
2222

@@ -75,6 +75,14 @@ def mappings(gz_msgs_ver):
7575
ros2_message_name=mapping.ros_type,
7676
gz_message_name=mapping.gz_type
7777
))
78+
if gz_msgs_ver >= (10, 1, 0):
79+
for (ros2_package_name, mappings) in MAPPINGS_10_1_0.items():
80+
for mapping in sorted(mappings):
81+
data.append(MessageMapping(
82+
ros2_package_name=ros2_package_name,
83+
ros2_message_name=mapping.ros_type,
84+
gz_message_name=mapping.gz_type
85+
))
7886
return sorted(data, key=lambda mm: mm.ros2_string())
7987

8088

‎ros_gz_bridge/ros_gz_bridge/mappings.py

+6
Original file line numberDiff line numberDiff line change
@@ -113,3 +113,9 @@
113113
Mapping('Dataframe', 'Dataframe'),
114114
],
115115
}
116+
117+
MAPPINGS_10_1_0 = {
118+
'ros_gz_interfaces': [
119+
Mapping('MaterialColor', 'MaterialColor'),
120+
],
121+
}

‎ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

+60
Original file line numberDiff line numberDiff line change
@@ -380,6 +380,66 @@ convert_gz_to_ros(
380380
ros_msg.intensity = gz_msg.intensity();
381381
}
382382

383+
#if HAVE_MATERIALCOLOR
384+
template<>
385+
void
386+
convert_ros_to_gz(
387+
const ros_gz_interfaces::msg::MaterialColor & ros_msg,
388+
gz::msgs::MaterialColor & gz_msg)
389+
{
390+
using EntityMatch = gz::msgs::MaterialColor::EntityMatch;
391+
392+
switch (ros_msg.entity_match) {
393+
case ros_gz_interfaces::msg::MaterialColor::FIRST:
394+
gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_FIRST);
395+
break;
396+
case ros_gz_interfaces::msg::MaterialColor::ALL:
397+
gz_msg.set_entity_match(EntityMatch::MaterialColor_EntityMatch_ALL);
398+
break;
399+
default:
400+
std::cerr << "Unsupported entity match type ["
401+
<< ros_msg.entity_match << "]\n";
402+
}
403+
404+
convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
405+
convert_ros_to_gz(ros_msg.entity, *gz_msg.mutable_entity());
406+
convert_ros_to_gz(ros_msg.ambient, *gz_msg.mutable_ambient());
407+
convert_ros_to_gz(ros_msg.diffuse, *gz_msg.mutable_diffuse());
408+
convert_ros_to_gz(ros_msg.specular, *gz_msg.mutable_specular());
409+
convert_ros_to_gz(ros_msg.emissive, *gz_msg.mutable_emissive());
410+
411+
gz_msg.set_shininess(ros_msg.shininess);
412+
}
413+
414+
template<>
415+
void
416+
convert_gz_to_ros(
417+
const gz::msgs::MaterialColor & gz_msg,
418+
ros_gz_interfaces::msg::MaterialColor & ros_msg)
419+
{
420+
using EntityMatch = gz::msgs::MaterialColor::EntityMatch;
421+
if (gz_msg.entity_match() == EntityMatch::MaterialColor_EntityMatch_FIRST) {
422+
ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::FIRST;
423+
/* *INDENT-OFF* */
424+
} else if (gz_msg.entity_match() ==
425+
EntityMatch::MaterialColor_EntityMatch_ALL) {
426+
/* *INDENT-ON* */
427+
ros_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL;
428+
} else {
429+
std::cerr << "Unsupported EntityMatch [" <<
430+
gz_msg.entity_match() << "]" << std::endl;
431+
}
432+
convert_gz_to_ros(gz_msg.header(), ros_msg.header);
433+
convert_gz_to_ros(gz_msg.entity(), ros_msg.entity);
434+
convert_gz_to_ros(gz_msg.ambient(), ros_msg.ambient);
435+
convert_gz_to_ros(gz_msg.diffuse(), ros_msg.diffuse);
436+
convert_gz_to_ros(gz_msg.specular(), ros_msg.specular);
437+
convert_gz_to_ros(gz_msg.emissive(), ros_msg.emissive);
438+
439+
ros_msg.shininess = gz_msg.shininess();
440+
}
441+
#endif // HAVE_MATERIALCOLOR
442+
383443
template<>
384444
void
385445
convert_ros_to_gz(

‎ros_gz_bridge/test/utils/gz_test_msg.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,10 @@
1919
#include <memory>
2020
#include <string>
2121

22+
#if GZ_MSGS_MAJOR_VERSION >= 10
23+
#define GZ_MSGS_IMU_HAS_COVARIANCE
24+
#endif
25+
2226
namespace ros_gz_bridge
2327
{
2428
namespace testing
@@ -1325,6 +1329,37 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _msg)
13251329
EXPECT_FLOAT_EQ(expected_msg.intensity(), _msg->intensity());
13261330
}
13271331

1332+
#if HAVE_MATERIALCOLOR
1333+
void createTestMsg(gz::msgs::MaterialColor & _msg)
1334+
{
1335+
createTestMsg(*_msg.mutable_header());
1336+
createTestMsg(*_msg.mutable_entity());
1337+
createTestMsg(*_msg.mutable_ambient());
1338+
createTestMsg(*_msg.mutable_diffuse());
1339+
createTestMsg(*_msg.mutable_specular());
1340+
createTestMsg(*_msg.mutable_emissive());
1341+
1342+
_msg.set_shininess(1.0);
1343+
_msg.set_entity_match(gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL);
1344+
}
1345+
1346+
void compareTestMsg(const std::shared_ptr<gz::msgs::MaterialColor> & _msg)
1347+
{
1348+
gz::msgs::MaterialColor expected_msg;
1349+
createTestMsg(expected_msg);
1350+
1351+
compareTestMsg(std::make_shared<gz::msgs::Header>(_msg->header()));
1352+
compareTestMsg(std::make_shared<gz::msgs::Entity>(_msg->entity()));
1353+
compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->ambient()));
1354+
compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->diffuse()));
1355+
compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->specular()));
1356+
compareTestMsg(std::make_shared<gz::msgs::Color>(_msg->emissive()));
1357+
1358+
EXPECT_EQ(expected_msg.shininess(), _msg->shininess());
1359+
EXPECT_EQ(expected_msg.entity_match(), _msg->entity_match());
1360+
}
1361+
#endif // HAVE_MATERIALCOLOR
1362+
13281363
void createTestMsg(gz::msgs::GUICamera & _msg)
13291364
{
13301365
gz::msgs::Header header_msg;

‎ros_gz_bridge/test/utils/gz_test_msg.hpp

+14
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,10 @@
7373
#include <gz/msgs/dataframe.pb.h>
7474
#endif // HAVE_DATAFRAME
7575

76+
#if HAVE_MATERIALCOLOR
77+
#include <gz/msgs/material_color.pb.h>
78+
#endif // HAVE_MATERIALCOLOR
79+
7680
namespace ros_gz_bridge
7781
{
7882
namespace testing
@@ -455,6 +459,16 @@ void createTestMsg(gz::msgs::Light & _msg);
455459
/// \param[in] _msg The message to compare.
456460
void compareTestMsg(const std::shared_ptr<gz::msgs::Light> & _msg);
457461

462+
#if HAVE_MATERIALCOLOR
463+
/// \brief Create a message used for testing.
464+
/// \param[out] _msg The message populated.
465+
void createTestMsg(gz::msgs::MaterialColor & _msg);
466+
467+
/// \brief Compare a message with the populated for testing.
468+
/// \param[in] _msg The message to compare.
469+
void compareTestMsg(const std::shared_ptr<gz::msgs::MaterialColor> & _msg);
470+
#endif // HAVE_MATERIALCOLOR
471+
458472
/// \brief Create a message used for testing.
459473
/// \param[out] _msg The message populated.
460474
void createTestMsg(gz::msgs::GUICamera & _msg);

‎ros_gz_bridge/test/utils/ros_test_msg.cpp

+30
Original file line numberDiff line numberDiff line change
@@ -559,6 +559,36 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Light> & _msg)
559559
EXPECT_FLOAT_EQ(expected_msg.intensity, _msg->intensity);
560560
}
561561

562+
#if HAVE_MATERIALCOLOR
563+
void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg)
564+
{
565+
createTestMsg(_msg.header);
566+
createTestMsg(_msg.entity);
567+
createTestMsg(_msg.ambient);
568+
createTestMsg(_msg.diffuse);
569+
createTestMsg(_msg.specular);
570+
createTestMsg(_msg.emissive);
571+
_msg.shininess = 1.0;
572+
_msg.entity_match = ros_gz_interfaces::msg::MaterialColor::ALL;
573+
}
574+
575+
void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::MaterialColor> & _msg)
576+
{
577+
ros_gz_interfaces::msg::MaterialColor expected_msg;
578+
createTestMsg(expected_msg);
579+
580+
compareTestMsg(_msg->header);
581+
compareTestMsg(std::make_shared<ros_gz_interfaces::msg::Entity>(_msg->entity));
582+
compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->ambient));
583+
compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->diffuse));
584+
compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->specular));
585+
compareTestMsg(std::make_shared<std_msgs::msg::ColorRGBA>(_msg->emissive));
586+
587+
EXPECT_EQ(expected_msg.shininess, _msg->shininess);
588+
EXPECT_EQ(expected_msg.entity_match, _msg->entity_match);
589+
}
590+
#endif // HAVE_MATERIALCOLOR
591+
562592
void createTestMsg(ros_gz_interfaces::msg::GuiCamera & _msg)
563593
{
564594
createTestMsg(_msg.header);

‎ros_gz_bridge/test/utils/ros_test_msg.hpp

+13
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,9 @@
5656
#include <ros_gz_interfaces/msg/dataframe.hpp>
5757
#endif // HAVE_DATAFRAME
5858
#include <ros_gz_interfaces/msg/light.hpp>
59+
#if HAVE_MATERIALCOLOR
60+
#include <ros_gz_interfaces/msg/material_color.hpp>
61+
#endif // HAVE_MATERIALCOLOR
5962
#include <ros_gz_interfaces/msg/param_vec.hpp>
6063
#include <ros_gz_interfaces/msg/sensor_noise.hpp>
6164
#include <ros_gz_interfaces/msg/string_vec.hpp>
@@ -376,6 +379,16 @@ void createTestMsg(ros_gz_interfaces::msg::Light & _msg);
376379
/// \param[in] _msg The message to compare.
377380
void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Light> & _msg);
378381

382+
#if HAVE_MATERIALCOLOR
383+
/// \brief Create a message used for testing.
384+
/// \param[out] _msg The message populated.
385+
void createTestMsg(ros_gz_interfaces::msg::MaterialColor & _msg);
386+
387+
/// \brief Compare a message with the populated for testing.
388+
/// \param[in] _msg The message to compare.
389+
void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::MaterialColor> & _msg);
390+
#endif // HAVE_MATERIALCOLOR
391+
379392
/// \brief Create a message used for testing.
380393
/// \param[out] _msg The message populated.
381394
void createTestMsg(ros_gz_interfaces::msg::Entity & _msg);

‎ros_gz_interfaces/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ set(msg_files
2727
"msg/GuiCamera.msg"
2828
"msg/JointWrench.msg"
2929
"msg/Light.msg"
30+
"msg/MaterialColor.msg"
3031
"msg/ParamVec.msg"
3132
"msg/SensorNoise.msg"
3233
"msg/StringVec.msg"

‎ros_gz_interfaces/msg/MaterialColor.msg

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
# Entities that match to apply material color: constant definition
2+
uint8 FIRST = 0
3+
uint8 ALL = 1
4+
5+
std_msgs/Header header # Optional header data
6+
ros_gz_interfaces/Entity entity # Entity to change material color
7+
std_msgs/ColorRGBA ambient # Ambient color
8+
std_msgs/ColorRGBA diffuse # Diffuse color
9+
std_msgs/ColorRGBA specular # Specular color
10+
std_msgs/ColorRGBA emissive # Emissive color
11+
float64 shininess # Specular exponent
12+
uint8 entity_match # Entities that match to apply material color

0 commit comments

Comments
 (0)