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>
 
@@ -326,6 +330,12 @@
   public: VisualCommand(msgs::Visual *_msg,
       std::shared_ptr<UserCommandsInterface> &_iface);
 
+  /// \brief Constructor
+  /// \param[in] _msg Message containing the material color parameters.
+  /// \param[in] _iface Pointer to user commands interface.
+  public: VisualCommand(msgs::MaterialColor *_msg,
+      std::shared_ptr<UserCommandsInterface> &_iface);
+
   // Documentation inherited
   public: bool Execute() final;
 
@@ -458,6 +468,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 +680,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 +972,21 @@
   return true;
 }
 
+//////////////////////////////////////////////////
+void UserCommandsPrivate::OnCmdMaterialColor(const msgs::MaterialColor &_msg)
+{
+  auto msg = _msg.New();
+  msg->CopyFrom(_msg);
+  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));
+  }
+
+  return;
+}
+
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::WheelSlipService(
     const msgs::WheelSlipParametersCmd &_req,
@@ -1720,57 +1754,128 @@
 {
 }
 
+//////////////////////////////////////////////////
+VisualCommand::VisualCommand(msgs::MaterialColor *_msg,
+    std::shared_ptr<UserCommandsInterface> &_iface)
+    : UserCommandBase(_msg, _iface)
+{
+}
+
 //////////////////////////////////////////////////
 bool VisualCommand::Execute()
 {
   auto visualMsg = dynamic_cast<const msgs::Visual *>(this->msg);
-  if (nullptr == visualMsg)
+  auto materialColorMsg = dynamic_cast<const msgs::MaterialColor *>(this->msg);
+  if (visualMsg != nullptr)
   {
-    gzerr << "Internal error, null visual message" << std::endl;
-    return false;
-  }
+    Entity visualEntity = kNullEntity;
+    if (visualMsg->id() != kNullEntity)
+    {
+      visualEntity = visualMsg->id();
+    }
+    else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty())
+    {
+      Entity parentEntity =
+        this->iface->ecm->EntityByComponents(
+          components::Name(visualMsg->parent_name()));
 
-  Entity visualEntity = kNullEntity;
-  if (visualMsg->id() != kNullEntity)
-  {
-    visualEntity = visualMsg->id();
-  }
-  else if (!visualMsg->name().empty() && !visualMsg->parent_name().empty())
-  {
-    Entity parentEntity =
-      this->iface->ecm->EntityByComponents(
-        components::Name(visualMsg->parent_name()));
+      auto entities =
+        this->iface->ecm->ChildrenByComponents(parentEntity,
+          components::Name(visualMsg->name()));
+
+      // When size > 1, we don't know which entity to modify
+      if (entities.size() == 1)
+      {
+        visualEntity = entities[0];
+      }
+    }
 
-    auto entities =
-      this->iface->ecm->ChildrenByComponents(parentEntity,
-        components::Name(visualMsg->name()));
+    if (visualEntity == kNullEntity)
+    {
+      gzerr << "Failed to find visual entity" << std::endl;
+      return false;
+    }
 
-    // When size > 1, we don't know which entity to modify
-    if (entities.size() == 1)
+    auto visualCmdComp =
+        this->iface->ecm->Component<components::VisualCmd>(visualEntity);
+    if (!visualCmdComp)
     {
-      visualEntity = entities[0];
+      this->iface->ecm->CreateComponent(
+          visualEntity, components::VisualCmd(*visualMsg));
+    }
+    else
+    {
+      auto state = visualCmdComp->SetData(*visualMsg, this->visualEql) ?
+          ComponentState::OneTimeChange : ComponentState::NoChange;
+      this->iface->ecm->SetChanged(
+          visualEntity, components::VisualCmd::typeId, state);
     }
   }
-
-  if (visualEntity == kNullEntity)
+  else if (materialColorMsg != nullptr)
   {
-    gzerr << "Failed to find visual entity" << std::endl;
-    return false;
-  }
+    Entity visualEntity = kNullEntity;
+    int numberOfEntities = 0;
+    auto entities = entitiesFromScopedName(materialColorMsg->entity().name(),
+      *this->iface->ecm);
+    if (entities.empty())
+    {
+      gzwarn << "Entity name: " << materialColorMsg->entity().name()
+             << ", is not found."
+             << std::endl;
+      return false;
+    }
+    for (const Entity &id : entities)
+    {
+      if ((numberOfEntities > 0) && (materialColorMsg->entity_match() !=
+        gz::msgs::MaterialColor::EntityMatch::MaterialColor_EntityMatch_ALL))
+      {
+        return true;
+      }
+      numberOfEntities++;
+      msgs::Visual visualMCMsg;
+      visualMCMsg.set_id(id);
+      visualMCMsg.mutable_material()->mutable_ambient()->CopyFrom(
+        materialColorMsg->ambient());
+      visualMCMsg.mutable_material()->mutable_diffuse()->CopyFrom(
+        materialColorMsg->diffuse());
+      visualMCMsg.mutable_material()->mutable_specular()->CopyFrom(
+        materialColorMsg->specular());
+      visualMCMsg.mutable_material()->mutable_emissive()->CopyFrom(
+        materialColorMsg->emissive());
+      // TODO(anyone) Enable setting shininess
+      visualEntity = kNullEntity;
+      if (visualMCMsg.id() != kNullEntity)
+      {
+        visualEntity = visualMCMsg.id();
+      }
+      if (visualEntity == kNullEntity)
+      {
+        gzerr << "Failed to find visual entity" << std::endl;
+        return false;
+      }
 
-  auto visualCmdComp =
-      this->iface->ecm->Component<components::VisualCmd>(visualEntity);
-  if (!visualCmdComp)
-  {
-    this->iface->ecm->CreateComponent(
-        visualEntity, components::VisualCmd(*visualMsg));
+      auto visualCmdComp =
+          this->iface->ecm->Component<components::VisualCmd>(visualEntity);
+      if (!visualCmdComp)
+      {
+        this->iface->ecm->CreateComponent(
+            visualEntity, components::VisualCmd(visualMCMsg));
+      }
+      else
+      {
+        auto state = visualCmdComp->SetData(visualMCMsg, this->visualEql) ?
+            ComponentState::OneTimeChange : ComponentState::NoChange;
+        this->iface->ecm->SetChanged(
+            visualEntity, components::VisualCmd::typeId, state);
+      }
+    }
   }
   else
   {
-    auto state = visualCmdComp->SetData(*visualMsg, this->visualEql) ?
-        ComponentState::OneTimeChange : ComponentState::NoChange;
-    this->iface->ecm->SetChanged(
-        visualEntity, components::VisualCmd::typeId, state);
+    gzerr <<
+      "VisualCommand _msg does not match MaterialColor or Visual msg type."
+      << std::endl;
+    return false;
   }
   return true;
 }

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,112 @@ 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));
+
+  Entity sphereEntity0 =
+    ecm->EntityByComponents(components::Name("sphere_0"));
+  Entity sphereEntity1 =
+    ecm->EntityByComponents(components::Name("sphere_1"));
+  auto sphereLinkEntity0 =
+    ecm->ChildrenByComponents(sphereEntity0,
+      components::Name("sphere_link_0"))[0];
+  auto sphereLinkEntity1 =
+    ecm->ChildrenByComponents(sphereEntity1,
+      components::Name("sphere_link_1"))[0];
+  auto sphereVisualEntity0 =
+    ecm->ChildrenByComponents(sphereLinkEntity0,
+      components::Name("sphere_visual"))[0];
+  auto sphereVisualEntity1 =
+    ecm->ChildrenByComponents(sphereLinkEntity1,
+      components::Name("sphere_visual"))[0];
+  auto updatedVisual0 =
+    ecm->Component<components::Material>(sphereVisualEntity0);
+  auto updatedVisual1 =
+    ecm->Component<components::Material>(sphereVisualEntity1);
+  EXPECT_TRUE((math::Color(0.0f, 0.0f, 0.0f, 1.0f) ==
+              updatedVisual0->Data().Diffuse()) ||
+              (math::Color(0.0f, 0.0f, 0.0f, 1.0f) ==
+              updatedVisual1->Data().Diffuse()));
+
+  // 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);
+
+  EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f),
+            updatedVisual0->Data().Diffuse());
+  EXPECT_EQ(math::Color(1.0f, 1.0f, 1.0f, 1.0f),
+            updatedVisual1->Data().Diffuse());
+}
+
 /////////////////////////////////////////////////
 TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics))
 {