From e1a5d548c0cd1865541da28f0369f1000c919ca6 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 19 Oct 2021 16:50:54 -0700
Subject: [PATCH] refactor of #2624 for simplicity (#2629)

---
 .../nav2_behavior_tree/bt_action_node.hpp     | 74 ++++++++-----------
 1 file changed, 32 insertions(+), 42 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 9021cc3094..ace1542b11 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
@@ -183,11 +183,11 @@ class BtActionNode : public BT::ActionNodeBase
       send_new_goal();
     }
 
-    // if new goal was sent and action server has not yet responded
-    // check the future goal handle
-    if (future_goal_handle_) {
-      auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
-      try {
+    try {
+      // if new goal was sent and action server has not yet responded
+      // check the future goal handle
+      if (future_goal_handle_) {
+        auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
         if (!is_future_goal_handle_complete(elapsed)) {
           // return RUNNING if there is still some time before timeout happens
           if (elapsed < server_timeout_) {
@@ -201,31 +201,20 @@ class BtActionNode : public BT::ActionNodeBase
           future_goal_handle_.reset();
           return BT::NodeStatus::FAILURE;
         }
-      } catch (const std::runtime_error & e) {
-        if (e.what() == std::string("send_goal failed") ||
-          e.what() == std::string("Goal was rejected by the action server"))
-        {
-          return BT::NodeStatus::FAILURE;
-        } else {
-          // Internal exception to propogate to the tree
-          throw e;
-        }
       }
-    }
 
-    // The following code corresponds to the "RUNNING" loop
-    if (rclcpp::ok() && !goal_result_available_) {
-      // user defined callback. May modify the value of "goal_updated_"
-      on_wait_for_result();
+      // The following code corresponds to the "RUNNING" loop
+      if (rclcpp::ok() && !goal_result_available_) {
+        // user defined callback. May modify the value of "goal_updated_"
+        on_wait_for_result();
 
-      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))
-      {
-        goal_updated_ = false;
-        send_new_goal();
-        auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
-        try {
+        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))
+        {
+          goal_updated_ = false;
+          send_new_goal();
+          auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
           if (!is_future_goal_handle_complete(elapsed)) {
             if (elapsed < server_timeout_) {
               return BT::NodeStatus::RUNNING;
@@ -237,24 +226,25 @@ class BtActionNode : public BT::ActionNodeBase
             future_goal_handle_.reset();
             return BT::NodeStatus::FAILURE;
           }
-        } catch (const std::runtime_error & e) {
-          if (e.what() == std::string("send_goal failed") ||
-            e.what() == std::string("Goal was rejected by the action server"))
-          {
-            return BT::NodeStatus::FAILURE;
-          } else {
-            // Internal exception to propogate to the tree
-            throw e;
-          }
         }
-      }
 
-      callback_group_executor_.spin_some();
+        callback_group_executor_.spin_some();
 
-      // check if, after invoking spin_some(), we finally received the result
-      if (!goal_result_available_) {
-        // Yield this Action, returning RUNNING
-        return BT::NodeStatus::RUNNING;
+        // check if, after invoking spin_some(), we finally received the result
+        if (!goal_result_available_) {
+          // Yield this Action, returning RUNNING
+          return BT::NodeStatus::RUNNING;
+        }
+      }
+    } catch (const std::runtime_error & e) {
+      if (e.what() == std::string("send_goal failed") ||
+        e.what() == std::string("Goal was rejected by the action server"))
+      {
+        // Action related failure that should not fail the tree, but the node
+        return BT::NodeStatus::FAILURE;
+      } else {
+        // Internal exception to propogate to the tree
+        throw e;
       }
     }