diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 972fa43a5fa8eba646811b913eb3957437b8cbe9..df7b51deaed41b1634dc72cde9bb4d102f0de3e9 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/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) diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95b1c96e926fc0ed175b85274a989d1192eea685 --- /dev/null +++ b/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"); +} diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 094c3db995e016c3178b8eb07165c600f5b7331f..d1dc8948a9e29dc365d8fa7143e2e8c0049d27ee 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/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: diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 20a05c9a2ec7c2ef0fddcff20d8f5111a2e063ac..b4ea4c516bf06a2f52559465b3d66535bce112a5 100644 --- a/nav2_bt_navigator/README.md +++ b/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. diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml new file mode 100644 index 0000000000000000000000000000000000000000..55cf0893e8b786d436ae0dbdd1adaf2293731b2f --- /dev/null +++ b/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> diff --git a/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png b/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png new file mode 100644 index 0000000000000000000000000000000000000000..c2a842e53212df4d8f23a53f0317cf5930bc5f29 Binary files /dev/null and b/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png differ diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 1be2b3f6a1be532113e4fba8ea0b7079df6889a0..a2ce6decb23453f7b76943dda5e3128bb0f5a3b0 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/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); diff --git a/tools/bt2img.py b/tools/bt2img.py index b6f828ea3b2752a511ef974278bd8cdc28d50969..1092b047f439acc6c63612197f852fa473c6dbaa 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -39,6 +39,7 @@ control_nodes = [ "RateController", "RecoveryNode", "PipelineSequence", + "RoundRobin" ] action_nodes = [ "AlwaysFailure", diff --git a/tools/update_bt_diagrams.bash b/tools/update_bt_diagrams.bash index 15cfe088c9fd88a9cda8f871b51191b5f6165ffe..94e5d00d0d7ef480cbbf9d0035eefec6307ca22b 100755 --- a/tools/update_bt_diagrams.bash +++ b/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