Set per-point color on markers

Signed-off-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Nov 25, 2021 · b7e3a24 · b7e3a24
1 parent 875b551
commit b7e3a24
Showing 1 changed file with 7 additions and 7 deletions.
diff --git a/src/plugins/marker_manager/MarkerManager.cc b/src/plugins/marker_manager/MarkerManager.cc
@@ -286,7 +286,8 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg)
 {
   // Get the namespace, if it exists. Otherwise, use the global namespace
   std::string ns;
-  if (!_msg.ns().empty()) {
+  if (!_msg.ns().empty())
+  {
     ns = _msg.ns();
   }
 
@@ -529,12 +530,6 @@ void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg,
     _markerPtr->ClearPoints();
   }
 
-  math::Color color(
-      _msg.material().diffuse().r(),
-      _msg.material().diffuse().g(),
-      _msg.material().diffuse().b(),
-      _msg.material().diffuse().a());
-
   // Set Marker Points
   for (int i = 0; i < _msg.point().size(); ++i)
   {
@@ -543,6 +538,11 @@ void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg,
         _msg.point(i).y(),
         _msg.point(i).z());
 
+    math::Color color = msgs::Convert(_msg.material().diffuse());
+    if (i < _msg.materials().size())
+    {
+      color = msgs::Convert(_msg.materials(i).diffuse());
+    }
     _markerPtr->AddPoint(vector, color);
   }
   if (_msg.has_scale())