From 52f867b9933fb24c5561fcd76a0d3b25efe3bf79 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Fri, 17 Jul 2020 20:55:04 -0700 Subject: [PATCH 01/24] add depth camera sensor with normals --- CMakeLists.txt | 16 +- ...epth_camera_sonar_single_beam_basic.launch | 19 + ...ebo_ros_depth_camera_sonar_single_beam.cpp | 706 ++++++++++++++++++ ...azebo_ros_depth_camera_sonar_single_beam.h | 171 +++++ urdf/depth_camera_sonar_single_beam.xacro | 84 +++ ...depth_camera_sonar_single_beam_robot.xacro | 23 + 6 files changed, 1014 insertions(+), 5 deletions(-) create mode 100644 launch/depth_camera_sonar_single_beam_basic.launch create mode 100644 src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp create mode 100644 src/nps_gazebo_ros_depth_camera_sonar_single_beam.h create mode 100644 urdf/depth_camera_sonar_single_beam.xacro create mode 100644 urdf/depth_camera_sonar_single_beam_robot.xacro diff --git a/CMakeLists.txt b/CMakeLists.txt index ea036b9..d19e598 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(nps_uw_sensors_gazebo) -find_package(catkin REQUIRED tf) +find_package(catkin REQUIRED tf gazebo_plugins) find_package(gazebo REQUIRED) find_package(roscpp REQUIRED) @@ -22,13 +22,19 @@ catkin_package() ## Plugins add_library(gazebo_ros_pulse_lidar_plugin src/gazebo_ros_pulse_lidar.cpp) +target_link_libraries(gazebo_ros_pulse_lidar_plugin + ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES}) + add_library(nps_gazebo_ros_gpu_sonar_single_beam_plugin src/nps_gazebo_ros_gpu_sonar_single_beam.cpp) - target_link_libraries(nps_gazebo_ros_gpu_sonar_single_beam_plugin - GpuRayPlugin ${catkin_LIBRARIES}) -target_link_libraries(gazebo_ros_pulse_lidar_plugin - ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES}) + GpuRayPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +add_library(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin + src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp + src/nps_gazebo_ros_depth_camera_sonar_single_beam.h) +target_link_libraries(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin + DepthCameraPlugin ${catkin_LIBRARIES}) # for launch install(DIRECTORY launch diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch new file mode 100644 index 0000000..e2b78a4 --- /dev/null +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp new file mode 100644 index 0000000..25d12ea --- /dev/null +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -0,0 +1,706 @@ +/* + * Copyright 2013 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + Desc: GazeboRosDepthCamera plugin for simulating cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 + */ + +#include +#include +#include +#include + +#include "nps_gazebo_ros_depth_camera_sonar_single_beam.h" + +#include +#include +#include + +#include + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosDepthCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosDepthCamera::GazeboRosDepthCamera() +{ + this->point_cloud_connect_count_ = 0; + this->depth_image_connect_count_ = 0; + this->normals_image_connect_count_ = 0; + this->depth_info_connect_count_ = 0; + this->last_depth_image_camera_info_update_time_ = common::Time(0); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosDepthCamera::~GazeboRosDepthCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + std::cout << "nps_gazebo_ros_depth_camera_sonar_single_beam zzzzzzzzzzzzzzzz"; + DepthCameraPlugin::Load(_parent, _sdf); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("depth_camera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // copying from DepthCameraPlugin into GazeboRosCameraUtils + this->parentSensor_ = this->parentSensor; + this->width_ = this->width; + this->height_ = this->height; + this->depth_ = this->depth; + this->format_ = this->format; + this->camera_ = this->depthCamera; + + // using a different default + if (!_sdf->HasElement("imageTopicName")) + this->image_topic_name_ = "ir/image_raw"; + if (!_sdf->HasElement("cameraInfoTopicName")) + this->camera_info_topic_name_ = "ir/camera_info"; + + // point cloud stuff + if (!_sdf->HasElement("pointCloudTopicName")) + this->point_cloud_topic_name_ = "points"; + else + this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + + // depth image and normals stuff + if (!_sdf->HasElement("depthImageTopicName")) + this->depth_image_topic_name_ = "depth/image_raw"; + else + this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get(); + + if (!_sdf->HasElement("normalsImageTopicName")) + this->normals_image_topic_name_ = "normals/image_raw"; + else + this->normals_image_topic_name_ = _sdf->GetElement("normalsImageTopicName")->Get(); + + if (!_sdf->HasElement("depthImageCameraInfoTopicName")) + this->depth_image_camera_info_topic_name_ = "depth/camera_info"; + else + this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get(); + + if (!_sdf->HasElement("pointCloudCutoff")) + this->point_cloud_cutoff_ = 0.4; + else + this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get(); + + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosDepthCamera::Advertise, this)); + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +void GazeboRosDepthCamera::Advertise() +{ + ros::AdvertiseOptions point_cloud_ao = + ros::AdvertiseOptions::create( + this->point_cloud_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::PointCloudConnect,this), + boost::bind( &GazeboRosDepthCamera::PointCloudDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); + + ros::AdvertiseOptions depth_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->depth_image_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::DepthImageConnect,this), + boost::bind( &GazeboRosDepthCamera::DepthImageDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao); + + ros::AdvertiseOptions normals_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->normals_image_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::NormalsImageConnect,this), + boost::bind( &GazeboRosDepthCamera::NormalsImageDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->normals_image_pub_ = this->rosnode_->advertise(normals_image_ao); + + ros::AdvertiseOptions depth_image_camera_info_ao = + ros::AdvertiseOptions::create( + this->depth_image_camera_info_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::DepthInfoConnect,this), + boost::bind( &GazeboRosDepthCamera::DepthInfoDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::PointCloudConnect() +{ + this->point_cloud_connect_count_++; + (*this->image_connect_count_)++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::PointCloudDisconnect() +{ + this->point_cloud_connect_count_--; + (*this->image_connect_count_)--; + if (this->point_cloud_connect_count_ <= 0) + this->parentSensor->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthImageConnect() +{ +std::cout << "DepthImageConnect\n"; + this->depth_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthImageDisconnect() +{ +std::cout << "DepthImageDisconnect\n"; + this->depth_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::NormalsImageConnect() +{ +std::cout << "NormalsImageConnect\n"; + this->normals_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::NormalsImageDisconnect() +{ +std::cout << "NormalsImageDisconnect\n"; + this->normals_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthInfoConnect() +{ + this->depth_info_connect_count_++; +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthInfoDisconnect() +{ + this->depth_info_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); +# else + this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); +# endif + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + this->normals_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { + if (this->point_cloud_connect_count_ > 0) + this->FillPointdCloud(_image); + + if (this->depth_image_connect_count_ > 0) + this->FillDepthImage(_image); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0 || // zz why? + this->normals_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); +# else + this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); +# endif + + if (!this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ > 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + else + { + if (this->point_cloud_connect_count_ > 0) + { + this->lock_.lock(); + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(_width*_height); + + point_cloud_msg_.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + for (unsigned int i = 0; i < _width; i++) + { + for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) + { + unsigned int index = (j * _width) + i; + *iter_x = _pcd[4 * index]; + *iter_y = _pcd[4 * index + 1]; + *iter_z = _pcd[4 * index + 2]; + *iter_rgb = _pcd[4 * index + 3]; + if (i == _width /2 && j == _height / 2) + { + uint32_t rgb = *reinterpret_cast(&(*iter_rgb)); + uint8_t r = (rgb >> 16) & 0x0000ff; + uint8_t g = (rgb >> 8) & 0x0000ff; + uint8_t b = (rgb) & 0x0000ff; + std::cerr << (int)r << " " << (int)g << " " << (int)b << "\n"; + } + } + } + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + this->lock_.unlock(); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + //ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); +# if GAZEBO_MAJOR_VERSION >= 7 + this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); +# else + this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); +# endif + + if (!this->parentSensor->IsActive()) + { + if ((*this->image_connect_count_) > 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + else + { + if ((*this->image_connect_count_) > 0) + { + this->PutCameraData(_image); + // TODO(lucasw) publish camera info with depth image + // this->PublishCameraInfo(sensor_update_time); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put camera data to the interface +void GazeboRosDepthCamera::FillPointdCloud(const float *_src) +{ + this->lock_.lock(); + + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + + ///copy from depth to point cloud message + FillPointCloudHelper(this->point_cloud_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + + this->lock_.unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Put depth image data to the interface +void GazeboRosDepthCamera::FillDepthImage(const float *_src) +{ + this->lock_.lock(); + // copy data into image + this->depth_image_msg_.header.frame_id = this->frame_name_; + this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + + ///copy from depth to depth image message + FillDepthImageHelper(this->depth_image_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->depth_image_pub_.publish(this->depth_image_msg_); + + this->lock_.unlock(); +} + + +// Fill depth information +bool GazeboRosDepthCamera::FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(rows_arg*cols_arg); + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + point_cloud_msg.is_dense = true; + + float* toCopyFrom = (float*)data_arg; + int index = 0; + + double hfov = this->parentSensor->DepthCamera()->HFOV().Radian(); + double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); + + // convert depth to point cloud + for (uint32_t j=0; j1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); + else pAngle = 0.0; + + for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); + else yAngle = 0.0; + + double depth = toCopyFrom[index++]; + + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); + if(depth > this->point_cloud_cutoff_) + { + *iter_z = depth; + } + else //point in the unseeable range + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + point_cloud_msg.is_dense = false; + } + + // put image color data for each point + uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0])); + if (this->image_msg_.data.size() == rows_arg*cols_arg*3) + { + // color + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; + } + else if (this->image_msg_.data.size() == rows_arg*cols_arg) + { + // mono (or bayer? @todo; fix for bayer) + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; + } + else + { + // no image + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; + } + } + } + + return true; +} + +// Fill depth information +bool GazeboRosDepthCamera::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.is_bigendian = 0; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + + // convert depth to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + + if (depth > this->point_cloud_cutoff_) + { + dest[i + j * cols_arg] = depth; + } + else //point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + } + return true; +} + +// ************************************************************************ +// normals +// ************************************************************************ + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewNormalsFrame(const float *_normals_image, + unsigned int _width, unsigned int _height, unsigned int _normals, + const std::string &_format) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->normals_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); +# else + this->normals_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); +# endif + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + this->normals_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { +// if (this->point_cloud_connect_count_ > 0) +// this->FillPointdCloud(_normals_image); + + if (this->normals_image_connect_count_ > 0) + this->FillNormalsImage(_normals_image); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0 || // zz why? + this->normals_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put normals image data to the interface +void GazeboRosDepthCamera::FillNormalsImage(const float *_src) +{ + this->lock_.lock(); + // copy data into image + this->normals_image_msg_.header.frame_id = this->frame_name_; + this->normals_image_msg_.header.stamp.sec = this->normals_sensor_update_time_.sec; + this->normals_image_msg_.header.stamp.nsec = this->normals_sensor_update_time_.nsec; + + ///copy from normals to normals image message + FillNormalsImageHelper(this->normals_image_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->normals_image_pub_.publish(this->normals_image_msg_); + + this->lock_.unlock(); +} + +// Fill normals information +bool GazeboRosDepthCamera::FillNormalsImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.is_bigendian = 0; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + + // convert normals to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float normals = toCopyFrom[index++]; + +// if (normals > this->point_cloud_cutoff_) +// { + dest[i + j * cols_arg] = normals; +// } +// else //point in the unseeable range +// { +// dest[i + j * cols_arg] = bad_point; +// } + } + } + return true; +} + + + + +// ************************************************************************ +// end normals +// ************************************************************************ + + + + +void GazeboRosDepthCamera::PublishCameraInfo() +{ + ROS_DEBUG_NAMED("depth_camera", "publishing default camera info, then depth camera info"); + GazeboRosCameraUtils::PublishCameraInfo(); + + if (this->depth_info_connect_count_ > 0) + { +# if GAZEBO_MAJOR_VERSION >= 7 + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); +# else + common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); +# endif + this->sensor_update_time_ = sensor_update_time; + if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + { + this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time); + this->last_depth_image_camera_info_update_time_ = sensor_update_time; + } + } +} + +//@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +/* +#include +pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); + +void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time) +{ + stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared (); + disp_msg->header.stamp = time; + disp_msg->header.frame_id = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + disp_msg->image.height = depth_height_; + disp_msg->image.width = depth_width_; + disp_msg->image.step = disp_msg->image.width * sizeof (float); + disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step); + disp_msg->T = depth.getBaseline (); + disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth (); + + /// @todo Compute these values from DepthGenerator::GetDeviceMaxDepth() and the like + disp_msg->min_disparity = 0.0; + disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3; + disp_msg->delta_d = 0.125; + + depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast(&disp_msg->image.data[0]), disp_msg->image.step); + + pub_disparity_.publish (disp_msg); +} +*/ + + +} + diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h new file mode 100644 index 0000000..65685f2 --- /dev/null +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor. + * Author: John Hsu + * Date: 24 Sept 2008 + */ + +#ifndef GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH +#define GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH + +// ros stuff +#include +#include +#include + +// ros messages stuff +#include +#include +#include +#include +#include +#include + +// gazebo stuff +#include +#include +#include +#include +#include +#include +#include + +// dynamic reconfigure stuff +#include +#include + +// boost stuff +#include + +// camera stuff +#include + +namespace gazebo +{ + class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosDepthCamera(); + + /// \brief Destructor + public: ~GazeboRosDepthCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Advertise point cloud and depth image + public: virtual void Advertise(); + + /// \brief Update the controller + protected: virtual void OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewNormalsFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewRGBPointCloud(const float *_pcd, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Put camera data to the ROS topic + private: void FillPointdCloud(const float *_src); + + /// \brief push depth image data into ros topic + private: void FillDepthImage(const float *_src); + + /// \brief push normals image data into ros topic + private: void FillNormalsImage(const float *_src); + + /// \brief Keep track of number of connctions for point clouds + private: int point_cloud_connect_count_; + private: void PointCloudConnect(); + private: void PointCloudDisconnect(); + + /// \brief Keep track of number of connctions for point clouds + private: int depth_image_connect_count_; + private: void DepthImageConnect(); + private: void DepthImageDisconnect(); + private: int normals_image_connect_count_; + private: void NormalsImageConnect(); + private: void NormalsImageDisconnect(); + private: common::Time last_depth_image_camera_info_update_time_; + + private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillDepthImageHelper(sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillNormalsImageHelper(sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::Publisher point_cloud_pub_; + private: ros::Publisher depth_image_pub_; + private: ros::Publisher normals_image_pub_; + + /// \brief PointCloud2 point cloud message + private: sensor_msgs::PointCloud2 point_cloud_msg_; + private: sensor_msgs::Image depth_image_msg_; + private: sensor_msgs::Image normals_image_msg_; + + private: double point_cloud_cutoff_; + + /// \brief ROS image topic name + private: std::string point_cloud_topic_name_; + + private: void InfoConnect(); + private: void InfoDisconnect(); + + using GazeboRosCameraUtils::PublishCameraInfo; + protected: virtual void PublishCameraInfo(); + + /// \brief image where each pixel contains the depth information + private: std::string depth_image_topic_name_; + private: std::string normals_image_topic_name_; + private: std::string depth_image_camera_info_topic_name_; + private: int depth_info_connect_count_; + private: void DepthInfoConnect(); + private: void DepthInfoDisconnect(); + + // overload with our own + private: common::Time depth_sensor_update_time_; + private: common::Time normals_sensor_update_time_; + protected: ros::Publisher depth_image_camera_info_pub_; + + private: event::ConnectionPtr load_connection_; + }; + +} +#endif + + diff --git a/urdf/depth_camera_sonar_single_beam.xacro b/urdf/depth_camera_sonar_single_beam.xacro new file mode 100644 index 0000000..d99f0f9 --- /dev/null +++ b/urdf/depth_camera_sonar_single_beam.xacro @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + false + + + true + ${update_rate} + true + ${topic} + 0 0 0 0 0 0 + + + depth_camera_sonar_single_beam_sensor_camera + + image_raw + image_raw/camera_info + point_cloud + image_depth + image_normals + image_depth/camera_info + + forward_sonar_optical_link + + + + + + + ${beam_width} + + ${samples} + 1 + R8G8B8 + + + ${min_range} + ${max_range} + + + depths normals + + + + + + + diff --git a/urdf/depth_camera_sonar_single_beam_robot.xacro b/urdf/depth_camera_sonar_single_beam_robot.xacro new file mode 100644 index 0000000..02c60dc --- /dev/null +++ b/urdf/depth_camera_sonar_single_beam_robot.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + From 259935f70388556f47d152275d92650e3b8d784c Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Fri, 24 Jul 2020 09:49:35 -0700 Subject: [PATCH 02/24] add ray topic --- ...epth_camera_sonar_single_beam_basic.launch | 15 ++ ...ebo_ros_depth_camera_sonar_single_beam.cpp | 222 ++++++++++++++---- ...azebo_ros_depth_camera_sonar_single_beam.h | 24 +- urdf/depth_camera_sonar_single_beam.xacro | 4 +- ...depth_camera_sonar_single_beam_robot.xacro | 16 ++ worlds/sonar_single_beam_basic.world | 2 + 6 files changed, 239 insertions(+), 44 deletions(-) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch index e2b78a4..78b62f5 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -15,5 +15,20 @@ respawn="false" output="screen" args="-urdf -model nps_depth_camera_sonar_single_beam_model -param depth_camera_sonar_single_beam_robot" /> + + + + + + + + + + + + diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index 25d12ea..b56e66e 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -92,6 +92,12 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + // Sonar point cloud stuff + if (!_sdf->HasElement("sonarPointCloudTopicName")) + this->sonar_point_cloud_topic_name_ = "sonar_points"; + else + this->sonar_point_cloud_topic_name_ = _sdf->GetElement("sonarPointCloudTopicName")->Get(); + // depth image and normals stuff if (!_sdf->HasElement("depthImageTopicName")) this->depth_image_topic_name_ = "depth/image_raw"; @@ -103,6 +109,11 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->normals_image_topic_name_ = _sdf->GetElement("normalsImageTopicName")->Get(); + if (!_sdf->HasElement("rayImageTopicName")) + this->ray_image_topic_name_ = "ray/image_raw"; + else + this->ray_image_topic_name_ = _sdf->GetElement("rayImageTopicName")->Get(); + if (!_sdf->HasElement("depthImageCameraInfoTopicName")) this->depth_image_camera_info_topic_name_ = "depth/camera_info"; else @@ -127,6 +138,14 @@ void GazeboRosDepthCamera::Advertise() ros::VoidPtr(), &this->camera_queue_); this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); + ros::AdvertiseOptions sonar_point_cloud_ao = + ros::AdvertiseOptions::create( + this->sonar_point_cloud_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::SonarPointCloudConnect,this), + boost::bind( &GazeboRosDepthCamera::SonarPointCloudDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->sonar_point_cloud_pub_ = this->rosnode_->advertise(sonar_point_cloud_ao); + ros::AdvertiseOptions depth_image_ao = ros::AdvertiseOptions::create< sensor_msgs::Image >( this->depth_image_topic_name_,1, @@ -143,6 +162,14 @@ void GazeboRosDepthCamera::Advertise() ros::VoidPtr(), &this->camera_queue_); this->normals_image_pub_ = this->rosnode_->advertise(normals_image_ao); + ros::AdvertiseOptions ray_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->ray_image_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::RayImageConnect,this), + boost::bind( &GazeboRosDepthCamera::RayImageDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->ray_image_pub_ = this->rosnode_->advertise(ray_image_ao); + ros::AdvertiseOptions depth_image_camera_info_ao = ros::AdvertiseOptions::create( this->depth_image_camera_info_topic_name_,1, @@ -171,6 +198,38 @@ void GazeboRosDepthCamera::PointCloudDisconnect() this->parentSensor->SetActive(false); } +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::SonarPointCloudConnect() +{ +std::cout << "SonarPointCloudConnect\n"; + this->sonar_point_cloud_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::SonarPointCloudDisconnect() +{ + // zz never turns off parentSensor + this->sonar_point_cloud_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::RayImageConnect() +{ +std::cout << "DepthImageConnect\n"; + this->ray_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::RayImageDisconnect() +{ +std::cout << "DepthImageDisconnect\n"; + this->ray_image_connect_count_--; +} + //////////////////////////////////////////////////////////////////////////////// // Increment count void GazeboRosDepthCamera::DepthImageConnect() @@ -222,6 +281,7 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { +std::cout << "OnNewDepthFrame\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; @@ -236,6 +296,8 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image, if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 && this->normals_image_connect_count_ <= 0 && + this->ray_image_connect_count_ <= 0 && + this->sonar_point_cloud_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) { this->parentSensor->SetActive(false); @@ -265,6 +327,7 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { +std::cout << "OnNewRGBPointCloud\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; @@ -335,6 +398,7 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { +std::cout << "OnNewImageFrame\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; @@ -493,45 +557,6 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( return true; } -// Fill depth information -bool GazeboRosDepthCamera::FillDepthImageHelper( - sensor_msgs::Image& image_msg, - uint32_t rows_arg, uint32_t cols_arg, - uint32_t step_arg, void* data_arg) -{ - image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - image_msg.height = rows_arg; - image_msg.width = cols_arg; - image_msg.step = sizeof(float) * cols_arg; - image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); - image_msg.is_bigendian = 0; - - const float bad_point = std::numeric_limits::quiet_NaN(); - - float* dest = (float*)(&(image_msg.data[0])); - float* toCopyFrom = (float*)data_arg; - int index = 0; - - // convert depth to point cloud - for (uint32_t j = 0; j < rows_arg; j++) - { - for (uint32_t i = 0; i < cols_arg; i++) - { - float depth = toCopyFrom[index++]; - - if (depth > this->point_cloud_cutoff_) - { - dest[i + j * cols_arg] = depth; - } - else //point in the unseeable range - { - dest[i + j * cols_arg] = bad_point; - } - } - } - return true; -} - // ************************************************************************ // normals // ************************************************************************ @@ -542,6 +567,7 @@ void GazeboRosDepthCamera::OnNewNormalsFrame(const float *_normals_image, unsigned int _width, unsigned int _height, unsigned int _normals, const std::string &_format) { +std::cout << "OnNewNormalsFrame\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; @@ -556,6 +582,8 @@ void GazeboRosDepthCamera::OnNewNormalsFrame(const float *_normals_image, if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 && this->normals_image_connect_count_ <= 0 && + this->ray_image_connect_count_ <= 0 && + this->sonar_point_cloud_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) { this->parentSensor->SetActive(false); @@ -567,6 +595,9 @@ void GazeboRosDepthCamera::OnNewNormalsFrame(const float *_normals_image, if (this->normals_image_connect_count_ > 0) this->FillNormalsImage(_normals_image); + + // lets perform Sonar calculations here + this->FillRayImage(); } } else @@ -607,11 +638,11 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg) { - image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC4; image_msg.height = rows_arg; image_msg.width = cols_arg; image_msg.step = sizeof(float) * cols_arg; - image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)*4); image_msg.is_bigendian = 0; const float bad_point = std::numeric_limits::quiet_NaN(); @@ -621,11 +652,13 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( int index = 0; // convert normals to point cloud +std::cout << "FillNormalsImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; for (uint32_t j = 0; j < rows_arg; j++) { - for (uint32_t i = 0; i < cols_arg; i++) + for (uint32_t i = 0; i < cols_arg*4; i++) { float normals = toCopyFrom[index++]; + std::cout << normals << " "; // if (normals > this->point_cloud_cutoff_) // { @@ -636,6 +669,7 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( // dest[i + j * cols_arg] = bad_point; // } } + std::cout << "\n"; } return true; } @@ -647,7 +681,111 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( // end normals // ************************************************************************ +// ************************************************************************ +// Sonar +// ************************************************************************ + +// void FillGaussianImage(); + +void GazeboRosDepthCamera::FillRayImage() +{ + if (this->depth_image_msg_.width == 0) { + std::cout << "FillRayImage depth_image_msg is empty\n"; + return; + } + if (this->normals_image_msg_.width == 0) { + std::cout << "FillRayImage normals_image_msg is empty\n"; + return; + } + + this->lock_.lock(); + // copy data into image + this->ray_image_msg_.header.frame_id = this->frame_name_; + // get metadata from depth data + this->ray_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->ray_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->ray_image_msg_.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + float rows_arg = this->depth_image_msg_.height; + float cols_arg = this->depth_image_msg_.width; +std::cout << "FillRayImage rows: " << rows_arg << " cols: " << cols_arg << "\n"; + this->ray_image_msg_.height = rows_arg; + this->ray_image_msg_.width = cols_arg; + this->ray_image_msg_.step = sizeof(float) * cols_arg; + this->ray_image_msg_.data.resize(rows_arg * cols_arg * sizeof(float)); + this->ray_image_msg_.is_bigendian = 0; + + float* from_depth = (float*)(&(this->depth_image_msg_.data[0])); + float* from_normals = (float*)(&(this->normals_image_msg_.data[0])); +// float* from_beam_kernel = (float*)(&(this->beam_kernel_msg_.data[0])); + float* to_ray = (float*)(&(this->ray_image_msg_.data[0])); + + // calculate intensity for each ray + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + int index = i + j * cols_arg; + float depth = from_depth[index]; + float normal = from_normals[index]; + + // Sonar equation for point (col, row) + float calculated_intensity = depth + normal; + + to_ray[index] = calculated_intensity; + } + } + + this->ray_image_pub_.publish(this->ray_image_msg_); + + this->lock_.unlock(); +} + +// Fill depth information +bool GazeboRosDepthCamera::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.is_bigendian = 0; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + // convert depth to image +std::cout << "FillDepthImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + std::cout << depth << " "; + + if (depth > this->point_cloud_cutoff_) + { + dest[i + j * cols_arg] = depth; + } + else //point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + std::cout << "\n"; + } + return true; +} + + +// ************************************************************************ +// end Sonar +// ************************************************************************ void GazeboRosDepthCamera::PublishCameraInfo() diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h index 65685f2..4fed114 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h @@ -107,13 +107,21 @@ namespace gazebo private: void PointCloudConnect(); private: void PointCloudDisconnect(); - /// \brief Keep track of number of connctions for point clouds + /// \brief Keep track of number of connctions for Sonar point clouds + private: int sonar_point_cloud_connect_count_; + private: void SonarPointCloudConnect(); + private: void SonarPointCloudDisconnect(); + + /// \brief Keep track of number of connctions for images private: int depth_image_connect_count_; private: void DepthImageConnect(); private: void DepthImageDisconnect(); private: int normals_image_connect_count_; private: void NormalsImageConnect(); private: void NormalsImageDisconnect(); + private: int ray_image_connect_count_; + private: void RayImageConnect(); + private: void RayImageDisconnect(); private: common::Time last_depth_image_camera_info_update_time_; private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, @@ -128,21 +136,34 @@ namespace gazebo uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg); + private: void FillRayImage(); + private: void FillSonarPointCloud(); + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. private: ros::Publisher point_cloud_pub_; + private: ros::Publisher sonar_point_cloud_pub_; + private: ros::Publisher depth_image_pub_; private: ros::Publisher normals_image_pub_; + private: ros::Publisher ray_image_pub_; /// \brief PointCloud2 point cloud message private: sensor_msgs::PointCloud2 point_cloud_msg_; + private: sensor_msgs::PointCloud2 sonar_point_cloud_msg_; + private: sensor_msgs::Image depth_image_msg_; private: sensor_msgs::Image normals_image_msg_; + private: sensor_msgs::Image ray_image_msg_; + private: double point_cloud_cutoff_; /// \brief ROS image topic name private: std::string point_cloud_topic_name_; + /// \brief ROS image Sonar topic name + private: std::string sonar_point_cloud_topic_name_; + private: void InfoConnect(); private: void InfoDisconnect(); @@ -152,6 +173,7 @@ namespace gazebo /// \brief image where each pixel contains the depth information private: std::string depth_image_topic_name_; private: std::string normals_image_topic_name_; + private: std::string ray_image_topic_name_; private: std::string depth_image_camera_info_topic_name_; private: int depth_info_connect_count_; private: void DepthInfoConnect(); diff --git a/urdf/depth_camera_sonar_single_beam.xacro b/urdf/depth_camera_sonar_single_beam.xacro index d99f0f9..b049903 100644 --- a/urdf/depth_camera_sonar_single_beam.xacro +++ b/urdf/depth_camera_sonar_single_beam.xacro @@ -48,8 +48,10 @@ image_raw image_raw/camera_info point_cloud + sonar_point_cloud image_depth image_normals + image_ray image_depth/camera_info forward_sonar_optical_link @@ -66,7 +68,7 @@ ${beam_width} ${samples} - 1 + ${samples} R8G8B8 diff --git a/urdf/depth_camera_sonar_single_beam_robot.xacro b/urdf/depth_camera_sonar_single_beam_robot.xacro index 02c60dc..e1b2335 100644 --- a/urdf/depth_camera_sonar_single_beam_robot.xacro +++ b/urdf/depth_camera_sonar_single_beam_robot.xacro @@ -6,6 +6,7 @@ + + + + + + diff --git a/worlds/sonar_single_beam_basic.world b/worlds/sonar_single_beam_basic.world index 3c729b6..9e62857 100644 --- a/worlds/sonar_single_beam_basic.world +++ b/worlds/sonar_single_beam_basic.world @@ -22,11 +22,13 @@ + From 597c66ed8598b1a2124122079d06cd2038b453e8 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Fri, 24 Jul 2020 19:45:50 -0700 Subject: [PATCH 03/24] add Python ROS node for calculating ray power --- CMakeLists.txt | 3 + ...epth_camera_sonar_single_beam_basic.launch | 6 + scripts/depth_camera_sonar.py | 224 ++++++++++++++++++ 3 files changed, 233 insertions(+) create mode 100755 scripts/depth_camera_sonar.py diff --git a/CMakeLists.txt b/CMakeLists.txt index d19e598..b886610 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,3 +41,6 @@ install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN "*~" EXCLUDE) +# for Python scripts +catkin_install_python(PROGRAMS scripts/depth_camera_sonar.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch index 78b62f5..72cd994 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -15,6 +15,12 @@ respawn="false" output="screen" args="-urdf -model nps_depth_camera_sonar_single_beam_model -param depth_camera_sonar_single_beam_robot" /> + + + + diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py new file mode 100755 index 0000000..f03e9d9 --- /dev/null +++ b/scripts/depth_camera_sonar.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python +#import sys +from argparse import ArgumentParser +from cv_bridge import CvBridge +import cv2 +import numpy as np +import rospy +from sensor_msgs.msg import Image + +# references: +# http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython +# http://docs.ros.org/melodic/api/cv_bridge/html/python/index.html + +# Notes: +# Arguments passed here from the launch file must match arguments in SDF. + +# ROS-CV bridge +bridge = CvBridge() + +# get angle width, height, num_rows, num_cols for one Sonar beam +def depth_camera_args(): + # args + parser = ArgumentParser(description="Start depth camera sonar ROS node") + parser.add_argument("width", type=float, help="Width angle") + parser.add_argument("height", type=float, help="Height angle") + parser.add_argument("num_rows", type=int, help="number of beams horiz") + parser.add_argument("num_cols", type=int, help="number of beams vert") + parser.add_argument("lobe_k1", type=float, help="Sonar lobe constant k1") + parser.add_argument("lobe_k2", type=float, help="Sonar lobe constant k2") + args = parser.parse_args() + return args.width, args.height, args.num_rows, args.num_cols, \ + args.lobe_k1, args.lobe_k2 + +# calculate angle steps +def angle_steps(width, height, num_rows, num_cols): + # horizontal and vertical starting angles and angle steps + if num_rows < 2: + width_0 = 0 + d_width = 0 + else: + width_0 = -width/2.0 + d_width = width / (num_rows - 1) + + if num_cols < 2: + height_0 = 0 + d_height = 0 + else: + height_0 = -height/2.0 + d_height = height / (num_cols - 1) + + return width_0, d_width, height_0, d_height + +# calculate Sonar head angle for each ray as tuples [height_angle, width_angle] +def camera_angle_matrix_constant(width, height, num_rows, num_cols): + + # horizontal and vertical starting angles and angle steps + width_0, d_width, height_0, d_height = angle_steps( + width, height, num_rows, num_cols) + + camera_angle_matrix = np.zeros((num_rows, num_cols, 2), np.float32) + height_i = height_0 + for i in range(num_rows): + width_j = width_0 + for j in range(num_cols): + camera_angle_matrix[i,j] = [height_i, width_j] + width_j += d_width + height_i += d_height + + return camera_angle_matrix + +# calculate lobe power matrix +def lobe_power_matrix_constant(width, height, num_rows, num_cols, k1, k2): + + # horizontal and vertical starting angles and angle steps + width_0, d_width, height_0, d_height = angle_steps( + width, height, num_rows, num_cols) + + lobe_power_matrix = np.zeros((num_rows, num_cols), np.float32) + height_i = height_0 + for i in range(num_rows): + width_j = width_0 + for j in range(num_cols): + ray_power = 1.0 # use angles and constants instead + lobe_power_matrix[i,j] = ray_power + width_j += d_width + height_i += d_height + + return lobe_power_matrix + +# currently retro is hardcoded to a uniform constant +def retro_power_matrix_constant(num_rows, num_cols): + retro_matrix = np.ones((num_rows, num_cols), np.float32) + return retro_matrix + +# calculate depth power matrix from depth values +def calculate_depth_power_matrix(depth_matrix, num_rows, num_cols): + depth_power_matrix = np.zeros((num_rows, num_cols), np.float32) + for i in range(num_rows): + for j in range(num_cols): + depth = depth_matrix[i,j] + depth_power = 1 # use equation + depth_power_matrix[i,j] = depth_power + return depth_power_matrix + +# calculate horizontal and vertical angles from x,y,z,1 +def calculate_normals_matrix(normals_f4, num_rows, num_cols): + normals_f2 = np.zeros((num_rows, num_cols, 2), np.float32) + for i in range(num_rows): + for j in range(num_cols): + x,y,z = normals_f4[i,j,:3] + # apply functions to get height and width angles + normals_f2[i,j] = (0,0) + return normals_f2 + +# calculate normals matrix as CV_32FC1 from CV_32FC4 x,y,z,1 image normals +def calculate_normals_power_matrix(normals_f2, num_rows, num_cols): + normals_power_matrix = np.zeros((num_rows, num_cols), np.float32) + for i in range(num_rows): + for j in range(num_cols): + normal_horiz, normal_vert = normals_f2[i,j] + + # apply math on normal_horiz, normal_vert to get some power value + # for now hardcoded answer = 1 + normals_power_matrix[i,j] = 1.0 + + return normals_power_matrix + +class SonarMaker: + def __init__(self): + + # user inputs warning: hardcoded in launch file, should match sdf + self.width, self.height, self.num_rows, self.num_cols, \ + self.lobe_k1, self.lobe_k2 = depth_camera_args() + # depth and normals matrix from GPU + self.depth_matrix = None + self.normals_matrix = None + + # matrices that hold constants + self.camera_angle_matrix = camera_angle_matrix_constant( + self.width, self.height, self.num_rows, self.num_cols) + self.lobe_power_matrix = lobe_power_matrix_constant( + self.width, self.height, self.num_rows, self.num_cols, + self.lobe_k1, self.lobe_k2) + + # matrices that should be calculated but are not + self.retro_power_matrix = retro_power_matrix_constant( + self.num_rows, self.num_cols) + + # ROS subscribers + self.depth_sub = rospy.Subscriber( + "depth_camera_sonar_single_beam_sensor_camera/image_depth", + Image, self.on_depth_image) + self.normals_sub = rospy.Subscriber( + "depth_camera_sonar_single_beam_sensor_camera/image_normals", + Image, self.on_normals_image) + + # ROS publishers + self.ray_pub = rospy.Publisher("image_sonar_rays_topic", Image) + + def images_to_rays(self): + # inputs + depth_power_matrix = self.depth_power_matrix + normals_power_matrix = self.normals_power_matrix + lobe_power_matrix = self.lobe_power_matrix + retro_power_matrix = self.retro_power_matrix + + # rays + ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) + return ray_matrix + + # get depth matrix from Gazebo and cache it + def on_depth_image(self, depth_image): + rospy.loginfo(rospy.get_caller_id() + "received depth_image") + depth_matrix = bridge.imgmsg_to_cv2(depth_image) + print("depth image shape: ", depth_matrix.shape) + if depth_matrix.shape != (self.num_rows, self.num_cols): + # bad + print("Invalid depth image shape", self.depth_matrix.shape) + return + + self.depth_power_matrix = calculate_depth_power_matrix( + depth_matrix, self.num_rows, self.num_cols) + + # get normals matrix from Gazebo and calculate Sonar + def on_normals_image(self, normals_image): + rospy.loginfo(rospy.get_caller_id() + "received normals_image") + normals_matrix_f4 = bridge.imgmsg_to_cv2(normals_image) + print("normals image shape: ", normals_matrix_f4.shape) + if normals_matrix_f4.shape != (self.num_rows, self.num_cols, 4): + # bad + print("Invalid normals image shape", normals_matrix_f4.shape) + return + + normals_matrix = calculate_normals_matrix( + normals_matrix_f4, self.num_rows, self.num_cols) + compensated_normals_matrix = cv2.add(normals_matrix, + self.camera_angle_matrix) + self.normals_power_matrix = calculate_normals_power_matrix( + compensated_normals_matrix, self.num_rows, self.num_cols) + + # use depth and normals matrices to make rays + ray_matrix = self.images_to_rays() + + # advertise ray_matrix to ROS, keep 32FC1 + self.ray_pub.publish(bridge.cv2_to_imgmsg(ray_matrix, "passthrough")) + print("Ray power matrix", ray_matrix) + + # use rays to make one beam + # make beam, TBD + # publish beam, TBD + +if __name__ == '__main__': + sonar_maker = SonarMaker() + rospy.init_node('depth_and_normals_to_sonar', anonymous=True) + + # spin until Ctrl-C + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") +# cv2.destroyAllWindows() + From 74a6355ace3aa795afa5a107bb7206cd2b690b6e Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Tue, 28 Jul 2020 11:02:42 -0700 Subject: [PATCH 04/24] add ROS node for calculating ray and beam --- scripts/depth_camera_sonar.py | 175 ++++++++++++++++------------------ 1 file changed, 80 insertions(+), 95 deletions(-) diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index f03e9d9..3f79b3c 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -23,128 +23,115 @@ def depth_camera_args(): parser = ArgumentParser(description="Start depth camera sonar ROS node") parser.add_argument("width", type=float, help="Width angle") parser.add_argument("height", type=float, help="Height angle") - parser.add_argument("num_rows", type=int, help="number of beams horiz") - parser.add_argument("num_cols", type=int, help="number of beams vert") + parser.add_argument("num_cols", type=int, help="number of beams horiz") + parser.add_argument("num_rows", type=int, help="number of beams vert") parser.add_argument("lobe_k1", type=float, help="Sonar lobe constant k1") parser.add_argument("lobe_k2", type=float, help="Sonar lobe constant k2") args = parser.parse_args() return args.width, args.height, args.num_rows, args.num_cols, \ args.lobe_k1, args.lobe_k2 -# calculate angle steps -def angle_steps(width, height, num_rows, num_cols): - # horizontal and vertical starting angles and angle steps - if num_rows < 2: +# matrix iterator given horizontal and vertical angle and count +def matrix_iterator(width, height, num_cols, num_rows): + print("num_cols %d,num_rows %d, width: %f, height: %f"%(width, height, num_cols, num_rows)) + print(7/0) # zzzzzzzzzzzzzzzzzzzzzzzzzzzzzz + + # set up horizontal and vertical starting angles and angle steps + # returns column, row, horizontal angle, vertical angle + if num_cols < 2: width_0 = 0 d_width = 0 else: width_0 = -width/2.0 - d_width = width / (num_rows - 1) + d_width = width / (num_cols - 1) - if num_cols < 2: + if num_rows < 2: height_0 = 0 d_height = 0 else: height_0 = -height/2.0 - d_height = height / (num_cols - 1) - - return width_0, d_width, height_0, d_height - -# calculate Sonar head angle for each ray as tuples [height_angle, width_angle] -def camera_angle_matrix_constant(width, height, num_rows, num_cols): - - # horizontal and vertical starting angles and angle steps - width_0, d_width, height_0, d_height = angle_steps( - width, height, num_rows, num_cols) + d_height = height / (num_rows - 1) - camera_angle_matrix = np.zeros((num_rows, num_cols, 2), np.float32) +# perform sweep height_i = height_0 for i in range(num_rows): width_j = width_0 for j in range(num_cols): - camera_angle_matrix[i,j] = [height_i, width_j] + yield j, i, width_j, height_i # yield width_j += d_width height_i += d_height - return camera_angle_matrix - # calculate lobe power matrix -def lobe_power_matrix_constant(width, height, num_rows, num_cols, k1, k2): - - # horizontal and vertical starting angles and angle steps - width_0, d_width, height_0, d_height = angle_steps( - width, height, num_rows, num_cols) +def lobe_power_matrix_constant(width, height, num_cols, num_rows, k1, k2): lobe_power_matrix = np.zeros((num_rows, num_cols), np.float32) - height_i = height_0 - for i in range(num_rows): - width_j = width_0 - for j in range(num_cols): - ray_power = 1.0 # use angles and constants instead - lobe_power_matrix[i,j] = ray_power - width_j += d_width - height_i += d_height + + # put lobe power constant in each each element + for column, row, horizontal_angle, vertical_angle in matrix_iterator( + width, height, num_cols, num_rows): + ray_power = 1.0 # should calculate using angles and constants + lobe_power_matrix[row,column] = ray_power return lobe_power_matrix -# currently retro is hardcoded to a uniform constant +# currently retro power is hardcoded to 1 def retro_power_matrix_constant(num_rows, num_cols): retro_matrix = np.ones((num_rows, num_cols), np.float32) return retro_matrix # calculate depth power matrix from depth values -def calculate_depth_power_matrix(depth_matrix, num_rows, num_cols): +def calculate_depth_power_matrix(depth_matrix, + width, height, num_cols, num_rows): depth_power_matrix = np.zeros((num_rows, num_cols), np.float32) - for i in range(num_rows): - for j in range(num_cols): - depth = depth_matrix[i,j] - depth_power = 1 # use equation - depth_power_matrix[i,j] = depth_power - return depth_power_matrix -# calculate horizontal and vertical angles from x,y,z,1 -def calculate_normals_matrix(normals_f4, num_rows, num_cols): - normals_f2 = np.zeros((num_rows, num_cols, 2), np.float32) - for i in range(num_rows): - for j in range(num_cols): - x,y,z = normals_f4[i,j,:3] - # apply functions to get height and width angles - normals_f2[i,j] = (0,0) - return normals_f2 + # put depth power in each each element + for column, row, horizontal_angle, vertical_angle in matrix_iterator( + width, height, num_cols, num_rows): + depth = depth_matrix[row,column] + depth_power = 1 # use equation + depth_power_matrix[row, column] = depth_power + return depth_power_matrix -# calculate normals matrix as CV_32FC1 from CV_32FC4 x,y,z,1 image normals -def calculate_normals_power_matrix(normals_f2, num_rows, num_cols): +def calculate_normals_power_matrix(normals_f4, + width, height, num_cols, num_rows): normals_power_matrix = np.zeros((num_rows, num_cols), np.float32) - for i in range(num_rows): - for j in range(num_cols): - normal_horiz, normal_vert = normals_f2[i,j] - # apply math on normal_horiz, normal_vert to get some power value - # for now hardcoded answer = 1 - normals_power_matrix[i,j] = 1.0 + # put normals angle tuple in each each element + for column, row, horizontal_angle, vertical_angle in matrix_iterator( + width, height, num_cols, num_rows): + + # calculate horizontal and vertical angles from x,y,z,1 + print("row: %d, col: %d"%(row, column)) + x,y,z,_unused = normals_f4[row, column] + horizontal_normal = 0.0 # replace with calculation that uses xyz + vertical_normal = 0.0 + horizontal_normal += horizontal_angle + vertical_normal += vertical_angle + + # apply math on vertical and horizontal normals to get normals power + normals_power_matrix[row, column] = 1.0 return normals_power_matrix -class SonarMaker: +class SonarCalculator: def __init__(self): - # user inputs warning: hardcoded in launch file, should match sdf + # user inputs from the launch file. They should match sdf. self.width, self.height, self.num_rows, self.num_cols, \ self.lobe_k1, self.lobe_k2 = depth_camera_args() - # depth and normals matrix from GPU - self.depth_matrix = None - self.normals_matrix = None - # matrices that hold constants - self.camera_angle_matrix = camera_angle_matrix_constant( - self.width, self.height, self.num_rows, self.num_cols) + # depth and normals power matrices from depth and normals from GPU + self.depth_power_matrix = None + self.normals_power_matrix = None + + # lobe power matrix self.lobe_power_matrix = lobe_power_matrix_constant( self.width, self.height, self.num_rows, self.num_cols, self.lobe_k1, self.lobe_k2) - # matrices that should be calculated but are not - self.retro_power_matrix = retro_power_matrix_constant( - self.num_rows, self.num_cols) + # retro power matrix should be calculated but retro power is hardcoded + self.retro_power_matrix = np.ones( + (self.num_rows, self.num_cols), np.float32) # ROS subscribers self.depth_sub = rospy.Subscriber( @@ -157,20 +144,7 @@ def __init__(self): # ROS publishers self.ray_pub = rospy.Publisher("image_sonar_rays_topic", Image) - def images_to_rays(self): - # inputs - depth_power_matrix = self.depth_power_matrix - normals_power_matrix = self.normals_power_matrix - lobe_power_matrix = self.lobe_power_matrix - retro_power_matrix = self.retro_power_matrix - - # rays - ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) - return ray_matrix - - # get depth matrix from Gazebo and cache it + # calculate depth power matrix from Gazebo depth_image and cache it def on_depth_image(self, depth_image): rospy.loginfo(rospy.get_caller_id() + "received depth_image") depth_matrix = bridge.imgmsg_to_cv2(depth_image) @@ -180,10 +154,11 @@ def on_depth_image(self, depth_image): print("Invalid depth image shape", self.depth_matrix.shape) return - self.depth_power_matrix = calculate_depth_power_matrix( - depth_matrix, self.num_rows, self.num_cols) + self.depth_power_matrix = calculate_depth_power_matrix(depth_matrix, + self.width, self.height, self.num_rows, self.num_cols) - # get normals matrix from Gazebo and calculate Sonar + # calculate normals power matrix from Gazebo normals_image + # then calculate ray power and beam power def on_normals_image(self, normals_image): rospy.loginfo(rospy.get_caller_id() + "received normals_image") normals_matrix_f4 = bridge.imgmsg_to_cv2(normals_image) @@ -193,17 +168,14 @@ def on_normals_image(self, normals_image): print("Invalid normals image shape", normals_matrix_f4.shape) return - normals_matrix = calculate_normals_matrix( - normals_matrix_f4, self.num_rows, self.num_cols) - compensated_normals_matrix = cv2.add(normals_matrix, - self.camera_angle_matrix) self.normals_power_matrix = calculate_normals_power_matrix( - compensated_normals_matrix, self.num_rows, self.num_cols) + normals_matrix_f4, + self.width, self.height, self.num_rows, self.num_cols) # use depth and normals matrices to make rays ray_matrix = self.images_to_rays() - # advertise ray_matrix to ROS, keep 32FC1 + # advertise ray_matrix to ROS, keep 32FC1 format self.ray_pub.publish(bridge.cv2_to_imgmsg(ray_matrix, "passthrough")) print("Ray power matrix", ray_matrix) @@ -211,8 +183,21 @@ def on_normals_image(self, normals_image): # make beam, TBD # publish beam, TBD + def images_to_rays(self): + # inputs + depth_power_matrix = self.depth_power_matrix + normals_power_matrix = self.normals_power_matrix + lobe_power_matrix = self.lobe_power_matrix + retro_power_matrix = self.retro_power_matrix + + # rays + ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) + return ray_matrix + if __name__ == '__main__': - sonar_maker = SonarMaker() + sonar_maker = SonarCalculator() rospy.init_node('depth_and_normals_to_sonar', anonymous=True) # spin until Ctrl-C From 2b79987232ac06fc0e962f476efa009e2292469f Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Tue, 28 Jul 2020 11:04:29 -0700 Subject: [PATCH 05/24] hardcode alternate value --- launch/depth_camera_sonar_single_beam_basic.launch | 2 +- urdf/depth_camera_sonar_single_beam.xacro | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch index 72cd994..9069ea6 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -18,7 +18,7 @@ + args="1.6 0.4 37 9 1.0 1.0"> diff --git a/urdf/depth_camera_sonar_single_beam.xacro b/urdf/depth_camera_sonar_single_beam.xacro index b049903..78f103b 100644 --- a/urdf/depth_camera_sonar_single_beam.xacro +++ b/urdf/depth_camera_sonar_single_beam.xacro @@ -67,8 +67,12 @@ ${beam_width} + 37 + 9 + R8G8B8 From 655165489abbd5859cca3585a30bfd91f1366c95 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Tue, 28 Jul 2020 18:12:30 -0700 Subject: [PATCH 06/24] add code --- ...epth_camera_sonar_single_beam_basic.launch | 2 +- scripts/depth_camera_sonar.py | 179 ++++++++++-------- urdf/depth_camera_sonar_single_beam.xacro | 12 +- ...depth_camera_sonar_single_beam_robot.xacro | 3 +- 4 files changed, 104 insertions(+), 92 deletions(-) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch index 9069ea6..e3b02b1 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -18,7 +18,7 @@ + args="1.6 37 9 1.0 1.0"> diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index 3f79b3c..3666f67 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -17,91 +17,92 @@ # ROS-CV bridge bridge = CvBridge() -# get angle width, height, num_rows, num_cols for one Sonar beam +# get angle beam width, vert_count, horiz_count for one Sonar beam def depth_camera_args(): # args parser = ArgumentParser(description="Start depth camera sonar ROS node") - parser.add_argument("width", type=float, help="Width angle") - parser.add_argument("height", type=float, help="Height angle") - parser.add_argument("num_cols", type=int, help="number of beams horiz") - parser.add_argument("num_rows", type=int, help="number of beams vert") + parser.add_argument("beam_width", type=float, help="Beam width angle") + parser.add_argument("horiz_count", type=int, help="number of rays horiz") + parser.add_argument("vert_count", type=int, help="number of rays vert") parser.add_argument("lobe_k1", type=float, help="Sonar lobe constant k1") parser.add_argument("lobe_k2", type=float, help="Sonar lobe constant k2") args = parser.parse_args() - return args.width, args.height, args.num_rows, args.num_cols, \ + return args.beam_width, args.horiz_count, args.vert_count, \ args.lobe_k1, args.lobe_k2 -# matrix iterator given horizontal and vertical angle and count -def matrix_iterator(width, height, num_cols, num_rows): - print("num_cols %d,num_rows %d, width: %f, height: %f"%(width, height, num_cols, num_rows)) - print(7/0) # zzzzzzzzzzzzzzzzzzzzzzzzzzzzzz +# ray matrix iterator given horizontal and vertical angle and count +# iterator returns: horiz_index, vert_index, horiz_angle, vert_angle +# sweeps rays horizontally from top to bottom +# returns horizontal index, vertical index, horizontal angle, vertical angle +def ray_matrix_iterator(beam_width, horiz_count, vert_count): + beam_height = beam_width * vert_count / horiz_count + print("beam_width: %f, beam_height: %f, horiz_count %d, vert_count %d"%( + beam_width, beam_height, horiz_count, vert_count)) # set up horizontal and vertical starting angles and angle steps - # returns column, row, horizontal angle, vertical angle - if num_cols < 2: - width_0 = 0 - d_width = 0 + if horiz_count < 2: + horiz_angle_0 = 0 + horiz_angle = 0 else: - width_0 = -width/2.0 - d_width = width / (num_cols - 1) + horiz_angle_0 = -beam_width/2.0 + horiz_angle = beam_width / (horiz_count - 1) - if num_rows < 2: + if vert_count < 2: height_0 = 0 - d_height = 0 + vert_angle = 0 else: - height_0 = -height/2.0 - d_height = height / (num_rows - 1) - -# perform sweep - height_i = height_0 - for i in range(num_rows): - width_j = width_0 - for j in range(num_cols): - yield j, i, width_j, height_i # yield - width_j += d_width - height_i += d_height + height_0 = -beam_height/2.0 + vert_angle = beam_height / (vert_count - 1) + + # perform sweep + vert_angle = height_0 + for vert_index in range(vert_count): + horiz_angle = horiz_angle_0 + for horiz_index in range(horiz_count): + yield horiz_index, vert_index, horiz_angle, vert_angle # yield + horiz_angle += horiz_angle + vert_angle += vert_angle # calculate lobe power matrix -def lobe_power_matrix_constant(width, height, num_cols, num_rows, k1, k2): +def lobe_power_matrix_constant(beam_width, horiz_count, vert_count, k1, k2): - lobe_power_matrix = np.zeros((num_rows, num_cols), np.float32) + lobe_power_matrix = np.zeros((vert_count, horiz_count), np.float32) # put lobe power constant in each each element - for column, row, horizontal_angle, vertical_angle in matrix_iterator( - width, height, num_cols, num_rows): + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): ray_power = 1.0 # should calculate using angles and constants lobe_power_matrix[row,column] = ray_power return lobe_power_matrix # currently retro power is hardcoded to 1 -def retro_power_matrix_constant(num_rows, num_cols): - retro_matrix = np.ones((num_rows, num_cols), np.float32) +def retro_power_matrix_constant(vert_count, horiz_count): + retro_matrix = np.ones((vert_count, horiz_count), np.float32) return retro_matrix # calculate depth power matrix from depth values def calculate_depth_power_matrix(depth_matrix, - width, height, num_cols, num_rows): - depth_power_matrix = np.zeros((num_rows, num_cols), np.float32) + beam_width, horiz_count, vert_count): + depth_power_matrix = np.zeros((vert_count, horiz_count), np.float32) # put depth power in each each element - for column, row, horizontal_angle, vertical_angle in matrix_iterator( - width, height, num_cols, num_rows): + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): depth = depth_matrix[row,column] depth_power = 1 # use equation depth_power_matrix[row, column] = depth_power return depth_power_matrix def calculate_normals_power_matrix(normals_f4, - width, height, num_cols, num_rows): - normals_power_matrix = np.zeros((num_rows, num_cols), np.float32) + beam_width, horiz_count, vert_count): + normals_power_matrix = np.zeros((vert_count, horiz_count), np.float32) # put normals angle tuple in each each element - for column, row, horizontal_angle, vertical_angle in matrix_iterator( - width, height, num_cols, num_rows): + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): # calculate horizontal and vertical angles from x,y,z,1 - print("row: %d, col: %d"%(row, column)) x,y,z,_unused = normals_f4[row, column] horizontal_normal = 0.0 # replace with calculation that uses xyz vertical_normal = 0.0 @@ -113,25 +114,36 @@ def calculate_normals_power_matrix(normals_f4, return normals_power_matrix -class SonarCalculator: +# calculate the ray power matrix from the power matrices +def powers_to_rays(depth_power_matrix, normals_power_matrix, + lobe_power_matrix, retro_power_matrix): + + # rays + ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) + ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) + return ray_matrix + +class SonarNode: def __init__(self): # user inputs from the launch file. They should match sdf. - self.width, self.height, self.num_rows, self.num_cols, \ + self.beam_width, self.horiz_count, self.vert_count, \ self.lobe_k1, self.lobe_k2 = depth_camera_args() # depth and normals power matrices from depth and normals from GPU + self.depth_matrix = None self.depth_power_matrix = None self.normals_power_matrix = None # lobe power matrix self.lobe_power_matrix = lobe_power_matrix_constant( - self.width, self.height, self.num_rows, self.num_cols, - self.lobe_k1, self.lobe_k2) + self.beam_width, self.horiz_count, self.vert_count, + self.lobe_k1, self.lobe_k2) # retro power matrix should be calculated but retro power is hardcoded self.retro_power_matrix = np.ones( - (self.num_rows, self.num_cols), np.float32) + (self.vert_count, self.horiz_count), np.float32) # ROS subscribers self.depth_sub = rospy.Subscriber( @@ -142,68 +154,71 @@ def __init__(self): Image, self.on_normals_image) # ROS publishers - self.ray_pub = rospy.Publisher("image_sonar_rays_topic", Image) + self.ray_pub = rospy.Publisher("image_sonar_rays_topic", Image, + queue_size=10) # calculate depth power matrix from Gazebo depth_image and cache it def on_depth_image(self, depth_image): - rospy.loginfo(rospy.get_caller_id() + "received depth_image") - depth_matrix = bridge.imgmsg_to_cv2(depth_image) - print("depth image shape: ", depth_matrix.shape) - if depth_matrix.shape != (self.num_rows, self.num_cols): + rospy.loginfo(rospy.get_caller_id() + " received depth_image") + self.depth_matrix = bridge.imgmsg_to_cv2(depth_image) + print("depth image shape: ", self.depth_matrix.shape) + if self.depth_matrix.shape != (self.vert_count, self.horiz_count): # bad print("Invalid depth image shape", self.depth_matrix.shape) return - self.depth_power_matrix = calculate_depth_power_matrix(depth_matrix, - self.width, self.height, self.num_rows, self.num_cols) + self.depth_power_matrix = calculate_depth_power_matrix( + self.depth_matrix, + self.beam_width, self.horiz_count, self.vert_count) # calculate normals power matrix from Gazebo normals_image # then calculate ray power and beam power def on_normals_image(self, normals_image): - rospy.loginfo(rospy.get_caller_id() + "received normals_image") + rospy.loginfo(rospy.get_caller_id() + " received normals_image") normals_matrix_f4 = bridge.imgmsg_to_cv2(normals_image) print("normals image shape: ", normals_matrix_f4.shape) - if normals_matrix_f4.shape != (self.num_rows, self.num_cols, 4): + if normals_matrix_f4.shape != (self.vert_count, self.horiz_count, 4): # bad print("Invalid normals image shape", normals_matrix_f4.shape) return self.normals_power_matrix = calculate_normals_power_matrix( - normals_matrix_f4, - self.width, self.height, self.num_rows, self.num_cols) + normals_matrix_f4, + self.beam_width, self.horiz_count, self.vert_count) - # use depth and normals matrices to make rays - ray_matrix = self.images_to_rays() + # generate all the outputs + self.generate_outputs() - # advertise ray_matrix to ROS, keep 32FC1 format - self.ray_pub.publish(bridge.cv2_to_imgmsg(ray_matrix, "passthrough")) - print("Ray power matrix", ray_matrix) + # generate all the outputs + def generate_outputs(self): - # use rays to make one beam - # make beam, TBD - # publish beam, TBD - - def images_to_rays(self): - # inputs - depth_power_matrix = self.depth_power_matrix - normals_power_matrix = self.normals_power_matrix - lobe_power_matrix = self.lobe_power_matrix - retro_power_matrix = self.retro_power_matrix + # use depth and normals matrices to make rays + ray_matrix = powers_to_rays(self.depth_power_matrix, + self.normals_power_matrix, + self.lobe_power_matrix, + self.retro_power_matrix) - # rays - ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) - return ray_matrix + # advertise ray_matrix to ROS, keep ray_matrix's 32FC1 format + self.ray_pub.publish(bridge.cv2_to_imgmsg(ray_matrix, "passthrough")) +# print("Ray power matrix", ray_matrix) + +# # create and publish the ray point cloud +# ray_cloud = ray_point_cloud(ray_matrix, +# self.beam_width, self.horiz_count, self.vert_count) +# self.ray_cloud_pub.publish(ray_cloud) +# +# # create and publish the beam point cloud +# beam_cloud = beam_point_cloud(ray_matrix, +# self.beam_width, self.horiz_count, self.vert_count) +# self.beam_cloud_pub.publish(ray_cloud) if __name__ == '__main__': - sonar_maker = SonarCalculator() rospy.init_node('depth_and_normals_to_sonar', anonymous=True) + ros_sonar_node = SonarNode() # spin until Ctrl-C try: rospy.spin() except KeyboardInterrupt: print("Shutting down") -# cv2.destroyAllWindows() diff --git a/urdf/depth_camera_sonar_single_beam.xacro b/urdf/depth_camera_sonar_single_beam.xacro index 78f103b..aad5780 100644 --- a/urdf/depth_camera_sonar_single_beam.xacro +++ b/urdf/depth_camera_sonar_single_beam.xacro @@ -1,8 +1,8 @@ - - + + @@ -67,12 +67,8 @@ ${beam_width} - 37 - 9 - + ${horiz_count} + ${vert_count} R8G8B8 diff --git a/urdf/depth_camera_sonar_single_beam_robot.xacro b/urdf/depth_camera_sonar_single_beam_robot.xacro index e1b2335..1245d31 100644 --- a/urdf/depth_camera_sonar_single_beam_robot.xacro +++ b/urdf/depth_camera_sonar_single_beam_robot.xacro @@ -25,7 +25,8 @@ parent_link="world" topic="depth_camera_sonar_single_beam" beam_width="1.6" - samples="3" + horiz_count="37" + vert_count="9" update_rate="15" min_range="0.1" max_range="7.0"> From 7fdb80405b4ead8d666b702c3a3b833b67d1fd67 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 29 Jul 2020 11:59:40 -0700 Subject: [PATCH 07/24] add code --- ...epth_camera_sonar_single_beam_basic.launch | 9 ++----- scripts/depth_camera_sonar.py | 24 ++++++++++++------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_single_beam_basic.launch index e3b02b1..17fc0ec 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_single_beam_basic.launch @@ -15,7 +15,7 @@ respawn="false" output="screen" args="-urdf -model nps_depth_camera_sonar_single_beam_model -param depth_camera_sonar_single_beam_robot" /> - + @@ -26,14 +26,9 @@ type="rqt_image_view" args="/depth_camera_sonar_single_beam_sensor_camera/image_depth"> - - - - + type="rqt_image_view" args="/sonar_ray_image"> diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index 3666f67..654319c 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -14,8 +14,11 @@ # Notes: # Arguments passed here from the launch file must match arguments in SDF. -# ROS-CV bridge -bridge = CvBridge() +# publisher name constants, should be replaced with sdf inputs +RAY_IMAGE_TOPIC = "sonar_ray_image" +RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" +BEAM_IMAGE_TOPIC = "sonar_beam_image" +BEAM_POINT_CLOUD_TOPIC = "sonar_beam_point_cloud" # get angle beam width, vert_count, horiz_count for one Sonar beam def depth_camera_args(): @@ -26,6 +29,8 @@ def depth_camera_args(): parser.add_argument("vert_count", type=int, help="number of rays vert") parser.add_argument("lobe_k1", type=float, help="Sonar lobe constant k1") parser.add_argument("lobe_k2", type=float, help="Sonar lobe constant k2") + parser.add_argument("__name", type=str, help="launch adds name, unused") + parser.add_argument("__log", type=str, help="launch adds log, unused") args = parser.parse_args() return args.beam_width, args.horiz_count, args.vert_count, \ args.lobe_k1, args.lobe_k2 @@ -90,7 +95,7 @@ def calculate_depth_power_matrix(depth_matrix, for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( beam_width, horiz_count, vert_count): depth = depth_matrix[row,column] - depth_power = 1 # use equation + depth_power = depth # use equation instead depth_power_matrix[row, column] = depth_power return depth_power_matrix @@ -154,13 +159,15 @@ def __init__(self): Image, self.on_normals_image) # ROS publishers - self.ray_pub = rospy.Publisher("image_sonar_rays_topic", Image, - queue_size=10) + self.ray_pub = rospy.Publisher(RAY_IMAGE_TOPIC, Image, queue_size=10) + + # ROS-CV bridge + self.bridge = CvBridge() # calculate depth power matrix from Gazebo depth_image and cache it def on_depth_image(self, depth_image): rospy.loginfo(rospy.get_caller_id() + " received depth_image") - self.depth_matrix = bridge.imgmsg_to_cv2(depth_image) + self.depth_matrix = self.bridge.imgmsg_to_cv2(depth_image) print("depth image shape: ", self.depth_matrix.shape) if self.depth_matrix.shape != (self.vert_count, self.horiz_count): # bad @@ -175,7 +182,7 @@ def on_depth_image(self, depth_image): # then calculate ray power and beam power def on_normals_image(self, normals_image): rospy.loginfo(rospy.get_caller_id() + " received normals_image") - normals_matrix_f4 = bridge.imgmsg_to_cv2(normals_image) + normals_matrix_f4 = self.bridge.imgmsg_to_cv2(normals_image) print("normals image shape: ", normals_matrix_f4.shape) if normals_matrix_f4.shape != (self.vert_count, self.horiz_count, 4): # bad @@ -199,7 +206,8 @@ def generate_outputs(self): self.retro_power_matrix) # advertise ray_matrix to ROS, keep ray_matrix's 32FC1 format - self.ray_pub.publish(bridge.cv2_to_imgmsg(ray_matrix, "passthrough")) + self.ray_pub.publish(self.bridge.cv2_to_imgmsg(ray_matrix, + "passthrough")) # print("Ray power matrix", ray_matrix) # # create and publish the ray point cloud From 819ed39a81fdb368b96f9e246557903e35ebeb11 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 29 Jul 2020 12:00:20 -0700 Subject: [PATCH 08/24] remove Sonar parts, ROS Python node does this now --- ...ebo_ros_depth_camera_sonar_single_beam.cpp | 223 ++++-------------- ...azebo_ros_depth_camera_sonar_single_beam.h | 13 - 2 files changed, 43 insertions(+), 193 deletions(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index b56e66e..5e6dd23 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -61,7 +61,7 @@ GazeboRosDepthCamera::~GazeboRosDepthCamera() // Load the controller void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { - std::cout << "nps_gazebo_ros_depth_camera_sonar_single_beam zzzzzzzzzzzzzzzz"; + std::cout << "nps_gazebo_ros_depth_camera_sonar_single_beam"; DepthCameraPlugin::Load(_parent, _sdf); // Make sure the ROS node for Gazebo has already been initialized @@ -92,12 +92,6 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); - // Sonar point cloud stuff - if (!_sdf->HasElement("sonarPointCloudTopicName")) - this->sonar_point_cloud_topic_name_ = "sonar_points"; - else - this->sonar_point_cloud_topic_name_ = _sdf->GetElement("sonarPointCloudTopicName")->Get(); - // depth image and normals stuff if (!_sdf->HasElement("depthImageTopicName")) this->depth_image_topic_name_ = "depth/image_raw"; @@ -109,11 +103,6 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->normals_image_topic_name_ = _sdf->GetElement("normalsImageTopicName")->Get(); - if (!_sdf->HasElement("rayImageTopicName")) - this->ray_image_topic_name_ = "ray/image_raw"; - else - this->ray_image_topic_name_ = _sdf->GetElement("rayImageTopicName")->Get(); - if (!_sdf->HasElement("depthImageCameraInfoTopicName")) this->depth_image_camera_info_topic_name_ = "depth/camera_info"; else @@ -138,14 +127,6 @@ void GazeboRosDepthCamera::Advertise() ros::VoidPtr(), &this->camera_queue_); this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); - ros::AdvertiseOptions sonar_point_cloud_ao = - ros::AdvertiseOptions::create( - this->sonar_point_cloud_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::SonarPointCloudConnect,this), - boost::bind( &GazeboRosDepthCamera::SonarPointCloudDisconnect,this), - ros::VoidPtr(), &this->camera_queue_); - this->sonar_point_cloud_pub_ = this->rosnode_->advertise(sonar_point_cloud_ao); - ros::AdvertiseOptions depth_image_ao = ros::AdvertiseOptions::create< sensor_msgs::Image >( this->depth_image_topic_name_,1, @@ -162,14 +143,6 @@ void GazeboRosDepthCamera::Advertise() ros::VoidPtr(), &this->camera_queue_); this->normals_image_pub_ = this->rosnode_->advertise(normals_image_ao); - ros::AdvertiseOptions ray_image_ao = - ros::AdvertiseOptions::create< sensor_msgs::Image >( - this->ray_image_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::RayImageConnect,this), - boost::bind( &GazeboRosDepthCamera::RayImageDisconnect,this), - ros::VoidPtr(), &this->camera_queue_); - this->ray_image_pub_ = this->rosnode_->advertise(ray_image_ao); - ros::AdvertiseOptions depth_image_camera_info_ao = ros::AdvertiseOptions::create( this->depth_image_camera_info_topic_name_,1, @@ -198,38 +171,6 @@ void GazeboRosDepthCamera::PointCloudDisconnect() this->parentSensor->SetActive(false); } -//////////////////////////////////////////////////////////////////////////////// -// Increment count -void GazeboRosDepthCamera::SonarPointCloudConnect() -{ -std::cout << "SonarPointCloudConnect\n"; - this->sonar_point_cloud_connect_count_++; - this->parentSensor->SetActive(true); -} -//////////////////////////////////////////////////////////////////////////////// -// Decrement count -void GazeboRosDepthCamera::SonarPointCloudDisconnect() -{ - // zz never turns off parentSensor - this->sonar_point_cloud_connect_count_--; -} - -//////////////////////////////////////////////////////////////////////////////// -// Increment count -void GazeboRosDepthCamera::RayImageConnect() -{ -std::cout << "DepthImageConnect\n"; - this->ray_image_connect_count_++; - this->parentSensor->SetActive(true); -} -//////////////////////////////////////////////////////////////////////////////// -// Decrement count -void GazeboRosDepthCamera::RayImageDisconnect() -{ -std::cout << "DepthImageDisconnect\n"; - this->ray_image_connect_count_--; -} - //////////////////////////////////////////////////////////////////////////////// // Increment count void GazeboRosDepthCamera::DepthImageConnect() @@ -296,8 +237,6 @@ std::cout << "OnNewDepthFrame\n"; if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 && this->normals_image_connect_count_ <= 0 && - this->ray_image_connect_count_ <= 0 && - this->sonar_point_cloud_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) { this->parentSensor->SetActive(false); @@ -473,6 +412,44 @@ void GazeboRosDepthCamera::FillDepthImage(const float *_src) this->lock_.unlock(); } +// Fill depth information +bool GazeboRosDepthCamera::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.is_bigendian = 0; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + + // convert depth to image +std::cout << "FillDepthImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + if (depth > this->point_cloud_cutoff_) + { + dest[i + j * cols_arg] = depth; + } + else //point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + } + return true; +} // Fill depth information bool GazeboRosDepthCamera::FillPointCloudHelper( @@ -582,8 +559,6 @@ std::cout << "OnNewNormalsFrame\n"; if (this->point_cloud_connect_count_ <= 0 && this->depth_image_connect_count_ <= 0 && this->normals_image_connect_count_ <= 0 && - this->ray_image_connect_count_ <= 0 && - this->sonar_point_cloud_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) { this->parentSensor->SetActive(false); @@ -596,8 +571,8 @@ std::cout << "OnNewNormalsFrame\n"; if (this->normals_image_connect_count_ > 0) this->FillNormalsImage(_normals_image); - // lets perform Sonar calculations here - this->FillRayImage(); +//zz // lets perform Sonar calculations here + //zz this->FillRayImage(); } } else @@ -641,8 +616,8 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC4; image_msg.height = rows_arg; image_msg.width = cols_arg; - image_msg.step = sizeof(float) * cols_arg; - image_msg.data.resize(rows_arg * cols_arg * sizeof(float)*4); + image_msg.step = 4 * sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float) * 4); image_msg.is_bigendian = 0; const float bad_point = std::numeric_limits::quiet_NaN(); @@ -658,8 +633,6 @@ std::cout << "FillNormalsImageHelper rows: " << rows_arg << " cols: " << cols_ar for (uint32_t i = 0; i < cols_arg*4; i++) { float normals = toCopyFrom[index++]; - std::cout << normals << " "; - // if (normals > this->point_cloud_cutoff_) // { dest[i + j * cols_arg] = normals; @@ -669,124 +642,14 @@ std::cout << "FillNormalsImageHelper rows: " << rows_arg << " cols: " << cols_ar // dest[i + j * cols_arg] = bad_point; // } } - std::cout << "\n"; } return true; } - - - // ************************************************************************ // end normals // ************************************************************************ -// ************************************************************************ -// Sonar -// ************************************************************************ - -// void FillGaussianImage(); - -void GazeboRosDepthCamera::FillRayImage() -{ - if (this->depth_image_msg_.width == 0) { - std::cout << "FillRayImage depth_image_msg is empty\n"; - return; - } - if (this->normals_image_msg_.width == 0) { - std::cout << "FillRayImage normals_image_msg is empty\n"; - return; - } - - this->lock_.lock(); - // copy data into image - this->ray_image_msg_.header.frame_id = this->frame_name_; - // get metadata from depth data - this->ray_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; - this->ray_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; - this->ray_image_msg_.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - float rows_arg = this->depth_image_msg_.height; - float cols_arg = this->depth_image_msg_.width; -std::cout << "FillRayImage rows: " << rows_arg << " cols: " << cols_arg << "\n"; - this->ray_image_msg_.height = rows_arg; - this->ray_image_msg_.width = cols_arg; - this->ray_image_msg_.step = sizeof(float) * cols_arg; - this->ray_image_msg_.data.resize(rows_arg * cols_arg * sizeof(float)); - this->ray_image_msg_.is_bigendian = 0; - - float* from_depth = (float*)(&(this->depth_image_msg_.data[0])); - float* from_normals = (float*)(&(this->normals_image_msg_.data[0])); -// float* from_beam_kernel = (float*)(&(this->beam_kernel_msg_.data[0])); - float* to_ray = (float*)(&(this->ray_image_msg_.data[0])); - - // calculate intensity for each ray - for (uint32_t j = 0; j < rows_arg; j++) - { - for (uint32_t i = 0; i < cols_arg; i++) - { - int index = i + j * cols_arg; - float depth = from_depth[index]; - float normal = from_normals[index]; - - // Sonar equation for point (col, row) - float calculated_intensity = depth + normal; - - to_ray[index] = calculated_intensity; - } - } - - this->ray_image_pub_.publish(this->ray_image_msg_); - - this->lock_.unlock(); -} - -// Fill depth information -bool GazeboRosDepthCamera::FillDepthImageHelper( - sensor_msgs::Image& image_msg, - uint32_t rows_arg, uint32_t cols_arg, - uint32_t step_arg, void* data_arg) -{ - image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - image_msg.height = rows_arg; - image_msg.width = cols_arg; - image_msg.step = sizeof(float) * cols_arg; - image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); - image_msg.is_bigendian = 0; - - const float bad_point = std::numeric_limits::quiet_NaN(); - - float* dest = (float*)(&(image_msg.data[0])); - float* toCopyFrom = (float*)data_arg; - int index = 0; - - // convert depth to image -std::cout << "FillDepthImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; - for (uint32_t j = 0; j < rows_arg; j++) - { - for (uint32_t i = 0; i < cols_arg; i++) - { - float depth = toCopyFrom[index++]; - std::cout << depth << " "; - - if (depth > this->point_cloud_cutoff_) - { - dest[i + j * cols_arg] = depth; - } - else //point in the unseeable range - { - dest[i + j * cols_arg] = bad_point; - } - } - std::cout << "\n"; - } - return true; -} - - -// ************************************************************************ -// end Sonar -// ************************************************************************ - void GazeboRosDepthCamera::PublishCameraInfo() { diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h index 4fed114..1b569c6 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h @@ -119,7 +119,6 @@ namespace gazebo private: int normals_image_connect_count_; private: void NormalsImageConnect(); private: void NormalsImageDisconnect(); - private: int ray_image_connect_count_; private: void RayImageConnect(); private: void RayImageDisconnect(); private: common::Time last_depth_image_camera_info_update_time_; @@ -136,34 +135,23 @@ namespace gazebo uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg); - private: void FillRayImage(); - private: void FillSonarPointCloud(); - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. private: ros::Publisher point_cloud_pub_; - private: ros::Publisher sonar_point_cloud_pub_; private: ros::Publisher depth_image_pub_; private: ros::Publisher normals_image_pub_; - private: ros::Publisher ray_image_pub_; /// \brief PointCloud2 point cloud message private: sensor_msgs::PointCloud2 point_cloud_msg_; - private: sensor_msgs::PointCloud2 sonar_point_cloud_msg_; private: sensor_msgs::Image depth_image_msg_; private: sensor_msgs::Image normals_image_msg_; - private: sensor_msgs::Image ray_image_msg_; - private: double point_cloud_cutoff_; /// \brief ROS image topic name private: std::string point_cloud_topic_name_; - /// \brief ROS image Sonar topic name - private: std::string sonar_point_cloud_topic_name_; - private: void InfoConnect(); private: void InfoDisconnect(); @@ -173,7 +161,6 @@ namespace gazebo /// \brief image where each pixel contains the depth information private: std::string depth_image_topic_name_; private: std::string normals_image_topic_name_; - private: std::string ray_image_topic_name_; private: std::string depth_image_camera_info_topic_name_; private: int depth_info_connect_count_; private: void DepthInfoConnect(); From 06dc594fff52d08002819f8c5e3fbf43e66cc175 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 29 Jul 2020 15:21:23 -0700 Subject: [PATCH 09/24] rename files --- ...launch => depth_camera_sonar_basic.launch} | 14 +++---- scripts/depth_camera_sonar.py | 14 ++++--- ...le_beam.xacro => depth_camera_sonar.xacro} | 16 ++++---- urdf/depth_camera_sonar_robot.xacro | 27 +++++++++++++ ...depth_camera_sonar_single_beam_robot.xacro | 40 ------------------- ...c.world => depth_camera_sonar_basic.world} | 2 - 6 files changed, 50 insertions(+), 63 deletions(-) rename launch/{depth_camera_sonar_single_beam_basic.launch => depth_camera_sonar_basic.launch} (66%) rename urdf/{depth_camera_sonar_single_beam.xacro => depth_camera_sonar.xacro} (75%) create mode 100644 urdf/depth_camera_sonar_robot.xacro delete mode 100644 urdf/depth_camera_sonar_single_beam_robot.xacro rename worlds/{sonar_single_beam_basic.world => depth_camera_sonar_basic.world} (98%) diff --git a/launch/depth_camera_sonar_single_beam_basic.launch b/launch/depth_camera_sonar_basic.launch similarity index 66% rename from launch/depth_camera_sonar_single_beam_basic.launch rename to launch/depth_camera_sonar_basic.launch index 17fc0ec..51446b7 100644 --- a/launch/depth_camera_sonar_single_beam_basic.launch +++ b/launch/depth_camera_sonar_basic.launch @@ -2,18 +2,18 @@ - + - - + + - + + args="-urdf -model nps_depth_camera_sonar_model -param depth_camera_sonar_robot" /> + type="rqt_image_view" args="/depth_camera_sonar_sensor_camera/image_depth"> diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index 654319c..5533c2d 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -14,6 +14,10 @@ # Notes: # Arguments passed here from the launch file must match arguments in SDF. +# subscriber name constants, should be replaced with sdf inputs +IMAGE_DEPTH_TOPIC = "depth_camera_sonar_sensor_camera/image_depth" +IMAGE_NORMALS_TOPIC = "depth_camera_sonar_sensor_camera/image_normals" + # publisher name constants, should be replaced with sdf inputs RAY_IMAGE_TOPIC = "sonar_ray_image" RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" @@ -151,12 +155,10 @@ def __init__(self): (self.vert_count, self.horiz_count), np.float32) # ROS subscribers - self.depth_sub = rospy.Subscriber( - "depth_camera_sonar_single_beam_sensor_camera/image_depth", - Image, self.on_depth_image) - self.normals_sub = rospy.Subscriber( - "depth_camera_sonar_single_beam_sensor_camera/image_normals", - Image, self.on_normals_image) + self.depth_sub = rospy.Subscriber(IMAGE_DEPTH_TOPIC, + Image, self.on_depth_image) + self.normals_sub = rospy.Subscriber(IMAGE_NORMALS_TOPIC, + Image, self.on_normals_image) # ROS publishers self.ray_pub = rospy.Publisher(RAY_IMAGE_TOPIC, Image, queue_size=10) diff --git a/urdf/depth_camera_sonar_single_beam.xacro b/urdf/depth_camera_sonar.xacro similarity index 75% rename from urdf/depth_camera_sonar_single_beam.xacro rename to urdf/depth_camera_sonar.xacro index aad5780..f98400e 100644 --- a/urdf/depth_camera_sonar_single_beam.xacro +++ b/urdf/depth_camera_sonar.xacro @@ -2,16 +2,16 @@ - + - + - + - + @@ -31,19 +31,19 @@ - + false - + true ${update_rate} true ${topic} 0 0 0 0 0 0 - + - depth_camera_sonar_single_beam_sensor_camera + depth_camera_sonar_sensor_camera image_raw image_raw/camera_info diff --git a/urdf/depth_camera_sonar_robot.xacro b/urdf/depth_camera_sonar_robot.xacro new file mode 100644 index 0000000..4bda28e --- /dev/null +++ b/urdf/depth_camera_sonar_robot.xacro @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + diff --git a/urdf/depth_camera_sonar_single_beam_robot.xacro b/urdf/depth_camera_sonar_single_beam_robot.xacro deleted file mode 100644 index 1245d31..0000000 --- a/urdf/depth_camera_sonar_single_beam_robot.xacro +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/worlds/sonar_single_beam_basic.world b/worlds/depth_camera_sonar_basic.world similarity index 98% rename from worlds/sonar_single_beam_basic.world rename to worlds/depth_camera_sonar_basic.world index 9e62857..3c729b6 100644 --- a/worlds/sonar_single_beam_basic.world +++ b/worlds/depth_camera_sonar_basic.world @@ -22,13 +22,11 @@ - From cda1de3a994742666f48bb9a1fc070da4abd1e10 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 29 Jul 2020 17:37:58 -0700 Subject: [PATCH 10/24] rename matrix --- scripts/depth_camera_sonar.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index 5533c2d..3f276dd 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -128,10 +128,10 @@ def powers_to_rays(depth_power_matrix, normals_power_matrix, lobe_power_matrix, retro_power_matrix): # rays - ray_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, lobe_power_matrix) - ray_matrix = cv2.multiply(ray_matrix, retro_power_matrix) - return ray_matrix + ray_power_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) + ray_power_matrix = cv2.multiply(ray_power_matrix, lobe_power_matrix) + ray_power_matrix = cv2.multiply(ray_power_matrix, retro_power_matrix) + return ray_power_matrix class SonarNode: def __init__(self): @@ -202,23 +202,23 @@ def on_normals_image(self, normals_image): def generate_outputs(self): # use depth and normals matrices to make rays - ray_matrix = powers_to_rays(self.depth_power_matrix, + ray_power_matrix = powers_to_rays(self.depth_power_matrix, self.normals_power_matrix, self.lobe_power_matrix, self.retro_power_matrix) - # advertise ray_matrix to ROS, keep ray_matrix's 32FC1 format - self.ray_pub.publish(self.bridge.cv2_to_imgmsg(ray_matrix, + # advertise ray_power_matrix to ROS, inheriting the 32FC1 format + self.ray_pub.publish(self.bridge.cv2_to_imgmsg(ray_power_matrix, "passthrough")) -# print("Ray power matrix", ray_matrix) +# print("Ray power matrix", ray_power_matrix) # # create and publish the ray point cloud -# ray_cloud = ray_point_cloud(ray_matrix, +# ray_cloud = ray_point_cloud(ray_power_matrix, # self.beam_width, self.horiz_count, self.vert_count) # self.ray_cloud_pub.publish(ray_cloud) # # # create and publish the beam point cloud -# beam_cloud = beam_point_cloud(ray_matrix, +# beam_cloud = beam_point_cloud(ray_power_matrix, # self.beam_width, self.horiz_count, self.vert_count) # self.beam_cloud_pub.publish(ray_cloud) From facb83ecbfbd15ffdb1ed5543621bfcd8d41059d Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Thu, 6 Aug 2020 14:25:37 -0700 Subject: [PATCH 11/24] fix comment --- scripts/depth_camera_sonar.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py index 3f276dd..261256b 100755 --- a/scripts/depth_camera_sonar.py +++ b/scripts/depth_camera_sonar.py @@ -39,10 +39,10 @@ def depth_camera_args(): return args.beam_width, args.horiz_count, args.vert_count, \ args.lobe_k1, args.lobe_k2 -# ray matrix iterator given horizontal and vertical angle and count -# iterator returns: horiz_index, vert_index, horiz_angle, vert_angle -# sweeps rays horizontally from top to bottom -# returns horizontal index, vertical index, horizontal angle, vertical angle +# Given a horizontal beam width and the horizontal and vertical ray count, +# calculates the vertical beam width then iteratively sweeps through ray points +# from top to bottom to return the following index and angular information +# about each ray: horiz_index, vert_index, horiz_angle, vert_angle. def ray_matrix_iterator(beam_width, horiz_count, vert_count): beam_height = beam_width * vert_count / horiz_count print("beam_width: %f, beam_height: %f, horiz_count %d, vert_count %d"%( From d91a6e62b07783652e68fe28f515fed44a3f14d4 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Fri, 28 Aug 2020 14:36:18 -0700 Subject: [PATCH 12/24] add Sonar equations --- .gitignore | 2 + launch/depth_camera_sonar_basic.launch | 5 + scripts/sonar_equations.py | 136 +++++++++++++++++++++++++ scripts/sonar_ros.py | 71 +++++++++++++ urdf/depth_camera_sonar_robot.xacro | 4 +- 5 files changed, 216 insertions(+), 2 deletions(-) create mode 100755 scripts/sonar_equations.py create mode 100755 scripts/sonar_ros.py diff --git a/.gitignore b/.gitignore index e7ea087..069f1d1 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,5 @@ # VIM *.swp +# Python +*.pyc diff --git a/launch/depth_camera_sonar_basic.launch b/launch/depth_camera_sonar_basic.launch index 51446b7..4a3d2c0 100644 --- a/launch/depth_camera_sonar_basic.launch +++ b/launch/depth_camera_sonar_basic.launch @@ -16,10 +16,15 @@ args="-urdf -model nps_depth_camera_sonar_model -param depth_camera_sonar_robot" /> + + + From eddbab525b25cc99b0f9802e9a3ecaa6a1c10183 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 2 Sep 2020 17:25:41 -0700 Subject: [PATCH 13/24] add incidence angle --- scripts/sonar_equations.py | 157 ++++++++++++++++++++++++++++++------- 1 file changed, 129 insertions(+), 28 deletions(-) diff --git a/scripts/sonar_equations.py b/scripts/sonar_equations.py index b13d5f0..cbe1c43 100755 --- a/scripts/sonar_equations.py +++ b/scripts/sonar_equations.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python +# We use Python 2 instead of python3 bacause ROS uses Python 2. # ref. https://numpy.org/doc/1.18/numpy-user.pdf # Single Beam Sonar @@ -6,12 +7,58 @@ # Contributors: Andreina Rascon, Derek Olson, Woeng-Sug Choi from random import random -from math import sqrt, sin, cos, pi, log +from math import sqrt, sin, cos, pi, log, acos import numpy as np import matplotlib.pyplot as plt +import rospy + +# diagnostics +def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2): + + # Plots + plt.figure(figsize=(14,10), dpi=80) + plt.suptitle("%d beam(s), %d elevation rays, %d azimuth rays " + "%d frequencies, %d buckets"%(nBeams, + ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets)) + + # inverse fast fourier transform + # figure (1) + plt.subplot(2,2,1) + plt.title("Power based on echo time") + plt.grid(True) + plt.plot(time1f, P_beam_tf2[0,:], linewidth=0.5) + plt.xlabel('Time, [s]') + plt.ylabel('Pressure, [Pa]') + + # Sound Pressure Level of Echo Level + # figure (2) + SPLf1 = 20 * np.log(np.abs(P_beam_tf2[0,:])) # sound pressure level, [dB] + plt.subplot(2,2,2) + plt.title("Sound pressure level based on echo time") + plt.grid(True) + plt.plot(time1f, SPLf1, linewidth=0.5) + plt.xlabel('Time, [s]') + plt.ylabel('Sound Pressure Level, [Pa]') + + # image for each nFreq + plt.subplot(2,2,3) + plt.title("Heatplot sound pressure level (SPL) based on frequency number") + plt.xlabel('Beam number') + plt.ylabel('Inverse FFT frequency number') + plt.imshow(P_beam_tf2.T, aspect="auto") + + # image for each nBuckets + plt.subplot(2,2,4) + plt.title("Bucketed heatplot SPL based on bucketed frequency number") + plt.xlabel('Beam number') + plt.ylabel('Inverse FFT frequency bucket number') + plt.imshow(P_bucket_tf2.T, aspect="auto") + + plt.show() # unnormalized sinc function -def unnormalized_sinc(t): +def _unnormalized_sinc(t): try: return sin(t)/t except ZeroDivisionError: @@ -32,8 +79,40 @@ def unnormalized_sinc(t): fmin = sonarFreq - bandwidth/2*4 # Calculated requency spectrum fmax = sonarFreq + bandwidth/2*4 # Calculated requency spectrum - -def process_rays(distances, _normals): +def _textf3(text, f): + return "%s: %f, %f, %f"%(text,f[0],f[1],f[2]) + +# incidence angle is target's normal angle compensated by ray's +# azimuth and elevation +def _ray_incidence(azimuth, elevation, normalf4): + # ray normal from camera azimuth and elevation + camera_x = cos(-azimuth)*cos(elevation) + camera_y = sin(-azimuth)*cos(elevation) + camera_z = sin(elevation) + ray_normal = np.array([camera_x, camera_y, camera_z]) + print("ray_normal", ray_normal) + + # target normal with axes compensated to camera axes zz verify + target_normal = np.array([normalf4[2], -normalf4[0], -normalf4[1]]) + + print("target_normal", target_normal) + + # dot product +#zz this fails because the dot product gets out of range. +#zz return pi - acos(ray_normal.dot(target_normal)) + + dot_product = ray_normal.dot(target_normal) + print("dot product: "%dot_product) + print(dot_product) + if dot_product < -1.0 or dot_product > 1.0: + text="ray_normal %f, %f, %f"%(ray_normal[0], ray_normal[1], ray_normal[2]) + rospy.logwarn(_textf3("ray_normal", ray_normal)) + rospy.logwarn(_textf3("target_normal", target_normal)) + rospy.logwarn("dot product = %f"%dot_product) + dot_product=-1.0 + return pi - acos(dot_product) + +def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): # Sonar sensor properties nBeams = 1 @@ -46,33 +125,36 @@ def process_rays(distances, _normals): nBuckets = 300 - if distances.shape != (ray_nElevationRays, ray_nAzimuthRays): - print("bad distances shape ", distances.shape) + if ray_distancesf2.shape != (ray_nElevationRays, ray_nAzimuthRays): + print("bad distances shape ", ray_distancesf2.shape) return np.zeros(nBeams,nBuckets) # calculated Sonar sensor properties ray_elevationAnglesf1 = beam_elevationAngle + np.linspace( - -beam_elevationAngleWidth / 2, beam_elevationAngleWidth / 2, - ray_nElevationRays) + -beam_elevationAngleWidth / 2, beam_elevationAngleWidth / 2, + ray_nElevationRays) ray_azimuthAnglesf1 = beam_azimuthAngle + np.linspace( - -beam_azimuthAngleWidth / 2, beam_azimuthAngleWidth / 2, - ray_nAzimuthRays) + -beam_azimuthAngleWidth / 2, beam_azimuthAngleWidth / 2, + ray_nAzimuthRays) ray_elevationAngleWidth = beam_elevationAngleWidth/(ray_nElevationRays - 1) ray_azimuthAngleWidth = beam_azimuthAngleWidth/(ray_nAzimuthRays - 1) - # Sonar inputs - ray_distancef2 = np.array([[15,5,10], [2,100,10], [15,15,15], [4,2,3]]) -# ray_alphaf2 = np.array([[0,0,0], [0,0,0], [0,0,0], [0,0,0]]) # zz TBD -# ray_distancef2 = distances - ray_alphaf2 = np.zeros((ray_nElevationRays, ray_nAzimuthRays)) # TBD + # azimuth angles for nAzimuthRays * nBeams + full_sweep_azimuthAnglesf1 = beam_azimuthAngle + np.linspace( + nBeams * -beam_azimuthAngleWidth / 2, + nBeams * beam_azimuthAngleWidth / 2, + nBeams * ray_nAzimuthRays) # calculated sampling periods - max_T = np.amax(ray_distancef2)*2/soundSpeed + max_T = np.amax(ray_distancesf2)*2/soundSpeed _delta_f = 1/max_T + # _delta_t = 1/(fmax - fmin) nFreq = int(round((fmax - fmin) / _delta_f)) + + # reduce nFreq because calculated nFreq is too large for looping + nFreq=10000 _freq1f = np.linspace(fmin,fmax,nFreq) - time1f = np.linspace(0,max_T,nFreq) # for diagnostics plot # calculated physics _absorption = 0.0354 # [dB/m] @@ -90,20 +172,25 @@ def process_rays(distances, _normals): elevationBeamPattern2f = np.zeros((ray_nElevationRays,ray_nAzimuthRays)) for k in range(ray_nElevationRays): for i in range(ray_nAzimuthRays): - azimuthBeamPattern2f[k,i] = (abs(unnormalized_sinc(pi * 0.884 - / ray_azimuthAngleWidth * sin(ray_azimuthAnglesf1[i]))))**2 - elevationBeamPattern2f[k,i] = (abs(unnormalized_sinc(pi * 0.884 - / ray_elevationAngleWidth * sin(ray_elevationAnglesf1[k]))))**2 + azimuthBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 + / ray_azimuthAngleWidth * sin(ray_azimuthAnglesf1[i]))))**2 + elevationBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 + / ray_elevationAngleWidth * sin(ray_elevationAnglesf1[k]))))**2 for k in range(ray_nElevationRays): for i in range(ray_nAzimuthRays): xi_z = random() # generate a random number, (Gaussian noise) xi_y = random() # generate another random number, (Gaussian noise) + # ray in beam + r = i % nBeams + # angle between ray vector and object normal vector, [rad] - alpha = ray_alphaf2[k,i] + alpha = _ray_incidence(full_sweep_azimuthAnglesf1[i], + ray_elevationAnglesf1[k], + ray_normalsf2_4[k, i]) - distance = ray_distancef2[k,i] + distance = ray_distancesf2[k,i] amplitude = (((xi_z + 1j * xi_y) / sqrt(2)) * (sqrt(mu * cos(alpha)**2 * distance**2 @@ -120,17 +207,31 @@ def process_rays(distances, _normals): # power level based on echo time for each beam P_beam_tf2 = np.zeros((nBeams, nFreq)) - #print("size1") - #print(P_beam_tf2.shape) for b in range(nBeams): P_beam_tf2[b,:] = np.fft.ifft(P_ray_f2c[b,:]) # power into buckets - P_bucket_tf2 = np.zeros((nBeams, nBuckets)) + P_bucket_tf2 = np.zeros((nBeams, nBuckets), dtype=np.float32) for b in range(nBeams): for f in range(nFreq): - bucket = int(P_beam_tf2[b,nBuckets]*300/max_T) + bucket = int(f*nBuckets/nFreq) P_bucket_tf2[b, bucket] += P_beam_tf2[b,f] + if show_plots: + time1f = np.linspace(0,max_T,nFreq) # for diagnostics plot + _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) + return P_bucket_tf2 +# test +if __name__ == '__main__': + # Note that dimensions must match dimensions hardcoded in process_rays. + ray_distancesf2 = np.array([[15,5,10], [2,100,10], [15,15,15], [4,2,3]]) + ray_normalsf2_4= np.array([[[1.0,0,0,0], [1,0,0,0], [1,0,0,0]], + [[1,0,0,0], [1,0,0,0], [1,0,0,0]], + [[1,0,0,0], [1,0,0,0], [1,0,0,0]], + [[1,0,0,0], [1,0,0,0], [1,0,0,0]]]) + + image = process_rays(ray_distancesf2, ray_normalsf2_4, True) + From 678a0a8eb818a308dee7126946dcf20f7961b935 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Tue, 15 Sep 2020 14:12:17 -0700 Subject: [PATCH 14/24] fix copy of normals data --- ...ebo_ros_depth_camera_sonar_single_beam.cpp | 23 +++---------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index 5e6dd23..0e29c91 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -620,29 +620,12 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( image_msg.data.resize(rows_arg * cols_arg * sizeof(float) * 4); image_msg.is_bigendian = 0; - const float bad_point = std::numeric_limits::quiet_NaN(); - float* dest = (float*)(&(image_msg.data[0])); float* toCopyFrom = (float*)data_arg; - int index = 0; - // convert normals to point cloud -std::cout << "FillNormalsImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; - for (uint32_t j = 0; j < rows_arg; j++) - { - for (uint32_t i = 0; i < cols_arg*4; i++) - { - float normals = toCopyFrom[index++]; -// if (normals > this->point_cloud_cutoff_) -// { - dest[i + j * cols_arg] = normals; -// } -// else //point in the unseeable range -// { -// dest[i + j * cols_arg] = bad_point; -// } - } - } + // accept values as they are so memcpy without looped checks + memcpy(dest, toCopyFrom, rows_arg * cols_arg * 4 * sizeof(float)); + return true; } From f3d34b515b47ee951d20105ce7d0ff83453cb916 Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Tue, 15 Sep 2020 16:03:33 -0700 Subject: [PATCH 15/24] add diagnostics --- scripts/sonar_equations.py | 59 +++++++++++++++++++++++++++++--------- scripts/sonar_ros.py | 20 ++++++++----- 2 files changed, 58 insertions(+), 21 deletions(-) diff --git a/scripts/sonar_equations.py b/scripts/sonar_equations.py index cbe1c43..d263472 100755 --- a/scripts/sonar_equations.py +++ b/scripts/sonar_equations.py @@ -102,17 +102,41 @@ def _ray_incidence(azimuth, elevation, normalf4): #zz return pi - acos(ray_normal.dot(target_normal)) dot_product = ray_normal.dot(target_normal) - print("dot product: "%dot_product) - print(dot_product) - if dot_product < -1.0 or dot_product > 1.0: - text="ray_normal %f, %f, %f"%(ray_normal[0], ray_normal[1], ray_normal[2]) - rospy.logwarn(_textf3("ray_normal", ray_normal)) - rospy.logwarn(_textf3("target_normal", target_normal)) - rospy.logwarn("dot product = %f"%dot_product) - dot_product=-1.0 +# print("dot product: "%dot_product) +# print(dot_product) +# if dot_product < -1.0 or dot_product > 1.0: +# text="ray_normal %f, %f, %f"%(ray_normal[0], ray_normal[1], ray_normal[2]) +# rospy.logwarn(_textf3("ray_normal", ray_normal)) +# rospy.logwarn(_textf3("target_normal", target_normal)) +# rospy.logwarn("dot product = %f"%dot_product) +# dot_product=-1.0 return pi - acos(dot_product) def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): +# for i in range(4): +# rospy.logwarn("ray distance %d: %f, %f, %f"%(i, +# ray_distancesf2[i,0], ray_distancesf2[i,1], ray_distancesf2[i,2])) +# +# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, +# ray_normalsf2_4[i,0,0], ray_normalsf2_4[i,0,1], ray_normalsf2_4[i,0,2], ray_normalsf2_4[i,0,3])) +# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, +# ray_normalsf2_4[i,1,2], ray_normalsf2_4[i,1,0], ray_normalsf2_4[i,1,1], ray_normalsf2_4[i,1,3])) +# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, +# ray_normalsf2_4[i,2,2], ray_normalsf2_4[i,2,0], ray_normalsf2_4[i,2,1], ray_normalsf2_4[i,2,3])) +# rospy.logwarn(" ") + + for i in range(4): + rospy.logwarn("ray distance %d: %f, %f, %f"%(i, + ray_distancesf2[i,0], ray_distancesf2[i,1], ray_distancesf2[i,2])) + + rospy.logwarn("ray normal %d: %f, %f, %f"%(i, + ray_normalsf2_4[i,0,2], ray_normalsf2_4[i,0,0], ray_normalsf2_4[i,0,1])) + rospy.logwarn("ray normal %d: %f, %f, %f"%(i, + ray_normalsf2_4[i,1,2], ray_normalsf2_4[i,1,0], ray_normalsf2_4[i,1,1])) + rospy.logwarn("ray normal %d: %f, %f, %f"%(i, + ray_normalsf2_4[i,2,2], ray_normalsf2_4[i,2,0], ray_normalsf2_4[i,2,1])) + rospy.logwarn(" ") + # Sonar sensor properties nBeams = 1 @@ -177,6 +201,7 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): elevationBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 / ray_elevationAngleWidth * sin(ray_elevationAnglesf1[k]))))**2 + incidences_f2 = np.zeros((ray_nElevationRays, ray_nAzimuthRays), dtype=np.float32) # diagnostics message for k in range(ray_nElevationRays): for i in range(ray_nAzimuthRays): xi_z = random() # generate a random number, (Gaussian noise) @@ -186,14 +211,15 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): r = i % nBeams # angle between ray vector and object normal vector, [rad] - alpha = _ray_incidence(full_sweep_azimuthAnglesf1[i], - ray_elevationAnglesf1[k], - ray_normalsf2_4[k, i]) + incidence = _ray_incidence(full_sweep_azimuthAnglesf1[i], + ray_elevationAnglesf1[k], + ray_normalsf2_4[k, i]) + incidences_f2[k,i] = incidence distance = ray_distancesf2[k,i] amplitude = (((xi_z + 1j * xi_y) / sqrt(2)) - * (sqrt(mu * cos(alpha)**2 * distance**2 + * (sqrt(mu * cos(incidence)**2 * distance**2 * ray_azimuthAngleWidth * ray_elevationAngleWidth)) * azimuthBeamPattern2f[k,i] @@ -205,6 +231,11 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): P_ray_f2c[b,m] = P_ray_f2c[b,m] + S_f1f[m] * amplitude \ * np.exp(-1j * K1f[m] * distance * 2) / (distance**2) + # diagnostics incidences + for i in range(4): + rospy.logwarn("incidences %d: %f, %f, %f"%(i, + incidences_f2[i,0], incidences_f2[i,1], incidences_f2[i,2])) + # power level based on echo time for each beam P_beam_tf2 = np.zeros((nBeams, nFreq)) for b in range(nBeams): @@ -222,7 +253,7 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) - return P_bucket_tf2 + return P_bucket_tf2, incidences_f2 # test if __name__ == '__main__': @@ -233,5 +264,5 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): [[1,0,0,0], [1,0,0,0], [1,0,0,0]], [[1,0,0,0], [1,0,0,0], [1,0,0,0]]]) - image = process_rays(ray_distancesf2, ray_normalsf2_4, True) + _image, _incidences = process_rays(ray_distancesf2, ray_normalsf2_4, True) diff --git a/scripts/sonar_ros.py b/scripts/sonar_ros.py index babb2f8..d82db7c 100755 --- a/scripts/sonar_ros.py +++ b/scripts/sonar_ros.py @@ -18,10 +18,11 @@ IMAGE_NORMALS_TOPIC = "depth_camera_sonar_sensor_camera/image_normals" # publisher name constants, should be replaced with sdf inputs -RAY_IMAGE_TOPIC = "sonar_ray_image" -RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" +#RAY_IMAGE_TOPIC = "sonar_ray_image" +#RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" BEAM_IMAGE_TOPIC = "sonar_beam_image" -BEAM_POINT_CLOUD_TOPIC = "sonar_beam_point_cloud" +#BEAM_POINT_CLOUD_TOPIC = "sonar_beam_point_cloud" +INCIDENCE_IMAGE_TOPIC = "sonar_ray_incidence_image" class SonarNode: def __init__(self): @@ -33,7 +34,8 @@ def __init__(self): Image, self.on_normals_image) # ROS publishers - self.ray_pub = rospy.Publisher(RAY_IMAGE_TOPIC, Image, queue_size=10) + self.beam_pub = rospy.Publisher(BEAM_IMAGE_TOPIC, Image, queue_size=10) + self.incidence_pub = rospy.Publisher(INCIDENCE_IMAGE_TOPIC, Image, queue_size=10) # ROS-CV bridge self.bridge = CvBridge() @@ -52,11 +54,15 @@ def on_normals_image(self, normals_image): print("normals image shape: ", self.normals_matrix_f4.shape) # generate all the outputs - beam_matrix = process_rays(self.depth_matrix, + beam_matrix, alphas = process_rays(self.depth_matrix, self.normals_matrix_f4) - # advertise ray_power_matrix to ROS, inheriting the 32FC1 format - self.ray_pub.publish(self.bridge.cv2_to_imgmsg(beam_matrix, + # advertise the bucketed beam power matrix + self.beam_pub.publish(self.bridge.cv2_to_imgmsg(beam_matrix, + "passthrough")) + + # advertise alphas to ROS for diagnostics + self.incidence_pub.publish(self.bridge.cv2_to_imgmsg(alphas, "passthrough")) if __name__ == '__main__': From d26958ea45fd8522062819b7266c14507b94249e Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Thu, 17 Sep 2020 15:56:43 -0700 Subject: [PATCH 16/24] add diagnostics --- launch/depth_camera_sonar_basic.launch | 17 +--- scripts/sonar_equations.py | 133 +++++++++++-------------- urdf/depth_camera_sonar.xacro | 11 +- urdf/depth_camera_sonar_robot.xacro | 1 + 4 files changed, 64 insertions(+), 98 deletions(-) diff --git a/launch/depth_camera_sonar_basic.launch b/launch/depth_camera_sonar_basic.launch index 4a3d2c0..65dc23e 100644 --- a/launch/depth_camera_sonar_basic.launch +++ b/launch/depth_camera_sonar_basic.launch @@ -16,24 +16,13 @@ args="-urdf -model nps_depth_camera_sonar_model -param depth_camera_sonar_robot" /> - - - - - - - + + diff --git a/scripts/sonar_equations.py b/scripts/sonar_equations.py index d263472..ae5966a 100755 --- a/scripts/sonar_equations.py +++ b/scripts/sonar_equations.py @@ -12,7 +12,7 @@ import matplotlib.pyplot as plt import rospy -# diagnostics +# various diagnostics for beam 0 def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2): @@ -23,7 +23,6 @@ def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets)) # inverse fast fourier transform - # figure (1) plt.subplot(2,2,1) plt.title("Power based on echo time") plt.grid(True) @@ -32,7 +31,6 @@ def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, plt.ylabel('Pressure, [Pa]') # Sound Pressure Level of Echo Level - # figure (2) SPLf1 = 20 * np.log(np.abs(P_beam_tf2[0,:])) # sound pressure level, [dB] plt.subplot(2,2,2) plt.title("Sound pressure level based on echo time") @@ -57,6 +55,27 @@ def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, plt.show() +# diagnostics showing pressure levels for each beam +def _show_plots_powers(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2): + + # Plots + plt.figure(figsize=(14,10), dpi=80) + plt.suptitle("Sound pressure level in Db based on echo time\n" + "%d beam(s), %d elevation rays, %d azimuth rays " + "%d frequencies"%(nBeams, + ray_nElevationRays, ray_nAzimuthRays, nFreq)) + + # Sound Pressure Level of Echo Level + for i in range(16): + SPLf1 = 20 * np.log(np.abs(P_beam_tf2[i,:])) +# SPLf1 = 20 * np.log(np.abs(P_bucket_tf2[i,:])) # bucketed + plt.subplot(16,1,i+1) + plt.grid(True) + plt.plot(time1f, SPLf1, linewidth=0.5) + plt.xlabel('Time, [s]') + plt.show() + # unnormalized sinc function def _unnormalized_sinc(t): try: @@ -82,64 +101,26 @@ def _unnormalized_sinc(t): def _textf3(text, f): return "%s: %f, %f, %f"%(text,f[0],f[1],f[2]) -# incidence angle is target's normal angle compensated by ray's -# azimuth and elevation +# incidence angle is target's normal angle accounting for the ray's azimuth +# and elevation def _ray_incidence(azimuth, elevation, normalf4): # ray normal from camera azimuth and elevation camera_x = cos(-azimuth)*cos(elevation) camera_y = sin(-azimuth)*cos(elevation) camera_z = sin(elevation) ray_normal = np.array([camera_x, camera_y, camera_z]) - print("ray_normal", ray_normal) - # target normal with axes compensated to camera axes zz verify + # target normal with axes compensated to camera axes target_normal = np.array([normalf4[2], -normalf4[0], -normalf4[1]]) - print("target_normal", target_normal) - # dot product -#zz this fails because the dot product gets out of range. -#zz return pi - acos(ray_normal.dot(target_normal)) - dot_product = ray_normal.dot(target_normal) -# print("dot product: "%dot_product) -# print(dot_product) -# if dot_product < -1.0 or dot_product > 1.0: -# text="ray_normal %f, %f, %f"%(ray_normal[0], ray_normal[1], ray_normal[2]) -# rospy.logwarn(_textf3("ray_normal", ray_normal)) -# rospy.logwarn(_textf3("target_normal", target_normal)) -# rospy.logwarn("dot product = %f"%dot_product) -# dot_product=-1.0 return pi - acos(dot_product) def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): -# for i in range(4): -# rospy.logwarn("ray distance %d: %f, %f, %f"%(i, -# ray_distancesf2[i,0], ray_distancesf2[i,1], ray_distancesf2[i,2])) -# -# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, -# ray_normalsf2_4[i,0,0], ray_normalsf2_4[i,0,1], ray_normalsf2_4[i,0,2], ray_normalsf2_4[i,0,3])) -# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, -# ray_normalsf2_4[i,1,2], ray_normalsf2_4[i,1,0], ray_normalsf2_4[i,1,1], ray_normalsf2_4[i,1,3])) -# rospy.logwarn("ray normal %d: %f, %f, %f, %f"%(i, -# ray_normalsf2_4[i,2,2], ray_normalsf2_4[i,2,0], ray_normalsf2_4[i,2,1], ray_normalsf2_4[i,2,3])) -# rospy.logwarn(" ") - - for i in range(4): - rospy.logwarn("ray distance %d: %f, %f, %f"%(i, - ray_distancesf2[i,0], ray_distancesf2[i,1], ray_distancesf2[i,2])) - - rospy.logwarn("ray normal %d: %f, %f, %f"%(i, - ray_normalsf2_4[i,0,2], ray_normalsf2_4[i,0,0], ray_normalsf2_4[i,0,1])) - rospy.logwarn("ray normal %d: %f, %f, %f"%(i, - ray_normalsf2_4[i,1,2], ray_normalsf2_4[i,1,0], ray_normalsf2_4[i,1,1])) - rospy.logwarn("ray normal %d: %f, %f, %f"%(i, - ray_normalsf2_4[i,2,2], ray_normalsf2_4[i,2,0], ray_normalsf2_4[i,2,1])) - rospy.logwarn(" ") - # Sonar sensor properties - nBeams = 1 + nBeams = 16 beam_elevationAngle = 0.0175 # Beam looking down in elevation direction beam_azimuthAngle = 0.0 # Beam at center line in azimuth direction beam_elevationAngleWidth = 0.1 # radians @@ -149,7 +130,7 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): nBuckets = 300 - if ray_distancesf2.shape != (ray_nElevationRays, ray_nAzimuthRays): + if ray_distancesf2.shape != (ray_nElevationRays, ray_nAzimuthRays * nBeams): print("bad distances shape ", ray_distancesf2.shape) return np.zeros(nBeams,nBuckets) @@ -163,12 +144,6 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): ray_elevationAngleWidth = beam_elevationAngleWidth/(ray_nElevationRays - 1) ray_azimuthAngleWidth = beam_azimuthAngleWidth/(ray_nAzimuthRays - 1) - # azimuth angles for nAzimuthRays * nBeams - full_sweep_azimuthAnglesf1 = beam_azimuthAngle + np.linspace( - nBeams * -beam_azimuthAngleWidth / 2, - nBeams * beam_azimuthAngleWidth / 2, - nBeams * ray_nAzimuthRays) - # calculated sampling periods max_T = np.amax(ray_distancesf2)*2/soundSpeed _delta_f = 1/max_T @@ -177,7 +152,7 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): nFreq = int(round((fmax - fmin) / _delta_f)) # reduce nFreq because calculated nFreq is too large for looping - nFreq=10000 + print("nFreq", nFreq) _freq1f = np.linspace(fmin,fmax,nFreq) # calculated physics @@ -201,17 +176,22 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): elevationBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 / ray_elevationAngleWidth * sin(ray_elevationAnglesf1[k]))))**2 - incidences_f2 = np.zeros((ray_nElevationRays, ray_nAzimuthRays), dtype=np.float32) # diagnostics message + # diagnostics image of ray incidences + incidences_f2 = np.zeros((ray_nElevationRays, ray_nAzimuthRays * nBeams), + dtype=np.float32) # diagnostics message + for k in range(ray_nElevationRays): - for i in range(ray_nAzimuthRays): + for i in range(ray_nAzimuthRays * nBeams): xi_z = random() # generate a random number, (Gaussian noise) xi_y = random() # generate another random number, (Gaussian noise) +# xi_z = 0.5 # turn off randomness +# xi_y = 0.5 # turn off randomness - # ray in beam - r = i % nBeams + # ray r in beam i + r = i % ray_nAzimuthRays - # angle between ray vector and object normal vector, [rad] - incidence = _ray_incidence(full_sweep_azimuthAnglesf1[i], + # angle between ray vector and object normal vector + incidence = _ray_incidence(ray_azimuthAnglesf1[r], ray_elevationAnglesf1[k], ray_normalsf2_4[k, i]) incidences_f2[k,i] = incidence @@ -222,8 +202,8 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): * (sqrt(mu * cos(incidence)**2 * distance**2 * ray_azimuthAngleWidth * ray_elevationAngleWidth)) - * azimuthBeamPattern2f[k,i] - * elevationBeamPattern2f[k,i]) + * azimuthBeamPattern2f[k,r] + * elevationBeamPattern2f[k,r]) # Summation of Echo returned from a signal (frequency domain) b = int(i/ray_nAzimuthRays) # beam @@ -231,13 +211,8 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): P_ray_f2c[b,m] = P_ray_f2c[b,m] + S_f1f[m] * amplitude \ * np.exp(-1j * K1f[m] * distance * 2) / (distance**2) - # diagnostics incidences - for i in range(4): - rospy.logwarn("incidences %d: %f, %f, %f"%(i, - incidences_f2[i,0], incidences_f2[i,1], incidences_f2[i,2])) - # power level based on echo time for each beam - P_beam_tf2 = np.zeros((nBeams, nFreq)) + P_beam_tf2 = np.zeros((nBeams, nFreq), dtype=np.float32) for b in range(nBeams): P_beam_tf2[b,:] = np.fft.ifft(P_ray_f2c[b,:]) @@ -248,21 +223,29 @@ def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): bucket = int(f*nBuckets/nFreq) P_bucket_tf2[b, bucket] += P_beam_tf2[b,f] +# show_plots = True if show_plots: time1f = np.linspace(0,max_T,nFreq) # for diagnostics plot - _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, +# _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, +# nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) + _show_plots_powers(nBeams, ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) - return P_bucket_tf2, incidences_f2 +# return P_beam_tf2.T, incidences_f2 # unbucketed beam + return P_bucket_tf2.T, incidences_f2 # bucketed beam # test if __name__ == '__main__': - # Note that dimensions must match dimensions hardcoded in process_rays. - ray_distancesf2 = np.array([[15,5,10], [2,100,10], [15,15,15], [4,2,3]]) - ray_normalsf2_4= np.array([[[1.0,0,0,0], [1,0,0,0], [1,0,0,0]], - [[1,0,0,0], [1,0,0,0], [1,0,0,0]], - [[1,0,0,0], [1,0,0,0], [1,0,0,0]], - [[1,0,0,0], [1,0,0,0], [1,0,0,0]]]) + # These dimensions must match hardcoded dimensions + # 16 beams 3 wide 4 tall + ray_distancesf2 = np.zeros((4,48), dtype=np.float32) + ray_distancesf2[:,] = np.linspace(0.5, 6.0, 48) + ray_normalsf2_4 = np.zeros((4,48,4), dtype=np.float32) + ray_normalsf2_4[:,:,0]=1.0 + + print("ray_distancesf2", ray_distancesf2) + print("ray_normalsf2_4", ray_normalsf2_4) + # run test dataset and show plots _image, _incidences = process_rays(ray_distancesf2, ray_normalsf2_4, True) diff --git a/urdf/depth_camera_sonar.xacro b/urdf/depth_camera_sonar.xacro index f98400e..eecd99e 100644 --- a/urdf/depth_camera_sonar.xacro +++ b/urdf/depth_camera_sonar.xacro @@ -2,7 +2,7 @@ - + @@ -56,18 +56,11 @@ forward_sonar_optical_link - - - ${beam_width} - ${horiz_count} + ${horiz_count * num_beams} ${vert_count} R8G8B8 diff --git a/urdf/depth_camera_sonar_robot.xacro b/urdf/depth_camera_sonar_robot.xacro index 644d6ed..e15faa5 100644 --- a/urdf/depth_camera_sonar_robot.xacro +++ b/urdf/depth_camera_sonar_robot.xacro @@ -14,6 +14,7 @@ beam_width="1.6" horiz_count="3" vert_count="4" + num_beams="16" update_rate="15" min_range="0.1" max_range="7.0"> From 93a3e6a788b52721e04430f41339f9b9f0973549 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 09:20:17 -0700 Subject: [PATCH 17/24] style tweaks for nps_gazebo_ros_depth_camera_sonar_single_beam.h --- ...azebo_ros_depth_camera_sonar_single_beam.h | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h index 1b569c6..1bb6c85 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h @@ -36,15 +36,6 @@ #include #include -// gazebo stuff -#include -#include -#include -#include -#include -#include -#include - // dynamic reconfigure stuff #include #include @@ -55,6 +46,17 @@ // camera stuff #include +// gazebo stuff +#include +#include +#include +#include +#include +#include +#include + +#include + namespace gazebo { class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils @@ -123,7 +125,8 @@ namespace gazebo private: void RayImageDisconnect(); private: common::Time last_depth_image_camera_info_update_time_; - private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, + private: bool FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg); @@ -135,7 +138,8 @@ namespace gazebo uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg); - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + /// \brief A pointer to the ROS node. + /// A node will be instantiated if it does not exist. private: ros::Publisher point_cloud_pub_; private: ros::Publisher depth_image_pub_; From 6c18818c70c77780940b4e0e50eebc326357a20b Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 09:22:10 -0700 Subject: [PATCH 18/24] rename header to match header guard and reflect our convention for cpp headers --- ...le_beam.h => nps_gazebo_ros_depth_camera_sonar_single_beam.hh} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/{nps_gazebo_ros_depth_camera_sonar_single_beam.h => nps_gazebo_ros_depth_camera_sonar_single_beam.hh} (100%) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.hh similarity index 100% rename from src/nps_gazebo_ros_depth_camera_sonar_single_beam.h rename to src/nps_gazebo_ros_depth_camera_sonar_single_beam.hh From 45bfe0df48b0c7f03f7618a4e3412e7f5dafdefe Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 23 Sep 2020 10:01:46 -0700 Subject: [PATCH 19/24] move header file --- .../nps_gazebo_ros_depth_camera_sonar_single_beam.h | 4 ++-- src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) rename {src => include/nps_uw_sensors_gazebo}/nps_gazebo_ros_depth_camera_sonar_single_beam.h (98%) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h b/include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.h similarity index 98% rename from src/nps_gazebo_ros_depth_camera_sonar_single_beam.h rename to include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.h index 1b569c6..7c52f7e 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.h +++ b/include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.h @@ -20,8 +20,8 @@ * Date: 24 Sept 2008 */ -#ifndef GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH -#define GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH +#ifndef GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_H +#define GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_H // ros stuff #include diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index 0e29c91..35f1f80 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -25,7 +25,7 @@ #include #include -#include "nps_gazebo_ros_depth_camera_sonar_single_beam.h" +#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.h" #include #include From 2767356ac57f8d77b7e9ef4c74817bcd615f1d9b Mon Sep 17 00:00:00 2001 From: Bruce Allen Date: Wed, 23 Sep 2020 10:18:19 -0700 Subject: [PATCH 20/24] fix rename from unexpected push --- src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index 35f1f80..d387d51 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -25,7 +25,7 @@ #include #include -#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.h" +#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh" #include #include From 05f7fd3e0a76d9e5691e66ce060e4be4dc7c5804 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 13:34:28 -0700 Subject: [PATCH 21/24] fix header path --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b886610..47f2b43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,7 @@ target_link_libraries(nps_gazebo_ros_gpu_sonar_single_beam_plugin add_library(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp - src/nps_gazebo_ros_depth_camera_sonar_single_beam.h) + include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh) target_link_libraries(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin DepthCameraPlugin ${catkin_LIBRARIES}) From f0920e706f41aa74fb3d50b9b8f795a8a9d4a231 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 13:34:43 -0700 Subject: [PATCH 22/24] fix whitespace and casting --- ...ebo_ros_depth_camera_sonar_single_beam.cpp | 111 ++++++++++-------- 1 file changed, 64 insertions(+), 47 deletions(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index d387d51..1514e4f 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -20,20 +20,22 @@ Date: 24 Sept 2008 */ -#include #include +#include +#include + #include #include -#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh" +#include +#include +#include #include #include #include -#include - -#include +#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh" namespace gazebo { @@ -121,33 +123,33 @@ void GazeboRosDepthCamera::Advertise() { ros::AdvertiseOptions point_cloud_ao = ros::AdvertiseOptions::create( - this->point_cloud_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::PointCloudConnect,this), - boost::bind( &GazeboRosDepthCamera::PointCloudDisconnect,this), + this->point_cloud_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::PointCloudConnect, this), + boost::bind(&GazeboRosDepthCamera::PointCloudDisconnect, this), ros::VoidPtr(), &this->camera_queue_); this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); ros::AdvertiseOptions depth_image_ao = ros::AdvertiseOptions::create< sensor_msgs::Image >( - this->depth_image_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::DepthImageConnect,this), - boost::bind( &GazeboRosDepthCamera::DepthImageDisconnect,this), + this->depth_image_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::DepthImageConnect, this), + boost::bind(&GazeboRosDepthCamera::DepthImageDisconnect, this), ros::VoidPtr(), &this->camera_queue_); this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao); ros::AdvertiseOptions normals_image_ao = ros::AdvertiseOptions::create< sensor_msgs::Image >( - this->normals_image_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::NormalsImageConnect,this), - boost::bind( &GazeboRosDepthCamera::NormalsImageDisconnect,this), + this->normals_image_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::NormalsImageConnect, this), + boost::bind(&GazeboRosDepthCamera::NormalsImageDisconnect, this), ros::VoidPtr(), &this->camera_queue_); this->normals_image_pub_ = this->rosnode_->advertise(normals_image_ao); ros::AdvertiseOptions depth_image_camera_info_ao = ros::AdvertiseOptions::create( - this->depth_image_camera_info_topic_name_,1, - boost::bind( &GazeboRosDepthCamera::DepthInfoConnect,this), - boost::bind( &GazeboRosDepthCamera::DepthInfoDisconnect,this), + this->depth_image_camera_info_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::DepthInfoConnect, this), + boost::bind(&GazeboRosDepthCamera::DepthInfoDisconnect, this), ros::VoidPtr(), &this->camera_queue_); this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); } @@ -253,7 +255,7 @@ std::cout << "OnNewDepthFrame\n"; else { if (this->point_cloud_connect_count_ > 0 || - this->depth_image_connect_count_ <= 0 || // zz why? + this->depth_image_connect_count_ <= 0 || // zz why? this->normals_image_connect_count_ <= 0) // do this first so there's chance for sensor to run 1 frame after activate this->parentSensor->SetActive(true); @@ -320,7 +322,8 @@ std::cout << "OnNewRGBPointCloud\n"; uint8_t r = (rgb >> 16) & 0x0000ff; uint8_t g = (rgb >> 8) & 0x0000ff; uint8_t b = (rgb) & 0x0000ff; - std::cerr << (int)r << " " << (int)g << " " << (int)b << "\n"; + std::cerr << static_cast(r) << " " << static_cast(g) + << " " << static_cast(b) << std::endl; } } } @@ -341,7 +344,7 @@ std::cout << "OnNewImageFrame\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; - //ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); + // ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); # if GAZEBO_MAJOR_VERSION >= 7 this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else @@ -378,12 +381,12 @@ void GazeboRosDepthCamera::FillPointdCloud(const float *_src) this->point_cloud_msg_.height = this->height; this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; - ///copy from depth to point cloud message + // copy from depth to point cloud message FillPointCloudHelper(this->point_cloud_msg_, this->height, this->width, this->skip_, - (void*)_src ); + const_cast(reinterpret_cast(_src))); this->point_cloud_pub_.publish(this->point_cloud_msg_); @@ -400,12 +403,12 @@ void GazeboRosDepthCamera::FillDepthImage(const float *_src) this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; - ///copy from depth to depth image message + // copy from depth to depth image message FillDepthImageHelper(this->depth_image_msg_, this->height, this->width, this->skip_, - (void*)_src ); + const_cast(reinterpret_cast(_src))); this->depth_image_pub_.publish(this->depth_image_msg_); @@ -427,8 +430,8 @@ bool GazeboRosDepthCamera::FillDepthImageHelper( const float bad_point = std::numeric_limits::quiet_NaN(); - float* dest = (float*)(&(image_msg.data[0])); - float* toCopyFrom = (float*)data_arg; + float* dest = reinterpret_cast(&(image_msg.data[0])); + float* toCopyFrom = reinterpret_cast(data_arg); int index = 0; // convert depth to image @@ -442,7 +445,7 @@ std::cout << "FillDepthImageHelper rows: " << rows_arg << " cols: " << cols_arg { dest[i + j * cols_arg] = depth; } - else //point in the unseeable range + else // point in the unseeable range { dest[i + j * cols_arg] = bad_point; } @@ -468,24 +471,38 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( point_cloud_msg.is_dense = true; - float* toCopyFrom = (float*)data_arg; + float* toCopyFrom = reinterpret_cast(data_arg); int index = 0; double hfov = this->parentSensor->DepthCamera()->HFOV().Radian(); - double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); + double fl = (static_cast(this->width)) / (2.0 *tan(hfov/2.0)); // convert depth to point cloud - for (uint32_t j=0; j1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); - else pAngle = 0.0; + if (rows_arg > 1) + { + pAngle = atan2(static_cast(j) - + 0.5*static_cast(rows_arg-1), fl); + } + else + { + pAngle = 0.0; + } - for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); - else yAngle = 0.0; + if (cols_arg > 1) + { + yAngle = atan2(static_cast(i) - + 0.5*static_cast(cols_arg-1), fl); + } + else + { + yAngle = 0.0; + } double depth = toCopyFrom[index++]; @@ -495,18 +512,18 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( // rotation from the physical camera *_frame *iter_x = depth * tan(yAngle); *iter_y = depth * tan(pAngle); - if(depth > this->point_cloud_cutoff_) + if (depth > this->point_cloud_cutoff_) { *iter_z = depth; } - else //point in the unseeable range + else // point in the unseeable range { - *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); point_cloud_msg.is_dense = false; } // put image color data for each point - uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0])); + uint8_t* image_src = reinterpret_cast(&(this->image_msg_.data[0])); if (this->image_msg_.data.size() == rows_arg*cols_arg*3) { // color @@ -571,14 +588,14 @@ std::cout << "OnNewNormalsFrame\n"; if (this->normals_image_connect_count_ > 0) this->FillNormalsImage(_normals_image); -//zz // lets perform Sonar calculations here - //zz this->FillRayImage(); +// zz // lets perform Sonar calculations here +// zz this->FillRayImage(); } } else { if (this->point_cloud_connect_count_ > 0 || - this->depth_image_connect_count_ <= 0 || // zz why? + this->depth_image_connect_count_ <= 0 || // zz why? this->normals_image_connect_count_ <= 0) // do this first so there's chance for sensor to run 1 frame after activate this->parentSensor->SetActive(true); @@ -595,12 +612,12 @@ void GazeboRosDepthCamera::FillNormalsImage(const float *_src) this->normals_image_msg_.header.stamp.sec = this->normals_sensor_update_time_.sec; this->normals_image_msg_.header.stamp.nsec = this->normals_sensor_update_time_.nsec; - ///copy from normals to normals image message + // copy from normals to normals image message FillNormalsImageHelper(this->normals_image_msg_, this->height, this->width, this->skip_, - (void*)_src ); + const_cast(reinterpret_cast(_src))); this->normals_image_pub_.publish(this->normals_image_msg_); @@ -620,8 +637,8 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( image_msg.data.resize(rows_arg * cols_arg * sizeof(float) * 4); image_msg.is_bigendian = 0; - float* dest = (float*)(&(image_msg.data[0])); - float* toCopyFrom = (float*)data_arg; + float* dest = reinterpret_cast(&(image_msg.data[0])); + float* toCopyFrom = reinterpret_cast(data_arg); // accept values as they are so memcpy without looped checks memcpy(dest, toCopyFrom, rows_arg * cols_arg * 4 * sizeof(float)); @@ -655,7 +672,7 @@ void GazeboRosDepthCamera::PublishCameraInfo() } } -//@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +// @todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. /* #include pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); From 0355a72151185687e15ce76f03920424b5fc3588 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 15:09:26 -0700 Subject: [PATCH 23/24] fix long lines --- ...ebo_ros_depth_camera_sonar_single_beam.cpp | 147 ++++++++++++------ 1 file changed, 97 insertions(+), 50 deletions(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index 1514e4f..dc850b9 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -50,6 +50,8 @@ GazeboRosDepthCamera::GazeboRosDepthCamera() this->depth_image_connect_count_ = 0; this->normals_image_connect_count_ = 0; this->depth_info_connect_count_ = 0; + this->point_cloud_cutoff_ = 0.4; + this->sonar_point_cloud_connect_count_ = 0; this->last_depth_image_camera_info_update_time_ = common::Time(0); } @@ -61,7 +63,8 @@ GazeboRosDepthCamera::~GazeboRosDepthCamera() //////////////////////////////////////////////////////////////////////////////// // Load the controller -void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf) { std::cout << "nps_gazebo_ros_depth_camera_sonar_single_beam"; DepthCameraPlugin::Load(_parent, _sdf); @@ -69,8 +72,10 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) { - ROS_FATAL_STREAM_NAMED("depth_camera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + ROS_FATAL_STREAM_NAMED("depth_camera", "A ROS node for Gazebo has not " + "been initialized, unable to load plugin. Load the " + "Gazebo system plugin 'libgazebo_ros_api_plugin.so' " + "in the gazebo_ros package)"); return; } @@ -92,30 +97,36 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf if (!_sdf->HasElement("pointCloudTopicName")) this->point_cloud_topic_name_ = "points"; else - this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + this->point_cloud_topic_name_ = + _sdf->GetElement("pointCloudTopicName")->Get(); // depth image and normals stuff if (!_sdf->HasElement("depthImageTopicName")) this->depth_image_topic_name_ = "depth/image_raw"; else - this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get(); + this->depth_image_topic_name_ = + _sdf->GetElement("depthImageTopicName")->Get(); if (!_sdf->HasElement("normalsImageTopicName")) this->normals_image_topic_name_ = "normals/image_raw"; else - this->normals_image_topic_name_ = _sdf->GetElement("normalsImageTopicName")->Get(); + this->normals_image_topic_name_ = + _sdf->GetElement("normalsImageTopicName")->Get(); if (!_sdf->HasElement("depthImageCameraInfoTopicName")) this->depth_image_camera_info_topic_name_ = "depth/camera_info"; else - this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get(); + this->depth_image_camera_info_topic_name_ = + _sdf->GetElement("depthImageCameraInfoTopicName")->Get(); if (!_sdf->HasElement("pointCloudCutoff")) this->point_cloud_cutoff_ = 0.4; else - this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get(); + this->point_cloud_cutoff_ = + _sdf->GetElement("pointCloudCutoff")->Get(); - load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosDepthCamera::Advertise, this)); + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind( + &GazeboRosDepthCamera::Advertise, this)); GazeboRosCameraUtils::Load(_parent, _sdf); } @@ -151,7 +162,8 @@ void GazeboRosDepthCamera::Advertise() boost::bind(&GazeboRosDepthCamera::DepthInfoConnect, this), boost::bind(&GazeboRosDepthCamera::DepthInfoDisconnect, this), ros::VoidPtr(), &this->camera_queue_); - this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); + this->depth_image_camera_info_pub_ = + this->rosnode_->advertise(depth_image_camera_info_ao); } @@ -229,9 +241,11 @@ std::cout << "OnNewDepthFrame\n"; return; # if GAZEBO_MAJOR_VERSION >= 7 - this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); + this->depth_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); # else - this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); + this->depth_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); # endif if (this->parentSensor->IsActive()) @@ -257,7 +271,8 @@ std::cout << "OnNewDepthFrame\n"; if (this->point_cloud_connect_count_ > 0 || this->depth_image_connect_count_ <= 0 || // zz why? this->normals_image_connect_count_ <= 0) - // do this first so there's chance for sensor to run 1 frame after activate + // do this first so there's chance for sensor to run 1 + // frame after activate this->parentSensor->SetActive(true); } } @@ -273,15 +288,18 @@ std::cout << "OnNewRGBPointCloud\n"; return; # if GAZEBO_MAJOR_VERSION >= 7 - this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); + this->depth_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); # else - this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); + this->depth_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); # endif if (!this->parentSensor->IsActive()) { if (this->point_cloud_connect_count_ > 0) - // do this first so there's chance for sensor to run 1 frame after activate + // do this first so there's chance for sensor to run 1 + // frame after activate this->parentSensor->SetActive(true); } else @@ -289,12 +307,15 @@ std::cout << "OnNewRGBPointCloud\n"; if (this->point_cloud_connect_count_ > 0) { this->lock_.lock(); - this->point_cloud_msg_.header.frame_id = this->frame_name_; - this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; - this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; - this->point_cloud_msg_.width = this->width; - this->point_cloud_msg_.height = this->height; - this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = + this->point_cloud_msg_.point_step * this->width; sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_); pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); @@ -309,7 +330,8 @@ std::cout << "OnNewRGBPointCloud\n"; for (unsigned int i = 0; i < _width; i++) { - for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) + for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, + ++iter_z, ++iter_rgb) { unsigned int index = (j * _width) + i; *iter_x = _pcd[4 * index]; @@ -322,7 +344,7 @@ std::cout << "OnNewRGBPointCloud\n"; uint8_t r = (rgb >> 16) & 0x0000ff; uint8_t g = (rgb >> 8) & 0x0000ff; uint8_t b = (rgb) & 0x0000ff; - std::cerr << static_cast(r) << " " << static_cast(g) + std::cerr << static_cast(r) << " " << static_cast(g) << " " << static_cast(b) << std::endl; } } @@ -344,7 +366,10 @@ std::cout << "OnNewImageFrame\n"; if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) return; - // ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); + // ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s", + // this->parentSensor_->GetName().c_str(), + // this->frame_name_.c_str()); + # if GAZEBO_MAJOR_VERSION >= 7 this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else @@ -354,7 +379,8 @@ std::cout << "OnNewImageFrame\n"; if (!this->parentSensor->IsActive()) { if ((*this->image_connect_count_) > 0) - // do this first so there's chance for sensor to run 1 frame after activate + // do this first so there's chance for sensor to run 1 + // frame after activate this->parentSensor->SetActive(true); } else @@ -374,12 +400,15 @@ void GazeboRosDepthCamera::FillPointdCloud(const float *_src) { this->lock_.lock(); - this->point_cloud_msg_.header.frame_id = this->frame_name_; - this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; - this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; - this->point_cloud_msg_.width = this->width; - this->point_cloud_msg_.height = this->height; - this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = + this->point_cloud_msg_.point_step * this->width; // copy from depth to point cloud message FillPointCloudHelper(this->point_cloud_msg_, @@ -399,9 +428,11 @@ void GazeboRosDepthCamera::FillDepthImage(const float *_src) { this->lock_.lock(); // copy data into image - this->depth_image_msg_.header.frame_id = this->frame_name_; - this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; - this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->depth_image_msg_.header.frame_id = this->frame_name_; + this->depth_image_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->depth_image_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; // copy from depth to depth image message FillDepthImageHelper(this->depth_image_msg_, @@ -435,7 +466,8 @@ bool GazeboRosDepthCamera::FillDepthImageHelper( int index = 0; // convert depth to image -std::cout << "FillDepthImageHelper rows: " << rows_arg << " cols: " << cols_arg << "\n"; + std::cout << "FillDepthImageHelper rows: " << rows_arg + << " cols: " << cols_arg << std:endl; for (uint32_t j = 0; j < rows_arg; j++) { for (uint32_t i = 0; i < cols_arg; i++) @@ -491,7 +523,8 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( pAngle = 0.0; } - for (uint32_t i = 0; i < cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) + for (uint32_t i = 0; i < cols_arg; i++, ++iter_x, ++iter_y, + ++iter_z, ++iter_rgb) { double yAngle; if (cols_arg > 1) @@ -523,7 +556,9 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( } // put image color data for each point - uint8_t* image_src = reinterpret_cast(&(this->image_msg_.data[0])); + uint8_t* image_src = + reinterpret_cast(&(this->image_msg_.data[0])); + if (this->image_msg_.data.size() == rows_arg*cols_arg*3) { // color @@ -566,9 +601,11 @@ std::cout << "OnNewNormalsFrame\n"; return; # if GAZEBO_MAJOR_VERSION >= 7 - this->normals_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); + this->normals_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); # else - this->normals_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); + this->normals_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); # endif if (this->parentSensor->IsActive()) @@ -597,7 +634,8 @@ std::cout << "OnNewNormalsFrame\n"; if (this->point_cloud_connect_count_ > 0 || this->depth_image_connect_count_ <= 0 || // zz why? this->normals_image_connect_count_ <= 0) - // do this first so there's chance for sensor to run 1 frame after activate + // do this first so there's chance for sensor to run 1 + // frame after activate this->parentSensor->SetActive(true); } } @@ -608,9 +646,11 @@ void GazeboRosDepthCamera::FillNormalsImage(const float *_src) { this->lock_.lock(); // copy data into image - this->normals_image_msg_.header.frame_id = this->frame_name_; - this->normals_image_msg_.header.stamp.sec = this->normals_sensor_update_time_.sec; - this->normals_image_msg_.header.stamp.nsec = this->normals_sensor_update_time_.nsec; + this->normals_image_msg_.header.frame_id = this->frame_name_; + this->normals_image_msg_.header.stamp.sec = + this->normals_sensor_update_time_.sec; + this->normals_image_msg_.header.stamp.nsec = + this->normals_sensor_update_time_.nsec; // copy from normals to normals image message FillNormalsImageHelper(this->normals_image_msg_, @@ -653,26 +693,33 @@ bool GazeboRosDepthCamera::FillNormalsImageHelper( void GazeboRosDepthCamera::PublishCameraInfo() { - ROS_DEBUG_NAMED("depth_camera", "publishing default camera info, then depth camera info"); + ROS_DEBUG_NAMED("depth_camera", "publishing default camera info," + " then depth camera info"); GazeboRosCameraUtils::PublishCameraInfo(); if (this->depth_info_connect_count_ > 0) { # if GAZEBO_MAJOR_VERSION >= 7 - common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); + common::Time sensor_update_time = + this->parentSensor_->LastMeasurementTime(); # else - common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); + common::Time sensor_update_time = + this->parentSensor_->GetLastMeasurementTime(); # endif this->sensor_update_time_ = sensor_update_time; - if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= + this->update_period_) { - this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time); +// this->PublishCameraInfo(this->depth_image_camera_info_pub_ , +// sensor_update_time); + this->PublishCameraInfo(this->depth_image_camera_info_pub_); this->last_depth_image_camera_info_update_time_ = sensor_update_time; } } } -// @todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +// @todo: publish disparity similar to +// openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. /* #include pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); From 85cc53188cfaae9e707f6414bb93b27590a1a318 Mon Sep 17 00:00:00 2001 From: M1chaelM Date: Wed, 23 Sep 2020 15:17:27 -0700 Subject: [PATCH 24/24] fix typo --- src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp index dc850b9..edf0587 100644 --- a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -467,7 +467,7 @@ bool GazeboRosDepthCamera::FillDepthImageHelper( // convert depth to image std::cout << "FillDepthImageHelper rows: " << rows_arg - << " cols: " << cols_arg << std:endl; + << " cols: " << cols_arg << std::endl; for (uint32_t j = 0; j < rows_arg; j++) { for (uint32_t i = 0; i < cols_arg; i++)