From ad1e058f41d6c366e5226f9087fae62dca621b49 Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Tue, 2 May 2023 23:40:59 +0200
Subject: [PATCH] [nav2_planner] Fix costmap thread reset on cleanup (#3548)

* remove costmap thread reset on cleanup

* Init costmap thread in on_configure method

* Move costmap_thread init in on_configure method

Signed-off-by: enricosutera <enricosutera@outlook.com>
---
 nav2_controller/src/controller_server.cpp | 4 ++--
 nav2_planner/src/planner_server.cpp       | 5 +++--
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 05bd9da04b..eff250c3a2 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -65,8 +65,6 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()}, "local_costmap",
     get_parameter("use_sim_time").as_bool());
-  // Launch a thread to run the costmap node
-  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 }
 
 ControllerServer::~ControllerServer()
@@ -124,6 +122,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   get_parameter("failure_tolerance", failure_tolerance_);
 
   costmap_ros_->configure();
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 
   try {
     progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 6b1d548171..e8c63fcd1e 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -64,8 +64,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "global_costmap", std::string{get_namespace()}, "global_costmap",
     get_parameter("use_sim_time").as_bool());
-  // Launch a thread to run the costmap node
-  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
 }
 
 PlannerServer::~PlannerServer()
@@ -86,6 +84,9 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   costmap_ros_->configure();
   costmap_ = costmap_ros_->getCostmap();
 
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+
   RCLCPP_DEBUG(
     get_logger(), "Costmap size: %d,%d",
     costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());