Created a MarkerArray to publish normals in ROS

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);
 
+    /// \brief Update the controller
+    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,11 @@ namespace gazebo
     private: void PointCloudConnect();
     private: void PointCloudDisconnect();
 
+    /// \brief Keep track of number of connctions for point clouds
+    private: int normals_connect_count_;
+    private: void NormalsConnect();
+    private: void NormalsDisconnect();
+
     /// \brief Keep track of number of connctions for point clouds
     private: int depth_image_connect_count_;
     private: void DepthImageConnect();
@@ -116,16 +128,22 @@ 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 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 +166,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,9 +45,11 @@ 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);
+
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -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";
@@ -135,8 +143,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 +171,24 @@ 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_--;
+  (*this->image_connect_count_)--;
+  if (this->normals_connect_count_ <= 0)
+    this->parentSensor->SetActive(false);
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 // Increment count
 void GazeboRosDepthCamera::DepthImageConnect()
@@ -250,6 +283,12 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
     if (this->point_cloud_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 +366,72 @@ 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;
+
+  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++)
+      {
+        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();
+
+        // plotting some of the normals, otherwise rviz will block it
+        if(index%50==0)
+          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,15 +104,23 @@
                             <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>
+                        <save enabled='0'>
+                          <path>__default__</path>
+                        </save>
+                        <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>