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); } }