diff --git a/include/laser_filters/box_filter.h b/include/laser_filters/box_filter.h index 7fbab20..ef90f46 100644 --- a/include/laser_filters/box_filter.h +++ b/include/laser_filters/box_filter.h @@ -49,27 +49,34 @@ #include +#include "tf2_ros/buffer.hpp" #include #include #include +#include typedef tf2::Vector3 Point; #include -#include + +#include "laser_filters/util.h" namespace laser_filters { /** * @brief This is a filter that removes points in a laser scan inside of a cartesian box. */ -class LaserScanBoxFilter : public filters::FilterBase, public rclcpp_lifecycle::LifecycleNode +class LaserScanBoxFilter : public filters::FilterBase { public: - LaserScanBoxFilter() : rclcpp_lifecycle::LifecycleNode("laser_scan_box_filter"), buffer_(get_clock()), tf_(buffer_){}; + LaserScanBoxFilter() {}; bool configure() { + node_ = getUniqueNode("box_filter", this); + buffer_ = std::make_shared(node_->get_clock()); + tf_ = std::make_shared(*buffer_); + up_and_running_ = true; double min_x, min_y, min_z, max_x, max_y, max_z; bool box_frame_set = getParam("box_frame", box_frame_); @@ -93,31 +100,31 @@ class LaserScanBoxFilter : public filters::FilterBaseget_logger(), "box_frame is not set!"); } if (!x_max_set) { - RCLCPP_ERROR(get_logger(), "max_x is not set!"); + RCLCPP_ERROR(node_->get_logger(), "max_x is not set!"); } if (!y_max_set) { - RCLCPP_ERROR(get_logger(), "max_y is not set!"); + RCLCPP_ERROR(node_->get_logger(), "max_y is not set!"); } if (!z_max_set) { - RCLCPP_ERROR(get_logger(), "max_z is not set!"); + RCLCPP_ERROR(node_->get_logger(), "max_z is not set!"); } if (!x_min_set) { - RCLCPP_ERROR(get_logger(), "min_x is not set!"); + RCLCPP_ERROR(node_->get_logger(), "min_x is not set!"); } if (!y_min_set) { - RCLCPP_ERROR(get_logger(), "min_y is not set!"); + RCLCPP_ERROR(node_->get_logger(), "min_y is not set!"); } if (!z_min_set) { - RCLCPP_ERROR(get_logger(), "min_z is not set!"); + RCLCPP_ERROR(node_->get_logger(), "min_z is not set!"); } return box_frame_set && x_max_set && y_max_set && z_max_set && @@ -134,7 +141,7 @@ class LaserScanBoxFilter : public filters::FilterBasecanTransform( box_frame_, input_scan.header.frame_id, rclcpp::Time(input_scan.header.stamp) + std::chrono::duration(input_scan.ranges.size() * input_scan.time_increment), @@ -142,25 +149,25 @@ class LaserScanBoxFilter : public filters::FilterBaseget_logger(), "Could not get transform, irgnoring laser scan! %s", error_msg.c_str()); return false; } rclcpp::Clock steady_clock(RCL_STEADY_TIME); try { - projector_.transformLaserScanToPointCloud(box_frame_, input_scan, laser_cloud, buffer_); + projector_.transformLaserScanToPointCloud(box_frame_, input_scan, laser_cloud, *buffer_); } catch (tf2::TransformException &ex) { if (up_and_running_) { - RCLCPP_WARN_THROTTLE(get_logger(), steady_clock, 1, "Dropping Scan: Tansform unavailable %s", ex.what()); + RCLCPP_WARN_THROTTLE(node_->get_logger(), steady_clock, 1, "Dropping Scan: Tansform unavailable %s", ex.what()); return true; } else { - RCLCPP_INFO_THROTTLE(get_logger(), steady_clock, .3, "Ignoring Scan: Waiting for TF"); + RCLCPP_INFO_THROTTLE(node_->get_logger(), steady_clock, .3, "Ignoring Scan: Waiting for TF"); } return false; } @@ -176,7 +183,7 @@ class LaserScanBoxFilter : public filters::FilterBaseget_logger(), steady_clock, .3, "x, y, z and index fields are required, skipping scan"); } for (; @@ -210,8 +217,9 @@ class LaserScanBoxFilter : public filters::FilterBase node_; + std::shared_ptr buffer_; + std::shared_ptr tf_; // parameter to decide if points in box or points outside of box are removed bool remove_box_points_ = true; diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index 9e7e92c..464b273 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -56,9 +57,12 @@ #include #include #include +#include #include #include +#include "laser_filters/util.h" + typedef tf2::Vector3 Point; using std::placeholders::_1; @@ -222,41 +226,46 @@ namespace laser_filters /** * @brief This is a filter that removes points in a laser scan inside of a polygon. */ -class LaserScanPolygonFilterBase : public filters::FilterBase, public rclcpp_lifecycle::LifecycleNode { +class LaserScanPolygonFilterBase : public filters::FilterBase +{ public: - LaserScanPolygonFilterBase() : rclcpp_lifecycle::LifecycleNode("laser_scan_polygon_filter"), buffer_(get_clock()), tf_(buffer_){}; + LaserScanPolygonFilterBase() {} virtual bool configure() { + node_ = getUniqueNode("polygon_filter", this); + buffer_ = std::make_shared(node_->get_clock()); + tf_ = std::make_shared(*buffer_); + // dynamic reconfigure parameters callback: - on_set_parameters_callback_handle_ = add_on_set_parameters_callback( + on_set_parameters_callback_handle_ = node_->add_on_set_parameters_callback( std::bind(&LaserScanPolygonFilterBase::reconfigureCB, this, std::placeholders::_1)); std::string polygon_string; if(!filters::FilterBase::getParam(std::string("footprint_topic"), footprint_topic_, false, "base_footprint_exclude")) { - RCLCPP_WARN(logging_interface_->get_logger(), "Footprint topic not set, assuming default: base_footprint_exclude"); + RCLCPP_WARN(node_->get_logger(), "Footprint topic not set, assuming default: base_footprint_exclude"); } if (!filters::FilterBase::getParam(std::string("polygon"), polygon_string)) { - RCLCPP_ERROR(logging_interface_->get_logger(), "Error: PolygonFilter was not given polygon.\n"); + RCLCPP_ERROR(node_->get_logger(), "Error: PolygonFilter was not given polygon.\n"); return false; }if (!filters::FilterBase::getParam(std::string("polygon_frame"), polygon_frame_, false)) { - RCLCPP_ERROR(logging_interface_->get_logger(), "Error: PolygonFilter was not given polygon_frame.\n"); + RCLCPP_ERROR(node_->get_logger(), "Error: PolygonFilter was not given polygon_frame.\n"); return false; }if (!filters::FilterBase::getParam(std::string("invert"), invert_filter_, false)) { - RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter invert filter not set, assuming false.\n"); + RCLCPP_INFO(node_->get_logger(), "Error: PolygonFilter invert filter not set, assuming false.\n"); }if (!filters::FilterBase::getParam(std::string("polygon_padding"), polygon_padding_, false)) { - RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter polygon_padding not set, assuming 0. \n"); + RCLCPP_INFO(node_->get_logger(), "Error: PolygonFilter polygon_padding not set, assuming 0. \n"); } polygon_ = makePolygonFromString(polygon_string, polygon_); padPolygon(polygon_, polygon_padding_); - polygon_pub_ = create_publisher("polygon", rclcpp::QoS(1).transient_local().keep_last(1)); + polygon_pub_ = node_->create_publisher("polygon", rclcpp::QoS(1).transient_local().keep_last(1)); is_polygon_published_ = false; return true; @@ -266,7 +275,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBasepoints.size() < 3) { - RCLCPP_WARN(logging_interface_->get_logger(), "Footprint needs at least three points for the robot polygon, ignoring message"); + RCLCPP_WARN(node_->get_logger(), "Footprint needs at least three points for the robot polygon, ignoring message"); return; } polygon_ = *polygon; @@ -289,8 +298,9 @@ class LaserScanPolygonFilterBase : public filters::FilterBase node_; + std::shared_ptr buffer_; + std::shared_ptr tf_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; virtual rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) @@ -301,8 +311,8 @@ class LaserScanPolygonFilterBase : public filters::FilterBaseget_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Unknown parameter"); + RCLCPP_WARN(node_->get_logger(), "Unknown parameter"); } } padPolygon(polygon_, polygon_padding_); @@ -348,7 +358,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBasenow(); + polygon_stamped.header.stamp = node_->get_clock()->now(); polygon_stamped.polygon = polygon_; polygon_pub_->publish(polygon_stamped); is_polygon_published_ = true; @@ -361,7 +371,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { bool configure() override { bool result = LaserScanPolygonFilterBase::configure(); - footprint_sub_ = create_subscription(footprint_topic_, 1, std::bind(&LaserScanPolygonFilterBase::footprintCB, this, std::placeholders::_1)); + footprint_sub_ = node_->create_subscription(footprint_topic_, 1, std::bind(&LaserScanPolygonFilterBase::footprintCB, this, std::placeholders::_1)); return result; } @@ -379,7 +389,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { std::string error_msg; - bool success = buffer_.canTransform( + bool success = buffer_->canTransform( polygon_frame_, input_scan.header.frame_id, rclcpp::Time(input_scan.header.stamp) + std::chrono::duration(input_scan.ranges.size() * input_scan.time_increment), @@ -387,15 +397,15 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { &error_msg ); if(!success){ - RCLCPP_WARN(logging_interface_->get_logger(), "Could not get transform, irgnoring laser scan! %s", error_msg.c_str()); + RCLCPP_WARN(node_->get_logger(), "Could not get transform, irgnoring laser scan! %s", error_msg.c_str()); return false; } try{ - projector_.transformLaserScanToPointCloud(polygon_frame_, input_scan, laser_cloud, buffer_); + projector_.transformLaserScanToPointCloud(polygon_frame_, input_scan, laser_cloud, *buffer_); } catch(tf2::TransformException& ex){ - RCLCPP_INFO_THROTTLE(logging_interface_->get_logger(), *get_clock(), 300, "Ignoring Scan: Waiting for TF"); + RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 300, "Ignoring Scan: Waiting for TF"); return false; } @@ -406,7 +416,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { if (i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1) { - RCLCPP_INFO_THROTTLE(logging_interface_->get_logger(), *get_clock(), 300, "x, y, z and index fields are required, skipping scan"); + RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 300, "x, y, z and index fields are required, skipping scan"); } const int i_idx_offset = laser_cloud.fields[i_idx_c].offset; @@ -453,7 +463,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { auto end = std::chrono::high_resolution_clock::now(); auto update_elapsed = std::chrono::duration_cast(end - start).count(); - RCLCPP_DEBUG(logging_interface_->get_logger(), "LaserScanPolygonFilter update took %lu microseconds", update_elapsed); + RCLCPP_DEBUG(node_->get_logger(), "LaserScanPolygonFilter update took %lu microseconds", update_elapsed); return true; } @@ -477,9 +487,9 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { transform_timeout_ = 5; // Default if (!filters::FilterBase::getParam(std::string("transform_timeout"), transform_timeout_)) { - RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter transform_timeout not set, assuming 5. \n"); + RCLCPP_INFO(node_->get_logger(), "Error: PolygonFilter transform_timeout not set, assuming 5. \n"); } - footprint_sub_ = create_subscription(footprint_topic_, 1, std::bind(&StaticLaserScanPolygonFilter::footprintCB, this, std::placeholders::_1)); + footprint_sub_ = node_->create_subscription(footprint_topic_, 1, std::bind(&StaticLaserScanPolygonFilter::footprintCB, this, std::placeholders::_1)); return result; } @@ -528,7 +538,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { bool transformPolygon(const std::string &input_scan_frame_id) { std::string error_msg; - RCLCPP_DEBUG(logging_interface_->get_logger(), + RCLCPP_DEBUG(node_->get_logger(), "waitForTransform %s -> %s", polygon_frame_.c_str(), input_scan_frame_id.c_str() ); @@ -536,20 +546,20 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { geometry_msgs::msg::TransformStamped transform; try { - transform = buffer_.lookupTransform(input_scan_frame_id, + transform = buffer_->lookupTransform(input_scan_frame_id, polygon_frame_, tf2::TimePointZero, tf2::durationFromSec(transform_timeout_)); } catch(tf2::TransformException& ex) { - RCLCPP_WARN_THROTTLE(logging_interface_->get_logger(), - *get_clock(), 1000, + RCLCPP_WARN_THROTTLE(node_->get_logger(), + *node_->get_clock(), 1000, "Could not get transform, ignoring laser scan! %s", ex.what()); return false; } - RCLCPP_INFO(logging_interface_->get_logger(), "Obtained transform"); + RCLCPP_INFO(node_->get_logger(), "Obtained transform"); for (int i = 0; i < polygon_.points.size(); ++i) { geometry_msgs::msg::PointStamped point_in = createPointStamped(polygon_.points[i].x, polygon_.points[i].y, 0, transform.header.stamp, polygon_frame_); @@ -584,7 +594,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { co_sine_map_angle_min_ != scan_in.angle_min || co_sine_map_angle_max_ != scan_in.angle_max ) { - RCLCPP_DEBUG(logging_interface_->get_logger(), "No precomputed map given. Computing one."); + RCLCPP_DEBUG(node_->get_logger(), "No precomputed map given. Computing one."); co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); co_sine_map_angle_min_ = scan_in.angle_min; co_sine_map_angle_max_ = scan_in.angle_max; diff --git a/include/laser_filters/util.h b/include/laser_filters/util.h new file mode 100644 index 0000000..591d7f4 --- /dev/null +++ b/include/laser_filters/util.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by Eurotec B.V. + * Copyright (c) 2020, Eurotec B.V. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. 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. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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. + * + * + * + * polygon_filter.h + */ + +#ifndef LASER_FILTERS__UTIL_H_ +#define LASER_FILTERS__UTIL_H_ + +#include +#include + +namespace laser_filters +{ + /** + * \brief Create a node that is unique to this filter. + * \param filter_name The name of the filter, used to help make the node name unique. + * \param filter A pointer to the filter instance, used to help make the node name unique. + * \return A shared pointer to a node handle in the filter's private namespace + */ + template + rclcpp::Node::SharedPtr getUniqueNode(const std::string &filter_name, const filters::FilterBase *filter) + { + char node_name[42]; + snprintf( + node_name, sizeof(node_name), "%s_%zx", + filter_name.c_str(), reinterpret_cast(filter) + ); + + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( + node_name, + rclcpp::NodeOptions().arguments( + {"--ros-args", "-r", "__node:=" + std::string(node_name)})); + + return node; + } +} + +#endif // LASER_FILTERS__UTIL_H_ \ No newline at end of file