From dc1cf04b07b4064cd14f19728e78129caab4dc79 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Sun, 5 Oct 2014 14:55:00 -0400 Subject: [PATCH] Finalize pointcloud to laserscan --- pointcloud_to_laserscan/CMakeLists.txt | 80 ++------ .../PointCloudToLaserScanBase.h | 72 ++++--- .../launch/sample_node.launch | 24 +++ .../launch/sample_nodelet.launch | 24 +++ pointcloud_to_laserscan/nodelets.xml | 11 + .../src/PointCloudToLaserScanBase.cpp | 193 +++++++++++++++--- .../src/PointCloudToLaserScanNodelet.cpp | 76 +++++++ .../src/pointcloud_to_laserscan_node.cpp | 54 ++++- 8 files changed, 408 insertions(+), 126 deletions(-) create mode 100644 pointcloud_to_laserscan/launch/sample_node.launch create mode 100644 pointcloud_to_laserscan/launch/sample_nodelet.launch create mode 100644 pointcloud_to_laserscan/nodelets.xml diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt index 6018a529..076adb62 100644 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ b/pointcloud_to_laserscan/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(pointcloud_to_laserscan) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_filters @@ -14,21 +11,10 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(PCL REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES pointcloud_to_laserscan -# CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs -# DEPENDS system_lib + LIBRARIES PointCloudToLaserScan PointCloudToLaserScanNodelet + CATKIN_DEPENDS dynamic_reconfigure libpcl-all-dev message_filters nodelet pcl_ros roscpp sensor_msgs ) include_directories( @@ -37,58 +23,22 @@ include_directories( ) add_library(PointCloudToLaserScan src/PointCloudToLaserScanBase.cpp) -#add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg) target_link_libraries(PointCloudToLaserScan ${catkin_LIBRARIES}) +add_library(PointCloudToLaserScanNodelet src/PointCloudToLaserScanNodelet.cpp) +target_link_libraries(PointCloudToLaserScanNodelet PointCloudToLaserScan ${catkin_LIBRARIES}) + add_executable(pointcloud_to_laserscan src/pointcloud_to_laserscan_node.cpp) -#add_dependencies(pointcloud_to_laserscan pointcloud_to_laserscan_generate_messages_cpp) target_link_libraries(pointcloud_to_laserscan PointCloudToLaserScan ${catkin_LIBRARIES}) +install(TARGETS PointCloudToLaserScan PointCloudToLaserScanNodelet pointcloud_to_laserscan + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_pointcloud_to_laserscan.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h index 9bc12ffd..7e5b9ba5 100644 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h +++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/PointCloudToLaserScanBase.h @@ -1,34 +1,41 @@ /* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. + * Software License Agreement (BSD License) * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * 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. * - * * 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 the 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: Chad Rockey +/* + * Author: Paul Bovbel */ #ifndef POINTCLOUD_TO_LASERSCAN_ROS @@ -47,15 +54,18 @@ namespace pointcloud_to_laserscan { typedef pcl::PointXYZ Point; -typedef pcl::PointCloud PointCloud; - +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: - PointCloudToLaserScanBase(ros::NodeHandle& n, ros::NodeHandle& pnh, const unsigned int concurrency); + PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh); ~PointCloudToLaserScanBase(); @@ -76,13 +86,13 @@ private: tf::TransformListener tf_; boost::mutex connect_mutex_; - const unsigned int concurrency_; + unsigned int input_queue_size_; std::string target_frame_; - float tolerance_; + double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; + bool use_inf_; }; - } // pointcloud_to_laserscan #endif diff --git a/pointcloud_to_laserscan/launch/sample_node.launch b/pointcloud_to_laserscan/launch/sample_node.launch new file mode 100644 index 00000000..91f7f186 --- /dev/null +++ b/pointcloud_to_laserscan/launch/sample_node.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch new file mode 100644 index 00000000..bd4372a7 --- /dev/null +++ b/pointcloud_to_laserscan/launch/sample_nodelet.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/pointcloud_to_laserscan/nodelets.xml b/pointcloud_to_laserscan/nodelets.xml new file mode 100644 index 00000000..c1143973 --- /dev/null +++ b/pointcloud_to_laserscan/nodelets.xml @@ -0,0 +1,11 @@ + + + + + Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans. + + + + diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp index 3374c9f1..8caaf208 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanBase.cpp @@ -1,44 +1,185 @@ +/* + * 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, const unsigned int concurrency) : - nh_(nh), private_nh_(private_nh), concurrency_(concurrency) { - boost::mutex::scoped_lock lock(connect_mutex_); - pub_ = nh.advertise("scan", 10, boost::bind(&PointCloudToLaserScanBase::connectCb, this), boost::bind(&PointCloudToLaserScanBase::disconnectCb, this)); - //todo params - target_frame_ = "base_link"; + PointCloudToLaserScanBase::PointCloudToLaserScanBase(ros::NodeHandle& nh, ros::NodeHandle& private_nh) : + nh_(nh), private_nh_(private_nh) + { -} + private_nh.param("target_frame", target_frame_, "base_link"); + private_nh_.param("min_height", min_height_, 0.0); + private_nh_.param("max_height", max_height_, 1.0); -PointCloudToLaserScanBase::~PointCloudToLaserScanBase(){ - sub_.shutdown(); -} + 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); -void PointCloudToLaserScanBase::cloudCb(const PointCloud::ConstPtr& cloud){ + 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)); -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", concurrency_, &PointCloudToLaserScanBase::cloudCb, this); } -} -void PointCloudToLaserScanBase::disconnectCb() { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) { - ROS_DEBUG("Unsubscribing from depth topic."); + 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_in_header = pcl_conversions::fromPCL(cloud_scan->header); + + //decide if pointcloud needs to be transformed to a target frame + if(!target_frame_.empty() && cloud_in_header.frame_id != target_frame_){ + bool found_transform = tf_.waitForTransform(cloud_in_header.frame_id, target_frame_, cloud_in_header.stamp, ros::Duration(10.0)); + if(found_transform){ + 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_in_header.frame_id << " into lasercan with frame " << target_frame_); + return; + } + }else{ + cloud_scan = cloud_msg; + } + + //build laserscan output + sensor_msgs::LaserScan output; + output.header = cloud_in_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_; + + 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 + 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); + + } } // pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp index e69de29b..b420ed1c 100644 --- a/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp +++ b/pointcloud_to_laserscan/src/PointCloudToLaserScanNodelet.cpp @@ -0,0 +1,76 @@ +/* + * 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 + + +namespace pointcloud_to_laserscan +{ + + class PointCloudToLaserScanNodelet : public nodelet::Nodelet + { + public: + PointCloudToLaserScanNodelet() {}; + + ~PointCloudToLaserScanNodelet() {} + + private: + virtual void onInit() + { + bool use_concurrency; + getPrivateNodeHandle().param("use_concurrency", use_concurrency, false); + + if(use_concurrency){ + cloud_to_scan.reset(new PointCloudToLaserScanBase(getMTNodeHandle(), getPrivateNodeHandle())); + }else{ + cloud_to_scan.reset(new PointCloudToLaserScanBase(getNodeHandle(), getPrivateNodeHandle())); + } + + }; + + boost::shared_ptr cloud_to_scan; + }; + +} + +#include +PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet); + diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp index 8c56f448..76661bea 100644 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp @@ -1,16 +1,62 @@ +/* + * 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 int main(int argc, char **argv){ ros::init(argc, argv, "pointcloud_to_laserscan"); ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); + ros::NodeHandle private_nh("~"); + bool use_concurrency; - unsigned int concurrency = 4; + private_nh.param("use_concurrency", use_concurrency, false); - pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, nh_private, concurrency); + pointcloud_to_laserscan::PointCloudToLaserScanBase node(nh, private_nh); - ros::spin(); + if(use_concurrency) { + ros::MultiThreadedSpinner spinner; + spinner.spin(); + }else{ + ros::spin(); + } return 0; }