UserCommands sdf light entity match.

Allow for setting all matching light entities by setting
<set_all_light_entities> boolean in usercommands plugin's sdf.

Signed-off-by: Benjamin Perseghetti <[email protected]>

src/systems/user_commands/UserCommands.cc

@@ -119,6 +119,9 @@ class UserCommandsInterface
   /// \return True if a contact sensor is connected to the collision entity,
   /// false otherwise
   public: bool HasContactSensor(const Entity _collision);
+
+  /// \brief Bool to set all matching light entities.
+  public: bool setAllLightEntities = false;
 };
 
 /// \brief All user commands should inherit from this class so they can be
@@ -545,6 +548,7 @@ class gz::sim::systems::UserCommandsPrivate
 
   /// \brief Mutex to protect pending queue.
   public: std::mutex pendingMutex;
+
 };
 
 /// \brief Pose3d equality comparison function.
@@ -610,7 +614,7 @@ bool UserCommandsInterface::HasContactSensor(const Entity _collision)
 
 //////////////////////////////////////////////////
 void UserCommands::Configure(const Entity &_entity,
-    const std::shared_ptr<const sdf::Element> &,
+    const std::shared_ptr<const sdf::Element> &_sdf,
     EntityComponentManager &_ecm,
     EventManager &_eventManager)
 {
@@ -680,6 +684,15 @@ void UserCommands::Configure(const Entity &_entity,
   this->dataPtr->node.Subscribe(lightTopic, &UserCommandsPrivate::OnCmdLight,
                                 this->dataPtr.get());
 
+  if (_sdf->HasElement("set_all_light_entities"))
+  {
+    this->dataPtr->iface->setAllLightEntities =
+      _sdf->Get<bool>("set_all_light_entities");
+    gzmsg << "Set all light entities: "
+          << this->dataPtr->iface->setAllLightEntities
+          << std::endl;
+  }
+
   std::string materialColorTopic{
     "/world/" + validWorldName + "/material_color"};
   this->dataPtr->node.Subscribe(materialColorTopic,
@@ -859,7 +872,6 @@ void UserCommandsPrivate::OnCmdLight(const msgs::Light &_msg)
   }
 }
 
-
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,
     msgs::Boolean &_res)
@@ -1362,79 +1374,150 @@ LightCommand::LightCommand(msgs::Light *_msg,
 //////////////////////////////////////////////////
 bool LightCommand::Execute()
 {
-  auto lightMsg = dynamic_cast<const msgs::Light *>(this->msg);
+  auto lightMsg = dynamic_cast<msgs::Light *>(this->msg);
   if (nullptr == lightMsg)
   {
     gzerr << "Internal error, null light message" << std::endl;
     return false;
   }
+  Entity lightEntity = kNullEntity;
+  if (this->iface->setAllLightEntities)
+  {
+    auto entities = entitiesFromScopedName(lightMsg->name(),
+        *this->iface->ecm);
+    if (entities.empty())
+    {
+      gzwarn << "Entity name: " << lightMsg->name() << ", is not found."
+             << std::endl;
+      return false;
+    }
+    for (const Entity &id : entities)
+    {
+      lightEntity = kNullEntity;
+      lightMsg->set_id(id);
+      if (lightMsg->id() != kNullEntity)
+      {
+        lightEntity = lightMsg->id();
+      }
+      if (kNullEntity == lightEntity)
+      {
+        gzerr << "Failed to find light with name [" << lightMsg->name()
+               << "] and parent ID ["
+               << lightMsg->parent_id() << "]." << std::endl;
+        return false;
+      }
+
+      if (!lightEntity)
+      {
+        gzmsg << "Failed to find light entity named [" << lightMsg->name()
+          << "]." << std::endl;
+        return false;
+      }
 
-  Entity lightEntity{kNullEntity};
+      auto lightPose =
+        this->iface->ecm->Component<components::Pose>(lightEntity);
+      if (nullptr == lightPose)
+      {
+        lightEntity = kNullEntity;
+      }
 
-  if (lightMsg->id() != kNullEntity)
-  {
-    lightEntity = lightMsg->id();
+      if (!lightEntity)
+      {
+        gzmsg << "Pose component not available" << std::endl;
+        return false;
+      }
+
+      if (lightMsg->has_pose())
+      {
+        lightPose->Data().Pos() = msgs::Convert(lightMsg->pose()).Pos();
+      }
+
+      auto lightCmdComp =
+        this->iface->ecm->Component<components::LightCmd>(lightEntity);
+      if (!lightCmdComp)
+      {
+        this->iface->ecm->CreateComponent(
+            lightEntity, components::LightCmd(*lightMsg));
+      }
+      else
+      {
+        auto state = lightCmdComp->SetData(*lightMsg, this->lightEql) ?
+            ComponentState::OneTimeChange :
+            ComponentState::NoChange;
+        this->iface->ecm->SetChanged(lightEntity, components::LightCmd::typeId,
+          state);
+      }
+    }
   }
-  else if (!lightMsg->name().empty())
+  else
   {
-    if (lightMsg->parent_id() != kNullEntity)
+    lightEntity = kNullEntity;
+    if (lightMsg->id() != kNullEntity)
     {
-      lightEntity = this->iface->ecm->EntityByComponents(
-        components::Name(lightMsg->name()),
-        components::ParentEntity(lightMsg->parent_id()));
+      lightEntity = lightMsg->id();
     }
-    else
+    else if (!lightMsg->name().empty())
+    {
+      if (lightMsg->parent_id() != kNullEntity)
+      {
+        lightEntity = this->iface->ecm->EntityByComponents(
+          components::Name(lightMsg->name()),
+          components::ParentEntity(lightMsg->parent_id()));
+      }
+      else
+      {
+        lightEntity = this->iface->ecm->EntityByComponents(
+          components::Name(lightMsg->name()));
+      }
+    }
+    if (kNullEntity == lightEntity)
     {
-      lightEntity = this->iface->ecm->EntityByComponents(
-        components::Name(lightMsg->name()));
+      gzerr << "Failed to find light with name [" << lightMsg->name()
+             << "], ID [" << lightMsg->id() << "] and parent ID ["
+             << lightMsg->parent_id() << "]." << std::endl;
+      return false;
     }
-  }
-  if (kNullEntity == lightEntity)
-  {
-    gzerr << "Failed to find light with name [" << lightMsg->name()
-           << "], ID [" << lightMsg->id() << "] and parent ID ["
-           << lightMsg->parent_id() << "]." << std::endl;
-    return false;
-  }
 
-  if (!lightEntity)
-  {
-    gzmsg << "Failed to find light entity named [" << lightMsg->name()
-      << "]." << std::endl;
-    return false;
-  }
+    if (!lightEntity)
+    {
+      gzmsg << "Failed to find light entity named [" << lightMsg->name()
+        << "]." << std::endl;
+      return false;
+    }
 
-  auto lightPose = this->iface->ecm->Component<components::Pose>(lightEntity);
-  if (nullptr == lightPose)
-    lightEntity = kNullEntity;
+    auto lightPose = this->iface->ecm->Component<components::Pose>(lightEntity);
+    if (nullptr == lightPose)
+    {
+      lightEntity = kNullEntity;
+    }
 
-  if (!lightEntity)
-  {
-    gzmsg << "Pose component not available" << std::endl;
-    return false;
-  }
+    if (!lightEntity)
+    {
+      gzmsg << "Pose component not available" << std::endl;
+      return false;
+    }
 
-  if (lightMsg->has_pose())
-  {
-    lightPose->Data().Pos() = msgs::Convert(lightMsg->pose()).Pos();
-  }
+    if (lightMsg->has_pose())
+    {
+      lightPose->Data().Pos() = msgs::Convert(lightMsg->pose()).Pos();
+    }
 
-  auto lightCmdComp =
-    this->iface->ecm->Component<components::LightCmd>(lightEntity);
-  if (!lightCmdComp)
-  {
-    this->iface->ecm->CreateComponent(
-        lightEntity, components::LightCmd(*lightMsg));
-  }
-  else
-  {
-    auto state = lightCmdComp->SetData(*lightMsg, this->lightEql) ?
-        ComponentState::OneTimeChange :
-        ComponentState::NoChange;
-    this->iface->ecm->SetChanged(lightEntity, components::LightCmd::typeId,
-      state);
+    auto lightCmdComp =
+      this->iface->ecm->Component<components::LightCmd>(lightEntity);
+    if (!lightCmdComp)
+    {
+      this->iface->ecm->CreateComponent(
+          lightEntity, components::LightCmd(*lightMsg));
+    }
+    else
+    {
+      auto state = lightCmdComp->SetData(*lightMsg, this->lightEql) ?
+          ComponentState::OneTimeChange :
+          ComponentState::NoChange;
+      this->iface->ecm->SetChanged(lightEntity, components::LightCmd::typeId,
+        state);
+    }
   }
-
   return true;
 }