reset all goal checkers on cleanup state (ros-navigation#2354)

ruffsl · Jul 2, 2021 · 667d5d4 · 667d5d4
1 parent 1647035
commit 667d5d4
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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;
 }