From aa6d4a197519e66cee07f41b67a95cb1b3a51038 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Sat, 7 Mar 2020 00:42:37 +0100 Subject: [PATCH] [Gazebo 9] Created a MarkerArray to publish normals in ROS (#1047) * created a MarkerArray to publish normals in ROS * Added visualization msgs dependencie depthcamerasensor * Added feedback * populate point cloud when normals is activated * argument to reduce the amount of normals to publish * reduced computation in normals publisher Signed-off-by: ahcorde --- gazebo_plugins/CMakeLists.txt | 2 + .../gazebo_plugins/gazebo_ros_depth_camera.h | 24 +++- gazebo_plugins/package.xml | 1 + .../src/gazebo_ros_depth_camera.cpp | 123 +++++++++++++++++- .../test_worlds/gazebo_ros_depth_camera.world | 12 +- 5 files changed, 156 insertions(+), 6 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 99a7f50c5..0d5c855a3 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED COMPONENTS diagnostic_updater camera_info_manager std_msgs + visualization_msgs ) # Through transitive dependencies in the packages above, gazebo_plugins depends @@ -157,6 +158,7 @@ catkin_package( rosconsole camera_info_manager std_msgs + visualization_msgs ) add_dependencies(${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h index 28c2ad848..af317ed4d 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -35,6 +35,8 @@ #include #include #include +#include +#include // gazebo stuff #include @@ -88,6 +90,11 @@ namespace gazebo unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format); + // Documentation inherited + protected: virtual void OnNewNormalsFrame(const float * _normals, + 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); @@ -99,6 +106,13 @@ namespace gazebo private: void PointCloudConnect(); private: void PointCloudDisconnect(); + /// \brief Keep track of number of connctions for point clouds + private: int normals_connect_count_; + /// \brief Increase the counter which count the subscribers are connected + private: void NormalsConnect(); + /// \brief Decrease the counter which count the subscribers are connected + private: void NormalsDisconnect(); + /// \brief Keep track of number of connctions for point clouds private: int depth_image_connect_count_; private: void DepthImageConnect(); @@ -116,16 +130,25 @@ namespace gazebo /// \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 normal_pub_; /// \brief PointCloud2 point cloud message private: sensor_msgs::PointCloud2 point_cloud_msg_; private: sensor_msgs::Image depth_image_msg_; + private: float * pcd_ = nullptr; + private: double point_cloud_cutoff_; + /// \brief adding one value each reduce_normals_ to the array marker + private: int reduce_normals_; + /// \brief ROS image topic name private: std::string point_cloud_topic_name_; + /// \brief ROS normals topic name + private: std::string normals_topic_name_; + private: void InfoConnect(); private: void InfoDisconnect(); @@ -148,4 +171,3 @@ namespace gazebo } #endif - diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index f78b02e26..1aa962b2a 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -30,6 +30,7 @@ geometry_msgs sensor_msgs trajectory_msgs + visualization_msgs std_srvs roscpp rospy diff --git a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp index e701c8b71..a3921204d 100644 --- a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp @@ -45,6 +45,7 @@ GZ_REGISTER_SENSOR_PLUGIN(GazeboRosDepthCamera) GazeboRosDepthCamera::GazeboRosDepthCamera() { this->point_cloud_connect_count_ = 0; + this->normals_connect_count_ = 0; this->depth_image_connect_count_ = 0; this->depth_info_connect_count_ = 0; this->last_depth_image_camera_info_update_time_ = common::Time(0); @@ -54,6 +55,7 @@ GazeboRosDepthCamera::GazeboRosDepthCamera() // Destructor GazeboRosDepthCamera::~GazeboRosDepthCamera() { + delete [] pcd_; } //////////////////////////////////////////////////////////////////////////////// @@ -90,6 +92,12 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + // normals stuff + if (!_sdf->HasElement("normalsTopicName")) + this->normals_topic_name_ = "normals"; + else + this->normals_topic_name_ = _sdf->GetElement("normalsTopicName")->Get(); + // depth image stuff if (!_sdf->HasElement("depthImageTopicName")) this->depth_image_topic_name_ = "depth/image_raw"; @@ -106,6 +114,11 @@ void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf else this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get(); + if (!_sdf->HasElement("reduceNormals")) + this->reduce_normals_ = 50; + else + this->reduce_normals_ = _sdf->GetElement("reduceNormals")->Get(); + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosDepthCamera::Advertise, this)); GazeboRosCameraUtils::Load(_parent, _sdf); } @@ -135,8 +148,15 @@ void GazeboRosDepthCamera::Advertise() boost::bind( &GazeboRosDepthCamera::DepthInfoDisconnect,this), ros::VoidPtr(), &this->camera_queue_); this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); -} + ros::AdvertiseOptions normals_ao = + ros::AdvertiseOptions::create( + normals_topic_name_, 1, + boost::bind( &GazeboRosDepthCamera::NormalsConnect,this), + boost::bind( &GazeboRosDepthCamera::NormalsDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->normal_pub_ = this->rosnode_->advertise(normals_ao); +} //////////////////////////////////////////////////////////////////////////////// // Increment count @@ -156,6 +176,21 @@ void GazeboRosDepthCamera::PointCloudDisconnect() this->parentSensor->SetActive(false); } +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::NormalsConnect() +{ + this->normals_connect_count_++; + (*this->image_connect_count_)++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::NormalsDisconnect() +{ + this->normals_connect_count_--; +} + //////////////////////////////////////////////////////////////////////////////// // Increment count void GazeboRosDepthCamera::DepthImageConnect() @@ -247,9 +282,15 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, } else { - if (this->point_cloud_connect_count_ > 0) + if (this->point_cloud_connect_count_ > 0 || this->normals_connect_count_ > 0) { this->lock_.lock(); + + if (pcd_ == nullptr) + pcd_ = new float[_width * _height * 4]; + + memcpy(pcd_, _pcd, sizeof(float)* _width * _height * 4); + 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; @@ -327,6 +368,84 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, } } +void GazeboRosDepthCamera::OnNewNormalsFrame(const float * _normals, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + visualization_msgs::MarkerArray m_array; + + if (!this->parentSensor->IsActive()) + { + if (this->normals_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->normals_connect_count_ > 0) + { + this->lock_.lock(); + if (this->point_cloud_msg_.data.size() > 0){ + + for (unsigned int i = 0; i < _width; i++) + { + for (unsigned int j = 0; j < _height; j++) + { + // plotting some of the normals, otherwise rviz will block it + if (index % this->reduce_normals_ == 0) + visualization_msgs::Marker m; + m.type = visualization_msgs::Marker::ARROW; + m.header.frame_id = this->frame_name_; + m.header.stamp.sec = this->depth_sensor_update_time_.sec; + m.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + m.action = visualization_msgs::Marker::ADD; + + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + m.color.a = 1.0; + m.scale.x = 1; + m.scale.y = 0.01; + m.scale.z = 0.01; + m.lifetime.sec = 1; + m.lifetime.nsec = 0; + + unsigned int index = (j * _width) + i; + m.id = index; + float x = _normals[4 * index]; + float y = _normals[4 * index + 1]; + float z = _normals[4 * index + 2]; + + m.pose.position.x = pcd_[4 * index]; + m.pose.position.y = pcd_[4 * index + 1]; + m.pose.position.z = pcd_[4 * index + 2]; + + // calculating the angle of the normal with the world + tf::Vector3 axis_vector(x, y, z); + tf::Vector3 vector(1.0, 0.0, 0.0); + tf::Vector3 right_vector = axis_vector.cross(vector); + right_vector.normalized(); + tf::Quaternion q(right_vector, -1.0*acos(axis_vector.dot(vector))); + q.normalize(); + + m.pose.orientation.x = q.x(); + m.pose.orientation.y = q.y(); + m.pose.orientation.z = q.z(); + m.pose.orientation.w = q.w(); + + m_array.markers.push_back(m); + } + } + } + this->lock_.unlock(); + this->normal_pub_.publish(m_array); + } + } +} + //////////////////////////////////////////////////////////////////////////////// // Put camera data to the interface void GazeboRosDepthCamera::FillPointdCloud(const float *_src) diff --git a/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world b/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world index 522a74209..b9808a7aa 100644 --- a/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world +++ b/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world @@ -104,18 +104,24 @@ 480 R8G8B8 - - - + + 0.1 + 100 + + + points;normals + 1 10.0 image_raw points + normals camera_info depth_cam /base_link + 50 0.001 0.00000001 0.00000001