From 1f9aecf1a4a872d2a25835ef0111bda34ecf52dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jeldrik=20Schro=CC=88er?= Date: Sat, 14 Feb 2026 23:08:03 +0100 Subject: [PATCH 1/5] Add Generic XYZI sensor type --- README.md | 3 ++- src/self_filter.cpp | 13 +++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index e54b7cd..37d5365 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ ros2 launch robot_self_filter self_filter.launch.py \ | `filter_config` | string | - | Path to YAML configuration file | | `in_pointcloud_topic` | string | `/cloud_in` | Input point cloud topic | | `out_pointcloud_topic` | string | `/cloud_out` | Filtered point cloud topic | -| `lidar_sensor_type` | int | `2` | Sensor type (0: XYZ, 1: XYZRGB, 2: Ouster, 3: Hesai, 4: Robosense, 5: Pandar) | +| `lidar_sensor_type` | int | `2` | Sensor type (0: XYZ, 1: XYZRGB, 2: Ouster, 3: Hesai, 4: Robosense, 5: Pandar, 6: XYZI) | | `zero_for_removed_points` | bool | `true` | Set filtered points to zero instead of removing | | `use_sim_time` | bool | `true` | Use simulation time | | `description_name` | string | `/robot_description` | Robot description parameter namespace | @@ -160,6 +160,7 @@ The package supports multiple sensor types through the `lidar_sensor_type` param | 3 | Hesai | Custom Hesai point type | | 4 | Robosense | Custom Robosense point type | | 5 | Pandar | Custom Pandar point type | +| 6 | Generic XYZI | `pcl::PointXYZI` | ## Examples diff --git a/src/self_filter.cpp b/src/self_filter.cpp index db5a973..3e69368 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -31,6 +31,7 @@ namespace robot_self_filter HesaiSensor = 3, RobosenseSensor = 4, PandarSensor = 5, + XYZISensor = 6, }; class SelfFilterNode : public rclcpp::Node @@ -110,6 +111,9 @@ namespace robot_self_filter case SensorType::PandarSensor: self_filter_ = std::make_shared>(this->shared_from_this()); break; + case SensorType::XYZISensor: + self_filter_ = std::make_shared>(this->shared_from_this()); + break; default: self_filter_ = std::make_shared>(this->shared_from_this()); break; @@ -161,6 +165,15 @@ namespace robot_self_filter publishShapesFromMask(mask, cloud->header.frame_id); break; } + case SensorType::XYZISensor: + { + auto sf_xyzi = std::dynamic_pointer_cast>(self_filter_); + if (!sf_xyzi) + return; + auto mask = sf_xyzi->getSelfMaskPtr(); + publishShapesFromMask(mask, cloud->header.frame_id); + break; + } default: RCLCPP_ERROR(this->get_logger(), "Sensor type not handled for shape publishing"); return; From 23ecde27c10ec1dd4f7c11a693d8ff6917393e60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jeldrik=20Schro=CC=88er?= Date: Sat, 14 Feb 2026 23:45:57 +0100 Subject: [PATCH 2/5] Subscribe to robot_description topic Add option to subscribe to the robot_description topic coming from robot_state_publisher node, rather than providing the URDF string as a parameter to this node. If robot_description URDF string is provided, it will take precedence over the topic. --- README.md | 22 +++++++++++++++++++--- launch/self_filter.launch.py | 15 ++++++++------- src/self_filter.cpp | 36 +++++++++++++++++++++++++++++++++++- 3 files changed, 62 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 37d5365..11a43f8 100644 --- a/README.md +++ b/README.md @@ -44,7 +44,20 @@ source install/setup.bash ### Quick Start -Launch the self filter node with your robot configuration: +Launch the self filter node with your robot configuration. The robot description can be provided either as a parameter or via a ROS 2 topic (e.g. published by `robot_state_publisher`). + +**Option 1: From topic (recommended)** + +If `robot_state_publisher` is running, the filter will automatically pick up the robot description from the `/robot_description` topic: + +```bash +ros2 launch robot_self_filter self_filter.launch.py \ + filter_config:=/path/to/filter_config.yaml \ + in_pointcloud_topic:=/lidar/points \ + out_pointcloud_topic:=/lidar/points_filtered +``` + +**Option 2: From parameter** ```bash ros2 launch robot_self_filter self_filter.launch.py \ @@ -54,18 +67,20 @@ ros2 launch robot_self_filter self_filter.launch.py \ out_pointcloud_topic:=/lidar/points_filtered ``` +When `robot_description` is provided as a non-empty parameter, it takes priority over the topic. + ### Launch Parameters | Parameter | Type | Default | Description | |-----------|------|---------|-------------| -| `robot_description` | string | - | Robot URDF/XACRO description | +| `robot_description` | string | - | Robot URDF/XACRO description (if empty, uses topic instead) | +| `robot_description_topic` | string | `/robot_description` | Topic to subscribe to for robot description | | `filter_config` | string | - | Path to YAML configuration file | | `in_pointcloud_topic` | string | `/cloud_in` | Input point cloud topic | | `out_pointcloud_topic` | string | `/cloud_out` | Filtered point cloud topic | | `lidar_sensor_type` | int | `2` | Sensor type (0: XYZ, 1: XYZRGB, 2: Ouster, 3: Hesai, 4: Robosense, 5: Pandar, 6: XYZI) | | `zero_for_removed_points` | bool | `true` | Set filtered points to zero instead of removing | | `use_sim_time` | bool | `true` | Use simulation time | -| `description_name` | string | `/robot_description` | Robot description parameter namespace | ## Configuration @@ -131,6 +146,7 @@ The filter automatically determines shape types from the robot's URDF collision ### Subscribed Topics - `` (sensor_msgs/PointCloud2): Raw point cloud from sensor +- `` (std_msgs/String): Robot URDF description (only when `robot_description` parameter is empty) - `/tf` (tf2_msgs/TFMessage): Transform data - `/tf_static` (tf2_msgs/TFMessage): Static transforms - `/joint_states` (sensor_msgs/JointState): Robot joint positions diff --git a/launch/self_filter.launch.py b/launch/self_filter.launch.py index f4d2142..4999cf3 100644 --- a/launch/self_filter.launch.py +++ b/launch/self_filter.launch.py @@ -7,10 +7,6 @@ from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): - description_name_arg = DeclareLaunchArgument( - 'description_name', - default_value='/robot_description' - ) zero_for_removed_points_arg = DeclareLaunchArgument( 'zero_for_removed_points', default_value='true' @@ -28,7 +24,12 @@ def generate_launch_description(): default_value='/cloud_out' ) robot_description_arg = DeclareLaunchArgument( - 'robot_description' + 'robot_description', + default_value='' + ) + robot_description_topic_arg = DeclareLaunchArgument( + 'robot_description_topic', + default_value='/robot_description' ) filter_config_arg = DeclareLaunchArgument( 'filter_config' @@ -56,24 +57,24 @@ def generate_launch_description(): LaunchConfiguration('robot_description'), value_type=str ), + 'robot_description_topic': LaunchConfiguration('robot_description_topic'), 'zero_for_removed_points': LaunchConfiguration('zero_for_removed_points'), 'use_sim_time': LaunchConfiguration('use_sim_time') # Use the launch argument } ], remappings=[ - ('/robot_description', LaunchConfiguration('description_name')), ('/cloud_in', LaunchConfiguration('in_pointcloud_topic')), ('/cloud_out', LaunchConfiguration('out_pointcloud_topic')), ], ) return LaunchDescription([ - description_name_arg, zero_for_removed_points_arg, lidar_sensor_type_arg, in_pointcloud_topic_arg, out_pointcloud_topic_arg, robot_description_arg, + robot_description_topic_arg, filter_config_arg, use_sim_time_arg, # Add to launch description log_config, diff --git a/src/self_filter.cpp b/src/self_filter.cpp index 3e69368..70702bf 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -55,6 +56,7 @@ namespace robot_self_filter this->declare_parameter("max_queue_size", 10); this->declare_parameter("lidar_sensor_type", 0); this->declare_parameter("robot_description", ""); + this->declare_parameter("robot_description_topic", "/robot_description"); this->declare_parameter("in_pointcloud_topic", "/cloud_in"); sensor_frame_ = this->get_parameter("sensor_frame").as_string(); @@ -91,6 +93,38 @@ namespace robot_self_filter { std::string robot_description_xml = this->get_parameter("robot_description").as_string(); + if (!robot_description_xml.empty()) + { + RCLCPP_INFO(this->get_logger(), "Using robot description from parameter"); + setupFilter(); + return; + } + + std::string topic = this->get_parameter("robot_description_topic").as_string(); + RCLCPP_INFO(this->get_logger(), "No robot_description parameter provided, subscribing to topic: %s", topic.c_str()); + + robot_desc_sub_ = this->create_subscription( + topic, + rclcpp::QoS(1).transient_local(), + std::bind(&SelfFilterNode::robotDescriptionCallback, this, std::placeholders::_1)); + } + + private: + void robotDescriptionCallback(const std_msgs::msg::String::SharedPtr msg) + { + if (msg->data.empty()) + { + RCLCPP_WARN(this->get_logger(), "Received empty robot description from topic, ignoring"); + return; + } + RCLCPP_INFO(this->get_logger(), "Received robot description from topic"); + this->set_parameter(rclcpp::Parameter("robot_description", msg->data)); + robot_desc_sub_.reset(); + setupFilter(); + } + + void setupFilter() + { switch (sensor_type_) { case SensorType::XYZSensor: @@ -129,7 +163,6 @@ namespace robot_self_filter std::bind(&SelfFilterNode::cloudCallback, this, std::placeholders::_1)); } - private: void cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud) { RCLCPP_INFO(this->get_logger(), "Received cloud message with timestamp %.6f", @@ -306,6 +339,7 @@ namespace robot_self_filter std::shared_ptr self_filter_; rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr robot_desc_sub_; rclcpp::Publisher::SharedPtr pointCloudPublisher_; rclcpp::Publisher::SharedPtr marker_pub_; From 6dade9ec76503c0b2337b5d8a63eef1c85269dfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jeldrik=20Schro=CC=88er?= Date: Sun, 15 Feb 2026 00:05:46 +0100 Subject: [PATCH 3/5] Provide sensor_frame via launch file To improve flexibility in multi-sensor scenarios, where only the frame differs, but the parameter file otherwise stays the same. --- README.md | 1 + launch/self_filter.launch.py | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/README.md b/README.md index 11a43f8..607df34 100644 --- a/README.md +++ b/README.md @@ -80,6 +80,7 @@ When `robot_description` is provided as a non-empty parameter, it takes priority | `out_pointcloud_topic` | string | `/cloud_out` | Filtered point cloud topic | | `lidar_sensor_type` | int | `2` | Sensor type (0: XYZ, 1: XYZRGB, 2: Ouster, 3: Hesai, 4: Robosense, 5: Pandar, 6: XYZI) | | `zero_for_removed_points` | bool | `true` | Set filtered points to zero instead of removing | +| `sensor_frame` | string | `Lidar` | TF frame of the sensor | | `use_sim_time` | bool | `true` | Use simulation time | ## Configuration diff --git a/launch/self_filter.launch.py b/launch/self_filter.launch.py index 4999cf3..bc57e01 100644 --- a/launch/self_filter.launch.py +++ b/launch/self_filter.launch.py @@ -34,6 +34,11 @@ def generate_launch_description(): filter_config_arg = DeclareLaunchArgument( 'filter_config' ) + sensor_frame_arg = DeclareLaunchArgument( + 'sensor_frame', + default_value='Lidar', + description='TF frame of the sensor' + ) # Declare use_sim_time argument use_sim_time_arg = DeclareLaunchArgument( 'use_sim_time', @@ -59,6 +64,7 @@ def generate_launch_description(): ), 'robot_description_topic': LaunchConfiguration('robot_description_topic'), 'zero_for_removed_points': LaunchConfiguration('zero_for_removed_points'), + 'sensor_frame': LaunchConfiguration('sensor_frame'), 'use_sim_time': LaunchConfiguration('use_sim_time') # Use the launch argument } ], @@ -76,6 +82,7 @@ def generate_launch_description(): robot_description_arg, robot_description_topic_arg, filter_config_arg, + sensor_frame_arg, use_sim_time_arg, # Add to launch description log_config, self_filter_node From baae84c8cc67b4b785deecd3db80dc825b44b15a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jeldrik=20Schro=CC=88er?= Date: Sun, 15 Feb 2026 01:42:44 +0100 Subject: [PATCH 4/5] Improve logging during startup --- src/self_filter.cpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/self_filter.cpp b/src/self_filter.cpp index 70702bf..eaab881 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -101,12 +101,19 @@ namespace robot_self_filter } std::string topic = this->get_parameter("robot_description_topic").as_string(); - RCLCPP_INFO(this->get_logger(), "No robot_description parameter provided, subscribing to topic: %s", topic.c_str()); + if (!topic.empty() && topic != "") + { + RCLCPP_INFO(this->get_logger(), "Subscribing to robot description topic: %s", topic.c_str()); + + robot_desc_sub_ = this->create_subscription( + topic, + rclcpp::QoS(1).transient_local(), + std::bind(&SelfFilterNode::robotDescriptionCallback, this, std::placeholders::_1)); - robot_desc_sub_ = this->create_subscription( - topic, - rclcpp::QoS(1).transient_local(), - std::bind(&SelfFilterNode::robotDescriptionCallback, this, std::placeholders::_1)); + return; + } + + RCLCPP_ERROR(this->get_logger(), "No robot description provided via parameter or topic"); } private: @@ -120,6 +127,7 @@ namespace robot_self_filter RCLCPP_INFO(this->get_logger(), "Received robot description from topic"); this->set_parameter(rclcpp::Parameter("robot_description", msg->data)); robot_desc_sub_.reset(); + RCLCPP_INFO(this->get_logger(), "Robot description subscription no longer needed, unsubscribed"); setupFilter(); } @@ -155,6 +163,9 @@ namespace robot_self_filter self_filter_->getLinkNames(frames_); + RCLCPP_INFO(this->get_logger(), "Initialized SelfFilter with %zu collision shapes", frames_.size()); + RCLCPP_INFO(this->get_logger(), "Subscribing to point cloud topic: %s", in_topic_.c_str()); + // Subscribe to input cloud with sensor data QoS (BEST_EFFORT) rclcpp::QoS input_qos = rclcpp::SensorDataQoS(); sub_ = this->create_subscription( From 7f814b80925678bbf0d3ff7a05252d59aefb8318 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jeldrik=20Schro=CC=88er?= Date: Sun, 15 Feb 2026 01:56:21 +0100 Subject: [PATCH 5/5] Add out_pointcloud_topic as a parameter to self_filter node Previously, only in_pointcloud_topic was a parameter to the node, while out_pointcloud_topic was a remapping from the launch file. This change enables better logging and more straightforward config. --- launch/self_filter.launch.py | 8 +++----- src/self_filter.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/launch/self_filter.launch.py b/launch/self_filter.launch.py index bc57e01..574ae1c 100644 --- a/launch/self_filter.launch.py +++ b/launch/self_filter.launch.py @@ -57,6 +57,8 @@ def generate_launch_description(): parameters=[ LaunchConfiguration('filter_config'), # loads the YAML file { + 'in_pointcloud_topic': LaunchConfiguration('in_pointcloud_topic'), + 'out_pointcloud_topic': LaunchConfiguration('out_pointcloud_topic'), 'lidar_sensor_type': LaunchConfiguration('lidar_sensor_type'), 'robot_description': ParameterValue( LaunchConfiguration('robot_description'), @@ -67,11 +69,7 @@ def generate_launch_description(): 'sensor_frame': LaunchConfiguration('sensor_frame'), 'use_sim_time': LaunchConfiguration('use_sim_time') # Use the launch argument } - ], - remappings=[ - ('/cloud_in', LaunchConfiguration('in_pointcloud_topic')), - ('/cloud_out', LaunchConfiguration('out_pointcloud_topic')), - ], + ] ) return LaunchDescription([ diff --git a/src/self_filter.cpp b/src/self_filter.cpp index eaab881..0b4af51 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -57,7 +57,8 @@ namespace robot_self_filter this->declare_parameter("lidar_sensor_type", 0); this->declare_parameter("robot_description", ""); this->declare_parameter("robot_description_topic", "/robot_description"); - this->declare_parameter("in_pointcloud_topic", "/cloud_in"); + this->declare_parameter("in_pointcloud_topic", "cloud_in"); + this->declare_parameter("out_pointcloud_topic", "cloud_out"); sensor_frame_ = this->get_parameter("sensor_frame").as_string(); use_rgb_ = this->get_parameter("use_rgb").as_bool(); @@ -65,6 +66,7 @@ namespace robot_self_filter int temp_sensor_type = this->get_parameter("lidar_sensor_type").as_int(); sensor_type_ = static_cast(temp_sensor_type); in_topic_ = this->get_parameter("in_pointcloud_topic").as_string(); + out_topic_ = this->get_parameter("out_pointcloud_topic").as_string(); RCLCPP_INFO(this->get_logger(), "Parameters:"); RCLCPP_INFO(this->get_logger(), " sensor_frame: %s", sensor_frame_.c_str()); @@ -72,6 +74,7 @@ namespace robot_self_filter RCLCPP_INFO(this->get_logger(), " max_queue_size: %d", max_queue_size_); RCLCPP_INFO(this->get_logger(), " lidar_sensor_type: %d", temp_sensor_type); RCLCPP_INFO(this->get_logger(), " in_pointcloud_topic: %s", in_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " out_pointcloud_topic: %s", out_topic_.c_str()); tf_buffer_ = std::make_shared(this->get_clock()); tf_buffer_->setCreateTimerInterface( @@ -83,7 +86,7 @@ namespace robot_self_filter // Publish filtered cloud as sensor data QoS (BEST_EFFORT) for high-rate streams pointCloudPublisher_ = this->create_publisher( - "cloud_out", rclcpp::SensorDataQoS()); + out_topic_, rclcpp::SensorDataQoS()); marker_pub_ = this->create_publisher("collision_shapes", 1); @@ -360,6 +363,7 @@ namespace robot_self_filter int max_queue_size_; std::vector frames_; std::string in_topic_; + std::string out_topic_; }; } // namespace robot_self_filter