feat: add XYZI sensor type and parameterize topics/frame (upstream PR #4)

CMakeLists.txt

@@ -26,11 +26,12 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(urdf REQUIRED)
 find_package(resource_retriever REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(pcl_conversions REQUIRED)
-find_package(pcl_msgs QUIET)  # optional
+find_package(pcl_msgs QUIET) # optional
 find_package(pcl_ros REQUIRED) # or use pure PCL, depending on your usage
 find_package(filters REQUIRED)
 find_package(tinyxml2_vendor REQUIRED)
@@ -43,7 +44,11 @@ find_package(assimp QUIET)
 if(NOT assimp_FOUND)
   message(WARNING "Assimp not found by find_package. Trying pkg-config fallback.")
   find_package(PkgConfig REQUIRED)
-  pkg_check_modules(ASSIMP REQUIRED assimp)
+  pkg_check_modules(
+    ASSIMP
+    REQUIRED
+    assimp
+  )
 endif()
 
 #
@@ -67,7 +72,12 @@ include_directories(
 add_definitions(${PCL_DEFINITIONS})
 if(ASSIMP_FOUND)
   # If the version is above 2.0, set -DASSIMP_UNIFIED_HEADER_NAMES
-  if(NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
+  if(
+    NOT
+    ${ASSIMP_VERSION}
+    VERSION_LESS
+    "2.0.1150"
+  )
     add_definitions(-DASSIMP_UNIFIED_HEADER_NAMES)
     message(STATUS "Assimp version has unified headers")
   else()
@@ -78,24 +88,25 @@ endif()
 #
 # Create libraries
 #
-add_library(robot_geometric_shapes
+add_library(
+  robot_geometric_shapes
   src/load_mesh.cpp
   src/shapes.cpp
   src/bodies.cpp
 )
 
-target_link_libraries(robot_geometric_shapes
+target_link_libraries(
+  robot_geometric_shapes
   ${BULLET_LIBRARIES}
   ${ASSIMP_LIBRARIES}
   ${PCL_LIBRARIES}
   tinyxml2::tinyxml2
 )
 
-add_library(${PROJECT_NAME}
-  src/self_mask.cpp
-)
+add_library(${PROJECT_NAME} src/self_mask.cpp)
 
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(
+  ${PROJECT_NAME}
   robot_geometric_shapes
   ${ASSIMP_LIBRARIES}
 )
@@ -113,11 +124,10 @@ target_link_libraries(${PROJECT_NAME}
 #   ${ASSIMP_LIBRARIES}
 # )
 
-add_executable(self_filter
-  src/self_filter.cpp
-)
+add_executable(self_filter src/self_filter.cpp)
 
-target_link_libraries(self_filter
+target_link_libraries(
+  self_filter
   ${PROJECT_NAME}
   robot_geometric_shapes
   ${ASSIMP_LIBRARIES}
@@ -127,7 +137,8 @@ target_link_libraries(self_filter
 #
 # Link dependencies
 #
-ament_target_dependencies(robot_geometric_shapes
+ament_target_dependencies(
+  robot_geometric_shapes
   rclcpp
   tf2
   tf2_ros
@@ -136,12 +147,13 @@ ament_target_dependencies(robot_geometric_shapes
   urdf
   resource_retriever
   visualization_msgs
-  pcl_ros  # or pcl_conversions
+  pcl_ros # or pcl_conversions
   filters
   tinyxml2_vendor
 )
 
-ament_target_dependencies(${PROJECT_NAME}
+ament_target_dependencies(
+  ${PROJECT_NAME}
   rclcpp
   tf2
   tf2_ros
@@ -150,7 +162,7 @@ ament_target_dependencies(${PROJECT_NAME}
   urdf
   resource_retriever
   visualization_msgs
-  pcl_ros  # or pcl_conversions
+  pcl_ros # or pcl_conversions
   filters
   tinyxml2_vendor
 )
@@ -168,16 +180,18 @@ ament_target_dependencies(${PROJECT_NAME}
 #   filters
 # )
 
-ament_target_dependencies(self_filter
+ament_target_dependencies(
+  self_filter
   rclcpp
   tf2
   tf2_ros
   geometry_msgs
   sensor_msgs
+  std_msgs
   urdf
   resource_retriever
   visualization_msgs
-  pcl_ros  # or pcl_conversions
+  pcl_ros # or pcl_conversions
   filters
   tinyxml2_vendor
 )
@@ -191,9 +205,7 @@ install(
 )
 
 install(
-  DIRECTORY
-    launch
-    params
+  DIRECTORY launch params
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -210,9 +222,7 @@ install(
 #
 # Export and finalize
 #
-ament_export_include_directories(
-  include
-)
+ament_export_include_directories(include)
 
 ament_export_libraries(
   robot_geometric_shapes

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,50 @@ 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). Always overrides `filter_config`, see note below |
+| `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) |
-| `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 |
+| `lidar_sensor_type` | int | `0` | Sensor type (0: XYZ, 1: XYZRGB, 2: Ouster, 3: Hesai, 4: Robosense, 5: Pandar, 6: XYZI) |
+| `zero_for_removed_points` | bool | `false` | 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. Always overrides `filter_config`, see note below |
+
+#### `filter_config` YAML vs. launch arguments
+
+`robot_description_topic`, `in_pointcloud_topic`, `out_pointcloud_topic`, `lidar_sensor_type`,
+`zero_for_removed_points`, and `sensor_frame` follow this priority: an explicit CLI argument
+(e.g. `sensor_frame:=...`) wins, otherwise `filter_config` YAML wins, otherwise the node's own
+declared default (the "Default" column above) applies. Passing a launch argument's default value
+literally does not count as "explicit" for this priority; leave the argument unset instead.
+
+Disclosed default change from previous versions of this launch file: when neither `filter_config`
+nor the CLI sets `zero_for_removed_points` or `lidar_sensor_type`, this launch file used to force
+`true` / `2` (Ouster) unconditionally. It now falls through to the node's own declared default
+(`false` / `0`, the values in the table above) like every other parameter in this section, so a
+launch invocation that omitted these and relied on the old forced values will observe different
+behavior. Set them explicitly (via `filter_config` or the CLI) if you need a specific sensor type
+or removed-point behavior. If `lidar_sensor_type` falls through to its default this way, the node
+logs a `WARN` at startup, since defaulting to XYZ silently drops extra fields (intensity, ring,
+etc.) from sensor types that have them.
+
+Two launch arguments always win over `filter_config`, regardless of what the YAML sets, so
+setting them in `filter_config` has no effect:
+
+- `robot_description`: this can be an arbitrarily large, multi-line URDF/XACRO string, and the
+  CLI-override mechanism above would fold its embedded newlines into spaces and silently corrupt
+  it, so it is intentionally excluded from the YAML-priority behavior. Use the
+  `robot_description_topic` route (see Usage above) if you need this driven by YAML/launch
+  context instead of a literal CLI value.
+- `use_sim_time`: a cross-cutting runtime setting expected to be driven by the overall launch
+  context, not by a per-filter config file.
 
 ## Configuration
 
@@ -131,6 +176,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 +206,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
 
@@ -235,4 +282,4 @@ Lorenzo Terenzi <lterenzi@ethz.ch>
 ## Author
 
 ROS 2 version by Lorenzo Terenzi
-Original ROS 1 version by Eitan Marder-Eppstein
+Original ROS 1 version by Eitan Marder-Eppstein