Fixed timed_behavior.hpp (#4602)

Signed-off-by: Vladyslav Hrynchak <[email protected]>
ros-navigation · Aug 23, 2024 · 9c2e770 · 9c2e770
1 parent a750f8c
commit 9c2e770
Showing 1 changed file with 9 additions and 9 deletions.
diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -237,15 +237,6 @@ class TimedBehavior : public nav2_core::Behavior
 
     while (rclcpp::ok()) {
       elasped_time_ = clock_->now() - start_time;
-      if (action_server_->is_cancel_requested()) {
-        RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
-        stopRobot();
-        result->total_elapsed_time = elasped_time_;
-        onActionCompletion(result);
-        action_server_->terminate_all(result);
-        return;
-      }
-
       // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping
       if (action_server_->is_preempt_requested()) {
         RCLCPP_ERROR(
@@ -259,6 +250,15 @@ class TimedBehavior : public nav2_core::Behavior
         return;
       }
 
+      if (action_server_->is_cancel_requested()) {
+        RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
+        stopRobot();
+        result->total_elapsed_time = elasped_time_;
+        onActionCompletion(result);
+        action_server_->terminate_all(result);
+        return;
+      }
+
       ResultStatus on_cycle_update_result = onCycleUpdate();
       switch (on_cycle_update_result.status) {
         case Status::SUCCEEDED: