From 086159338a615977bc409f0e1a90b1f9a5b0bcd3 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 20 May 2021 13:57:47 -0700
Subject: [PATCH] reset all goal checkers on cleanup state

---
 nav2_controller/src/nav2_controller.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index 82781aab30e..1c13117d5be 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -256,6 +256,8 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
     it->second->cleanup();
   }
   controllers_.clear();
+
+  goal_checkers_.clear();
   costmap_ros_->on_cleanup(state);
 
   // Release any allocated resources
@@ -264,7 +266,6 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
   vel_publisher_.reset();
   speed_limit_sub_.reset();
   action_server_.reset();
-  goal_checkers_[current_goal_checker_]->reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }