diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc index 310e9568..2840e20a 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc @@ -387,6 +387,15 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo sensorElement->Get("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()) @@ -395,6 +404,7 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo } this->ros_node_ = std::make_shared("multibeam_sonar_node"); + this->ros_node_->set_parameters({rclcpp::Parameter("use_sim_time", true)}); // Create the point cloud publisher this->pointPub = this->node.Advertise( @@ -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(now.seconds()); this->sonarRawDataMsg.header.stamp.nanosec = @@ -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 intensities; + std::vector 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(this->sensorGain * abs(P_Beams[beam][f])); - uchar counts = static_cast(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); diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh index b7790e3a..3138bd10 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh @@ -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; 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..81c91798 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,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( @@ -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("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 +323,12 @@ 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); }); } ////////////////////////////////////////////////// @@ -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("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; - }); + { return this->RequestCreateSensor(_entity, _custom, _parent, _ecm); }); } //////////////////////////////////////////////////