Skip to content

Allows sensor to properly initialize when plugin is used in a model. #36

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rolker wants to merge 2 commits into
base: ros2
Choose a base branch
from
Open

Allows sensor to properly initialize when plugin is used in a model. #36

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f0f2bda
Allows sensor to properly initialize when plugin is used in a model.
rolker Nov 20, 2025
920ea95
Fix formatting
rolker Nov 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 61 additions & 46 deletions gazebo/dave_gz_multibeam_sonar/multibeam_sonar_system/MultibeamSonarSystem.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,11 @@ 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(
Expand Down Expand Up @@ -252,10 +257,58 @@ 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<std::string>("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<components::Name>(_parent->Data());

enableComponent<components::WorldPose>(_ecm, _entity);
enableComponent<components::WorldAngularVelocity>(_ecm, _entity);
enableComponent<components::WorldLinearVelocity>(_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<const sdf::Element> &,
gz::sim::EntityComponentManager &, gz::sim::EventManager & _eventMgr)
gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr)
{
this->preRenderConn =
_eventMgr.Connect<gz::sim::events::PreRender>(std::bind(&Implementation::OnPreRender, this));
Expand All @@ -270,6 +323,12 @@ void MultibeamSonarSystem::Implementation::DoConfigure(
std::bind(&Implementation::OnRenderTeardown, this));

this->eventMgr = &_eventMgr;

_ecm.EachNew<gz::sim::components::CustomSensor, gz::sim::components::ParentEntity>(
[&](
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); });
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -299,51 +358,7 @@ void MultibeamSonarSystem::Implementation::DoPreUpdate(
[&](
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<std::string>("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<components::Name>(_parent->Data());

enableComponent<components::WorldPose>(_ecm, _entity);
enableComponent<components::WorldAngularVelocity>(_ecm, _entity);
enableComponent<components::WorldLinearVelocity>(_ecm, _entity);

this->perStepRequests.push_back(
requests::CreateSensor{sdf, _entity, _parent->Data(), parentName->Data()});

this->knownSensorEntities.insert(_entity);
return true;
});
{ return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); });
}

//////////////////////////////////////////////////
Expand Down