From 400b4e4b7d6a3e3c17c43933267c59ff596d8fd3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 30 Jun 2023 10:17:34 +0200 Subject: [PATCH] Merge pull request #34 from logivations/backport-reset AMRNAV-4963 add a controller reset (#3326) --- nav2_controller/src/controller_server.cpp | 6 ++++++ nav2_core/include/nav2_core/controller.hpp | 5 +++++ 2 files changed, 11 insertions(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 247b1d31a53..95bf680e1f1 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -583,6 +583,12 @@ void ControllerServer::publishZeroVelocity() velocity.header.frame_id = costmap_ros_->getBaseFrameID(); velocity.header.stamp = now(); publishVelocity(velocity); + + // Reset the state of the controllers after the task has ended + ControllerMap::iterator it; + for (it = controllers_.begin(); it != controllers_.end(); ++it) { + it->second->reset(); + } } bool ControllerServer::isGoalReached() diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 4e89e0b1177..dab79176e94 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -124,6 +124,11 @@ class Controller * or in absolute values in false case. */ virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0; + + /** + * @brief Reset the state of the controller if necessary after task is exited + */ + virtual void reset() {} }; } // namespace nav2_core