Update controller server goal checker (#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>
ros-navigation · Oct 12, 2022 · b8bd93f · b8bd93f
1 parent 1c10617
commit b8bd93f
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);
   }
 }