From 3fb881f8eba20cfd17a41e4e654833a054ca3913 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 15 Jan 2015 15:22:48 -0500 Subject: [PATCH] Refactor with tf2 and message filters --- pointcloud_to_laserscan/CMakeLists.txt | 8 +- .../pointcloud_to_laserscan_nodelet.h | 32 +-- pointcloud_to_laserscan/package.xml | 2 + .../src/pointcloud_to_laserscan_nodelet.cpp | 218 +++++++++--------- 4 files changed, 135 insertions(+), 125 deletions(-) diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 37e27e26..395b8b4e 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/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( diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index 366cebb8..c15bf8f2 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ b/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 -#include +#include "ros/ros.h" +#include "boost/thread/mutex.hpp" -#include -#include -#include -#include +#include "nodelet/nodelet.h" +#include "tf2/buffer_core.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.h" +#include "message_filters/subscriber.h" +#include "sensor_msgs/PointCloud2.h" -#include -#include namespace pointcloud_to_laserscan { - typedef pcl::PointXYZ Point; - typedef pcl::PointCloud PointCloud; - + typedef message_filters::Subscriber FilteredSub; + typedef tf2_ros::MessageFilter 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 sub_; + boost::shared_ptr message_filter_; + // ROS Parameters unsigned int input_queue_size_; std::string target_frame_; diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index cb6cb0b8..d601cd6b 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -24,6 +24,7 @@ roslaunch sensor_msgs tf2 + tf2_ros dynamic_reconfigure libpcl-all-dev @@ -33,6 +34,7 @@ roscpp sensor_msgs tf2 + tf2_ros diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 0c63065f..96e8b5c9 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -39,147 +39,155 @@ */ #include -#include #include -#include -#include #include +#include +#include namespace pointcloud_to_laserscan { + PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() + : tf2_(), tf2_listener_(tf2_) + { } + + void PointCloudToLaserScanNodelet::onInit() { - nh_ = getMTNodeHandle(); - private_nh_ = getMTPrivateNodeHandle(); + nh_ = getMTNodeHandle(); + private_nh_ = getMTPrivateNodeHandle(); - private_nh_.param("target_frame", target_frame_, ""); - private_nh_.param("min_height", min_height_, 0.0); - private_nh_.param("max_height", max_height_, 1.0); + private_nh_.param("target_frame", target_frame_, ""); + private_nh_.param("min_height", min_height_, 0.0); + private_nh_.param("max_height", max_height_, 1.0); - private_nh_.param("angle_min", angle_min_, -M_PI/2.0); - private_nh_.param("angle_max", angle_max_, M_PI/2.0); - private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); - private_nh_.param("scan_time", scan_time_, 1.0/30.0); - private_nh_.param("range_min", range_min_, 0.45); - private_nh_.param("range_max", range_max_, 4.0); + private_nh_.param("angle_min", angle_min_, -M_PI/2.0); + private_nh_.param("angle_max", angle_max_, M_PI/2.0); + private_nh_.param("angle_increment", angle_increment_, M_PI/360.0); + private_nh_.param("scan_time", scan_time_, 1.0/30.0); + private_nh_.param("range_min", range_min_, 0.45); + private_nh_.param("range_max", range_max_, 4.0); - int concurrency_level; - private_nh_.param("concurrency_level", concurrency_level, true); - private_nh_.param("use_inf", use_inf_, true); + int concurrency_level; + private_nh_.param("concurrency_level", concurrency_level, true); + private_nh_.param("use_inf", use_inf_, true); - boost::mutex::scoped_lock lock(connect_mutex_); + boost::mutex::scoped_lock lock(connect_mutex_); - // Only queue one pointcloud per running thread - if(concurrency_level > 0) - { - input_queue_size_ = concurrency_level; - }else{ - input_queue_size_ = boost::thread::hardware_concurrency(); - } + // Only queue one pointcloud per running thread + if(concurrency_level > 0) + { + input_queue_size_ = concurrency_level; + }else{ + input_queue_size_ = boost::thread::hardware_concurrency(); + } - pub_ = nh_.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), - boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); + pub_ = nh_.advertise("scan", 10, + boost::bind(&PointCloudToLaserScanNodelet::connectCb, this), + boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this)); } void PointCloudToLaserScanNodelet::connectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - NODELET_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanNodelet::cloudCb, this); + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud"); + 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)); } + } } void PointCloudToLaserScanNodelet::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - NODELET_DEBUG("Unsubscribing from depth topic."); - sub_.shutdown(); + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud"); + if(!target_frame_.empty()) + { + message_filter_.reset(); } + sub_.reset(); + } } - void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg) + void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) { - //pointer to pointcloud data to transform to laserscan - PointCloud::ConstPtr cloud_scan; + if(target_frame_.empty()){ + target_frame_ = cloud_msg->header.frame_id; + } - std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); + //build laserscan output + sensor_msgs::LaserScan output; + 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_; + output.time_increment = 0.0; + output.scan_time = scan_time_; + output.range_min = range_min_; + output.range_max = range_max_; - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_header; - output.angle_min = angle_min_; - output.angle_max = angle_max_; - output.angle_increment = angle_increment_; - output.time_increment = 0.0; - output.scan_time = scan_time_; - output.range_min = range_min_; - output.range_max = range_max_; + //determine amount of rays to create + uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - //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_; + //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range + if(use_inf_){ + output.ranges.assign(ranges_size, std::numeric_limits::infinity()); + }else{ + output.ranges.assign(ranges_size, output.range_max + 1.0); + } - 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; + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud_msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud_msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud_msg, "z"); + + geometry_msgs::Point32 point; + 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); } - //determine amount of rays to create - uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment); - - //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range - if(use_inf_){ - output.ranges.assign(ranges_size, std::numeric_limits::infinity()); - }else{ - output.ranges.assign(ranges_size, output.range_max + 1.0); + 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; } - for (PointCloud::const_iterator it = cloud_scan->begin(); it != cloud_scan->end(); ++it){ - const float &x = it->x; - const float &y = it->y; - const float &z = it->z; - - if ( std::isnan(x) || std::isnan(y) || std::isnan(z) ){ - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); - continue; - } - - if (z > max_height_ || z < min_height_){ - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_); - continue; - } - - double range = hypot(x,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); - continue; - } - - double angle = atan2(y, 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; - } - - //overwrite range at laserscan ray if new range is smaller - int index = (angle - output.angle_min) / output.angle_increment; - if (range < output.ranges[index]){ - output.ranges[index] = range; - } + if (point.z > max_height_ || point.z < min_height_){ + NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", point.z, min_height_, max_height_); + continue; } - pub_.publish(output); + + 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_, point.x, point.y, point.z); + continue; + } + + 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; + } + + //overwrite range at laserscan ray if new range is smaller + int index = (angle - output.angle_min) / output.angle_increment; + if (range < output.ranges[index]){ + output.ranges[index] = range; + } + + } + pub_.publish(output); } }