Skip to content

refactor: convert SelfFilterNode to LifecycleNode#7

Open
Lakshya-04 wants to merge 1 commit into
leggedrobotics:mainfrom
Lakshya-04:production-patches
Open

refactor: convert SelfFilterNode to LifecycleNode#7
Lakshya-04 wants to merge 1 commit into
leggedrobotics:mainfrom
Lakshya-04:production-patches

Conversation

@Lakshya-04
Copy link
Copy Markdown

Summary

Converts SelfFilterNode from rclcpp::Node to rclcpp_lifecycle::LifecycleNode so the filter can integrate with lifecycle-managed perception stacks. Changes gathered from production deployments on mobile manipulators where the self-filter needs to start/stop coordinated with an upstream perception pipeline.

Changes

1. LifecycleNode conversion (src/self_filter.cpp)

  • Inherit from rclcpp_lifecycle::LifecycleNode instead of rclcpp::Node.
  • Move setup logic (parameter reads, subscribers, publishers, self_mask init) from the constructor into on_configure.
  • Logs restructured to announce configuration phase cleanly.

2. TF buffer created in constructor, not on_configure

On sim-time / HIL deployments, the ROS clock ticks slowly at startup and there's a warm-up window where TF messages arrive before on_configure fires. Creating the TF buffer + listener in the constructor ensures early TF history is accumulated:

// In constructor, not on_configure:
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

This eliminated a race where the first dock cycle on sim-time could fail because no TF history was available yet.

3. use_sim_time default: truefalse

The original default targeted simulation workflows. Most real deployments run with wall-clock time; simulation users can still flip via parameter. Matches the default convention for ROS 2 nodes.

4. Build system updates

  • package.xml: add rclcpp_lifecycle to build and exec deps.
  • CMakeLists.txt: add rclcpp_lifecycle to find_package, bump cmake_minimum_required to 3.22 (aligns with ROS 2 Humble's CMake minimum), minor reordering.

Backwards compatibility

LifecycleNode is a superset of Node in message handling but requires explicit configure/activate transitions. Existing callers that launched self_filter as a plain node will now start in the UNCONFIGURED state and need to trigger the transitions explicitly (via ros2 lifecycle set /self_filter_node configure activate or a lifecycle_manager).

If this is a concern, one option is to add a "classic node" build mode guarded by a CMake flag — happy to add that if preferred.

Testing

Deployed for several months on a mobile manipulator (UR10e + custom base + Livox LiDAR) as part of a dock → nvblox → planner pipeline. The TF warm-up fix specifically addressed intermittent first-cycle failures that only reproduced on sim-time.

Happy to split into smaller commits or separate PRs if preferred. Thanks for maintaining this package!

Production patches for HIL / sim-time deployments:

- Convert rclcpp::Node to rclcpp_lifecycle::LifecycleNode so the filter
  can be (de)activated in sync with the rest of the lifecycle-managed
  perception stack (nvblox, etc.)
- Move TF buffer + listener creation from on_configure back into the
  constructor. On sim-time deployments with slow clock startup, creating
  them inside on_configure misses early TF messages and causes a race
  on the first dock cycle — constructor creation accumulates transforms
  from node start.
- Change default use_sim_time from true to false (the wrong default for
  most non-Gazebo deployments; sim users can flip via param).
- Add rclcpp_lifecycle to CMakeLists + package.xml.
- Bump cmake_minimum_required to 3.22 (aligns with ROS 2 Humble minimum
  and simplifies target properties).
- Logs restructured under on_configure with clearer startup message.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant