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

@@ -539,6 +539,9 @@ class gz::sim::systems::UserCommandsPrivate
 
   /// \brief Mutex to protect pending queue.
   public: std::mutex pendingMutex;
+
+  /// \brief Bool to set all matching light entities.
+  public: bool setAllLightEntities = false;
 };
 
 /// \brief Pose3d equality comparison function.
@@ -604,7 +607,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)
 {
@@ -674,6 +677,14 @@ 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->setAllLightEntities =
+      _sdf->Get<bool>("set_all_light_entities");
+    gzmsg << "Set all light entities: " << this->dataPtr->setAllLightEntities
+            << std::endl;
+  }
+
   std::string materialColorTopic{
     "/world/" + validWorldName + "/material_color"};
   this->dataPtr->node.Subscribe(materialColorTopic,
@@ -842,17 +853,35 @@ bool UserCommandsPrivate::LightService(const msgs::Light &_req,
 //////////////////////////////////////////////////
 void UserCommandsPrivate::OnCmdLight(const msgs::Light &_msg)
 {
-  auto msg = _msg.New();
-  msg->CopyFrom(_msg);
-  auto cmd = std::make_unique<LightCommand>(msg, this->iface);
-
-  // Push to pending
+  int numberOfEntities = 0;
+  auto entities = entitiesFromScopedName(_msg.name(),
+      *this->iface->ecm);
+  if (entities.empty())
   {
-    std::lock_guard<std::mutex> lock(this->pendingMutex);
-    this->pendingCmds.push_back(std::move(cmd));
+    gzwarn << "Entity name: " << _msg.name() << ", is not found."
+           << std::endl;
+    return;
   }
-}
+  for (const Entity &id : entities)
+  {
+    if ((numberOfEntities > 0) && (!setAllLightEntities))
+    {
+      break;
+    }
+    auto msg = _msg.New();
+    msg->CopyFrom(_msg);
+    numberOfEntities++;
+    msg->set_id(id);
+    auto cmd = std::make_unique<LightCommand>(msg, this->iface);
 
+    // Push to pending
+    {
+      std::lock_guard<std::mutex> lock(this->pendingMutex);
+      this->pendingCmds.push_back(std::move(cmd));
+    }
+  }
+  return;
+}
 
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,