36 #include <boost/thread/thread.hpp> 37 #include <boost/bind.hpp> 41 #include <gazebo/sensors/Sensor.hh> 43 #include <gazebo/sensors/SensorTypes.hh> 51 #include <opencv2/core/core.hpp> 56 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosImageSonar)
62 this->point_cloud_connect_count_ = 0;
63 this->depth_info_connect_count_ = 0;
64 this->last_depth_image_camera_info_update_time_ = common::Time(0);
84 std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_parent);
89 gzerr <<
"DepthCameraPlugin not attached to a depthCamera sensor\n";
93 this->
width = this->depthCamera->ImageWidth();
94 this->
height = this->depthCamera->ImageHeight();
95 this->
depth = this->depthCamera->ImageDepth();
96 this->
format = this->depthCamera->ImageFormat();
100 this, std::placeholders::_1, std::placeholders::_2,
101 std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
105 this, std::placeholders::_1, std::placeholders::_2,
106 std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
110 this, std::placeholders::_1, std::placeholders::_2,
111 std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
118 ROS_FATAL_STREAM_NAMED(
"depth_camera",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 119 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
132 if (!_sdf->HasElement(
"imageTopicName"))
134 if (!_sdf->HasElement(
"cameraInfoTopicName"))
138 if (!_sdf->HasElement(
"pointCloudTopicName"))
144 if (!_sdf->HasElement(
"depthImageTopicName"))
149 if (!_sdf->HasElement(
"depthImageCameraInfoTopicName"))
154 if (!_sdf->HasElement(
"pointCloudCutoff"))
159 if (!_sdf->HasElement(
"clip")) {
160 gzerr <<
"We do not have clip" << std::endl;
163 gzerr <<
"We do have clip" << std::endl;
164 gzerr << _sdf->GetElement(
"clip")->GetElement(
"far")->Get<
double>() << std::endl;
174 ros::AdvertiseOptions::create<sensor_msgs::PointCloud2 >(
182 ros::AdvertiseOptions::create< sensor_msgs::Image >(
190 ros::AdvertiseOptions::create< sensor_msgs::Image >(
191 this->depth_image_topic_name_+
"_normals",1,
198 ros::AdvertiseOptions::create< sensor_msgs::Image >(
199 this->depth_image_topic_name_+
"_multibeam",1,
206 ros::AdvertiseOptions::create< sensor_msgs::Image >(
207 this->depth_image_topic_name_+
"_sonar",1,
214 ros::AdvertiseOptions::create< sensor_msgs::Image >(
215 this->depth_image_topic_name_+
"_raw_sonar",1,
222 ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
335 unsigned int _width,
unsigned int _height,
unsigned int _depth,
336 const std::string &_format)
373 unsigned int _width,
unsigned int _height,
unsigned int _depth,
374 const std::string &_format)
401 pcd_modifier.
resize(_width*_height);
410 for (
unsigned int i = 0; i < _width; i++)
412 for (
unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
414 unsigned int index = (j * _width) + i;
415 *iter_x = _pcd[4 * index];
416 *iter_y = _pcd[4 * index + 1];
417 *iter_z = _pcd[4 * index + 2];
418 *iter_rgb = _pcd[4 * index + 3];
419 if (i == _width /2 && j == _height / 2)
421 uint32_t rgb = *
reinterpret_cast<int*
>(&(*iter_rgb));
422 uint8_t r = (rgb >> 16) & 0x0000ff;
423 uint8_t g = (rgb >> 8) & 0x0000ff;
424 uint8_t b = (rgb) & 0x0000ff;
425 std::cerr << (int)r <<
" " << (
int)g <<
" " << (int)b <<
"\n";
431 this->
lock_.unlock();
439 unsigned int _width,
unsigned int _height,
unsigned int _depth,
440 const std::string &_format)
487 this->
lock_.unlock();
509 this->
lock_.unlock();
515 sensor_msgs::PointCloud2 &point_cloud_msg,
516 uint32_t rows_arg, uint32_t cols_arg,
517 uint32_t step_arg,
void* data_arg)
521 pcd_modifier.
resize(rows_arg*cols_arg);
528 point_cloud_msg.is_dense =
true;
530 float* toCopyFrom = (
float*)data_arg;
533 double hfov = this->
parentSensor->DepthCamera()->HFOV().Radian();
534 double fl = ((double)this->
width) / (2.0 *tan(hfov/2.0));
537 for (uint32_t j=0; j<rows_arg; j++)
540 if (rows_arg>1) pAngle = atan2( (
double)j - 0.5*(
double)(rows_arg-1), fl);
543 for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
546 if (cols_arg>1) yAngle = atan2( (
double)i - 0.5*(
double)(cols_arg-1), fl);
549 double depth = toCopyFrom[index++];
555 *iter_x = depth * tan(yAngle);
556 *iter_y = depth * tan(pAngle);
563 *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN ();
564 point_cloud_msg.is_dense =
false;
568 uint8_t* image_src = (uint8_t*)(&(this->
image_msg_.data[0]));
569 if (this->
image_msg_.data.size() == rows_arg*cols_arg*3)
572 iter_rgb[0] = image_src[i*3+j*cols_arg*3+0];
573 iter_rgb[1] = image_src[i*3+j*cols_arg*3+1];
574 iter_rgb[2] = image_src[i*3+j*cols_arg*3+2];
576 else if (this->
image_msg_.data.size() == rows_arg*cols_arg)
579 iter_rgb[0] = image_src[i+j*cols_arg];
580 iter_rgb[1] = image_src[i+j*cols_arg];
581 iter_rgb[2] = image_src[i+j*cols_arg];
598 sensor_msgs::Image& image_msg,
599 uint32_t rows_arg, uint32_t cols_arg,
600 uint32_t step_arg,
void* data_arg)
603 image_msg.height = rows_arg;
604 image_msg.width = cols_arg;
605 image_msg.step =
sizeof(float) * cols_arg;
606 image_msg.data.resize(rows_arg * cols_arg *
sizeof(
float));
607 image_msg.is_bigendian = 0;
609 const float bad_point = std::numeric_limits<float>::quiet_NaN();
611 float* dest = (
float*)(&(image_msg.data[0]));
612 float* toCopyFrom = (
float*)data_arg;
616 for (uint32_t j = 0; j < rows_arg; j++)
618 for (uint32_t i = 0; i < cols_arg; i++)
620 float depth = toCopyFrom[index++];
624 dest[i + j * cols_arg] =
depth;
628 dest[i + j * cols_arg] = bad_point;
644 cv::Mat_<float> f1 = (cv::Mat_<float>(3, 3) << 1, 2, 1,
648 cv::Mat_<float> f2 = (cv::Mat_<float>(3, 3) << 1, 0, -1,
653 cv::flip(f1, f1m, 0);
654 cv::flip(f2, f2m, 1);
657 cv::filter2D(depth, n1, -1, f1m, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
658 cv::filter2D(depth, n2, -1, f2m, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
661 cv::erode(depth == 0, no_readings, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
663 n1.setTo(0, no_readings);
664 n2.setTo(0, no_readings);
666 std::vector<cv::Mat> images(3);
667 cv::Mat white = cv::Mat::ones(depth.rows, depth.cols, CV_32FC1);
675 cv::Mat normal_image;
676 cv::merge(images, normal_image);
679 for (
int i = 0; i < normal_image.rows; ++i) {
680 for (
int j = 0; j < normal_image.cols; ++j) {
681 cv::Vec3f& n = normal_image.at<cv::Vec3f>(i, j);
682 n = cv::normalize(n);
686 cv::split(normal_image.clone(), images);
687 cv::Vec3d minVec, maxVec;
688 for (
int i = 0; i < 3; ++i) {
689 cv::minMaxLoc(images[i], &minVec[i], &maxVec[i]);
690 images[i] -= minVec[i];
691 images[i] *= 1./(maxVec[i] - minVec[i]);
694 cv::merge(images, normal_image);
695 cv::Mat normal_image8;
696 normal_image.convertTo(normal_image8, CV_8UC3, 255.0);
709 std::vector<cv::Mat> images(3);
710 cv::split(normals, images);
712 float intensity = 100.;
718 std::vector<float> t_x, t_y;
719 for (
int i = 0; i < depth.cols; i++) t_x.push_back((
float(i) - this->cx_)/this->focal_length_);
720 for (
int i = 0; i < depth.rows; i++) t_y.push_back((
float(i) - this->cy_)/this->focal_length_);
722 cv::repeat(cv::Mat(t_x).reshape(1,1), t_y.size(), 1, X);
723 cv::repeat(cv::Mat(t_y).reshape(1,1).t(), 1, t_x.size(), Y);
724 dist_matrix_ = cv::Mat::zeros(depth.rows, depth.cols, CV_32FC1);
725 cv::multiply(X, X, X);
726 cv::multiply(Y, Y, Y);
727 cv::sqrt(X + Y + 1, dist_matrix_);
731 cv::Mat TS = intensity*images[2];
732 cv::Mat TL = 5*
depth;
734 cv::Mat SNR = SL - 2.0*TL - (NL-DI) + TS;
735 SNR.setTo(0., SNR < 0.);
737 double minVal, maxVal;
738 cv::minMaxLoc(SNR, &minVal, &maxVal);
740 SNR *= 1./(maxVal - minVal);
742 cv::Mat sonar_image8;
743 SNR.convertTo(sonar_image8, CV_8UC3, 255.0);
756 std::normal_distribution<double> speckle_dist(1.0, 0.1);
758 for (
int i = 0; i < scan.rows; ++i) {
759 for (
int j = 0; j < scan.cols; ++j) {
760 float& a = scan.at<
float>(i, j);
764 float speckle = fabs(speckle_dist(
generator));
773 int window_size = 30;
778 float threshold = tan(fov);
779 for (
int j = 0; j < scan.cols; ++j) {
780 for (
int i = 0; i < scan.rows; ++i) {
781 float x = fabs(
float(scan.cols)/2. - j);
782 float y = scan.rows - i;
783 int dist = int(sqrt(x*x+y*y))/2;
784 if (dist >= scan.rows/2 || fabs(x)/y > threshold) {
794 std::uniform_real_distribution<double> index_dist(0.0, 1.0);
796 std::vector<float> kernel(window_size);
797 for (
int i = 0; i < window_size; ++i) {
798 float diff = float(i-window_size/2)/float(window_size/4.);
799 kernel[i] = exp(-0.5*diff);
802 std::vector<float> conv_results(2*window_size);
803 for (
int i = 0; i < nrolls; ++i) {
804 int sampled_range = range_dist(
generator);
810 int window_start = std::max(0, sampled_index-window_size);
811 int window_end = std::min(
angle_nbr_indices_[sampled_range], sampled_index+window_size);
812 for (
int i = window_start; i < window_end; ++i) {
813 conv_results[i - window_start] = 0.;
814 float conv_mass = 0.;
815 for (
int j = 0; j < window_size; ++j) {
816 int index = i + j - window_size/2;
818 conv_results[i - window_start] += kernel[j]*scan.at<
float>(
angle_range_indices_[sampled_range][index]);
819 conv_mass += kernel[j];
822 if (conv_mass == 0.) {
826 conv_results[i - window_start] *= 1./conv_mass;
830 for (
int i = window_start; i < window_end; ++i) {
840 cv::Mat is_zero = scan == 0.;
841 cv::Mat is_bg = scan == 0.2;
842 cv::bitwise_or(is_zero, is_bg, is_zero);
844 cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 9));
845 cv::Mat scan_dilated;
846 cv::dilate(scan, scan_dilated, element, cv::Point(-1, -1), 1, 1, 1);
848 scan_dilated.copyTo(scan, is_zero);
857 int cols = 2*int(
float(rows)*sin(M_PI/180.*fov/2.))+20;
859 cv::Mat scan = cv::Mat::zeros(rows, cols, CV_32FC1);
861 cv::Point center(scan.cols/2, scan.rows);
862 cv::Size full_axes(scan.rows, scan.rows);
863 cv::ellipse(scan, center, full_axes, -90, -fov/2., fov/2., 0.2, -1);
864 cv::Size third_axes(scan.rows/3, scan.rows/3);
865 cv::ellipse(scan, center, third_axes, -90, -fov/2., fov/2., 0, -1);
867 float mapped_range = float(scan.rows);
869 for (
int i = 0; i < depth.rows; ++i) {
870 for (
int j = 0; j < depth.cols; ++j) {
871 float d = depth.at<
float>(i, j);
873 float a = SNR.at<
float>(i, j);
874 if (d == 0 || a == 0) {
885 z = d*sqrt(y*y + z*z);
889 int pi = scan.rows - 1 - int(z/range*mapped_range);
890 int pj = scan.cols/2 + int(x/range*mapped_range);
891 if (pi < scan.rows && pi > 0 && pj < scan.cols && pj > 0 && x*x + z*z < range*range) {
892 scan.at<
float>(pi, pj) = a;
912 float mapped_range = float(raw_scan.rows);
914 cv::Scalar blue(15, 48, 102);
915 cv::Scalar black(0, 0, 0);
916 cv::Mat scan(raw_scan.rows, raw_scan.cols, CV_8UC3);
919 cv::Point center(scan.cols/2, scan.rows);
920 cv::Size axes(scan.rows+3, scan.rows+3);
921 cv::ellipse(scan, center, axes, -90, -fov/2., fov/2., black, -1);
923 for (
int i = 0; i < scan.rows; ++i) {
924 for (
int j = 0; j < scan.cols; ++j) {
925 float a = raw_scan.at<
float>(i, j);
931 scan.at<cv::Vec3b>(i, j) = cv::Vec3b(255*1.25*a, 255*0.78*a, 255*0.50*a);
934 scan.at<cv::Vec3b>(i, j) = cv::Vec3b(255*a, 255*(1.88*a-0.88), 255*(-1.99*a+1.99));
937 scan.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255*(1.88-0.88), 255*(-1.99+1.99));
942 cv::Scalar white(255, 255, 255);
943 cv::Size axes1(2./3.*scan.rows, 2./3.*scan.rows);
944 cv::Size axes2(1./3.*scan.rows, 1./3.*scan.rows);
945 cv::ellipse(scan, center, axes, -90, -fov/2.-0.5, fov/2., white, 1, CV_AA);
946 cv::ellipse(scan, center, axes1, -90, -fov/2., fov/2., white, 1, CV_AA);
947 cv::ellipse(scan, center, axes2, -90, -fov/2., fov/2., white, 1, CV_AA);
949 for (
int i = 0; i < 5; ++i) {
950 float angle = -fov/2.-0.5 + (fov+0.5)*i/float(5-1);
951 int cornerx = int(mapped_range*sin(M_PI/180.*angle));
952 int cornery = int(mapped_range*cos(M_PI/180.*angle));
955 cv::Point corner(scan.cols/2+cornerx, scan.rows-cornery);
958 cv::line(scan, center, corner, white, 1, CV_AA);
981 int rows_arg = this->
height;
982 int cols_arg = this->
width;
983 int step_arg = this->
skip_;
985 sensor_msgs::Image image_msg;
987 image_msg.height = rows_arg;
988 image_msg.width = cols_arg;
989 image_msg.step =
sizeof(float) * cols_arg;
990 image_msg.data.resize(rows_arg * cols_arg *
sizeof(
float));
991 image_msg.is_bigendian = 0;
994 cv::Mat depth_image(rows_arg, cols_arg, CV_32FC1, (
float*)_src);
1009 this->
lock_.unlock();
1014 ROS_DEBUG_NAMED(
"depth_camera",
"publishing default camera info, then depth camera info");
1019 common::Time sensor_update_time = this->
parentSensor_->LastMeasurementTime();
ros::Publisher depth_image_camera_info_pub_
sensor_msgs::Image multibeam_image_msg_
cv::Mat ConstructSonarImage(cv::Mat &depth, cv::Mat &normals)
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
std::string depth_image_topic_name_
image where each pixel contains the depth information
event::ConnectionPtr newRGBPointCloudConnection
std::string image_topic_name_
bool FillDepthImageHelper(sensor_msgs::Image &image_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
void ComputeSonarImage(const float *_src)
push depth image data into ros topic
void publish(const boost::shared_ptr< M > &message) const
common::Time sensor_update_time_
virtual void OnNewRGBPointCloud(const float *_pcd, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
sensor_msgs::Image raw_sonar_image_msg_
void RawSonarImageDisconnect()
void ApplySpeckleNoise(cv::Mat &scan, float fov)
sensor_msgs::Image sonar_image_msg_
void ApplySmoothing(cv::Mat &scan, float fov)
double point_cloud_cutoff_
void MultibeamImageConnect()
ROSCPP_DECL bool isInitialized()
std::vector< int > angle_nbr_indices_
ros::Publisher multibeam_image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
event::ConnectionPtr load_connection_
common::Time last_depth_image_camera_info_update_time_
void DepthInfoDisconnect()
TFSIMD_FORCE_INLINE const tfScalar & y() const
~GazeboRosImageSonar()
Destructor.
cv::Mat ComputeNormalImage(cv::Mat &depth)
ros::CallbackQueue camera_queue_
cv::Mat ConstructVisualScanImage(cv::Mat &raw_scan)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
boost::shared_ptr< int > image_connect_count_
rendering::DepthCameraPtr depthCamera
int depth_info_connect_count_
sensor_msgs::Image image_msg_
void MultibeamImageDisconnect()
ros::Publisher point_cloud_pub_
A pointer to the ROS node. A node will be instantiated if it does not exist.
#define ROS_DEBUG_NAMED(name,...)
void FillDepthImage(const float *_src)
push depth image data into ros topic
std::string point_cloud_topic_name_
ROS image topic name.
event::ConnectionPtr OnLoad(const boost::function< void()> &)
virtual void PublishCameraInfo()
common::Time depth_sensor_update_time_
ros::Publisher normal_image_pub_
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
#define ROS_FATAL_STREAM_NAMED(name, args)
const std::string TYPE_32FC1
virtual void Advertise()
Advertise point cloud and depth image.
void DepthImageDisconnect()
sensors::DepthCameraSensorPtr parentSensor
void RawSonarImageConnect()
void SonarImageDisconnect()
void NormalImageDisconnect()
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::default_random_engine generator
bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
TFSIMD_FORCE_INLINE const tfScalar & z() const
sensor_msgs::PointCloud2 point_cloud_msg_
PointCloud2 point cloud message.
ros::Publisher raw_sonar_image_pub_
cv::Mat ConstructScanImage(cv::Mat &depth, cv::Mat &SNR)
std::string camera_info_topic_name_
event::ConnectionPtr newDepthFrameConnection
void PutCameraData(const unsigned char *_src)
rendering::CameraPtr camera_
sensor_msgs::Image depth_image_msg_
event::ConnectionPtr newImageFrameConnection
void PointCloudDisconnect()
ros::NodeHandle * rosnode_
int depth_image_connect_count_
Keep track of number of connctions for point clouds.
sensor_msgs::Image normal_image_msg_
void ApplyMedianFilter(cv::Mat &scan)
void NormalImageConnect()
void FillPointdCloud(const float *_src)
Put camera data to the ROS topic.
std::vector< std::vector< int > > angle_range_indices_
sensors::SensorPtr parentSensor_
void setPointCloud2FieldsByString(int n_fields,...)
boost::shared_ptr< void > VoidPtr
ros::Publisher depth_image_pub_
ros::Publisher sonar_image_pub_
int point_cloud_connect_count_
Keep track of number of connctions for point clouds.
sensor_msgs::ImagePtr toImageMsg() const
std::string depth_image_camera_info_topic_name_