Copy fix-terminate diff from opennav_docking repo

Signed-off-by: redvinaa <[email protected]>

nav2_docking/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"

nav2_docking/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"
@@ -67,11 +68,14 @@ class Navigator
    * May throw exception if fails to navigate or communicate
    * Blocks until completion.
    * @param pose Pose to go to
-   * @param max_staging_duration Maximum time to get to the staging pose
+   * @param remaining_staging_duration Remaining time to get to the staging pose
+   * @param isPreempted Function to check if preempted
+   * @param recursed True if recursed (used to retry once)
    */
   void goToPose(
     const geometry_msgs::msg::PoseStamped & pose,
-    const rclcpp::Duration & max_staging_duration,
+    rclcpp::Duration remaining_staging_duration,
+    std::function<bool()> isPreempted,
     bool recursed = false);
 
 protected:

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -253,8 +253,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");
     }
 

nav2_docking/opennav_docking/src/navigator.cpp

@@ -51,13 +51,16 @@ void Navigator::deactivate()
 
 void Navigator::goToPose(
   const geometry_msgs::msg::PoseStamped & pose,
-  const rclcpp::Duration & max_staging_duration,
+  rclcpp::Duration remaining_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,41 @@ 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 > remaining_staging_duration) {
+        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 timed out.");
+      }
+
+      if (executor_.spin_until_future_complete(
+          future_result, 10ms) == rclcpp::FutureReturnCode::SUCCESS)
+      {
+        auto result = future_result.get();
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED &&
+          result.result->error_code == 0)
+        {
+          return;  // Success!
+        } else {
+          RCLCPP_WARN(node->get_logger(), "Navigation request to staging pose failed.");
+          break;
+        }
       }
     }
   }
 
   // Attempt to retry once using single iteration recursion
   if (!recursed) {
-    goToPose(pose, max_staging_duration, true);
+    auto elapsed_time = node->now() - start_time;
+    remaining_staging_duration = remaining_staging_duration - elapsed_time;
+    goToPose(pose, remaining_staging_duration, isPreempted, true);
     return;
   }
 

nav2_docking/opennav_docking/test/test_navigator.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/simple_action_server.hpp"
@@ -94,26 +96,35 @@ TEST(NavigatorTests, TestNavigator)
   auto navigator = std::make_unique<Navigator>(node);
   navigator->activate();
 
+  std::function<bool()> is_preempted_true = []() {return true;};
+  std::function<bool()> is_preempted_false = []() {return false;};
+
   // Should succeed, action server set to succeed
   dummy_navigator_node->setReturn(true);
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false));
 
   // Should fail, timeout exceeded
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0), is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // Should fail, action server set to succeed
   dummy_navigator_node->setReturn(false);
   EXPECT_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)),
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false),
     opennav_docking_core::FailedToStage);
 
   // First should fail, recursion should succeed
   dummy_navigator_node->setToggle();
   EXPECT_NO_THROW(
-    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)));
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false));
+
+  // Should fail, preempted
+  dummy_navigator_node->setReturn(true);
+  EXPECT_THROW(
+    navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_true),
+    opennav_docking_core::FailedToStage);
 
   navigator->deactivate();
   navigator.reset();