diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 7bb2fc38..f7981fc1 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -23,7 +23,6 @@ pcl_conversions pcl_msgs pcl_ros - pointcloud_to_laserscan diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst deleted file mode 100644 index 5c2b3e50..00000000 --- a/pointcloud_to_laserscan/CHANGELOG.rst +++ /dev/null @@ -1,43 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package pointcloud_to_laserscan -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.6 (2015-02-04) ------------------- -* Fix default value for concurrency -* Fix multithreaded lazy pub sub -* Contributors: Paul Bovbel - -1.2.5 (2015-01-20) ------------------- -* Switch to tf_sensor_msgs for transform -* Set parameters in sample launch files to default -* Add tolerance parameter -* Contributors: Paul Bovbel - -1.2.4 (2015-01-15) ------------------- -* Remove stray dependencies -* Refactor with tf2 and message filters -* Remove roslaunch check -* Fix regressions -* Refactor to allow debug messages from node and nodelet -* Contributors: Paul Bovbel - -1.2.3 (2015-01-10) ------------------- -* add launch tests -* refactor naming and fix nodelet export -* set default target frame to empty -* clean up package.xml -* Contributors: Paul Bovbel - -1.2.2 (2014-10-25) ------------------- -* clean up package.xml -* Fix header reference -* Fix flow -* Fix pointer assertion -* Finalize pointcloud to laserscan -* Initial pointcloud to laserscan commit -* Contributors: Paul Bovbel \ No newline at end of file diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt deleted file mode 100644 index 193e3ac6..00000000 --- a/pointcloud_to_laserscan/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pointcloud_to_laserscan) - -find_package(catkin REQUIRED COMPONENTS - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES pointcloud_to_laserscan - CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -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) -target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES}) - -install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(FILES nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) 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 deleted file mode 100644 index bed70952..00000000 --- a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ /dev/null @@ -1,96 +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 - */ - -#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET -#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET - -#include "ros/ros.h" -#include "boost/thread/mutex.hpp" - -#include "nodelet/nodelet.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/message_filter.h" -#include "message_filters/subscriber.h" -#include "sensor_msgs/PointCloud2.h" - - -namespace pointcloud_to_laserscan -{ - 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. -*/ - class PointCloudToLaserScanNodelet : public nodelet::Nodelet - { - - public: - PointCloudToLaserScanNodelet(); - - private: - virtual void onInit(); - - void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg); - void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, - tf2_ros::filter_failure_reasons::FilterFailureReason reason); - - void connectCb(); - - void disconnectCb(); - - ros::NodeHandle nh_, private_nh_; - ros::Publisher pub_; - boost::mutex connect_mutex_; - - tf2_ros::Buffer tf2_; - message_filters::Subscriber sub_; - boost::shared_ptr message_filter_; - - // ROS Parameters - unsigned int input_queue_size_; - std::string target_frame_; - double 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 // 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 deleted file mode 100644 index 1d346365..00000000 --- a/pointcloud_to_laserscan/launch/sample_node.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.01 - 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 - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/launch/sample_nodelet.launch b/pointcloud_to_laserscan/launch/sample_nodelet.launch deleted file mode 100644 index ef784310..00000000 --- a/pointcloud_to_laserscan/launch/sample_nodelet.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - #target_frame: # Leave disabled to output scan in pointcloud frame - transform_tolerance: 0.01 - 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 - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/nodelets.xml b/pointcloud_to_laserscan/nodelets.xml deleted file mode 100644 index 327a819e..00000000 --- a/pointcloud_to_laserscan/nodelets.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans. - - - - diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml deleted file mode 100644 index b3985006..00000000 --- a/pointcloud_to_laserscan/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - pointcloud_to_laserscan - 1.2.6 - 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). - - Paul Bovbel - Tully Foote - - BSD - - http://ros.org/wiki/perception_pcl - https://github.com/ros-perception/perception_pcl/issues - https://github.com/ros-perception/perception_pcl - - catkin - - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs - - message_filters - nodelet - roscpp - sensor_msgs - tf2 - tf2_ros - tf2_sensor_msgs - - - - - \ No newline at end of file diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp deleted file mode 100644 index 7eb7c219..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp +++ /dev/null @@ -1,65 +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 - -int main(int argc, char **argv){ - ros::init(argc, argv, "pointcloud_to_laserscan_node"); - ros::NodeHandle private_nh("~"); - int concurrency_level; - private_nh.param("concurrency_level", concurrency_level, 0); - - 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); - - boost::shared_ptr spinner; - if(concurrency_level) { - spinner.reset(new ros::MultiThreadedSpinner(concurrency_level)); - }else{ - spinner.reset(new ros::MultiThreadedSpinner()); - } - spinner->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 deleted file mode 100644 index 256fddff..00000000 --- a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp +++ /dev/null @@ -1,240 +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 -#include - -namespace pointcloud_to_laserscan -{ - - PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {} - - void PointCloudToLaserScanNodelet::onInit() - { - boost::mutex::scoped_lock lock(connect_mutex_); - private_nh_ = getPrivateNodeHandle(); - - private_nh_.param("target_frame", target_frame_, ""); - private_nh_.param("tolerance", tolerance_, 0.01); - 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); - - int concurrency_level; - private_nh_.param("concurrency_level", concurrency_level, 1); - private_nh_.param("use_inf", use_inf_, true); - - //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size - if (concurrency_level == 1) - { - nh_ = getNodeHandle(); - } - else - { - nh_ = getMTNodeHandle(); - } - - // Only queue one pointcloud per running thread - if (concurrency_level > 0) - { - input_queue_size_ = concurrency_level; - } - else - { - input_queue_size_ = boost::thread::hardware_concurrency(); - } - - // 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_)); - message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2)); - } - else // otherwise setup direct subscription - { - sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); - } - - 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 (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0) - { - NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud"); - sub_.subscribe(nh_, "cloud_in", input_queue_size_); - } - } - - void PointCloudToLaserScanNodelet::disconnectCb() - { - boost::mutex::scoped_lock lock(connect_mutex_); - if (pub_.getNumSubscribers() == 0) - { - NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud"); - sub_.unsubscribe(); - } - } - - void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, - 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_); - } - - void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg) - { - - //build laserscan output - sensor_msgs::LaserScan output; - output.header = cloud_msg->header; - if (!target_frame_.empty()) - { - 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_; - - //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); - } - - sensor_msgs::PointCloud2ConstPtr cloud_out; - sensor_msgs::PointCloud2Ptr cloud; - - // Transform cloud if necessary - if (!(output.header.frame_id == cloud_msg->header.frame_id)) - { - try - { - cloud.reset(new sensor_msgs::PointCloud2); - tf2_.transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_)); - cloud_out = cloud; - } - catch (tf2::TransformException ex) - { - NODELET_ERROR_STREAM("Transform failure: " << ex.what()); - return; - } - } - else - { - cloud_out = cloud_msg; - } - - // Iterate through pointcloud - for (sensor_msgs::PointCloud2ConstIterator - iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z"); - iter_x != iter_x.end(); - ++iter_x, ++iter_y, ++iter_z) - { - - if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) - { - NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); - continue; - } - - if (*iter_z > max_height_ || *iter_z < min_height_) - { - NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_); - continue; - } - - double range = hypot(*iter_x, *iter_y); - if (range < range_min_) - { - NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y, - *iter_z); - continue; - } - - double angle = atan2(*iter_y, *iter_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); - } - -} - -PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);