diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8dd284f7aed..0ef61c7a70f 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/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 std::shared_ptr &_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("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(msg, this->iface); - - // Push to pending + int numberOfEntities = 0; + auto entities = entitiesFromScopedName(_msg.name(), + *this->iface->ecm); + if (entities.empty()) { - std::lock_guard 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(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + } + return; +} ////////////////////////////////////////////////// bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,