From 1ae2372a1af6f8bfffeb017444782bc99aaf2583 Mon Sep 17 00:00:00 2001 From: Lakshya-04 <100lakshyaagarwal@gmail.com> Date: Sat, 14 Mar 2026 11:36:51 +0530 Subject: [PATCH] feat(grid_map_visualization): convert to rclcpp_lifecycle::LifecycleNode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit on_configure: reads parameters, creates VisualizationFactory and all configured visualizations on_activate: creates grid map subscription and starts processing on_deactivate: resets subscription to stop processing and save CPU on_cleanup/on_shutdown: teardown visualizations and factory Removes the wall-timer-based lazy subscription mechanism — lifecycle state transitions now control when the node subscribes to the grid map topic, giving external controllers explicit management of visualization CPU cost. --- grid_map_visualization/CMakeLists.txt | 2 + .../GridMapVisualization.hpp | 56 ++++--- grid_map_visualization/package.xml | 1 + .../src/GridMapVisualization.cpp | 146 ++++++++++-------- .../src/grid_map_visualization_node.cpp | 3 +- 5 files changed, 116 insertions(+), 92 deletions(-) diff --git a/grid_map_visualization/CMakeLists.txt b/grid_map_visualization/CMakeLists.txt index f46704c14..d17b4b3c9 100644 --- a/grid_map_visualization/CMakeLists.txt +++ b/grid_map_visualization/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(grid_map_msgs REQUIRED) find_package(grid_map_ros REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(Eigen3 REQUIRED) @@ -21,6 +22,7 @@ set(dependencies grid_map_ros nav_msgs rclcpp + rclcpp_lifecycle sensor_msgs visualization_msgs ) diff --git a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp index 674c7ba91..6f3e61e90 100644 --- a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp @@ -19,6 +19,7 @@ // ROS #include +#include // STD #include @@ -30,10 +31,14 @@ namespace grid_map_visualization /*! * Visualizes a grid map by publishing different topics that can be viewed in Rviz. + * Implemented as a lifecycle node so the visualization pipeline can be activated + * only when needed, saving CPU when no operator is monitoring. */ -class GridMapVisualization +class GridMapVisualization : public rclcpp_lifecycle::LifecycleNode { public: + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + /*! * Constructor. * @param parameterName The config parameter name. @@ -43,7 +48,32 @@ class GridMapVisualization /*! * Destructor. */ - virtual ~GridMapVisualization(); + virtual ~GridMapVisualization() = default; + + /*! + * Reads parameters and sets up visualizations. + */ + CallbackReturn on_configure(const rclcpp_lifecycle::State & state); + + /*! + * Creates the grid map subscription and starts processing. + */ + CallbackReturn on_activate(const rclcpp_lifecycle::State & state); + + /*! + * Destroys the grid map subscription and stops processing. + */ + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state); + + /*! + * Cleans up visualizations and factory. + */ + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state); + + /*! + * Shuts down the node. + */ + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state); /*! * Callback function for the grid map. @@ -51,9 +81,6 @@ class GridMapVisualization */ void callback(const grid_map_msgs::msg::GridMap::SharedPtr message); - //! ROS node shared pointer - rclcpp::Node::SharedPtr nodePtr; - private: /*! * Read parameters from ROS. @@ -67,14 +94,6 @@ class GridMapVisualization */ bool initialize(); - /*! - * Check if visualizations are active (subscribed to), - * and accordingly cancels/activates the subscription to the - * grid map to save bandwidth. - * @param timerEvent the timer event. - */ - void updateSubscriptionCallback(); - //! Parameter name of the visualizer configuration list. std::string visualizationsParameter_; @@ -90,16 +109,7 @@ class GridMapVisualization //! Visualization factory. std::shared_ptr factory_; - //! Timer to check the activity of the visualizations. - rclcpp::TimerBase::SharedPtr activityCheckTimer_; - - //! Rate of checking the activity of the visualizations. - double activityCheckRate_; - - //! If the grid map visualization is subscribed to the grid map. - bool isSubscribed_; - - //! If the grid map subscriber is Transient local. + //! If the grid map subscriber uses Transient Local durability. bool isGridMapSubLatched_; }; diff --git a/grid_map_visualization/package.xml b/grid_map_visualization/package.xml index 8cef45fe0..a1766f5bd 100644 --- a/grid_map_visualization/package.xml +++ b/grid_map_visualization/package.xml @@ -20,6 +20,7 @@ grid_map_ros nav_msgs rclcpp + rclcpp_lifecycle sensor_msgs visualization_msgs diff --git a/grid_map_visualization/src/GridMapVisualization.cpp b/grid_map_visualization/src/GridMapVisualization.cpp index 91a57f65b..79fc5fd80 100644 --- a/grid_map_visualization/src/GridMapVisualization.cpp +++ b/grid_map_visualization/src/GridMapVisualization.cpp @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -21,44 +20,89 @@ namespace grid_map_visualization { GridMapVisualization::GridMapVisualization(const std::string & parameterName) -: visualizationsParameter_(parameterName), - isSubscribed_(false) +: rclcpp_lifecycle::LifecycleNode("grid_map_visualization"), + visualizationsParameter_(parameterName), + isGridMapSubLatched_(false) { - nodePtr = std::make_shared("grid_map_visualization"); - factory_ = std::make_shared(nodePtr); + declare_parameter("grid_map_topic", std::string("/grid_map")); + declare_parameter(visualizationsParameter_, std::vector()); + declare_parameter("transient_local", rclcpp::ParameterValue(false)); +} + +GridMapVisualization::CallbackReturn GridMapVisualization::on_configure( + const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring grid map visualization."); - RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization nodePtr started."); - readParameters(); + if (!readParameters()) { + return CallbackReturn::FAILURE; + } + if (!initialize()) { + return CallbackReturn::FAILURE; + } - activityCheckTimer_ = nodePtr->create_wall_timer( - std::chrono::duration(1.0 / activityCheckRate_), - std::bind(&GridMapVisualization::updateSubscriptionCallback, this)); - initialize(); + RCLCPP_INFO(get_logger(), "Grid map visualization configured."); + return CallbackReturn::SUCCESS; } -GridMapVisualization::~GridMapVisualization() +GridMapVisualization::CallbackReturn GridMapVisualization::on_activate( + const rclcpp_lifecycle::State & state) { + LifecycleNode::on_activate(state); + + rclcpp::QoS qos_setting = rclcpp::SystemDefaultsQoS(); + if (isGridMapSubLatched_) { + qos_setting = rclcpp::QoS(1).transient_local(); + } + + mapSubscriber_ = create_subscription( + mapTopic_, qos_setting, + std::bind(&GridMapVisualization::callback, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Subscribed to grid map at '%s'.", mapTopic_.c_str()); + return CallbackReturn::SUCCESS; } -bool GridMapVisualization::readParameters() +GridMapVisualization::CallbackReturn GridMapVisualization::on_deactivate( + const rclcpp_lifecycle::State & state) +{ + mapSubscriber_.reset(); + RCLCPP_INFO(get_logger(), "Cancelled subscription to grid map."); + LifecycleNode::on_deactivate(state); + return CallbackReturn::SUCCESS; +} + +GridMapVisualization::CallbackReturn GridMapVisualization::on_cleanup( + const rclcpp_lifecycle::State & /*state*/) { - nodePtr->declare_parameter("grid_map_topic", std::string("/grid_map")); - nodePtr->declare_parameter("activity_check_rate", 2.0); - nodePtr->declare_parameter(visualizationsParameter_, std::vector()); - nodePtr->declare_parameter("transient_local", rclcpp::ParameterValue(false)); + visualizations_.clear(); + factory_.reset(); + return CallbackReturn::SUCCESS; +} + +GridMapVisualization::CallbackReturn GridMapVisualization::on_shutdown( + const rclcpp_lifecycle::State & /*state*/) +{ + mapSubscriber_.reset(); + visualizations_.clear(); + factory_.reset(); + return CallbackReturn::SUCCESS; +} - nodePtr->get_parameter("grid_map_topic", mapTopic_); - nodePtr->get_parameter("activity_check_rate", activityCheckRate_); - nodePtr->get_parameter("transient_local", isGridMapSubLatched_); +bool GridMapVisualization::readParameters() +{ + get_parameter("grid_map_topic", mapTopic_); + get_parameter("transient_local", isGridMapSubLatched_); - assert(activityCheckRate_); + auto node_ptr = std::static_pointer_cast(shared_from_this()); + factory_ = std::make_shared(node_ptr); // Configure the visualizations from a configuration stored on the parameter server. std::vector config; - if (!nodePtr->get_parameter(visualizationsParameter_, config)) { + if (!get_parameter(visualizationsParameter_, config)) { RCLCPP_WARN( - nodePtr->get_logger(), - "Could not load the visualizations configuration from parameter %s,are you sure it" + get_logger(), + "Could not load the visualizations configuration from parameter %s, are you sure it " "was pushed to the parameter server? Assuming that you meant to leave it empty.", visualizationsParameter_.c_str()); return false; @@ -66,7 +110,7 @@ bool GridMapVisualization::readParameters() std::unordered_set config_check; - // Iterate over all visualizations (may be just one), + // Iterate over all visualizations (may be just one). for (auto name : config) { std::string type; @@ -75,24 +119,24 @@ bool GridMapVisualization::readParameters() config_check.insert(name); } else { RCLCPP_ERROR( - nodePtr->get_logger(), + get_logger(), "%s: A visualization with the name '%s' already exists.", visualizationsParameter_.c_str(), name.c_str()); return false; } - nodePtr->declare_parameter(name + ".type"); + declare_parameter(name + ".type"); try { - if (!nodePtr->get_parameter(name + ".type", type)) { + if (!get_parameter(name + ".type", type)) { RCLCPP_ERROR( - nodePtr->get_logger(), + get_logger(), "%s: Could not add a visualization because no type was given", name.c_str()); return false; } } catch (const rclcpp::ParameterTypeException & e) { RCLCPP_ERROR( - nodePtr->get_logger(), + get_logger(), "Could not add %s visualization, because the %s.type parameter is not a string.", name.c_str(), name.c_str()); return false; @@ -101,7 +145,7 @@ bool GridMapVisualization::readParameters() // Make sure the visualization has a valid type. if (!factory_->isValidType(type)) { RCLCPP_ERROR( - nodePtr->get_logger(), + get_logger(), "Could not add %s visualization, no visualization of type '%s' found.", name.c_str(), type.c_str()); return false; @@ -111,7 +155,7 @@ bool GridMapVisualization::readParameters() visualization->readParameters(); visualizations_.push_back(visualization); RCLCPP_INFO( - nodePtr->get_logger(), "%s: Configured visualization of type '%s' with name '%s'.", + get_logger(), "%s: Configured visualization of type '%s' with name '%s'.", visualizationsParameter_.c_str(), type.c_str(), name.c_str()); } return true; @@ -122,48 +166,14 @@ bool GridMapVisualization::initialize() for (auto & visualization : visualizations_) { visualization->initialize(); } - - updateSubscriptionCallback(); - RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization initialized."); + RCLCPP_INFO(get_logger(), "Grid map visualization initialized."); return true; } -void GridMapVisualization::updateSubscriptionCallback() -{ - bool isActive = false; - - for (auto & visualization : visualizations_) { - if (visualization->isActive()) { - isActive = true; - break; - } - } - - if (!isSubscribed_ && isActive) { - rclcpp::QoS qos_setting = rclcpp::SystemDefaultsQoS(); - - if (isGridMapSubLatched_) { - qos_setting = rclcpp::QoS(1).transient_local(); - } - - mapSubscriber_ = nodePtr->create_subscription( - mapTopic_, qos_setting, - std::bind(&GridMapVisualization::callback, this, std::placeholders::_1)); - - isSubscribed_ = true; - RCLCPP_DEBUG(nodePtr->get_logger(), "Subscribed to grid map at '%s'.", mapTopic_.c_str()); - } - if (isSubscribed_ && !isActive) { - mapSubscriber_.reset(); - isSubscribed_ = false; - RCLCPP_DEBUG(nodePtr->get_logger(), "Cancelled subscription to grid map."); - } -} - void GridMapVisualization::callback(const grid_map_msgs::msg::GridMap::SharedPtr message) { RCLCPP_DEBUG( - nodePtr->get_logger(), + get_logger(), "Grid map visualization received a map (timestamp %f) for visualization.", rclcpp::Time(message->header.stamp).seconds()); grid_map::GridMap map; diff --git a/grid_map_visualization/src/grid_map_visualization_node.cpp b/grid_map_visualization/src/grid_map_visualization_node.cpp index 743b8026e..adf77d8b5 100644 --- a/grid_map_visualization/src/grid_map_visualization_node.cpp +++ b/grid_map_visualization/src/grid_map_visualization_node.cpp @@ -7,6 +7,7 @@ */ #include +#include #include #include @@ -16,7 +17,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto node = std::make_shared( "grid_map_visualizations"); - rclcpp::spin(node->nodePtr); + rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); return 0; }