added methods to publish the pointcloud information

Author: saikishor

include/realsense_gazebo_plugin/RealSensePlugin.h

@@ -126,7 +126,7 @@ class RealSensePlugin : public ModelPlugin {
 
   bool pointCloud_ = false;
   std::string pointCloudTopic_;
-  double pointCloudCutOff_;
+  double pointCloudCutOff_, pointCloudCutOffMax_;
 
   double colorUpdateRate_;
   double infraredUpdateRate_;

include/realsense_gazebo_plugin/gazebo_ros_realsense.h

@@ -6,6 +6,7 @@
 #include <ros/ros.h>
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/image_encodings.h>
 
 #include <camera_info_manager/camera_info_manager.h>
@@ -34,6 +35,10 @@ class GazeboRosRealsense : public RealSensePlugin {
 public:
   virtual void OnNewDepthFrame();
 
+  /// \brief Helper function to fill the pointcloud information
+  bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg,
+                            uint32_t cols_arg, uint32_t step_arg, void *data_arg);
+
   /// \brief Callback that publishes a received Camera Frame as an
   /// ImageStamped message.
 public:
@@ -51,13 +56,15 @@ class GazeboRosRealsense : public RealSensePlugin {
 
 private:
   image_transport::ImageTransport *itnode_;
+  ros::Publisher pointcloud_pub_;
 
 protected:
   image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_;
 
   /// \brief ROS image messages
 protected:
   sensor_msgs::Image image_msg_, depth_msg_;
+  sensor_msgs::PointCloud2 pointcloud_msg_;
 };
 }
 #endif /* _GAZEBO_ROS_REALSENSE_PLUGIN_ */

src/RealSensePlugin.cpp

@@ -35,6 +35,7 @@ RealSensePlugin::RealSensePlugin() {
   this->ired2Cam = nullptr;
   this->colorCam = nullptr;
   this->prefix = "";
+  this->pointCloudCutOffMax_ = 5.0;
 }
 
 /////////////////////////////////////////////////
@@ -109,6 +110,8 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
       pointCloudTopic_ = _sdf->GetValue()->GetAsString();
     else if (name == "pointCloudCutoff")
       _sdf->GetValue()->Get(pointCloudCutOff_);
+    else if (name == "pointCloudCutoffMax")
+      _sdf->GetValue()->Get(pointCloudCutOffMax_);
     else if (name == "prefix")
       this->prefix = _sdf->GetValue()->GetAsString();
     else if (name == "robotNamespace")

src/gazebo_ros_realsense.cpp

@@ -1,5 +1,6 @@
 #include "realsense_gazebo_plugin/gazebo_ros_realsense.h"
 #include <sensor_msgs/fill_image.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
 
 namespace {
 std::string extractCameraName(const std::string &name);
@@ -47,6 +48,11 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
       cameraParamsMap_[IRED2_CAMERA_NAME].topic_name, 2);
   this->depth_pub_ = this->itnode_->advertiseCamera(
       cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name, 2);
+  if (pointCloud_)
+  {
+    this->pointcloud_pub_ =
+        this->rosnode_->advertise<sensor_msgs::PointCloud2>(pointCloudTopic_, 2, false);
+  }
 }
 
 void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam,
@@ -93,6 +99,99 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam,
   image_pub->publish(this->image_msg_, camera_info_msg);
 }
 
+// Referenced from gazebo_plugins
+// https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp#L302
+// Fill depth information
+bool GazeboRosRealsense::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");
+  // convert to flat array shape, we need to reconvert later
+  pcd_modifier.resize(rows_arg * cols_arg);
+  point_cloud_msg.is_dense = true;
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg_, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg_, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(pointcloud_msg_, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(pointcloud_msg_, "rgb");
+
+  float *toCopyFrom = (float *)data_arg;
+  int index = 0;
+
+  double hfov = this->depthCam->HFOV().Radian();
+  double fl = ((double)this->depthCam->ImageWidth()) / (2.0 * tan(hfov / 2.0));
+
+  // convert depth to point cloud
+  for (uint32_t j = 0; j < rows_arg; j++)
+  {
+    double pAngle;
+    if (rows_arg > 1)
+      pAngle = atan2((double)j - 0.5 * (double)(rows_arg - 1), fl);
+    else
+      pAngle = 0.0;
+
+    for (uint32_t i = 0; i < cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb)
+    {
+      double yAngle;
+      if (cols_arg > 1)
+        yAngle = atan2((double)i - 0.5 * (double)(cols_arg - 1), fl);
+      else
+        yAngle = 0.0;
+
+      double depth = toCopyFrom[index++];  // + 0.0*this->myParent->GetNearClip();
+
+      if (depth > pointCloudCutOff_ && depth < pointCloudCutOffMax_)
+      {
+        // 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);
+        *iter_z = depth;
+      }
+      else  // point in the unseeable range
+      {
+        *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::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;
+      }
+    }
+  }
+
+  // reconvert to original height and width after the flat reshape
+  point_cloud_msg.height = rows_arg;
+  point_cloud_msg.width = cols_arg;
+  point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width;
+
+  return true;
+}
+
 void GazeboRosRealsense::OnNewDepthFrame() {
   // get current time
   common::Time current_time = this->world->SimTime();
@@ -118,6 +217,19 @@ void GazeboRosRealsense::OnNewDepthFrame() {
   auto depth_info_msg =
       cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian());
   this->depth_pub_.publish(this->depth_msg_, depth_info_msg);
+
+  if (pointCloud_ && this->pointcloud_pub_.getNumSubscribers() > 0)
+  {
+    this->pointcloud_msg_.header = this->depth_msg_.header;
+    this->pointcloud_msg_.width = this->depthCam->ImageWidth();
+    this->pointcloud_msg_.height = this->depthCam->ImageHeight();
+    this->pointcloud_msg_.row_step =
+        this->pointcloud_msg_.point_step * this->depthCam->ImageWidth();
+    FillPointCloudHelper(this->pointcloud_msg_, this->depthCam->ImageHeight(),
+                         this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(),
+                         (void *)this->depthCam->DepthData());
+    this->pointcloud_pub_.publish(this->pointcloud_msg_);
+  }
 }
 }