Skip to content

Give box/footprint/polygon filters unique private node names #266

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rafal-gorecki wants to merge 2 commits into
base: rolling
Choose a base branch
from
Open

Give box/footprint/polygon filters unique private node names #266

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3598612
Give box/footprint/polygon filters unique private node names
rafal-gorecki Aug 14, 2026
19f4005
Reuse each filter's private node for its TransformListener
rafal-gorecki Aug 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 27 additions & 18 deletions include/laser_filters/box_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,20 +56,25 @@
typedef tf2::Vector3 Point;

#include <laser_geometry/laser_geometry.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>

#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<sensor_msgs::msg::LaserScan>, public rclcpp_lifecycle::LifecycleNode
class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserScan>
{
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<tf2_ros::Buffer>(node_->get_clock());
tf_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, node_);

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_);
Expand All @@ -93,31 +98,31 @@ class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserSca

if (!box_frame_set)
{
RCLCPP_ERROR(get_logger(), "box_frame is not set!");
RCLCPP_ERROR(node_->get_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 &&
Expand All @@ -134,33 +139,33 @@ class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserSca

std::string error_msg;

bool success = buffer_.canTransform(
bool success = buffer_->canTransform(
box_frame_,
input_scan.header.frame_id,
rclcpp::Time(input_scan.header.stamp) + std::chrono::duration<double>(input_scan.ranges.size() * input_scan.time_increment),
1.0s,
&error_msg);
if (!success)
{
RCLCPP_WARN(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;
}

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;
}
Expand All @@ -176,7 +181,7 @@ class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserSca
!(iter_y != iter_y.end()) ||
!(iter_z != iter_z.end()))
{
RCLCPP_INFO_THROTTLE(get_logger(), steady_clock, .3, "x, y, z and index fields are required, skipping scan");
RCLCPP_INFO_THROTTLE(node_->get_logger(), steady_clock, .3, "x, y, z and index fields are required, skipping scan");
}

for (;
Expand Down Expand Up @@ -209,9 +214,13 @@ class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::msg::LaserSca
std::string box_frame_;
laser_geometry::LaserProjection projector_;

// node private to this filter, so its name stays unique even under a
// process-wide "-r __node:=<name>" 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<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_;

// parameter to decide if points in box or points outside of box are removed
bool remove_box_points_ = true;
Expand Down
30 changes: 18 additions & 12 deletions include/laser_filters/footprint_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,25 +50,27 @@ This is useful for ground plane extraction
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp> // PointCloud2ConstIterator
#include <geometry_msgs/msg/point32.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>

#include "laser_geometry/laser_geometry.hpp"
#include "laser_filters/util.h"

namespace laser_filters
{

class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::LaserScan>, public rclcpp_lifecycle::LifecycleNode
class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::LaserScan>
{
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<tf2_ros::Buffer>(node_->get_clock());
tf_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, node_);

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;
Expand All @@ -84,18 +86,18 @@ class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::La
sensor_msgs::msg::PointCloud2 laser_cloud;

try{
projector_.transformLaserScanToPointCloud("base_link", input_scan, laser_cloud, buffer_);
projector_.transformLaserScanToPointCloud("base_link", input_scan, laser_cloud, *buffer_);
}
catch (tf2::TransformException &ex)
{
rclcpp::Clock steady_clock(RCL_STEADY_TIME);
if (up_and_running_)
{
RCLCPP_WARN_THROTTLE(get_logger(), steady_clock, 1, "Dropping Scan: Transform unavailable %s", ex.what());
RCLCPP_WARN_THROTTLE(node_->get_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;
}
Expand All @@ -107,7 +109,7 @@ class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::La

if (!(iter_i != iter_i.end()))
{
RCLCPP_ERROR(get_logger(), "We need an index channel to be able to filter out the footprint");
RCLCPP_ERROR(node_->get_logger(), "We need an index channel to be able to filter out the footprint");
return false;
}

Expand Down Expand Up @@ -142,8 +144,12 @@ class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::msg::La
}

private:
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf_;
// node private to this filter, so its name stays unique even under a
// process-wide "-r __node:=<name>" remap (see laser_filters/util.h)
rclcpp::Node::SharedPtr node_;

std::shared_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_;
laser_geometry::LaserProjection projector_;
double inscribed_radius_;
bool up_and_running_;
Expand Down
42 changes: 26 additions & 16 deletions include/laser_filters/polygon_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,12 +53,14 @@
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/polygon_stamped.hpp>

#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/transform_listener.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>

#include "laser_filters/util.h"


typedef tf2::Vector3 Point;
using std::placeholders::_1;
Expand Down Expand Up @@ -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<sensor_msgs::msg::LaserScan>, public rclcpp_lifecycle::LifecycleNode {
class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::LaserScan> {
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<tf2_ros::Buffer>(node_->get_clock());
tf_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, node_);

// 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;
Expand Down Expand Up @@ -258,7 +264,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::
polygon_ = makePolygonFromString(polygon_string, polygon_);
padPolygon(polygon_, polygon_padding_);

polygon_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>("polygon", rclcpp::QoS(1).transient_local().keep_last(1));
polygon_pub_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>("polygon", rclcpp::QoS(1).transient_local().keep_last(1));
is_polygon_published_ = false;

return true;
Expand Down Expand Up @@ -290,9 +296,13 @@ class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::
bool is_polygon_published_ = false;


// node private to this filter, so its name stays unique even under a
// process-wide "-r __node:=<name>" 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<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_;

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
virtual rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector<rclcpp::Parameter> parameters)
Expand Down Expand Up @@ -350,7 +360,7 @@ class LaserScanPolygonFilterBase : public filters::FilterBase<sensor_msgs::msg::
{
geometry_msgs::msg::PolygonStamped polygon_stamped;
polygon_stamped.header.frame_id = polygon_frame_;
polygon_stamped.header.stamp = get_clock()->now();
polygon_stamped.header.stamp = node_->get_clock()->now();
polygon_stamped.polygon = polygon_;
polygon_pub_->publish(polygon_stamped);
is_polygon_published_ = true;
Expand All @@ -363,7 +373,7 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase {
bool configure() override
{
bool result = LaserScanPolygonFilterBase::configure();
footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(footprint_topic_, 1, std::bind(&LaserScanPolygonFilterBase::footprintCB, this, std::placeholders::_1));
footprint_sub_ = node_->create_subscription<geometry_msgs::msg::Polygon>(footprint_topic_, 1, std::bind(&LaserScanPolygonFilterBase::footprintCB, this, std::placeholders::_1));
return result;
}

Expand All @@ -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<double>(input_scan.ranges.size() * input_scan.time_increment),
Expand All @@ -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;
}

Expand All @@ -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;
Expand Down Expand Up @@ -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<geometry_msgs::msg::Polygon>(footprint_topic_, 1, std::bind(&StaticLaserScanPolygonFilter::footprintCB, this, std::placeholders::_1));
footprint_sub_ = node_->create_subscription<geometry_msgs::msg::Polygon>(footprint_topic_, 1, std::bind(&StaticLaserScanPolygonFilter::footprintCB, this, std::placeholders::_1));
return result;
}

Expand Down Expand Up @@ -538,15 +548,15 @@ 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,
*node_->get_clock(), 1000,
"Could not get transform, ignoring laser scan! %s", ex.what());
return false;
}
Expand Down
Loading