From 0cec123eb2dc4330e24b5a4842e47cae191a9e87 Mon Sep 17 00:00:00 2001
From: Aitor Miguel Blanco <aitormibl@gmail.com>
Date: Mon, 20 Apr 2020 17:35:02 +1000
Subject: [PATCH] Added preemption to recovery server

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
---
 .../include/nav2_behavior_tree/bt_action_node.hpp    | 11 +++++++++++
 nav2_behavior_tree/plugins/action/spin_action.cpp    | 10 ++++++++++
 nav2_behavior_tree/plugins/action/wait_action.cpp    | 10 ++++++++++
 nav2_bt_navigator/src/bt_navigator.cpp               |  2 ++
 nav2_recoveries/include/nav2_recoveries/recovery.hpp | 12 +++++++-----
 5 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index c6e606bedb..240ae369c0 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -100,6 +100,11 @@ class BtActionNode : public BT::ActionNodeBase
   {
   }
 
+  // Can be overwritten to trigger specific behaviours when a new goal is sent to the BtNavigator
+  virtual void on_goal_preempted()
+  {
+  }
+
   // Called upon successful completion of the action. A derived class can override this
   // method to put a value on the blackboard, for example.
   virtual BT::NodeStatus on_success()
@@ -140,6 +145,12 @@ class BtActionNode : public BT::ActionNodeBase
       // user defined callback. May modify the value of "goal_updated_"
       on_wait_for_result();
 
+      // Check if the navigation goal has been updated
+      if (config().blackboard->get<bool>("goal_preempted")) {
+        config().blackboard->set("goal_preempted", false);
+        on_goal_preempted();
+      }
+
       auto goal_status = goal_handle_->get_status();
       if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
         goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp
index afa5af2bcd..c70ebdd881 100644
--- a/nav2_behavior_tree/plugins/action/spin_action.cpp
+++ b/nav2_behavior_tree/plugins/action/spin_action.cpp
@@ -36,12 +36,22 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
   : BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
+  {
+  }
+
+  void on_tick() override
   {
     double dist;
     getInput("spin_dist", dist);
     goal_.target_yaw = dist;
   }
 
+  void on_goal_preempted() override
+  {
+    goal_.target_yaw = 0;
+    goal_updated_ = true;
+  }
+
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(
diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp
index 85d0af0cd7..10ea6128fd 100644
--- a/nav2_behavior_tree/plugins/action/wait_action.cpp
+++ b/nav2_behavior_tree/plugins/action/wait_action.cpp
@@ -33,6 +33,10 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
   : BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
+  {
+  }
+
+  void on_tick() override
   {
     int duration;
     getInput("wait_duration", duration);
@@ -45,6 +49,12 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
 
     goal_.time.sec = duration;
   }
+  void on_goal_preempted() override
+  {
+    // Stop the wait if a new goal has been preempted.
+    goal_.time.sec = 0;
+    goal_updated_ = true;
+  }
 
   // Any BT node that accepts parameters must provide a requiredNodeParameters method
   static BT::PortsList providedPorts()
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 0d9f03dd49..f1cfaacc73 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -109,6 +109,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT
   blackboard_->set<bool>("initial_pose_received", false);  // NOLINT
+  blackboard_->set<bool>("goal_preempted", false); // NOLINT
 
   // Get the BT filename to use from the node parameter
   std::string bt_xml_filename;
@@ -221,6 +222,7 @@ BtNavigator::navigateToPose()
         RCLCPP_INFO(get_logger(), "Received goal preemption request");
         action_server_->accept_pending_goal();
         initializeGoalPose();
+        blackboard_->set("goal_preempted", true);
       }
       topic_logger.flush();
     };
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index 731188cecb..6cbdb59b56 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
@@ -173,12 +173,14 @@ class Recovery : public nav2_core::Recovery
       // TODO(orduno) #868 Enable preempting a Recovery on-the-fly without stopping
       if (action_server_->is_preempt_requested()) {
         RCLCPP_ERROR(
-          node_->get_logger(), "Received a preemption request for %s,"
-          " however feature is currently not implemented. Aborting and stopping.",
+          node_->get_logger(), "Received a preemption request for %s,",
           recovery_name_.c_str());
-        stopRobot();
-        action_server_->terminate_current();
-        return;
+        action_server_->accept_pending_goal();
+        if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
+          RCLCPP_INFO(node_->get_logger(), "Preemption failed for %s", recovery_name_.c_str());
+          action_server_->terminate_current();
+          return;
+        }
       }
 
       switch (onCycleUpdate()) {