From f0f2bdaa8c971de8049cc81a6f09a33d7f446f4c Mon Sep 17 00:00:00 2001 From: Roland Arsenault Date: Thu, 20 Nov 2025 10:53:19 -0500 Subject: [PATCH 1/2] Allows sensor to properly initialize when plugin is used in a model. --- .../MultibeamSonarSystem.cc | 120 +++++++++++------- 1 file changed, 74 insertions(+), 46 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc index 2f28c240..1e88b93c 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc @@ -139,6 +139,13 @@ class gz::sim::systems::MultibeamSonarSystem::Implementation public: void Handle(requests::SetEnvironmentalData _request); + /// \brief Create a MultibeamSonarSensor sensor. + bool RequestCreateSensor( + const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent, + gz::sim::EntityComponentManager & _ecm); + /// \brief Implementation for Configure() hook. public: void DoConfigure( @@ -252,10 +259,62 @@ using namespace gz; using namespace sim; using namespace systems; +bool MultibeamSonarSystem::Implementation::RequestCreateSensor( + const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent, + gz::sim::EntityComponentManager & _ecm) +{ + using namespace gz::sim; + // Get sensor's scoped name without the world + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + + // Check sensor's type before proceeding + sdf::Sensor sdf = _custom->Data(); + sdf::ElementPtr root = sdf.Element(); + if (!root->HasAttribute("gz:type")) + { + gzmsg << "No 'gz:type' attribute in custom sensor " + << "[" << sensorScopedName << "]. Ignoring." << std::endl; + return true; + } + auto type = root->Get("gz:type"); + if (type != "multibeam_sonar") + { + gzdbg << "Found custom sensor [" << sensorScopedName << "]" + << " of '" << type << "' type. Ignoring." << std::endl; + return true; + } + gzdbg << "Found custom sensor [" << sensorScopedName << "]" + << " of '" << type << "' type!" << std::endl; + + sdf.SetName(sensorScopedName); + + if (sdf.Topic().empty()) + { + // Default to scoped name as topic + sdf.SetTopic(scopedName(_entity, _ecm) + "/multibeam_sonar/velocity"); + } + + auto parentName = _ecm.Component(_parent->Data()); + + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + + this->perStepRequests.push_back( + requests::CreateSensor{sdf, _entity, _parent->Data(), parentName->Data()}); + + this->knownSensorEntities.insert(_entity); + return true; + +} + ////////////////////////////////////////////////// void MultibeamSonarSystem::Implementation::DoConfigure( const gz::sim::Entity &, const std::shared_ptr &, - gz::sim::EntityComponentManager &, gz::sim::EventManager & _eventMgr) + gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager & _eventMgr) { this->preRenderConn = _eventMgr.Connect(std::bind(&Implementation::OnPreRender, this)); @@ -270,6 +329,14 @@ void MultibeamSonarSystem::Implementation::DoConfigure( std::bind(&Implementation::OnRenderTeardown, this)); this->eventMgr = &_eventMgr; + + _ecm.EachNew([&]( + const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent) -> bool + { + return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); + }); + } ////////////////////////////////////////////////// @@ -295,55 +362,16 @@ void MultibeamSonarSystem::Implementation::DoPreUpdate( return true; }); - _ecm.EachNew( - [&]( + + _ecm.EachNew([&]( const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, const gz::sim::components::ParentEntity * _parent) -> bool - { - using namespace gz::sim; - // Get sensor's scoped name without the world - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - - // Check sensor's type before proceeding - sdf::Sensor sdf = _custom->Data(); - sdf::ElementPtr root = sdf.Element(); - if (!root->HasAttribute("gz:type")) - { - gzmsg << "No 'gz:type' attribute in custom sensor " - << "[" << sensorScopedName << "]. Ignoring." << std::endl; - return true; - } - auto type = root->Get("gz:type"); - if (type != "multibeam_sonar") - { - gzdbg << "Found custom sensor [" << sensorScopedName << "]" - << " of '" << type << "' type. Ignoring." << std::endl; - return true; - } - gzdbg << "Found custom sensor [" << sensorScopedName << "]" - << " of '" << type << "' type!" << std::endl; - - sdf.SetName(sensorScopedName); - - if (sdf.Topic().empty()) - { - // Default to scoped name as topic - sdf.SetTopic(scopedName(_entity, _ecm) + "/multibeam_sonar/velocity"); - } - - auto parentName = _ecm.Component(_parent->Data()); - - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); - enableComponent(_ecm, _entity); + { + return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); + }); + - this->perStepRequests.push_back( - requests::CreateSensor{sdf, _entity, _parent->Data(), parentName->Data()}); - this->knownSensorEntities.insert(_entity); - return true; - }); } ////////////////////////////////////////////////// From 920ea95fe2200dbb6a28e28d5d328ac69f44e5a6 Mon Sep 17 00:00:00 2001 From: Roland Arsenault Date: Thu, 20 Nov 2025 11:27:19 -0500 Subject: [PATCH 2/2] Fix formatting --- .../MultibeamSonarSystem.cc | 37 ++++++------------- 1 file changed, 12 insertions(+), 25 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc index 1e88b93c..81c91798 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc @@ -141,10 +141,8 @@ class gz::sim::systems::MultibeamSonarSystem::Implementation /// \brief Create a MultibeamSonarSensor sensor. bool RequestCreateSensor( - const gz::sim::Entity & _entity, - const gz::sim::components::CustomSensor * _custom, - const gz::sim::components::ParentEntity * _parent, - gz::sim::EntityComponentManager & _ecm); + const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent, gz::sim::EntityComponentManager & _ecm); /// \brief Implementation for Configure() hook. public: @@ -260,15 +258,12 @@ using namespace sim; using namespace systems; bool MultibeamSonarSystem::Implementation::RequestCreateSensor( - const gz::sim::Entity & _entity, - const gz::sim::components::CustomSensor * _custom, - const gz::sim::components::ParentEntity * _parent, - gz::sim::EntityComponentManager & _ecm) + const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent, gz::sim::EntityComponentManager & _ecm) { using namespace gz::sim; // Get sensor's scoped name without the world - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + std::string sensorScopedName = removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); // Check sensor's type before proceeding sdf::Sensor sdf = _custom->Data(); @@ -308,13 +303,12 @@ bool MultibeamSonarSystem::Implementation::RequestCreateSensor( this->knownSensorEntities.insert(_entity); return true; - } ////////////////////////////////////////////////// void MultibeamSonarSystem::Implementation::DoConfigure( const gz::sim::Entity &, const std::shared_ptr &, - gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager & _eventMgr) + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) { this->preRenderConn = _eventMgr.Connect(std::bind(&Implementation::OnPreRender, this)); @@ -330,13 +324,11 @@ void MultibeamSonarSystem::Implementation::DoConfigure( this->eventMgr = &_eventMgr; - _ecm.EachNew([&]( + _ecm.EachNew( + [&]( const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, const gz::sim::components::ParentEntity * _parent) -> bool - { - return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); - }); - + { return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); }); } ////////////////////////////////////////////////// @@ -362,16 +354,11 @@ void MultibeamSonarSystem::Implementation::DoPreUpdate( return true; }); - - _ecm.EachNew([&]( + _ecm.EachNew( + [&]( const gz::sim::Entity & _entity, const gz::sim::components::CustomSensor * _custom, const gz::sim::components::ParentEntity * _parent) -> bool - { - return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); - }); - - - + { return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); }); } //////////////////////////////////////////////////