feat(grid_map_visualization): convert GridMapVisualization to rclcpp_lifecycle::LifecycleNode

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
 )

grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp

@@ -19,6 +19,7 @@
 
 // ROS
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 
 // STD
 #include <memory>
@@ -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,17 +48,39 @@ 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.
    * @param message the grid map message to be visualized.
    */
   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<VisualizationFactory> 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_;
 };
 

grid_map_visualization/package.xml

@@ -20,6 +20,7 @@
   <depend>grid_map_ros</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
   <depend>visualization_msgs</depend>
 

grid_map_visualization/src/GridMapVisualization.cpp

@@ -9,7 +9,6 @@
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
-#include <chrono>
 #include <memory>
 #include <string>
 #include <unordered_set>
@@ -21,52 +20,97 @@ 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<rclcpp::Node>("grid_map_visualization");
-  factory_ = std::make_shared<VisualizationFactory>(nodePtr);
+  declare_parameter("grid_map_topic", std::string("/grid_map"));
+  declare_parameter(visualizationsParameter_, std::vector<std::string>());
+  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<double>(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<grid_map_msgs::msg::GridMap>(
+    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<std::string>());
-  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<rclcpp::Node>(shared_from_this());
+  factory_ = std::make_shared<VisualizationFactory>(node_ptr);
 
   // Configure the visualizations from a configuration stored on the parameter server.
   std::vector<std::string> 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;
   }
 
   std::unordered_set<std::string> 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<std::string>(name + ".type");
+    declare_parameter<std::string>(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<grid_map_msgs::msg::GridMap>(
-      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;

grid_map_visualization/src/grid_map_visualization_node.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <grid_map_visualization/GridMapVisualization.hpp>
 
 #include <memory>
@@ -16,7 +17,7 @@ int main(int argc, char ** argv)
   rclcpp::init(argc, argv);
   auto node = std::make_shared<grid_map_visualization::GridMapVisualization>(
     "grid_map_visualizations");
-  rclcpp::spin(node->nodePtr);
+  rclcpp::spin(node->get_node_base_interface());
   rclcpp::shutdown();
   return 0;
 }