Fix pointcloud to laserscan transform tolerance issues

Author: Paul Bovbel

include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h

@@ -46,6 +46,7 @@
 
 #include "nodelet/nodelet.h"
 #include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
 #include "tf2_ros/message_filter.h"
 #include "message_filters/subscriber.h"
 #include "sensor_msgs/PointCloud2.h"
@@ -79,7 +80,8 @@ namespace pointcloud_to_laserscan
     ros::Publisher pub_;
     boost::mutex connect_mutex_;
 
-    tf2_ros::Buffer tf2_;
+    boost::shared_ptr<tf2_ros::Buffer> tf2_;
+    boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
     message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
     boost::shared_ptr<MessageFilter> message_filter_;
 

launch/sample_node.launch

@@ -15,7 +15,7 @@
         <remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
         <remap from="scan" to="$(arg camera)/scan"/>
         <rosparam>
-            target_frame: camera_link # Leave empty to output scan in the pointcloud frame
+            target_frame: camera_link # Leave disabled to output scan in pointcloud frame
             transform_tolerance: 0.01
             min_height: 0.0
             max_height: 1.0

launch/sample_nodelet.launch

@@ -15,7 +15,7 @@
         <remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
         <remap from="scan" to="$(arg camera)/scan"/>
         <rosparam>
-            target_frame: camera_link # Leave empty to output scan in the pointcloud frame
+            target_frame: camera_link # Leave disabled to output scan in pointcloud frame
             transform_tolerance: 0.01
             min_height: 0.0
             max_height: 1.0

package.xml

@@ -5,16 +5,17 @@
     <description>Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM).</description>
 
     <maintainer email="[email protected]">Paul Bovbel</maintainer>
+    <author email="[email protected]">Paul Bovbel</author>
     <author>Tully Foote</author>
 
     <license>BSD</license>
-    
+
     <url type="website">http://ros.org/wiki/perception_pcl</url>
     <url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
     <url type="repository">https://github.com/ros-perception/perception_pcl</url>
 
     <buildtool_depend>catkin</buildtool_depend>
-    
+
     <build_depend>message_filters</build_depend>
     <build_depend>nodelet</build_depend>
     <build_depend>roscpp</build_depend>
@@ -34,4 +35,4 @@
     <export>
         <nodelet plugin="${prefix}/nodelets.xml"/>
     </export>
-</package>
+</package>

src/pointcloud_to_laserscan_nodelet.cpp

@@ -55,7 +55,7 @@ namespace pointcloud_to_laserscan
     private_nh_ = getPrivateNodeHandle();
 
     private_nh_.param<std::string>("target_frame", target_frame_, "");
-    private_nh_.param<double>("tolerance", tolerance_, 0.01);
+    private_nh_.param<double>("transform_tolerance", tolerance_, 0.01);
     private_nh_.param<double>("min_height", min_height_, 0.0);
     private_nh_.param<double>("max_height", max_height_, 1.0);
 
@@ -93,8 +93,9 @@ namespace pointcloud_to_laserscan
     // if pointcloud target frame specified, we need to filter by transform availability
     if (!target_frame_.empty())
     {
-      message_filter_.reset(new MessageFilter(sub_, tf2_, target_frame_, input_queue_size_, nh_));
-      message_filter_->setTolerance(ros::Duration(tolerance_));
+      tf2_.reset(new tf2_ros::Buffer());
+      tf2_listener_.reset(new tf2_ros::TransformListener(*tf2_));
+      message_filter_.reset(new MessageFilter(sub_, *tf2_, target_frame_, input_queue_size_, nh_));
       message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
       message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
     }
@@ -132,7 +133,7 @@ namespace pointcloud_to_laserscan
                                                tf2_ros::filter_failure_reasons::FilterFailureReason reason)
   {
     NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
-        << message_filter_->getTargetFramesString() << " with tolerance " << tolerance_);
+        << message_filter_->getTargetFramesString());
   }
 
   void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
@@ -176,7 +177,7 @@ namespace pointcloud_to_laserscan
       try
       {
         cloud.reset(new sensor_msgs::PointCloud2);
-        tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
+        tf2_->transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
         cloud_out = cloud;
       }
       catch (tf2::TransformException ex)