BehaviorTree refactoring (#1606)

* Proposed refactoring to avoid issues with CoroAction

* correctly haltAllActions (related to #1600)

* not really needed and will be deprecated soon

* Applying changes suggested in the comments of #1606

- fix haltAllActions
- changes method signature on_success()
- reverts the changes made here:
https://github.com/ros-planning/navigation2/pull/1515/files

* fix warnings and errors

* make uncrustify happy?

* Update bt_navigator.cpp

* Update bt_navigator.cpp

* uncrustify fix

Co-authored-by: daf@blue-ocean-robotics.com <Davide Faconti>

Author: facontidavide

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -44,21 +44,19 @@ class BehaviorTreeEngine
     const std::string & xml_string,
     BT::Blackboard::Ptr blackboard);
 
+  // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
   void haltAllActions(BT::TreeNode * root_node)
   {
-    auto visitor = [](BT::TreeNode * node) {
-        if (auto action = dynamic_cast<BT::CoroActionNode *>(node)) {
-          action->halt();
-        }
-      };
-    BT::applyRecursiveVisitor(root_node, visitor);
-  }
+    // this halt signal should propagate through the entire tree.
+    root_node->halt();
+    root_node->setStatus(BT::NodeStatus::IDLE);
 
-  // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
-  void resetTree(BT::TreeNode * root_node)
-  {
+    // but, just in case...
     auto visitor = [](BT::TreeNode * node) {
-        node->setStatus(BT::NodeStatus::IDLE);
+        if (node->status() == BT::NodeStatus::RUNNING) {
+          node->halt();
+          node->setStatus(BT::NodeStatus::IDLE);
+        }
       };
     BT::applyRecursiveVisitor(root_node, visitor);
   }

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -26,14 +26,14 @@ namespace nav2_behavior_tree
 {
 
 template<class ActionT>
-class BtActionNode : public BT::CoroActionNode
+class BtActionNode : public BT::ActionNodeBase
 {
 public:
   BtActionNode(
     const std::string & xml_tag_name,
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
-  : BT::CoroActionNode(xml_tag_name, conf), action_name_(action_name)
+  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
   {
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
@@ -96,66 +96,71 @@ class BtActionNode : public BT::CoroActionNode
   }
 
   // Called upon successful completion of the action. A derived class can override this
-  // method to put a value on the blackboard, for example
-  virtual void on_success()
+  // method to put a value on the blackboard, for example.
+  virtual BT::NodeStatus on_success()
   {
+    return BT::NodeStatus::SUCCESS;
   }
 
-  // The main override required by a BT action
-  BT::NodeStatus tick() override
+  // Called when a the action is aborted. By default, the node will return FAILURE.
+  // The user may override it to return another value, instead.
+  virtual BT::NodeStatus on_aborted()
   {
-    on_tick();
+    return BT::NodeStatus::FAILURE;
+  }
 
-new_goal_received:
-    goal_result_available_ = false;
-    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
-    send_goal_options.result_callback =
-      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
-        if (result.code != rclcpp_action::ResultCode::ABORTED) {
-          goal_result_available_ = true;
-          result_ = result;
-        }
-      };
+  // Called when a the action is cancelled. By default, the node will return SUCCESS.
+  // The user may override it to return another value, instead.
+  virtual BT::NodeStatus on_cancelled()
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
 
-    auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+  // The main override required by a BT action
+  BT::NodeStatus tick() override
+  {
+    // first step to be done only at the beginning of the Action
+    if (status() == BT::NodeStatus::IDLE) {
+      // setting the status to RUNNING to notify the BT Loggers (if any)
+      setStatus(BT::NodeStatus::RUNNING);
 
-    if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
-      rclcpp::executor::FutureReturnCode::SUCCESS)
-    {
-      throw std::runtime_error("send_goal failed");
-    }
+      // user defined callback
+      on_tick();
 
-    goal_handle_ = future_goal_handle.get();
-    if (!goal_handle_) {
-      throw std::runtime_error("Goal was rejected by the action server");
+      on_new_goal_received();
     }
 
-    while (rclcpp::ok() && !goal_result_available_) {
+    // 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 status = goal_handle_->get_status();
-      if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
-        status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
+      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;
-        goto new_goal_received;
+        on_new_goal_received();
       }
 
-      // Yield to any other nodes
-      setStatusRunningAndYield();
       rclcpp::spin_some(node_);
+
+      // check if, after invoking spin_some(), we finally received the result
+      if (!goal_result_available_) {
+        // Yield this Action, returning RUNNING
+        return BT::NodeStatus::RUNNING;
+      }
     }
 
     switch (result_.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
-        on_success();
-        return BT::NodeStatus::SUCCESS;
+        return on_success();
 
       case rclcpp_action::ResultCode::ABORTED:
-        return BT::NodeStatus::FAILURE;
+        return on_aborted();
 
       case rclcpp_action::ResultCode::CANCELED:
-        return BT::NodeStatus::SUCCESS;
+        return on_cancelled();
 
       default:
         throw std::logic_error("BtActionNode::Tick: invalid status value");
@@ -178,7 +183,6 @@ class BtActionNode : public BT::CoroActionNode
     }
 
     setStatus(BT::NodeStatus::IDLE);
-    CoroActionNode::halt();
   }
 
 protected:
@@ -202,6 +206,33 @@ class BtActionNode : public BT::CoroActionNode
     return false;
   }
 
+
+  void on_new_goal_received()
+  {
+    goal_result_available_ = false;
+    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+    send_goal_options.result_callback =
+      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
+        if (result.code != rclcpp_action::ResultCode::ABORTED) {
+          goal_result_available_ = true;
+          result_ = result;
+        }
+      };
+
+    auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+
+    if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
+      rclcpp::executor::FutureReturnCode::SUCCESS)
+    {
+      throw std::runtime_error("send_goal failed");
+    }
+
+    goal_handle_ = future_goal_handle.get();
+    if (!goal_handle_) {
+      throw std::runtime_error("Goal was rejected by the action server");
+    }
+  }
+
   const std::string action_name_;
   typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -26,13 +26,13 @@ namespace nav2_behavior_tree
 {
 
 template<class ServiceT>
-class BtServiceNode : public BT::CoroActionNode
+class BtServiceNode : public BT::SyncActionNode
 {
 public:
   BtServiceNode(
     const std::string & service_node_name,
     const BT::NodeConfiguration & conf)
-  : BT::CoroActionNode(service_node_name, conf), service_node_name_(service_node_name)
+  : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name)
   {
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -47,7 +47,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
     getInput("planner_id", goal_.planner_id);
   }
 
-  void on_success() override
+  BT::NodeStatus on_success() override
   {
     setOutput("path", result_.result->path);
 
@@ -56,6 +56,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
     } else {
       config().blackboard->set("path_updated", true);
     }
+    return BT::NodeStatus::SUCCESS;
   }
 
   static BT::PortsList providedPorts()

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -110,6 +110,8 @@ class BtNavigator : public nav2_util::LifecycleNode
   void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
 
+  BT::Tree tree_;
+
   // The blackboard shared by all of the nodes in the tree
   BT::Blackboard::Ptr blackboard_;
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -129,6 +129,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
   RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str());
 
+  // Create the Behavior Tree from the XML input
+  tree_ = bt_->buildTreeFromText(xml_string_, blackboard_);
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -159,7 +162,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   // TODO(orduno) Fix the race condition between the worker thread ticking the tree
   //              and the main thread resetting the resources, see #1344
-
   goal_sub_.reset();
   client_node_.reset();
   self_client_.reset();
@@ -172,6 +174,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   plugin_lib_names_.clear();
   xml_string_.clear();
   blackboard_.reset();
+  bt_->haltAllActions(tree_.root_node);
   bt_.reset();
 
   RCLCPP_INFO(get_logger(), "Completed Cleaning up");
@@ -211,11 +214,7 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-
-  // Create the Behavior Tree from the XML input
-  BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_);
-
-  RosTopicLogger topic_logger(client_node_, tree);
+  RosTopicLogger topic_logger(client_node_, tree_);
 
   auto on_loop = [&]() {
       if (action_server_->is_preempt_requested()) {
@@ -227,7 +226,10 @@ BtNavigator::navigateToPose()
     };
 
   // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling);
+  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
+  // Make sure that the Bt is not in a running state from a previous execution
+  // note: if all the ControlNodes are implemented correctly, this is not needed.
+  bt_->haltAllActions(tree_.root_node);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED: