Update controller server goal checker (ros-navigation#3240)

* [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>
moss-ag · Nov 8, 2022 · c255ba6 · c255ba6
1 parent c888561
commit c255ba6
Showing 1 changed file with 10 additions and 0 deletions.
diff --git 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);
  }
 }