Be able to invoke a single recovery action at a time (#1388)

Added parallel recovery behaviors

nav2_behavior_tree/CMakeLists.txt

@@ -95,6 +95,9 @@ list(APPEND plugin_libs nav2_random_crawl_action_bt_node)
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 
+add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
+list(APPEND plugin_libs nav2_round_robin_node_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${dependencies})
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

nav2_behavior_tree/plugins/control/round_robin_node.cpp

@@ -0,0 +1,82 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "behaviortree_cpp_v3/control_node.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace nav2_behavior_tree
+{
+
+class RoundRobinNode : public BT::ControlNode
+{
+public:
+  explicit RoundRobinNode(const std::string & name)
+  : BT::ControlNode::ControlNode(name, {})
+  {
+    setRegistrationID("RoundRobin");
+  }
+
+  BT::NodeStatus tick() override
+  {
+    const unsigned num_children = children_nodes_.size();
+
+    setStatus(BT::NodeStatus::RUNNING);
+
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    switch (child_status) {
+      case BT::NodeStatus::SUCCESS:
+        // Wrap around to the first child
+        if (++current_child_idx_ == num_children) {
+          // TODO(mjeronimo): halt this child (not all children)
+          current_child_idx_ = 0;
+        }
+
+        haltChildren(0);
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::FAILURE:
+        haltChildren(0);
+        return BT::NodeStatus::FAILURE;
+
+      case BT::NodeStatus::RUNNING:
+        break;
+
+      default:
+        throw BT::LogicError("Invalid status return from BT node");
+        break;
+    }
+
+    return BT::NodeStatus::RUNNING;
+  }
+
+  void halt() override
+  {
+    ControlNode::halt();
+    current_child_idx_ = 0;
+  }
+
+private:
+  unsigned int current_child_idx_{0};
+};
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RoundRobinNode>("RoundRobin");
+}

nav2_bringup/bringup/params/nav2_params.yaml

@@ -64,6 +64,7 @@ bt_navigator:
     - nav2_rate_controller_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bt_navigator/README.md

@@ -56,6 +56,8 @@ children till the second one succeeds. Then it will tick the first three till th
 third succeeds and so on, till there are no more children. This will return RUNNING,
 till the last child succeeds, at which time it also returns SUCCESS. If any child
 returns FAILURE, all nodes are halted and this node returns FAILURE.
+* RoundRobin: This is a custom control node introduced to the Behavior Tree. When this node is ticked, it will tick the first child until it returns SUCCESS or FAILURE. If the child returns either SUCCESS or FAILURE, it will tick its next child. Once the node reaches the last child, it will restart ticking from the first child. The main difference between the RoundRobin node versus the Sequence node is that when a child returns FAILURE the RoundRobin node will tick the next child but in the Sequence node, it will return FAILURE.
+
 * Recovery: This is a control flow type node with two children.  It returns success if and only if the first child returns success. The second child will be executed only if the first child returns failure.  The second child is responsible for recovery actions such as re-initializing system or other recovery behaviors. If the recovery behaviors are succeeded, then the first child will be executed again.  The user can specify how many times the recovery actions should be taken before returning failure. The figure below depicts a simple recovery node.
 
 <img src="./doc/recovery_node.png" title="" width="40%" align="middle">
@@ -88,12 +90,17 @@ With the recovery node, simple recoverable navigation with replanning can be imp
 
 This tree is currently our default tree in the stack and the xml file is located here: [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml).
 
-## Future Work
-Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc.  Currently, in the navigation stack, multi-scope recovery actions are not implemented. The figure below highlights a simple multi-scope recovery handling for the navigation task.
+## Multi-Scope Recoveries
+Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc.
+
+### Navigate with replanning and simple Multi-Scope Recoveries
+In the navigation stack, multi-scope recovery actions are implemented for each module.  Currently, the recovery actions for the Global planner are: Clear Entire Global Costmap and Wait.  The recovery actions for the Local planner are: Clear Entire Local Costmap and Spin; the recovery actions for the system level is just Wait. The figure below highlights a simple multi-scope recovery handling for the navigation task.  With this tree, if the Global Planner fails, the ClearEntireCostmap which is the first recovery action for this module will be ticked, then the ComputePathToPose will be ticked again. If the ComputePathToPose fails again, the Wait which is the second recovery action for this module will be ticked. After trying the second recovery action, the ComputePathToPose will be ticked again. These actions can be repeated n times until ComputePathToPose succeeds.  If the ComputePathToPose fails and the Global Planner cannot be recovered locally, the higher-level recovery actions will be ticked.  In this simple example, our higher-level recovery action is just a longer wait. The same strategy is applied to the Local Planner. If the Local Planner fails, the tree will first tick the ClearEntireCostmap and then if it fails again the tree will tick the Spin.
 
-<img src="./doc/proposed_recovery.png" title="" width="95%" align="middle">
+<img src="./doc/parallel_w_round_robin_recovery.png" title="" width="95%" align="middle">
 <br/>
 
+This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml).
+
 ## Open Issues
 
 * **Schema definition and XML document validation** - Currently, there is no dynamic validation of incoming XML. The Behavior-Tree.CPP library is using tinyxml2, which doesn't have a validator. Instead, we can create a schema for the Mission Planning-level XML and use build-time validation of the XML input to ensure that it is well-formed and valid.

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml

@@ -0,0 +1,30 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz and it also has
+  recovery actions.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="2" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="4" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+            <RoundRobin name="GlobalPlannerRecoveryActions">
+              <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
+              <Wait wait_duration="2"/>
+            </RoundRobin>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="4" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <RoundRobin name="PlannerRecoveryActions">
+            <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
+            <Spin spin_dist="1.57"/>
+          </RoundRobin>
+        </RecoveryNode>
+      </PipelineSequence>
+        <Wait wait_duration="5"/>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

nav2_controller/src/nav2_controller.cpp

@@ -260,6 +260,9 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
 {
   RCLCPP_DEBUG(get_logger(),
     "Providing path to the controller %s", current_controller_);
+  if (path.poses.empty()) {
+    throw nav2_core::PlannerException("Invalid path, Path is empty.");
+  }
   controllers_[current_controller_]->setPlan(path);
 
   auto end_pose = *(path.poses.end() - 1);

tools/bt2img.py

@@ -39,6 +39,7 @@
     "RateController",
     "RecoveryNode",
     "PipelineSequence",
+    "RoundRobin"
     ]
 action_nodes = [
     "AlwaysFailure",

tools/update_bt_diagrams.bash

@@ -9,3 +9,6 @@ navigation2/tools/bt2img.py \
 navigation2/tools/bt2img.py \
   --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml \
   --image_out navigation2/nav2_bt_navigator/doc/parallel_w_recovery
+navigation2/tools/bt2img.py \
+  --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml \
+  --image_out navigation2/nav2_bt_navigator/doc/parallel_w_round_robin_recovery