Change an entities visual material color by topic.

src/systems/user_commands/UserCommands.cc

@@ -1,5 +1,7 @@
 /*
  * Copyright (C) 2019 Open Source Robotics Foundation
+ * Copyright (C) 2024 CogniPilot Foundation
+ * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -33,6 +35,7 @@
 #include <gz/msgs/entity_factory.pb.h>
 #include <gz/msgs/entity_factory_v.pb.h>
 #include <gz/msgs/light.pb.h>
+#include <gz/msgs/material_color.pb.h>
 #include <gz/msgs/physics.pb.h>
 #include <gz/msgs/pose.pb.h>
 #include <gz/msgs/pose_v.pb.h>
@@ -45,6 +48,7 @@
 #include <unordered_set>
 #include <vector>
 
+#include <gz/math/Color.hh>
 #include <gz/math/SphericalCoordinates.hh>
 #include <gz/msgs/Utility.hh>
 
@@ -458,6 +462,10 @@
   /// \param[in] _msg Light message
   public: void OnCmdLight(const msgs::Light &_msg);
 
+  /// \brief Callback for MaterialColor subscription
+  /// \param[in] _msg MaterialColor message
+  public: void OnCmdMaterialColor(const msgs::MaterialColor &_msg);
+
   /// \brief Callback for pose service
   /// \param[in] _req Request containing pose update of an entity.
   /// \param[out] _res True if message successfully received and queued.
@@ -666,6 +674,11 @@
   this->dataPtr->node.Subscribe(lightTopic, &UserCommandsPrivate::OnCmdLight,
                                 this->dataPtr.get());
 
+  std::string materialColorTopic{
+    "/world/" + validWorldName + "/material_color"};
+  this->dataPtr->node.Subscribe(materialColorTopic,
+      &UserCommandsPrivate::OnCmdMaterialColor, this->dataPtr.get());
+
   // Physics service
   std::string physicsService{"/world/" + validWorldName + "/set_physics"};
   this->dataPtr->node.Advertise(physicsService,
@@ -953,6 +966,56 @@
   return true;
 }
 
+//////////////////////////////////////////////////
+void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg)
+{
+  msgs::Visual _req;
+  int numberOfEntities = 0;
+  auto entities = entitiesFromScopedName(_msg.entity().name(),
+      *this->iface->ecm);
+  if (entities.empty())
+  {
+    gzwarn << "Entity name: " << _msg.entity().name() << ", is not found."
+           << std::endl;
+    return;
+  }
+  for (const Entity &id : entities)
+  {
+    if ((numberOfEntities > 0) && (_msg.entity_match() !=
+      gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL))
+    {
+      break;
+    }
+    numberOfEntities++;
+    auto msg = _req.New();
+    msg->set_id(id);
+    msg->mutable_material()->mutable_ambient()->set_r(_msg.ambient().r());
+    msg->mutable_material()->mutable_ambient()->set_g(_msg.ambient().g());
+    msg->mutable_material()->mutable_ambient()->set_b(_msg.ambient().b());
+    msg->mutable_material()->mutable_ambient()->set_a(_msg.ambient().a());
+    msg->mutable_material()->mutable_diffuse()->set_r(_msg.diffuse().r());
+    msg->mutable_material()->mutable_diffuse()->set_g(_msg.diffuse().g());
+    msg->mutable_material()->mutable_diffuse()->set_b(_msg.diffuse().b());
+    msg->mutable_material()->mutable_diffuse()->set_a(_msg.diffuse().a());
+    msg->mutable_material()->mutable_specular()->set_r(_msg.specular().r());
+    msg->mutable_material()->mutable_specular()->set_g(_msg.specular().g());
+    msg->mutable_material()->mutable_specular()->set_b(_msg.specular().b());
+    msg->mutable_material()->mutable_specular()->set_a(_msg.specular().a());
+    msg->mutable_material()->mutable_emissive()->set_r(_msg.emissive().r());
+    msg->mutable_material()->mutable_emissive()->set_g(_msg.emissive().g());
+    msg->mutable_material()->mutable_emissive()->set_b(_msg.emissive().b());
+    msg->mutable_material()->mutable_emissive()->set_a(_msg.emissive().a());
+
+    auto cmd = std::make_unique<VisualCommand>(msg, this->iface);
+
+    // Push to pending
+    {
+      std::lock_guard<std::mutex> lock(this->pendingMutex);
+      this->pendingCmds.push_back(std::move(cmd));
+    }
+  }
+}
+
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::WheelSlipService(
     const msgs::WheelSlipParametersCmd &_req,

test/integration/user_commands.cc

@@ -21,6 +21,7 @@
 #include <gz/msgs/entity.pb.h>
 #include <gz/msgs/entity_factory.pb.h>
 #include <gz/msgs/light.pb.h>
+#include <gz/msgs/material_color.pb.h>
 #include <gz/msgs/physics.pb.h>
 #include <gz/msgs/pose.pb.h>
 #include <gz/msgs/pose_v.pb.h>
@@ -1046,6 +1047,87 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light))
     spotLightComp->Data().Diffuse());
 }
 
+/////////////////////////////////////////////////
+TEST_F(UserCommandsTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MaterialColor))
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = gz::common::joinPaths(
+    std::string(PROJECT_SOURCE_PATH), "test", "worlds", "material_color.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system just to get the ECM
+  EntityComponentManager *ecm{nullptr};
+  test::Relay testSystem;
+  testSystem.OnPreUpdate([&](const sim::UpdateInfo &,
+                             sim::EntityComponentManager &_ecm)
+      {
+        ecm = &_ecm;
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+
+  // Run server and check we have the ECM
+  EXPECT_EQ(nullptr, ecm);
+  server.Run(true, 1, false);
+  EXPECT_NE(nullptr, ecm);
+
+  transport::Node node;
+
+  // box
+  auto sphereVisualEntity =
+    ecm->EntityByComponents(components::Name("sphere_visual"));
+  ASSERT_NE(kNullEntity, sphereVisualEntity);
+
+  // check box visual's initial values
+  auto sphereVisualComp =
+    ecm->Component<components::Material>(sphereVisualEntity);
+  ASSERT_NE(nullptr, sphereVisualComp);
+  EXPECT_EQ(math::Color(0.3f, 0.3f, 0.3f, 1.0f),
+            sphereVisualComp->Data().Diffuse());
+
+  // Test material_color topic
+  const std::string materialColorTopic =
+    "/world/material_color/material_color";
+
+  // Test first return logic (no direct compare as returns unordered set)
+  msgs::MaterialColor materialColorMsgFirst;
+  materialColorMsgFirst.mutable_entity()->set_name("sphere_visual");
+  materialColorMsgFirst.set_entity_match(
+    gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_FIRST);
+  gz::msgs::Set(materialColorMsgFirst.mutable_diffuse(),
+    gz::math::Color(0.0f, 0.0f, 0.0f, 1.0f));
+
+  // Publish material color
+  auto pub = node.Advertise<msgs::MaterialColor>(materialColorTopic);
+  pub.Publish(materialColorMsgFirst);
+  server.Run(true, 100, false);
+  // Sleep for a small duration to allow Run thread to start
+  GZ_SLEEP_MS(100);
+
+  msgs::MaterialColor materialColorMsg;
+  materialColorMsg.mutable_entity()->set_name("sphere_visual");
+  materialColorMsg.set_entity_match(
+    gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL);
+  gz::msgs::Set(materialColorMsg.mutable_diffuse(),
+    gz::math::Color(1.0f, 1.0f, 1.0f, 1.0f));
+
+  // Publish material color
+  pub.Publish(materialColorMsg);
+  server.Run(true, 100, false);
+  // Sleep for a small duration to allow Run thread to start
+  GZ_SLEEP_MS(100);
+
+  auto updatedVisual =
+    ecm->Component<components::Material>(sphereVisualEntity);
+  EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f),
+            updatedVisual->Data().Diffuse());
+}
+
 /////////////////////////////////////////////////
 TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics))
 {

test/worlds/material_color.sdf

@@ -0,0 +1,76 @@
+<?xml version="1.0" ?>
+<sdf version="1.10">
+  <world name="material_color">
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <model name="sphere">
+      <pose>0 0.0 0.0 0 0 0</pose>
+      <link name="sphere_link">
+        <!-- Added a render sensor to trigger RenderUtil:Update -->
+        <sensor name="camera" type="camera">
+          <pose>1 0 1.3 0 0 0</pose>
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>false</visualize>
+          <topic>camera</topic>
+        </sensor>
+        <visual name="sphere_visual">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <ambient>0.3 0.3 0.3 1</ambient>
+            <diffuse>0.3 0.3 0.3 1</diffuse>
+            <specular>0.3 0.3 0.3 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <model name="sphere_1">
+      <pose>0.5 1.0 0.0 0 0 0</pose>
+      <link name="sphere_link">
+        <visual name="sphere_visual">
+          <geometry>
+            <sphere>
+              <radius>0.5</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <ambient>0.3 0.3 0.3 1</ambient>
+            <diffuse>0.3 0.3 0.3 1</diffuse>
+            <specular>0.3 0.3 0.3 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>