From 35986126920ad51350a2f95805f4f89719e1f7bd Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 14 Aug 2026 14:54:15 +0200 Subject: [PATCH 1/2] Give box/footprint/polygon filters unique private node names These filters spin up their own internal node for TF and (for polygon) pub/sub/param-callback needs. That internal node used a fixed name, so a process-wide "-r __node:=" remap (e.g. Node(name="laser_filter") in a launch file) silently renamed it too, colliding with the parent filter-chain node under the same name and breaking rosout logging. Give each internal node a name unique to the filter instance, pinned via its own local "-r __node:=" argument so it survives a process-wide bare remap. Fixes #214 --- include/laser_filters/box_filter.h | 45 +++++++------ include/laser_filters/footprint_filter.h | 30 +++++---- include/laser_filters/polygon_filter.h | 42 +++++++----- include/laser_filters/util.h | 81 ++++++++++++++++++++++++ 4 files changed, 152 insertions(+), 46 deletions(-) create mode 100644 include/laser_filters/util.h diff --git a/include/laser_filters/box_filter.h b/include/laser_filters/box_filter.h index 5465b12b..86c8a909 100644 --- a/include/laser_filters/box_filter.h +++ b/include/laser_filters/box_filter.h @@ -56,20 +56,25 @@ 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 +98,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 +139,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 +147,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 +181,7 @@ class LaserScanBoxFilter : public filters::FilterBaseget_logger(), steady_clock, .3, "x, y, z and index fields are required, skipping scan"); } for (; @@ -209,9 +214,13 @@ class LaserScanBoxFilter : public filters::FilterBase" remap (see laser_filters/util.h) + rclcpp::Node::SharedPtr node_; + // tf listener to transform scans into the box_frame - tf2_ros::Buffer buffer_; - tf2_ros::TransformListener tf_; + 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/footprint_filter.h b/include/laser_filters/footprint_filter.h index 33975908..8970928e 100644 --- a/include/laser_filters/footprint_filter.h +++ b/include/laser_filters/footprint_filter.h @@ -50,25 +50,27 @@ This is useful for ground plane extraction #include #include // PointCloud2ConstIterator #include -#include #include "laser_geometry/laser_geometry.hpp" +#include "laser_filters/util.h" namespace laser_filters { -class LaserScanFootprintFilter : public filters::FilterBase, public rclcpp_lifecycle::LifecycleNode +class LaserScanFootprintFilter : public filters::FilterBase { public: - LaserScanFootprintFilter() - : rclcpp_lifecycle::LifecycleNode("laser_scan_footprint_filter"), - buffer_(get_clock()), tf_(buffer_), up_and_running_(false) {} + LaserScanFootprintFilter() : up_and_running_(false) {} bool configure() { + node_ = getUniqueNode("footprint_filter", this); + buffer_ = std::make_shared(node_->get_clock()); + tf_ = std::make_shared(*buffer_); + if(!getParam("inscribed_radius", inscribed_radius_)) { - RCLCPP_ERROR(get_logger(), "LaserScanFootprintFilter needs inscribed_radius to be set"); + RCLCPP_ERROR(node_->get_logger(), "LaserScanFootprintFilter needs inscribed_radius to be set"); return false; } return true; @@ -84,18 +86,18 @@ class LaserScanFootprintFilter : public filters::FilterBaseget_logger(), steady_clock, 1, "Dropping Scan: Transform unavailable %s", ex.what()); } 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; } @@ -107,7 +109,7 @@ class LaserScanFootprintFilter : public filters::FilterBaseget_logger(), "We need an index channel to be able to filter out the footprint"); return false; } @@ -142,8 +144,12 @@ class LaserScanFootprintFilter : public filters::FilterBase" remap (see laser_filters/util.h) + rclcpp::Node::SharedPtr node_; + + std::shared_ptr buffer_; + std::shared_ptr tf_; laser_geometry::LaserProjection projector_; double inscribed_radius_; bool up_and_running_; diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index fb15096f..290c2523 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -53,12 +53,14 @@ #include #include -#include #include #include +#include #include #include +#include "laser_filters/util.h" + typedef tf2::Vector3 Point; using std::placeholders::_1; @@ -222,15 +224,19 @@ 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; @@ -258,7 +264,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase("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; @@ -290,9 +296,13 @@ class LaserScanPolygonFilterBase : public filters::FilterBase" remap (see laser_filters/util.h) + rclcpp::Node::SharedPtr node_; + // tf listener to transform scans into the right frame - tf2_ros::Buffer buffer_; - tf2_ros::TransformListener tf_; + 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) @@ -350,7 +360,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; @@ -363,7 +373,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; } @@ -381,7 +391,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), @@ -394,10 +404,10 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { } 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(logging_interface_->get_logger(), *node_->get_clock(), 300, "Ignoring Scan: Waiting for TF"); return false; } @@ -408,7 +418,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(logging_interface_->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; @@ -481,7 +491,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { { RCLCPP_INFO(logging_interface_->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; } @@ -538,7 +548,7 @@ 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_)); @@ -546,7 +556,7 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { catch(tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(logging_interface_->get_logger(), - *get_clock(), 1000, + *node_->get_clock(), 1000, "Could not get transform, ignoring laser scan! %s", ex.what()); return false; } diff --git a/include/laser_filters/util.h b/include/laser_filters/util.h new file mode 100644 index 00000000..06f6e6ad --- /dev/null +++ b/include/laser_filters/util.h @@ -0,0 +1,81 @@ +/* + * 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. + * + * + * + * util.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. + * + * The node name is pinned via an explicit "-r __node:=" argument local + * to this node, so it stays unique even when the parent process is launched + * with a bare "--ros-args -r __node:=" rule, which otherwise renames + * every node created in the process to the same name and collides with it. + * + * \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_ From 19f4005c9f721fcca203346505c7a25426960e8a Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 14 Aug 2026 15:19:31 +0200 Subject: [PATCH 2/2] Reuse each filter's private node for its TransformListener box_filter, footprint_filter and polygon_filter each construct their TransformListener with the buffer-only convenience constructor, which spins up yet another hidden node (and dedicated thread) purely to subscribe to /tf and /tf_static, on top of the private node these filters already carry. Pass that private node through instead, so there's one node per filter, not two, matching the same cleanup already done for the top-level filter-chain node in 1801589 ("Re-use the main filter node for the TransformListener"). --- include/laser_filters/box_filter.h | 2 +- include/laser_filters/footprint_filter.h | 2 +- include/laser_filters/polygon_filter.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/laser_filters/box_filter.h b/include/laser_filters/box_filter.h index 86c8a909..9ee48f54 100644 --- a/include/laser_filters/box_filter.h +++ b/include/laser_filters/box_filter.h @@ -73,7 +73,7 @@ class LaserScanBoxFilter : public filters::FilterBase(node_->get_clock()); - tf_ = std::make_shared(*buffer_); + tf_ = std::make_shared(*buffer_, node_); up_and_running_ = true; double min_x, min_y, min_z, max_x, max_y, max_z; diff --git a/include/laser_filters/footprint_filter.h b/include/laser_filters/footprint_filter.h index 8970928e..33eed82d 100644 --- a/include/laser_filters/footprint_filter.h +++ b/include/laser_filters/footprint_filter.h @@ -66,7 +66,7 @@ class LaserScanFootprintFilter : public filters::FilterBase(node_->get_clock()); - tf_ = std::make_shared(*buffer_); + tf_ = std::make_shared(*buffer_, node_); if(!getParam("inscribed_radius", inscribed_radius_)) { diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index 290c2523..f32caf2e 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -233,7 +233,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase(node_->get_clock()); - tf_ = std::make_shared(*buffer_); + tf_ = std::make_shared(*buffer_, node_); // dynamic reconfigure parameters callback: on_set_parameters_callback_handle_ = node_->add_on_set_parameters_callback(