diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index eab6330d50..2e560fb721 100644 --- 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: