From 93d8706877aabdd1d37de85fe2f479e945cd7c8d Mon Sep 17 00:00:00 2001
From: mbryan <matthew.bryan@dell.com>
Date: Wed, 18 Jan 2023 15:55:54 +0700
Subject: [PATCH] Ensure planner_server shuts down cleanly on SIGINT.

Needed to ensure code coverage flushes. See Issue #3271.
---
 .../include/nav2_planner/planner_server.hpp       |  7 +++++++
 nav2_planner/src/planner_server.cpp               | 15 +++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
index fee711f974a..282ca366f73 100644
--- a/nav2_planner/include/nav2_planner/planner_server.hpp
+++ b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -189,6 +189,13 @@ class PlannerServer : public nav2_util::LifecycleNode
    */
   void computePlan();
 
+  /**
+   * @brief Perform destruction activity also performed by rcl preshutdown.
+   */
+  void prepDestruction();
+
+  void on_rcl_preshutdown() override;
+
   /**
    * @brief The action server callback which calls planner to get the path
    * ComputePathThroughPoses
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index f58bdac31fd..0da3faaa7f4 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -67,12 +67,27 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 }
 
+void
+PlannerServer::prepDestruction()
+{
+  planners_.clear();
+  costmap_thread_.reset();
+}
+
 PlannerServer::~PlannerServer()
 {
+  prepDestruction();
   planners_.clear();
   costmap_thread_.reset();
 }
 
+
+void PlannerServer::on_rcl_preshutdown()
+{
+  prepDestruction();
+  LifecycleNode::on_rcl_preshutdown();
+}
+
 nav2_util::CallbackReturn
 PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {