Skip to content

Fix raw multibeam data output #38

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 3 commits into
base: ros2
Choose a base branch
from
Open

Fix raw multibeam data output #38

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 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
c3d852d
Fix formatting
rolker Nov 20, 2025
7fea313
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
26 changes: 18 additions & 8 deletions gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -387,6 +387,15 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo
sensorElement->Get<std::string>("frameName", "forward_sonar_optical_link").first;
gzmsg << "frameName: " << this->frameName << std::endl;

this->frameId = _sensor->FrameId();
size_t pos = 0;
while ((pos = this->frameId.find("::", pos)) != std::string::npos)
{
this->frameId.replace(pos, 2, "/");
pos += 1;
}
gzmsg << "frameId: " << this->frameId << std::endl;

// ROS Initialization

if (!rclcpp::ok())
Expand All @@ -395,6 +404,7 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo
}

this->ros_node_ = std::make_shared<rclcpp::Node>("multibeam_sonar_node");
this->ros_node_->set_parameters({rclcpp::Parameter("use_sim_time", true)});

// Create the point cloud publisher
this->pointPub = this->node.Advertise<gz::msgs::PointCloudPacked>(
Expand Down Expand Up @@ -1156,7 +1166,7 @@ void MultibeamSonarSensor::Implementation::ComputeSonarImage()

rclcpp::Time now = this->ros_node_->now();

this->sonarRawDataMsg.header.frame_id = this->frameName;
this->sonarRawDataMsg.header.frame_id = this->frameId;

this->sonarRawDataMsg.header.stamp.sec = static_cast<int32_t>(now.seconds());
this->sonarRawDataMsg.header.stamp.nanosec =
Expand Down Expand Up @@ -1193,22 +1203,22 @@ void MultibeamSonarSensor::Implementation::ComputeSonarImage()
this->sonarRawDataMsg.ranges = ranges;
marine_acoustic_msgs::msg::SonarImageData sonar_image_data;
sonar_image_data.is_bigendian = false;
sonar_image_data.dtype = 0; // DTYPE_UINT8
sonar_image_data.dtype = marine_acoustic_msgs::msg::SonarImageData::DTYPE_FLOAT32;
sonar_image_data.beam_count = this->nBeams;
// this->sonar_image_raw_msg_.data_size = 1; // sizeof(float) * nFreq * nBeams;
std::vector<uchar> intensities;
std::vector<float> intensities;
int Intensity[this->nBeams][this->nFreq];

for (size_t f = 0; f < this->nFreq; f++)
for (size_t r = 0; r < P_Beams[0].size(); r++)
{
for (size_t beam = 0; beam < this->nBeams; beam++)
{
Intensity[beam][f] = static_cast<int>(this->sensorGain * abs(P_Beams[beam][f]));
uchar counts = static_cast<uchar>(std::min(UCHAR_MAX, Intensity[beam][f]));
intensities.push_back(counts);
float power_dB = 20.0f * log10(std::abs(P_Beams[beam][r]));
intensities.push_back(power_dB);
}
}
sonar_image_data.data = intensities;
sonar_image_data.data.resize(intensities.size() * sizeof(float));
memcpy(sonar_image_data.data.data(), intensities.data(), intensities.size() * sizeof(float));
this->sonarRawDataMsg.image = sonar_image_data;
this->sonarImageRawPub->publish(this->sonarRawDataMsg);

Expand Down
1 change: 1 addition & 0 deletions gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,7 @@ private:
std::string sonarImageRawTopicName;
std::string sonarImageTopicName;
std::string frameName;
std::string frameId; // for non-optical frame id from sensor

// Ray parameters
int raySkips;
Expand Down
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
Loading