diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 1bca25ae..47872edb 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -23,7 +23,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_base.cpp src/pointcloud_to_laserscan_nodelet.cpp) +add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp) target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES}) add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h similarity index 80% rename from pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h rename to pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h index 7e5b9ba5..366cebb8 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_base.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h @@ -38,8 +38,8 @@ * Author: Paul Bovbel */ -#ifndef POINTCLOUD_TO_LASERSCAN_ROS -#define POINTCLOUD_TO_LASERSCAN_ROS +#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET +#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET #include #include @@ -50,49 +50,45 @@ #include #include +#include namespace pointcloud_to_laserscan -{ -typedef pcl::PointXYZ Point; -typedef pcl::PointCloud PointCloud; +{ + typedef pcl::PointXYZ Point; + typedef pcl::PointCloud PointCloud; /** * Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot * pointcloud_to_laserscan implementation. */ -class PointCloudToLaserScanBase -{ -public: - + class PointCloudToLaserScanNodelet : public nodelet::Nodelet + { - PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh); - - ~PointCloudToLaserScanBase(); + public: + PointCloudToLaserScanNodelet() { }; -private: + private: + virtual void onInit(); - void cloudCb(const PointCloud::ConstPtr& cloud); + void cloudCb(const PointCloud::ConstPtr &cloud_msg); void connectCb(); void disconnectCb(); - - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; + ros::NodeHandle nh_, private_nh_; ros::Publisher pub_; ros::Subscriber sub_; - tf::TransformListener tf_; - boost::mutex connect_mutex_; + // ROS Parameters unsigned int input_queue_size_; std::string target_frame_; double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; bool use_inf_; -}; + }; -} // pointcloud_to_laserscan +} // pointcloud_to_laserscan -#endif +#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch index 3161f0a9..48c0ef47 100644 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -12,12 +12,27 @@ - - - - + + target_frame: base_link + min_height: 0.0 + max_height: 1.0 + + angle_min: -1.5708 # -M_PI/2 + angle_max: 1.5708 # M_PI/2 + angle_increment: 0.087 # M_PI/360.0 + scan_time: 0.3333 + range_min: 0.45 + range_max: 4.0 + use_inf: true + + # Concurrency level, affects number of pointclouds queued for processing and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + concurrency_level: 0 + diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch index d8b00f99..84943f55 100644 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -12,12 +12,27 @@ - - - - + + target_frame: base_link + min_height: 0.0 + max_height: 1.0 + + angle_min: -1.5708 # -M_PI/2 + angle_max: 1.5708 # M_PI/2 + angle_increment: 0.087 # M_PI/360.0 + scan_time: 0.3333 + range_min: 0.45 + range_max: 4.0 + use_inf: true + + # Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + concurrency_level: 0 + diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml index f957d2ca..63420f17 100644 --- a/pointcloud_to_laserscan/package.xml +++ b/pointcloud_to_laserscan/package.xml @@ -14,6 +14,7 @@ https://github.com/ros-perception/perception_pcl catkin + dynamic_reconfigure libpcl-all-dev message_filters @@ -22,6 +23,7 @@ roscpp roslaunch sensor_msgs + dynamic_reconfigure libpcl-all-dev message_filters diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp deleted file mode 100644 index 40d2d840..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_base.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010-2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * - */ - -/* - * Author: Paul Bovbel - */ - -#include -#include -#include -#include - -namespace pointcloud_to_laserscan -{ - - PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh) : - nh_(nh), private_nh_(private_nh) - { - - 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); - - bool use_concurrency; - private_nh_.param("use_concurrency", use_concurrency, true); - private_nh_.param("use_inf", use_inf_, true); - - boost::mutex::scoped_lock lock(connect_mutex_); - - //Only allow #threads pointclouds to wait in queue - if(use_concurrency){ - input_queue_size_ = boost::thread::hardware_concurrency(); - }else{ - input_queue_size_ = 1; - } - - pub_ = nh.advertise("scan", 10, - boost::bind(&PointCloudToLaserScanBase::connectCb, this), - boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); - - - } - - PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ - sub_.shutdown(); - } - - - void PointCloudToLaserScanBase::connectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (!sub_ && pub_.getNumSubscribers() > 0) { - ROS_DEBUG("Connecting to depth topic."); - sub_ = nh_.subscribe("cloud_in", input_queue_size_, &PointCloudToLaserScanBase::cloudCb, this); - } - } - - void PointCloudToLaserScanBase::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - ROS_DEBUG("Unsubscribing from depth topic."); - sub_.shutdown(); - } - } - - void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud_msg){ - - //pointer to pointcloud data to transform to laserscan - PointCloud::ConstPtr cloud_scan; - - std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); - - //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_; - - //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{ - ROS_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); - - //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); - } - - 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) ){ - //ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z); - continue; - } - - if (z > max_height_ || z < min_height_){ - //ROS_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_){ - //ROS_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){ - //ROS_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); - - } - -} // pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp index b8acb363..7eb7c219 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -39,24 +39,27 @@ */ #include -#include +#include int main(int argc, char **argv){ - ros::init(argc, argv, "pointcloud_to_laserscan"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - bool use_concurrency; + ros::init(argc, argv, "pointcloud_to_laserscan_node"); + ros::NodeHandle private_nh("~"); + int concurrency_level; + private_nh.param("concurrency_level", concurrency_level, 0); - private_nh.param("use_concurrency", use_concurrency, false); + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + std::string nodelet_name = ros::this_node::getName(); + nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv); - pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh); + boost::shared_ptr spinner; + if(concurrency_level) { + spinner.reset(new ros::MultiThreadedSpinner(concurrency_level)); + }else{ + spinner.reset(new ros::MultiThreadedSpinner()); + } + spinner->spin(); + return 0; - if(use_concurrency) { - ros::MultiThreadedSpinner spinner; - spinner.spin(); - }else{ - ros::spin(); - } - - return 0; } diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp index 2ce17dd6..de9ec102 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp @@ -38,39 +38,150 @@ * Author: Paul Bovbel */ -#include +#include #include - +#include +#include +#include +#include namespace pointcloud_to_laserscan { - class PointCloudToLaserScanNodelet : public nodelet::Nodelet - { - public: - PointCloudToLaserScanNodelet() {}; + void PointCloudToLaserScanNodelet::onInit() + { + nh_ = getMTPrivateNodeHandle(); + private_nh_ = getMTPrivateNodeHandle(); - ~PointCloudToLaserScanNodelet() {} + 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: - virtual void onInit() - { - bool use_concurrency; - getPrivateNodeHandle().param("use_concurrency", use_concurrency, false); + 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); - if(use_concurrency){ - cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle())); - }else{ - cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle())); - } + 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::shared_ptr cloud_to_scan; - }; + // 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)); + } + + void PointCloudToLaserScanNodelet::cloudCb(const PointCloud::ConstPtr &cloud_msg) + { + //pointer to pointcloud data to transform to laserscan + PointCloud::ConstPtr cloud_scan; + + std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud_msg->header); + + //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_; + + //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); + + //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); + } + + 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; + } + } + pub_.publish(output); + } + + 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); + } + } + + void PointCloudToLaserScanNodelet::disconnectCb() + { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + NODELET_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } + } } -#include PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);