Subscribe to /robot_description, provide sensor_frame via launch file and add XYZI sensor type

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,21 @@ 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) |
+| `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 |
-| `description_name` | string | `/robot_description` | Robot description parameter namespace |
 
 ## Configuration
 
@@ -131,6 +147,7 @@ The filter automatically determines shape types from the robot's URDF collision
 ### Subscribed Topics
 
 - `<in_pointcloud_topic>` (sensor_msgs/PointCloud2): Raw point cloud from sensor
+- `<robot_description_topic>` (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
@@ -160,6 +177,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
 

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,11 +24,21 @@ 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'
     )
+    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',
@@ -51,30 +57,30 @@ 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'),
                     value_type=str
                 ),
+                '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
             }
-        ],
-        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,
+        sensor_frame_arg,
         use_sim_time_arg, # Add to launch description
         log_config,
         self_filter_node

src/self_filter.cpp

@@ -5,6 +5,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/create_timer_ros.h>
 #include <tf2_ros/transform_listener.h>
@@ -31,6 +32,7 @@ namespace robot_self_filter
     HesaiSensor = 3,
     RobosenseSensor = 4,
     PandarSensor = 5,
+    XYZISensor = 6,
   };
 
   class SelfFilterNode : public rclcpp::Node
@@ -54,21 +56,25 @@ namespace robot_self_filter
       this->declare_parameter<int>("max_queue_size", 10);
       this->declare_parameter<int>("lidar_sensor_type", 0);
       this->declare_parameter<std::string>("robot_description", "");
-      this->declare_parameter<std::string>("in_pointcloud_topic", "/cloud_in");
+      this->declare_parameter<std::string>("robot_description_topic", "/robot_description");
+      this->declare_parameter<std::string>("in_pointcloud_topic", "cloud_in");
+      this->declare_parameter<std::string>("out_pointcloud_topic", "cloud_out");
 
       sensor_frame_ = this->get_parameter("sensor_frame").as_string();
       use_rgb_ = this->get_parameter("use_rgb").as_bool();
       max_queue_size_ = this->get_parameter("max_queue_size").as_int();
       int temp_sensor_type = this->get_parameter("lidar_sensor_type").as_int();
       sensor_type_ = static_cast<SensorType>(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());
       RCLCPP_INFO(this->get_logger(), "  use_rgb: %s", use_rgb_ ? "true" : "false");
       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<tf2_ros::Buffer>(this->get_clock());
       tf_buffer_->setCreateTimerInterface(
@@ -80,7 +86,7 @@ namespace robot_self_filter
       // Publish filtered cloud as sensor data QoS (BEST_EFFORT) for high-rate streams
       pointCloudPublisher_ =
           this->create_publisher<sensor_msgs::msg::PointCloud2>(
-              "cloud_out", rclcpp::SensorDataQoS());
+              out_topic_, rclcpp::SensorDataQoS());
 
       marker_pub_ =
           this->create_publisher<visualization_msgs::msg::MarkerArray>("collision_shapes", 1);
@@ -90,6 +96,46 @@ 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();
+      if (!topic.empty() && topic != "")
+      {
+        RCLCPP_INFO(this->get_logger(), "Subscribing to robot description topic: %s", topic.c_str());
+
+        robot_desc_sub_ = this->create_subscription<std_msgs::msg::String>(
+            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:
+    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();
+      RCLCPP_INFO(this->get_logger(), "Robot description subscription no longer needed, unsubscribed");
+      setupFilter();
+    }
+
+    void setupFilter()
+    {
       switch (sensor_type_)
       {
       case SensorType::XYZSensor:
@@ -110,13 +156,19 @@ namespace robot_self_filter
       case SensorType::PandarSensor:
         self_filter_ = std::make_shared<filters::SelfFilter<PointPandar>>(this->shared_from_this());
         break;
+      case SensorType::XYZISensor:
+        self_filter_ = std::make_shared<filters::SelfFilter<pcl::PointXYZI>>(this->shared_from_this());
+        break;
       default:
         self_filter_ = std::make_shared<filters::SelfFilter<pcl::PointXYZ>>(this->shared_from_this());
         break;
       }
 
       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<sensor_msgs::msg::PointCloud2>(
@@ -125,7 +177,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",
@@ -161,6 +212,15 @@ namespace robot_self_filter
         publishShapesFromMask(mask, cloud->header.frame_id);
         break;
       }
+      case SensorType::XYZISensor:
+      {
+        auto sf_xyzi = std::dynamic_pointer_cast<filters::SelfFilter<pcl::PointXYZI>>(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;
@@ -293,6 +353,7 @@ namespace robot_self_filter
     std::shared_ptr<filters::SelfFilterInterface> self_filter_;
 
     rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_desc_sub_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudPublisher_;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
 
@@ -302,6 +363,7 @@ namespace robot_self_filter
     int max_queue_size_;
     std::vector<std::string> frames_;
     std::string in_topic_;
+    std::string out_topic_;
   };
 
 } // namespace robot_self_filter