From 8751096c81b687594b1852446bc1bb389a52b44e Mon Sep 17 00:00:00 2001 From: Lakshya-04 <100lakshyaagarwal@gmail.com> Date: Thu, 23 Apr 2026 00:02:10 +0530 Subject: [PATCH] refactor: convert SelfFilterNode to LifecycleNode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- CMakeLists.txt | 12 +-- include/robot_self_filter/self_mask.h | 26 +++-- include/robot_self_filter/self_see_filter.h | 90 +++++++++--------- package.xml | 2 + src/self_filter.cpp | 100 +++++++++++++------- 5 files changed, 135 insertions(+), 95 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 59d4490..c0ef138 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,4 @@ -cmake_minimum_required(VERSION 3.8) -cmake_policy(VERSION 3.28) +cmake_minimum_required(VERSION 3.22) project(robot_self_filter) @@ -22,6 +21,7 @@ find_package(yaml-cpp REQUIRED) # ROS 2 packages find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) @@ -170,6 +170,7 @@ ament_target_dependencies(${PROJECT_NAME} ament_target_dependencies(self_filter rclcpp + rclcpp_lifecycle tf2 tf2_ros geometry_msgs @@ -201,10 +202,9 @@ install( TARGETS robot_geometric_shapes ${PROJECT_NAME} +# test_filter self_filter - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) # @@ -217,8 +217,6 @@ ament_export_include_directories( ament_export_libraries( robot_geometric_shapes ${PROJECT_NAME} - ${BULLET_LIBRARIES} - ${ASSIMP_LIBRARIES} ) # Exporting Bullet as a standard ament dependency doesn't typically do anything, diff --git a/include/robot_self_filter/self_mask.h b/include/robot_self_filter/self_mask.h index 668425f..4cdf160 100644 --- a/include/robot_self_filter/self_mask.h +++ b/include/robot_self_filter/self_mask.h @@ -3,6 +3,7 @@ #define ROBOT_SELF_FILTER_SELF_MASK_ #include +#include #include #include #include @@ -140,10 +141,13 @@ class SelfMask public: using PointCloud = pcl::PointCloud; - SelfMask(rclcpp::Node::SharedPtr node, + template + SelfMask(std::shared_ptr node, tf2_ros::Buffer &tf_buffer, const std::vector &links) - : node_(node) + : node_clock_(node->get_clock()) + , node_params_(node->get_node_parameters_interface()) + , node_logger_(node->get_logger()) , tf_buffer_(tf_buffer) { configure(links); @@ -216,14 +220,13 @@ class SelfMask void assumeFrame(const std_msgs::msg::Header &header) { - rclcpp::Time transform_time(header.stamp.sec, header.stamp.nanosec, node_->get_clock()->get_clock_type()); for (auto &sl : bodies_) { try { auto transform_stamped = tf_buffer_.lookupTransform( header.frame_id, sl.name, - transform_time, rclcpp::Duration(std::chrono::milliseconds(100))); + tf2::TimePointZero); tf2::Quaternion q( transform_stamped.transform.rotation.x, transform_stamped.transform.rotation.y, @@ -260,7 +263,6 @@ class SelfMask const double min_sensor_dist) { assumeFrame(header); - rclcpp::Time transform_time(header.stamp.sec, header.stamp.nanosec, node_->get_clock()->get_clock_type()); if (!sensor_frame.empty()) { @@ -268,7 +270,7 @@ class SelfMask { auto transform_stamped = tf_buffer_.lookupTransform( header.frame_id, sensor_frame, - transform_time, rclcpp::Duration(std::chrono::milliseconds(100))); + tf2::TimePointZero); tf2::Vector3 t( transform_stamped.transform.translation.x, transform_stamped.transform.translation.y, @@ -359,15 +361,17 @@ class SelfMask sensor_pos_ = tf2::Vector3(0, 0, 0); std::string content; - if (!node_->get_parameter("robot_description", content) || content.empty()) + rclcpp::Parameter rd_param; + if (!node_params_->get_parameter("robot_description", rd_param) || + (content = rd_param.as_string()).empty()) { - RCLCPP_ERROR(node_->get_logger(), "Robot model not found!"); + RCLCPP_ERROR(node_logger_, "Robot model not found!"); return false; } auto urdfModel = std::make_shared(); if (!urdfModel->initString(content)) { - RCLCPP_ERROR(node_->get_logger(), "Unable to parse URDF!"); + RCLCPP_ERROR(node_logger_, "Unable to parse URDF!"); return false; } @@ -588,7 +592,9 @@ class SelfMask } } - rclcpp::Node::SharedPtr node_; + rclcpp::Clock::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_; + rclcpp::Logger node_logger_; tf2_ros::Buffer &tf_buffer_; tf2::Vector3 sensor_pos_{0, 0, 0}; diff --git a/include/robot_self_filter/self_see_filter.h b/include/robot_self_filter/self_see_filter.h index cd5ab4c..698f6bd 100644 --- a/include/robot_self_filter/self_see_filter.h +++ b/include/robot_self_filter/self_see_filter.h @@ -39,46 +39,51 @@ class SelfFilter : public FilterBase>, public SelfFilter public: using PointCloud = pcl::PointCloud; - explicit SelfFilter(const rclcpp::Node::SharedPtr &node) - : node_(node) - , tf_buffer_(std::make_shared(node_->get_clock())) - , tf_listener_(std::make_shared(*tf_buffer_)) + template + explicit SelfFilter(const std::shared_ptr &node, + std::shared_ptr existing_tf_buffer = nullptr) + : tf_buffer_(existing_tf_buffer + ? existing_tf_buffer + : std::make_shared(node->get_clock())) + , tf_listener_(existing_tf_buffer + ? nullptr + : std::make_shared(*tf_buffer_)) { - node_->declare_parameter("min_sensor_dist", 0.01, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter("keep_organized", false, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter("zero_for_removed_points", false, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter("invert", false, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter("min_sensor_dist", 0.01, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter("keep_organized", false, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter("zero_for_removed_points", false, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter("invert", false, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>("default_box_scale", + node->template declare_parameter>("default_box_scale", {1.0, 1.0, 1.0}, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>("default_box_padding", + node->template declare_parameter>("default_box_padding", {0.01, 0.01, 0.01}, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>("default_cylinder_scale", + node->template declare_parameter>("default_cylinder_scale", {1.0, 1.0}, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>("default_cylinder_padding", + node->template declare_parameter>("default_cylinder_padding", {0.01, 0.01}, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter("default_sphere_scale", 1.0, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter("default_sphere_padding", 0.01, rcl_interfaces::msg::ParameterDescriptor()); - - node_->get_parameter("min_sensor_dist", min_sensor_dist_); - node_->get_parameter("keep_organized", keep_organized_); - node_->get_parameter("zero_for_removed_points", zero_for_removed_points_); - node_->get_parameter("invert", invert_); - - node_->get_parameter("default_box_scale", default_box_scale_); - node_->get_parameter("default_box_padding", default_box_pad_); - node_->get_parameter("default_cylinder_scale", default_cyl_scale_); - node_->get_parameter("default_cylinder_padding", default_cyl_pad_); - node_->get_parameter("default_sphere_scale", default_sphere_scale_); - node_->get_parameter("default_sphere_padding", default_sphere_pad_); - - node_->declare_parameter>( + node->template declare_parameter("default_sphere_scale", 1.0, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter("default_sphere_padding", 0.01, rcl_interfaces::msg::ParameterDescriptor()); + + node->get_parameter("min_sensor_dist", min_sensor_dist_); + node->get_parameter("keep_organized", keep_organized_); + node->get_parameter("zero_for_removed_points", zero_for_removed_points_); + node->get_parameter("invert", invert_); + + node->get_parameter("default_box_scale", default_box_scale_); + node->get_parameter("default_box_padding", default_box_pad_); + node->get_parameter("default_cylinder_scale", default_cyl_scale_); + node->get_parameter("default_cylinder_padding", default_cyl_pad_); + node->get_parameter("default_sphere_scale", default_sphere_scale_); + node->get_parameter("default_sphere_padding", default_sphere_pad_); + + node->template declare_parameter>( "self_see_links.names", std::vector(), rcl_interfaces::msg::ParameterDescriptor() ); std::vector link_names; - node_->get_parameter("self_see_links.names", link_names); + node->get_parameter("self_see_links.names", link_names); std::vector links; for (auto &lname : link_names) @@ -95,33 +100,33 @@ class SelfFilter : public FilterBase>, public SelfFilter std::string padding_key = "self_see_links." + lname + ".padding"; std::string scale_key = "self_see_links." + lname + ".scale"; - node_->declare_parameter>(box_scale_key, + node->template declare_parameter>(box_scale_key, std::vector(), rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>(box_padding_key, + node->template declare_parameter>(box_padding_key, std::vector(), rcl_interfaces::msg::ParameterDescriptor()); - node_->get_parameter(box_scale_key, li.box_scale); - node_->get_parameter(box_padding_key, li.box_padding); + node->get_parameter(box_scale_key, li.box_scale); + node->get_parameter(box_padding_key, li.box_padding); - node_->declare_parameter>(cyl_scale_key, + node->template declare_parameter>(cyl_scale_key, std::vector(), rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter>(cyl_padding_key, + node->template declare_parameter>(cyl_padding_key, std::vector(), rcl_interfaces::msg::ParameterDescriptor()); - node_->get_parameter(cyl_scale_key, li.cylinder_scale); - node_->get_parameter(cyl_padding_key, li.cylinder_padding); + node->get_parameter(cyl_scale_key, li.cylinder_scale); + node->get_parameter(cyl_padding_key, li.cylinder_padding); - node_->declare_parameter(padding_key, default_sphere_pad_, rcl_interfaces::msg::ParameterDescriptor()); - node_->declare_parameter(scale_key, default_sphere_scale_, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter(padding_key, default_sphere_pad_, rcl_interfaces::msg::ParameterDescriptor()); + node->template declare_parameter(scale_key, default_sphere_scale_, rcl_interfaces::msg::ParameterDescriptor()); double link_pad = default_sphere_pad_; double link_scl = default_sphere_scale_; - node_->get_parameter(padding_key, link_pad); - node_->get_parameter(scale_key, link_scl); + node->get_parameter(padding_key, link_pad); + node->get_parameter(scale_key, link_scl); li.padding = link_pad; li.scale = link_scl; links.push_back(li); } - sm_ = std::make_shared>(node_, *tf_buffer_, links); + sm_ = std::make_shared>(node, *tf_buffer_, links); } ~SelfFilter() override = default; @@ -223,7 +228,6 @@ class SelfFilter : public FilterBase>, public SelfFilter } } - rclcpp::Node::SharedPtr node_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; std::shared_ptr> sm_; diff --git a/package.xml b/package.xml index b8cc653..7b6727f 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,7 @@ rclcpp + rclcpp_lifecycle tf2 tf2_ros geometry_msgs @@ -30,6 +31,7 @@ rclcpp + rclcpp_lifecycle tf2 tf2_ros geometry_msgs diff --git a/src/self_filter.cpp b/src/self_filter.cpp index db5a973..d7021c1 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -33,29 +34,38 @@ namespace robot_self_filter PandarSensor = 5, }; - class SelfFilterNode : public rclcpp::Node + class SelfFilterNode : public rclcpp_lifecycle::LifecycleNode { public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + SelfFilterNode() - : Node("self_filter") + : rclcpp_lifecycle::LifecycleNode("self_filter") { try { - this->declare_parameter("use_sim_time", true); - this->set_parameter(rclcpp::Parameter("use_sim_time", true)); + this->declare_parameter("use_sim_time", false); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { } - this->declare_parameter("sensor_frame", "Lidar"); // Default value - // this->set_parameter(rclcpp::Parameter("sensor_frame", "Lidar")); // Removed explicit set + this->declare_parameter("sensor_frame", "Lidar"); this->declare_parameter("use_rgb", false); this->declare_parameter("max_queue_size", 10); this->declare_parameter("lidar_sensor_type", 0); this->declare_parameter("robot_description", ""); this->declare_parameter("in_pointcloud_topic", "/cloud_in"); + // Create the TF buffer + listener immediately in the constructor so it + // accumulates transforms well before the first dock cycle triggers configure. + // This eliminates the warm-up race condition on HIL (sim-time) setups. + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { 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(); @@ -63,66 +73,85 @@ namespace robot_self_filter sensor_type_ = static_cast(temp_sensor_type); in_topic_ = this->get_parameter("in_pointcloud_topic").as_string(); - RCLCPP_INFO(this->get_logger(), "Parameters:"); + RCLCPP_INFO(this->get_logger(), "Configuring SelfFilterNode:"); 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()); - tf_buffer_ = std::make_shared(this->get_clock()); - tf_buffer_->setCreateTimerInterface( - std::make_shared( - this->get_node_base_interface(), - this->get_node_timers_interface())); - tf_listener_ = std::make_shared(*tf_buffer_); - - // Publish filtered cloud as sensor data QoS (BEST_EFFORT) for high-rate streams pointCloudPublisher_ = this->create_publisher( "cloud_out", rclcpp::SensorDataQoS()); marker_pub_ = this->create_publisher("collision_shapes", 1); - } - - void initSelfFilter() - { - std::string robot_description_xml = this->get_parameter("robot_description").as_string(); + // Create SelfFilter (and its TF buffer + listener) once at configure time so + // the TF buffer accumulates transforms continuously — including while inactive + // between dock cycles. Re-creating it on every activate would leave an empty + // buffer on the first processed cloud. switch (sensor_type_) { case SensorType::XYZSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; case SensorType::XYZRGBSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; case SensorType::OusterSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; case SensorType::HesaiSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; case SensorType::RobosenseSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; case SensorType::PandarSensor: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; default: - self_filter_ = std::make_shared>(this->shared_from_this()); + self_filter_ = std::make_shared>(this->shared_from_this(), tf_buffer_); break; } - self_filter_->getLinkNames(frames_); - // Subscribe to input cloud with sensor data QoS (BEST_EFFORT) + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & state) + { + LifecycleNode::on_activate(state); + RCLCPP_INFO(this->get_logger(), "Activating SelfFilterNode — starting point cloud subscription"); rclcpp::QoS input_qos = rclcpp::SensorDataQoS(); sub_ = this->create_subscription( in_topic_, input_qos, std::bind(&SelfFilterNode::cloudCallback, this, std::placeholders::_1)); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) + { + RCLCPP_INFO(this->get_logger(), "Deactivating SelfFilterNode — stopping subscription"); + sub_.reset(); + LifecycleNode::on_deactivate(state); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + self_filter_.reset(); + frames_.clear(); + pointCloudPublisher_.reset(); + marker_pub_.reset(); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) + { + sub_.reset(); + self_filter_.reset(); + return CallbackReturn::SUCCESS; } private: @@ -293,8 +322,8 @@ namespace robot_self_filter std::shared_ptr self_filter_; rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pointCloudPublisher_; - rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pointCloudPublisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr marker_pub_; std::string sensor_frame_; bool use_rgb_; @@ -309,9 +338,10 @@ namespace robot_self_filter int main(int argc, char **argv) { rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; auto node = std::make_shared(); - node->initSelfFilter(); - rclcpp::spin(node); + executor.add_node(node->get_node_base_interface()); + executor.spin(); rclcpp::shutdown(); return 0; }