Check preempted in navigating to staging pose

opennav_docking/include/opennav_docking/docking_server.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <mutex>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"

opennav_docking/include/opennav_docking/navigator.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
@@ -72,6 +73,7 @@ class Navigator
   void goToPose(
     const geometry_msgs::msg::PoseStamped & pose,
     const rclcpp::Duration & max_staging_duration,
+    std::function<bool()> isPreempted,
     bool recursed = false);
 
 protected:

opennav_docking/src/docking_server.cpp

@@ -246,8 +246,15 @@ void DockingServer::dockRobot()
     {
       RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock");
     } else {
+      std::function<bool()> isPreempted = [this]() {
+        return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
+        checkAndWarnIfPreempted(docking_action_server_, "dock_robot");
+      };
+
       navigator_->goToPose(
-        initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
+        initial_staging_pose,
+        rclcpp::Duration::from_seconds(goal->max_staging_time),
+        isPreempted);
       RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
     }
 

opennav_docking/src/navigator.cpp

@@ -52,12 +52,15 @@ void Navigator::deactivate()
 void Navigator::goToPose(
   const geometry_msgs::msg::PoseStamped & pose,
   const rclcpp::Duration & max_staging_duration,
+  std::function<bool()> isPreempted,
   bool recursed)
 {
+  auto node = node_.lock();
+
   Nav2Pose::Goal goal;
   goal.pose = pose;
   goal.behavior_tree = navigator_bt_xml_;
-  const auto timeout = max_staging_duration.to_chrono<std::chrono::milliseconds>();
+  const auto start_time = node->now();
 
   // Wait for server to be active
   nav_to_pose_client_->wait_for_action_server(1s);
@@ -66,19 +69,37 @@ void Navigator::goToPose(
       future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS)
   {
     auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get());
-    if (executor_.spin_until_future_complete(
-        future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS)
-    {
-      auto result = future_result.get();
-      if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) {
-        return;  // Success!
+
+    while (rclcpp::ok()) {
+      if (isPreempted()) {
+        auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        executor_.spin_until_future_complete(cancel_future, 1s);
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose preempted.");
+      }
+
+      if (node->now() - start_time > max_staging_duration) {
+        nav_to_pose_client_->async_cancel_goal(future_goal_handle.get());
+        throw opennav_docking_core::FailedToStage("Navigation request to staging pose timed out.");
+      }
+
+      if (executor_.spin_until_future_complete(
+          future_result, 100ms) == rclcpp::FutureReturnCode::SUCCESS)
+      {
+        auto result = future_result.get();
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED &&
+          result.result->error_code == 0)
+        {
+          return;  // Success!
+        } else {
+          throw opennav_docking_core::FailedToStage("Navigation request to staging pose failed.");
+        }
       }
     }
   }
 
   // Attempt to retry once using single iteration recursion
   if (!recursed) {
-    goToPose(pose, max_staging_duration, true);
+    goToPose(pose, max_staging_duration, isPreempted, true);
     return;
   }