[Gazebo 9] Created a MarkerArray to publish normals in ROS (ros-simul…

…ation#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 <ahcorde@gmail.com>

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})
 

gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h

@@ -35,6 +35,8 @@
 #include <sensor_msgs/fill_image.h>
 #include <std_msgs/Float64.h>
 #include <image_transport/image_transport.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/Marker.h>
 
 // gazebo stuff
 #include <sdf/Param.hh>
@@ -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
-

gazebo_plugins/package.xml

@@ -30,6 +30,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>trajectory_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>std_srvs</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>

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<std::string>();
 
+  // normals stuff
+  if (!_sdf->HasElement("normalsTopicName"))
+    this->normals_topic_name_ = "normals";
+  else
+    this->normals_topic_name_ = _sdf->GetElement("normalsTopicName")->Get<std::string>();
+
   // 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<double>();
 
+  if (!_sdf->HasElement("reduceNormals"))
+    this->reduce_normals_ = 50;
+  else
+    this->reduce_normals_ = _sdf->GetElement("reduceNormals")->Get<int>();
+
   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<visualization_msgs::MarkerArray >(
+      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)

gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world

@@ -104,18 +104,24 @@
                             <height>480</height>
                             <format>R8G8B8</format>
                         </image>
-                        <clip near="0.01" far="100.0" />
-                        <save enabled="false" path="/tmp" />
-                        <depth_camera output="points" />
+                        <clip>
+                          <near>0.1</near>
+                          <far>100</far>
+                        </clip>
+                        <depth_camera>
+                          <output>points;normals</output>
+                        </depth_camera>
                     </camera>
                     <plugin name="plugin_1" filename="libgazebo_ros_depth_camera.so">
                         <alwaysOn>1</alwaysOn>
                         <updateRate>10.0</updateRate>
                         <imageTopicName>image_raw</imageTopicName>
                         <pointCloudTopicName>points</pointCloudTopicName>
+                        <normalsTopicName>normals</normalsTopicName>
                         <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                         <cameraName>depth_cam</cameraName>
                         <frameName>/base_link</frameName>
+                        <reduceNormals>50</reduceNormals>
                         <pointCloudCutoff>0.001</pointCloudCutoff>
                         <distortionK1>0.00000001</distortionK1>
                         <distortionK2>0.00000001</distortionK2>