diff --git a/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_raster_based.hh b/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_raster_based.hh index 910e701..42bd4a1 100644 --- a/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_raster_based.hh +++ b/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_raster_based.hh @@ -30,7 +30,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include // dynamic reconfigure stuff #include @@ -208,7 +212,7 @@ namespace gazebo private: sensor_msgs::Image depth_image_msg_; private: sensor_msgs::Image normal_image_msg_; private: sensor_msgs::PointCloud2 point_cloud_msg_; - private: acoustic_msgs::SonarImage sonar_image_raw_msg_; + private: acoustic_msgs::ProjectedSonarImage sonar_image_raw_msg_; private: sensor_msgs::Image sonar_image_msg_; private: sensor_msgs::Image sonar_image_mono_msg_; private: cv::Mat point_cloud_image_; diff --git a/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_ray_based.hh b/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_ray_based.hh index bcc13bf..452652e 100644 --- a/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_ray_based.hh +++ b/include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_ray_based.hh @@ -54,7 +54,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include // dynamic reconfigure stuff #include @@ -182,7 +186,7 @@ namespace gazebo private: sensor_msgs::PointCloud2 point_cloud_msg_; private: sensor_msgs::Image normal_image_msg_; - private: acoustic_msgs::SonarImage sonar_image_raw_msg_; + private: acoustic_msgs::ProjectedSonarImage sonar_image_raw_msg_; private: sensor_msgs::Image sonar_image_msg_; private: sensor_msgs::Image sonar_image_mono_msg_; private: cv::Mat point_cloud_image_; diff --git a/src/gazebo_multibeam_sonar_raster_based.cpp b/src/gazebo_multibeam_sonar_raster_based.cpp index f82778b..113e6e7 100644 --- a/src/gazebo_multibeam_sonar_raster_based.cpp +++ b/src/gazebo_multibeam_sonar_raster_based.cpp @@ -524,7 +524,7 @@ void NpsGazeboRosMultibeamSonar::Advertise() // Publisher for sonar image ros::AdvertiseOptions sonar_image_raw_ao = - ros::AdvertiseOptions::create( + ros::AdvertiseOptions::create( this->sonar_image_raw_topic_name_, 1, boost::bind(&NpsGazeboRosMultibeamSonar::DepthImageConnect, this), boost::bind(&NpsGazeboRosMultibeamSonar::DepthImageDisconnect, this), @@ -918,23 +918,38 @@ void NpsGazeboRosMultibeamSonar::ComputeSonarImage(const float *_src) = this->depth_sensor_update_time_.sec; this->sonar_image_raw_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; - this->sonar_image_raw_msg_.frequency = this->sonarFreq; - this->sonar_image_raw_msg_.sound_speed = this->soundSpeed; - this->sonar_image_raw_msg_.azimuth_beamwidth = hPixelSize; - this->sonar_image_raw_msg_.elevation_beamwidth = hPixelSize*this->nRays; + acoustic_msgs::PingInfo ping_info_msg_; + ping_info_msg_.frequency = this->sonarFreq; + ping_info_msg_.sound_speed = this->soundSpeed; std::vector azimuth_angles; double fl = static_cast(width) / (2.0 * tan(hFOV/2.0)); for (size_t beam = 0; beam < nBeams; beam ++) + { + ping_info_msg_.rx_beamwidths.push_back(static_cast( + abs(atan2(static_cast(beam) - 1.0 * static_cast(width), fl) + - atan2(static_cast(beam), fl)))); + ping_info_msg_.tx_beamwidths.push_back(static_cast(vFOV)); azimuth_angles.push_back(atan2(static_cast(beam) - 0.5 * static_cast(width), fl)); - this->sonar_image_raw_msg_.azimuth_angles = azimuth_angles; + } + this->sonar_image_raw_msg_.ping_info = ping_info_msg_; + for (size_t beam = 0; beam < nBeams; beam ++) + { + geometry_msgs::Vector3 beam_direction; + beam_direction.x = cos(azimuth_angles[beam]); + beam_direction.y = sin(azimuth_angles[beam]); + beam_direction.z = 0.0; + this->sonar_image_raw_msg_.beam_directions.push_back(beam_direction); + } std::vector ranges; for (size_t i = 0; i < P_Beams[0].size(); i ++) ranges.push_back(rangeVector[i]); this->sonar_image_raw_msg_.ranges = ranges; - - // this->sonar_image_raw_msg_.is_bigendian = false; - this->sonar_image_raw_msg_.data_size = 1; // sizeof(float) * nFreq * nBeams; + acoustic_msgs::SonarImageData sonar_image_data; + sonar_image_data.is_bigendian = false; + sonar_image_data.dtype = 0; //DTYPE_UINT8 + sonar_image_data.beam_count = nBeams; + //this->sonar_image_raw_msg_.data_size = 1; // sizeof(float) * nFreq * nBeams; std::vector intensities; int Intensity[nBeams][nFreq]; for (size_t f = 0; f < nFreq; f ++) @@ -949,7 +964,8 @@ void NpsGazeboRosMultibeamSonar::ComputeSonarImage(const float *_src) intensities.push_back(counts); } } - this->sonar_image_raw_msg_.intensities = intensities; + sonar_image_data.data = intensities; + this->sonar_image_raw_msg_.image = sonar_image_data; this->sonar_image_raw_pub_.publish(this->sonar_image_raw_msg_); // Construct visual sonar image for rqt plot in sensor::image msg format diff --git a/src/gazebo_multibeam_sonar_ray_based.cpp b/src/gazebo_multibeam_sonar_ray_based.cpp index 3a1e278..e927135 100644 --- a/src/gazebo_multibeam_sonar_ray_based.cpp +++ b/src/gazebo_multibeam_sonar_ray_based.cpp @@ -340,7 +340,7 @@ void NpsGazeboRosMultibeamSonarRay::Advertise() // Publisher for sonar raw data ros::AdvertiseOptions sonar_image_raw_ao = - ros::AdvertiseOptions::create( + ros::AdvertiseOptions::create( this->sonar_image_raw_topic_name_, 1, boost::bind(&NpsGazeboRosMultibeamSonarRay::SonarImageConnect, this), boost::bind(&NpsGazeboRosMultibeamSonarRay::SonarImageDisconnect, this), @@ -528,17 +528,32 @@ void NpsGazeboRosMultibeamSonarRay::ComputeSonarImage() = this->sensor_update_time_.sec; this->sonar_image_raw_msg_.header.stamp.nsec = this->sensor_update_time_.nsec; - this->sonar_image_raw_msg_.frequency = this->sonarFreq; - this->sonar_image_raw_msg_.sound_speed = this->soundSpeed; - this->sonar_image_raw_msg_.azimuth_beamwidth = hPixelSize; - this->sonar_image_raw_msg_.elevation_beamwidth = hPixelSize*this->nRays; - this->sonar_image_raw_msg_.azimuth_angles = this->azimuth_angles; + acoustic_msgs::PingInfo ping_info_msg_; + ping_info_msg_.frequency = this->sonarFreq; + ping_info_msg_.sound_speed = this->soundSpeed; + for (size_t beam = 0; beam < nBeams; beam ++) + { + ping_info_msg_.rx_beamwidths.push_back(static_cast(hFOV/floor(nBeams*2.0-2.0)*2.0)); + ping_info_msg_.tx_beamwidths.push_back(static_cast(vFOV)); + } + this->sonar_image_raw_msg_.ping_info = ping_info_msg_; + for (size_t beam = 0; beam < nBeams; beam ++) + { + geometry_msgs::Vector3 beam_direction; + beam_direction.x = cos(azimuth_angles[beam]); + beam_direction.y = sin(azimuth_angles[beam]); + beam_direction.z = 0.0; + this->sonar_image_raw_msg_.beam_directions.push_back(beam_direction); + } std::vector ranges; for (size_t i = 0; i < P_Beams[0].size(); i ++) ranges.push_back(rangeVector[i]); this->sonar_image_raw_msg_.ranges = ranges; - // this->sonar_image_raw_msg_.is_bigendian = false; - this->sonar_image_raw_msg_.data_size = 1; // sizeof(float) * nFreq * nBeams; + acoustic_msgs::SonarImageData sonar_image_data; + sonar_image_data.is_bigendian = false; + sonar_image_data.dtype = 0; //DTYPE_UINT8 + sonar_image_data.beam_count = nBeams; + //this->sonar_image_raw_msg_.data_size = 1; // sizeof(float) * nFreq * nBeams; std::vector intensities; int Intensity[nBeams][nFreq]; for (size_t f = 0; f < nFreq; f ++) @@ -550,9 +565,12 @@ void NpsGazeboRosMultibeamSonarRay::ComputeSonarImage() intensities.push_back(counts); } } - this->sonar_image_raw_msg_.intensities = intensities; + sonar_image_data.data = intensities; + this->sonar_image_raw_msg_.image = sonar_image_data; this->sonar_image_raw_pub_.publish(this->sonar_image_raw_msg_); + + // Construct visual sonar image for rqt plot in sensor::image msg format cv_bridge::CvImage img_bridge;