From cc13d410368752b01465a0cdb0448dd1386c791b Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 14 Feb 2024 14:24:53 +0100 Subject: [PATCH] remove obsolete code --- CMakeLists.txt | 10 +- .../pointcloud_to_laserscan_nodelet.h | 98 ------- nodelets.xml | 12 - src/pointcloud_to_laserscan_node.cpp | 65 ----- src/pointcloud_to_laserscan_nodelet.cpp | 241 ------------------ 5 files changed, 1 insertion(+), 425 deletions(-) delete mode 100644 include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h delete mode 100644 src/pointcloud_to_laserscan_node.cpp delete mode 100644 src/pointcloud_to_laserscan_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 079fe060..c10bf8cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} ipa_${PROJECT_NAME} roi_outlier_removal + LIBRARIES ipa_${PROJECT_NAME} roi_outlier_removal CATKIN_DEPENDS message_filters nodelet pcl_ros roscpp sensor_msgs tf tf2 tf2_geometry_msgs tf2_ros tf2_sensor_msgs ) @@ -34,12 +34,6 @@ target_link_libraries(frame_publisher_node frame_publisher ${catkin_LIBRARIES}) add_library(roi_outlier_removal src/roi_outlier_removal_nodelet.cpp) target_link_libraries(roi_outlier_removal ${catkin_LIBRARIES}) -add_library(${PROJECT_NAME} src/${PROJECT_NAME}_nodelet.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) - add_library(ipa_${PROJECT_NAME} src/ipa_${PROJECT_NAME}_nodelet.cpp src/scan_outlier_removal_filter.cpp) target_link_libraries(ipa_${PROJECT_NAME} ${catkin_LIBRARIES}) @@ -50,8 +44,6 @@ install(TARGETS frame_publisher frame_publisher_node ipa_${PROJECT_NAME} ipa_${PROJECT_NAME}_node - ${PROJECT_NAME} - ${PROJECT_NAME}_node roi_outlier_removal RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h b/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h deleted file mode 100644 index 5b44fc6e..00000000 --- a/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h +++ /dev/null @@ -1,98 +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/transform_listener.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_; - - boost::shared_ptr tf2_; - boost::shared_ptr tf2_listener_; - 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/nodelets.xml b/nodelets.xml index 3ea50202..45a903ca 100644 --- a/nodelets.xml +++ b/nodelets.xml @@ -1,15 +1,3 @@ - - - - - Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans. - - - - - -#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/src/pointcloud_to_laserscan_nodelet.cpp b/src/pointcloud_to_laserscan_nodelet.cpp deleted file mode 100644 index 5918f0bc..00000000 --- a/src/pointcloud_to_laserscan_nodelet.cpp +++ /dev/null @@ -1,241 +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("transform_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()) - { - 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)); - } - 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()); - } - - 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_EXPORT_CLASS(pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet);