Update controller server goal checker (#3240)

NicolasRochaPacheco · pepisg · SteveMacenski · commit 28474a92756a · 2022-11-08T15:15:00.000-08:00
* [FIX] Update controller server goal checker

* [FIX] Autoformat code

* [FIX] Misplaced tabs.

Co-authored-by: Pedro Alejandro González &lt;71234974+pepisg@users.noreply.github.com&gt;
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);
   }
 }

Original file line number	Diff line number	Diff line change
`@@ -528,6 +528,16 @@ void ControllerServer::updateGlobalPath()`
`528`	`528`	`action_server_->terminate_current();`
`529`	`529`	`return;`
`530`	`530`	`}`
	`531`	`+ std::string current_goal_checker;`
	`532`	`+ if (findGoalCheckerId(goal->goal_checker_id, current_goal_checker)) {`
	`533`	`+ current_goal_checker_ = current_goal_checker;`
	`534`	`+ } else {`
	`535`	`+ RCLCPP_INFO(`
	`536`	`+ get_logger(), "Terminating action, invalid goal checker %s requested.",`
	`537`	`+ goal->goal_checker_id.c_str());`
	`538`	`+ action_server_->terminate_current();`
	`539`	`+ return;`
	`540`	`+ }`
`531`	`541`	`setPlannerPath(goal->path);`
`532`	`542`	`}`
`533`	`543`	`}`