Skip to content

Adapt to new acoustic_msg format #43

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

Merged
merged 3 commits into from
Jul 6, 2022
Merged

Adapt to new acoustic_msg format #43

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
daea087
adapt to new acoustic_msg format
woensug-choi May 9, 2022
b71cda6
Encode direction as vector.
rolker Jun 1, 2022
0c3b820
Merge pull request #44 from rolker/fix-direction
woensug-choi Jul 6, 2022
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
8 changes: 6 additions & 2 deletions include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_raster_based.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,11 @@
#include <sensor_msgs/fill_image.h>
#include <std_msgs/Float64.h>
#include <image_transport/image_transport.h>
#include <acoustic_msgs/SonarImage.h>
#include <acoustic_msgs/ProjectedSonarImage.h>
#include <acoustic_msgs/SonarImageData.h>
#include <acoustic_msgs/PingInfo.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Float32.h>

// dynamic reconfigure stuff
#include <gazebo_plugins/GazeboRosCameraConfig.h>
Expand Down Expand Up @@ -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_;
Expand Down
8 changes: 6 additions & 2 deletions include/nps_uw_multibeam_sonar/gazebo_multibeam_sonar_ray_based.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,11 @@
#include <sensor_msgs/fill_image.h>
#include <std_msgs/Float64.h>
#include <image_transport/image_transport.h>
#include <acoustic_msgs/SonarImage.h>
#include <acoustic_msgs/ProjectedSonarImage.h>
#include <acoustic_msgs/SonarImageData.h>
#include <acoustic_msgs/PingInfo.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Float32.h>

// dynamic reconfigure stuff
#include <gazebo_plugins/GazeboRosCameraConfig.h>
Expand Down Expand Up @@ -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_;
Expand Down
36 changes: 26 additions & 10 deletions src/gazebo_multibeam_sonar_raster_based.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -524,7 +524,7 @@ void NpsGazeboRosMultibeamSonar::Advertise()

// Publisher for sonar image
ros::AdvertiseOptions sonar_image_raw_ao =
ros::AdvertiseOptions::create<acoustic_msgs::SonarImage>(
ros::AdvertiseOptions::create<acoustic_msgs::ProjectedSonarImage>(
this->sonar_image_raw_topic_name_, 1,
boost::bind(&NpsGazeboRosMultibeamSonar::DepthImageConnect, this),
boost::bind(&NpsGazeboRosMultibeamSonar::DepthImageDisconnect, this),
Expand Down Expand Up @@ -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<float> azimuth_angles;
double fl = static_cast<double>(width) / (2.0 * tan(hFOV/2.0));
for (size_t beam = 0; beam < nBeams; beam ++)
{
ping_info_msg_.rx_beamwidths.push_back(static_cast<float>(
abs(atan2(static_cast<double>(beam) - 1.0 * static_cast<double>(width), fl)
- atan2(static_cast<double>(beam), fl))));
ping_info_msg_.tx_beamwidths.push_back(static_cast<float>(vFOV));
azimuth_angles.push_back(atan2(static_cast<double>(beam) -
0.5 * static_cast<double>(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<float> 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<uchar> intensities;
int Intensity[nBeams][nFreq];
for (size_t f = 0; f < nFreq; f ++)
Expand All @@ -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
Expand Down
36 changes: 27 additions & 9 deletions src/gazebo_multibeam_sonar_ray_based.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,7 +340,7 @@ void NpsGazeboRosMultibeamSonarRay::Advertise()

// Publisher for sonar raw data
ros::AdvertiseOptions sonar_image_raw_ao =
ros::AdvertiseOptions::create<acoustic_msgs::SonarImage>(
ros::AdvertiseOptions::create<acoustic_msgs::ProjectedSonarImage>(
this->sonar_image_raw_topic_name_, 1,
boost::bind(&NpsGazeboRosMultibeamSonarRay::SonarImageConnect, this),
boost::bind(&NpsGazeboRosMultibeamSonarRay::SonarImageDisconnect, this),
Expand Down Expand Up @@ -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<float>(hFOV/floor(nBeams*2.0-2.0)*2.0));
ping_info_msg_.tx_beamwidths.push_back(static_cast<float>(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<float> 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<uchar> intensities;
int Intensity[nBeams][nFreq];
for (size_t f = 0; f < nFreq; f ++)
Expand All @@ -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;

Expand Down