Added motion compensation feature to rslidar_pointcloud

rslidar_pointcloud/CMakeLists.txt

@@ -5,6 +5,7 @@ add_compile_options(-std=c++11)
 set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo
 set(${PROJECT_NAME}_CATKIN_DEPS
     angles
+    eigen_conversions
     nodelet
     pcl_ros
     roscpp

rslidar_pointcloud/launch/rs_bpearl.launch

@@ -23,6 +23,9 @@
     <param name="min_distance" value="0.1"/>
     <param name="resolution_type" value="0.5cm"/>
     <param name="intensity_mode" value="3"/>
+    <param name="compensate_motion" value="false"/>
+    <param name="fixed_frame" value="odom"/>
+    <param name="tf_wait_time" value="0.02"/>
   </node>
 
   <node name="rviz" pkg="rviz" type="rviz"  args="-d $(find rslidar_pointcloud)/rviz_cfg/rslidar.rviz" />

rslidar_pointcloud/package.xml

@@ -31,9 +31,11 @@
   <build_depend>roslaunch</build_depend>
   <build_depend>rostest</build_depend>
   <build_depend>tf2_ros</build_depend>
+  <build_depend>eigen_conversions</build_depend>
 
   <run_depend>angles</run_depend>
   <run_depend>nodelet</run_depend>
+  <run_depend>eigen_conversions</run_depend>
   <run_depend>pcl_ros</run_depend>
   <run_depend>pluginlib</run_depend>
   <!-- <run_depend>python-yaml</run_depend> -->

rslidar_pointcloud/src/convert.cc

@@ -15,16 +15,33 @@
 */
 #include "convert.h"
 #include <pcl_conversions/pcl_conversions.h>
+#include <eigen_conversions/eigen_msg.h>
 
 namespace rslidar_pointcloud
 {
 std::string model;
 
 /** @brief Constructor. */
-Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new rslidar_rawdata::RawData())
+Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) :
+    data_(new rslidar_rawdata::RawData()),
+    tf_buffer_(),
+    tf_listener_(tf_buffer_)
 {
   data_->loadConfigFile(node, private_nh);  // load lidar parameters
   private_nh.param("model", model, std::string("RS16"));
+  private_nh.param("compensate_motion", compensate_motion_, false);
+  private_nh.param("fixed_frame", fixed_frame_, std::string(""));
+  private_nh.param("tf_wait_time", tf_wait_time_, 0.02);
+
+  ROS_INFO_STREAM("[cloud][convert] compensate motion: " << compensate_motion_);
+  if (compensate_motion_) {
+    if (fixed_frame_.empty()) {
+      ROS_ERROR_STREAM("[cloud][convert] Motion compensation enabled but got empty fixed frame id");
+    } else {
+      ROS_INFO_STREAM("[cloud][convert] fixed frame: " << fixed_frame_);
+      ROS_INFO_STREAM("[cloud][convert] Tf wait time: " << tf_wait_time_);
+    }
+  }
 
   // advertise output point cloud (before subscribing to input data)
   std::string output_points_topic;
@@ -74,9 +91,31 @@ void Convert::processScan(const rslidar_msgs::rslidarScan::ConstPtr& scanMsg)
   // process each packet provided by the driver
 
   data_->block_num = 0;
+  geometry_msgs::TransformStamped tf_comp_stamped;
+  Eigen::Affine3d tf_comp_eigen;
   for (size_t i = 0; i < scanMsg->packets.size(); ++i)
   {
-    data_->unpack(scanMsg->packets[i], outPoints);
+    if (compensate_motion_) {
+      // Set transform to identity, in case transform lookup fails.
+      tf_comp_eigen.setIdentity();
+      try{
+        tf_comp_stamped = tf_buffer_.lookupTransform(scanMsg->header.frame_id, // Target frame.
+                                                     scanMsg->header.stamp,  // Target time.
+                                                     scanMsg->header.frame_id, // Source frame.
+                                                     scanMsg->packets[i].stamp,  // Source time.
+                                                     fixed_frame_, // Fixed frame.
+                                                     ros::Duration(tf_wait_time_));  // Wait time for transform.
+        ROS_DEBUG_STREAM(tf_comp_stamped);
+        tf::transformMsgToEigen(tf_comp_stamped.transform, tf_comp_eigen);
+      }
+      catch (tf2::TransformException &ex) {
+        ROS_WARN("%s",ex.what());
+      }
+      data_->unpack(scanMsg->packets[i], outPoints, tf_comp_eigen.cast<float>());
+    } else {
+      data_->unpack(scanMsg->packets[i], outPoints);
+    }
+
   }
   sensor_msgs::PointCloud2 outMsg;
   pcl::toROSMsg(*outPoints, outMsg);

rslidar_pointcloud/src/convert.h

@@ -44,6 +44,13 @@ class Convert
   boost::shared_ptr<rslidar_rawdata::RawData> data_;
   ros::Subscriber rslidar_scan_;
   ros::Publisher output_;
+
+  // Motion compensation.
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  bool compensate_motion_;
+  std::string fixed_frame_;
+  double tf_wait_time_;
 };
 
 }  // namespace rslidar_pointcloud

rslidar_pointcloud/src/rawdata.cc

@@ -827,7 +827,9 @@ int RawData::estimateTemperature(float Temper)
  *  @param pkt raw packet to unpack
  *  @param pc shared pointer to point cloud (points are appended)
  */
-void RawData::unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud)
+void RawData::unpack(const rslidar_msgs::rslidarPacket& pkt,
+                     pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud,
+                     const Eigen::Affine3f& tf_comp)
 {
   // check pkt header
   if (pkt.data[0] != 0x55 || pkt.data[1] != 0xAA || pkt.data[2] != 0x05 || pkt.data[3] != 0x0A)
@@ -837,7 +839,7 @@ void RawData::unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl
 
   if (numOfLasers == 32)
   {
-    unpack_RS32(pkt, pointcloud);
+    unpack_RS32(pkt, pointcloud, tf_comp);
     return;
   }
   float azimuth;  // 0.01 dgree
@@ -952,14 +954,17 @@ void RawData::unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl
                     Rx_ * this->sin_lookup_table_[arg_horiz_orginal];
           point.z = distance2 * this->sin_lookup_table_[arg_vert] + Rz_;
           point.intensity = intensity;
+          point = pcl::transformPoint(point, tf_comp);
           pointcloud->at(2 * this->block_num + firing, dsr) = point;
         }
       }
     }
   }
 }
 
-void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud)
+void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt,
+                          pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud,
+                          const Eigen::Affine3f& tf_comp)
 {
   float azimuth;  // 0.01 dgree
   float intensity;
@@ -1076,6 +1081,7 @@ void RawData::unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointClou
                     Rx_ * this->sin_lookup_table_[arg_horiz_orginal];
           point.z = distance2 * this->sin_lookup_table_[arg_vert] + Rz_;
           point.intensity = intensity;
+          point = pcl::transformPoint(point, tf_comp);
           pointcloud->at(this->block_num, dsr) = point;
         }
       }

rslidar_pointcloud/src/rawdata.h

@@ -136,10 +136,14 @@ class RawData
   void loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh);
 
   /*unpack the RS16 UDP packet and opuput PCL PointXYZI type*/
-  void unpack(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud);
+  void unpack(const rslidar_msgs::rslidarPacket& pkt,
+              pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud,
+              const Eigen::Affine3f& tf_comp = Eigen::Affine3f::Identity());
 
   /*unpack the RS32 UDP packet and opuput PCL PointXYZI type*/
-  void unpack_RS32(const rslidar_msgs::rslidarPacket& pkt, pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud);
+  void unpack_RS32(const rslidar_msgs::rslidarPacket& pkt,
+                   pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud,
+                   const Eigen::Affine3f& tf_comp = Eigen::Affine3f::Identity());
 
   /*compute temperature*/
   float computeTemperature(unsigned char bit1, unsigned char bit2);