Ensure that we break bonds in lifecycle node/manager prior to rcl_shu…

…tdown.

Partial fix for Issue ros-navigation#3271

Signed-off-by: mbryan <matthew.bryan@dell.com>

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -114,6 +114,13 @@ class LifecycleManager : public rclcpp::Node
    */
   bool resume();
 
+  /**
+   * @brief Perform preshutdown activities before our Context is shutdown.
+   * Note that this is related to our Context's shutdown sequence, not the
+   * lifecycle node state machine or shutdown().
+   */
+  void onRclPreshutdown();
+
   // Support function for creating service clients
   /**
    * @brief Support function for creating service clients
@@ -186,6 +193,14 @@ class LifecycleManager : public rclcpp::Node
    */
   void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  /**
+   * Register our preshutdown callback for this Node's rcl Context.
+   * The callback fires before this Node's Context is shutdown.
+   * Note this is not directly related to the lifecycle state machine or the
+   * shutdown() instance function.
+   */
+  void registerRclPreshutdownCallback();
+
   // Timer thread to look at bond connections
   rclcpp::TimerBase::SharedPtr init_timer_;
   rclcpp::TimerBase::SharedPtr bond_timer_;

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -45,6 +45,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   declare_parameter("bond_respawn_max_duration", 10.0);
   declare_parameter("attempt_respawn_reconnection", true);
 
+  registerRclPreshutdownCallback();
+
   node_names_ = get_parameter("node_names").as_string_array();
   get_parameter("autostart", autostart_);
   double bond_timeout_s;
@@ -378,6 +380,35 @@ LifecycleManager::destroyBondTimer()
   }
 }
 
+void
+LifecycleManager::onRclPreshutdown()
+{
+  RCLCPP_INFO(
+    get_logger(), "Running Nav2 LifecycleManager rcl preshutdown (%s)",
+    this->get_name());
+
+  destroyBondTimer();
+
+  /*
+   * Dropping the bond map is what we really need here, but we drop the others
+   * to prevent the bond map being used. Likewise, squash the service thread.
+   */
+  service_thread_.reset();
+  node_names_.clear();
+  node_map_.clear();
+  bond_map_.clear();
+}
+
+void
+LifecycleManager::registerRclPreshutdownCallback()
+{
+  rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+
+  context->add_pre_shutdown_callback(
+    std::bind(&LifecycleManager::onRclPreshutdown, this)
+  );
+}
+
 void
 LifecycleManager::checkBondConnections()
 {

nav2_util/include/nav2_util/lifecycle_node.hpp

@@ -167,6 +167,13 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
     return nav2_util::CallbackReturn::SUCCESS;
   }
 
+  /**
+   * @brief Perform preshutdown activities before our Context is shutdown.
+   * Note that this is related to our Context's shutdown sequence, not the
+   * lifecycle node state machine.
+   */
+  virtual void on_rcl_preshutdown();
+
   /**
    * @brief Create bond connection to lifecycle manager
    */
@@ -183,6 +190,14 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
    */
   void printLifecycleNodeNotification();
 
+  /**
+   * Register our preshutdown callback for this Node's rcl Context.
+   * The callback fires before this Node's Context is shutdown.
+   * Note this is not directly related to the lifecycle state machine.
+   */
+  void register_rcl_preshutdown_callback();
+  std::unique_ptr<rclcpp::PreShutdownCallbackHandle> rcl_preshutdown_cb_handle_{nullptr};
+
   // Connection to tell that server is still up
   std::unique_ptr<bond::Bond> bond_{nullptr};
 };

nav2_util/src/lifecycle_node.cpp

@@ -36,6 +36,8 @@ LifecycleNode::LifecycleNode(
       bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true));
 
   printLifecycleNodeNotification();
+
+  register_rcl_preshutdown_callback();
 }
 
 LifecycleNode::~LifecycleNode()
@@ -48,6 +50,13 @@ LifecycleNode::~LifecycleNode()
     on_deactivate(get_current_state());
     on_cleanup(get_current_state());
   }
+
+  if (rcl_preshutdown_cb_handle_) {
+    rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+    context->remove_pre_shutdown_callback(*(rcl_preshutdown_cb_handle_.get()));
+    rcl_preshutdown_cb_handle_.reset();
+    destroyBond();
+  }
 }
 
 void LifecycleNode::createBond()
@@ -64,6 +73,24 @@ void LifecycleNode::createBond()
   bond_->start();
 }
 
+void LifecycleNode::on_rcl_preshutdown()
+{
+  RCLCPP_INFO(
+    get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)",
+    this->get_name());
+  destroyBond();
+}
+
+void LifecycleNode::register_rcl_preshutdown_callback()
+{
+  rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+
+  rcl_preshutdown_cb_handle_ = std::make_unique<rclcpp::PreShutdownCallbackHandle>(
+    context->add_pre_shutdown_callback(
+      std::bind(&LifecycleNode::on_rcl_preshutdown, this))
+  );
+}
+
 void LifecycleNode::destroyBond()
 {
   RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());