Refactor with tf2 and message filters

pointcloud_to_laserscan/CMakeLists.txt

@@ -2,21 +2,19 @@ cmake_minimum_required(VERSION 2.8.3)
 project(pointcloud_to_laserscan)
 
 find_package(catkin REQUIRED COMPONENTS
-  dynamic_reconfigure
   message_filters
   nodelet
-  pcl_ros
   roscpp
   sensor_msgs
-  roslaunch
   tf2
+  tf2_ros
+  geometry_msgs
 )
-find_package(PCL REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES pointcloud_to_laserscan
-  CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs
+  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
 )
 
 include_directories(

pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h

@@ -41,22 +41,21 @@
 #ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
 #define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
 
-#include <ros/ros.h>
+#include "ros/ros.h"
-#include <boost/thread/mutex.hpp>
+#include "boost/thread/mutex.hpp"
 
-#include <pcl/point_cloud.h>
+#include "nodelet/nodelet.h"
-#include <pcl_ros/point_cloud.h>
+#include "tf2/buffer_core.h"
-#include <pcl/point_types.h>
+#include "tf2_ros/transform_listener.h"
-#include <pcl/ros/conversions.h>
+#include "tf2_ros/message_filter.h"
+#include "message_filters/subscriber.h"
+#include "sensor_msgs/PointCloud2.h"
 
-#include <tf/transform_listener.h>
-#include <nodelet/nodelet.h>
 
 namespace pointcloud_to_laserscan
 {
-  typedef pcl::PointXYZ Point;
+  typedef message_filters::Subscriber<sensor_msgs::PointCloud2> FilteredSub;
-  typedef pcl::PointCloud<Point> PointCloud;
+  typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
-
 /**
 * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
 * pointcloud_to_laserscan implementation.
@@ -65,12 +64,12 @@ namespace pointcloud_to_laserscan
   {
 
   public:
-    PointCloudToLaserScanNodelet() { };
+    PointCloudToLaserScanNodelet();
 
   private:
     virtual void onInit();
 
-    void cloudCb(const PointCloud::ConstPtr &cloud_msg);
+    void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
 
     void connectCb();
 
@@ -78,10 +77,13 @@ namespace pointcloud_to_laserscan
 
     ros::NodeHandle nh_, private_nh_;
     ros::Publisher pub_;
-    ros::Subscriber sub_;
-    tf::TransformListener tf_;
     boost::mutex connect_mutex_;
 
+    tf2_ros::Buffer tf2_;
+    tf2_ros::TransformListener tf2_listener_;
+    boost::shared_ptr<FilteredSub> sub_;
+    boost::shared_ptr<MessageFilter> message_filter_;
+
     // ROS Parameters
     unsigned int input_queue_size_;
     std::string target_frame_;

pointcloud_to_laserscan/package.xml

@@ -24,6 +24,7 @@
     <build_depend>roslaunch</build_depend>
     <build_depend>sensor_msgs</build_depend>
     <build_depend>tf2</build_depend>
+    <build_depend>tf2_ros</build_depend>
 
     <run_depend>dynamic_reconfigure</run_depend>
     <run_depend>libpcl-all-dev</run_depend>
@@ -33,6 +34,7 @@
     <run_depend>roscpp</run_depend>
     <run_depend>sensor_msgs</run_depend>
     <run_depend>tf2</run_depend>
+    <run_depend>tf2_ros</run_depend>
 
     <export>
         <nodelet plugin="${prefix}/nodelets.xml"/>

pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp

@@ -39,15 +39,19 @@
  */
 
 #include <pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h>
-#include <nodelet/nodelet.h>
 #include <sensor_msgs/LaserScan.h>
-#include <pcl_ros/transforms.h>
-#include <math.h>
 #include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <geometry_msgs/Point32.h>
 
 namespace pointcloud_to_laserscan
 {
 
+  PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet()
+      : tf2_(), tf2_listener_(tf2_)
+  { }
+
+
   void PointCloudToLaserScanNodelet::onInit()
   {
     nh_ = getMTNodeHandle();
@@ -87,8 +91,15 @@ namespace pointcloud_to_laserscan
   {
     boost::mutex::scoped_lock lock(connect_mutex_);
     if (!sub_ && pub_.getNumSubscribers() > 0) {
-          NODELET_DEBUG("Connecting to depth topic.");
+      NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud");
-          sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this);
+      sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_));
+      if(!target_frame_.empty())
+      {
+        message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_));
+        message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
+      }else{
+        sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
+      }
     }
   }
 
@@ -96,21 +107,25 @@ namespace pointcloud_to_laserscan
   {
     boost::mutex::scoped_lock lock(connect_mutex_);
     if (pub_.getNumSubscribers() == 0) {
-          NODELET_DEBUG("Unsubscribing from depth topic.");
+      NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud");
-          sub_.shutdown();
+      if(!target_frame_.empty())
-      }
-  }
-
-  void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg)
       {
-      //pointer to pointcloud data to transform to laserscan
+        message_filter_.reset();
-      PointCloud::ConstPtr cloud_scan;
+      }
+      sub_.reset();
+    }
+  }
 
-      std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header);
+  void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
+  {
+    if(target_frame_.empty()){
+      target_frame_ = cloud_msg->header.frame_id;
+    }
 
     //build laserscan output
     sensor_msgs::LaserScan output;
-      output.header = cloud_header;
+    output.header = cloud_msg->header;
+    output.header.frame_id = target_frame_;
     output.angle_min = angle_min_;
     output.angle_max = angle_max_;
     output.angle_increment = angle_increment_;
@@ -119,23 +134,6 @@ namespace pointcloud_to_laserscan
     output.range_min = range_min_;
     output.range_max = range_max_;
 
-      //decide if pointcloud needs to be transformed to a target frame
-      if(!target_frame_.empty() && cloud_header.frame_id != target_frame_){
-          output.header.frame_id = target_frame_;
-
-          if(tf_.waitForTransform(cloud_header.frame_id, target_frame_, cloud_header.stamp, ros::Duration(10.0))){
-              PointCloud::Ptr cloud_tf(new PointCloud);
-              pcl_ros::transformPointCloud(target_frame_, *cloud_msg, *cloud_tf, tf_);
-              cloud_scan = cloud_tf;
-          }else{
-              NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform cloud with frame " << cloud_header.frame_id << " into "
-                  "lasercan with frame " << target_frame_);
-              return;
-          }
-      }else{
-          cloud_scan = cloud_msg;
-      }
-
     //determine amount of rays to create
     uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
 
@@ -146,28 +144,37 @@ namespace pointcloud_to_laserscan
       output.ranges.assign(ranges_size, output.range_max + 1.0);
     }
 
-      for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x");
-          const float &x = it->x;
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud_msg, "y");
-          const float &y = it->y;
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud_msg, "z");
-          const float &z = it->z;
 
-          if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){
+    geometry_msgs::Point32 point;
-              NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
+    for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
+
+      point.x = *iter_x;
+      point.y = *iter_y;
+      point.z = *iter_z;
+      if(!(output.header.frame_id == cloud_msg->header.frame_id)){
+        point = tf2_.transform(point, output.header.frame_id, output.header.stamp, cloud_msg->header.frame_id);
+      }
+
+      if ( std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z) ){
+        NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", point.x, point.y, point.z);
         continue;
       }
 
-          if (z > max_height_ || z < min_height_){
+      if (point.z > max_height_ || point.z < min_height_){
-              NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_);
+        NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", point.z, min_height_, max_height_);
         continue;
       }
 
-          double range = hypot(x,y);
+      double range = hypot(point.x,point.y);
       if (range < range_min_){
-              NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, x, y, z);
+        NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, point.x, point.y, point.z);
         continue;
       }
 
-          double angle = atan2(y, x);
+      double angle = atan2(point.y, point.x);
       if (angle < output.angle_min || angle > output.angle_max){
         NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
         continue;
@@ -178,6 +185,7 @@ namespace pointcloud_to_laserscan
       if (range < output.ranges[index]){
         output.ranges[index] = range;
       }
+
     }
     pub_.publish(output);
   }