From b8bd93fa658ef37bf25c64df478cc3b0cec0c608 Mon Sep 17 00:00:00 2001 From: Nicolas Rocha Pacheco Date: Wed, 12 Oct 2022 15:07:26 -0500 Subject: [PATCH] Update controller server goal checker (#3240) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> --- nav2_controller/src/controller_server.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2d017b1186..1712453b6b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -528,6 +528,16 @@ void ControllerServer::updateGlobalPath() action_server_->terminate_current(); return; } + std::string current_goal_checker; + if (findGoalCheckerId(goal->goal_checker_id, current_goal_checker)) { + current_goal_checker_ = current_goal_checker; + } else { + RCLCPP_INFO( + get_logger(), "Terminating action, invalid goal checker %s requested.", + goal->goal_checker_id.c_str()); + action_server_->terminate_current(); + return; + } setPlannerPath(goal->path); } }