Skip to content
Snippets Groups Projects
Unverified Commit 4ece520b authored by Carl Delsey's avatar Carl Delsey Committed by GitHub
Browse files

Fix parallel replanning in the behavior tree (alternate implementation) (#1371)

* Adding the PipelineSequence node

* Rewrite tree to take advantage of the new node.

* Add image generator

* Adding documentation

* Clarifying documentation on PipelineSequence
parent b229f354
No related branches found
No related tags found
No related merge requests found
Showing
with 335 additions and 32 deletions
......@@ -92,6 +92,9 @@ list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node)
add_library(nav2_random_crawl_action_bt_node SHARED plugins/action/random_crawl_action.cpp)
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)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
......
......@@ -2,10 +2,10 @@
The nav2_behavior_tree module provides:
* A C++ template class for integrating ROS2 actions into Behavior Trees,
* Navigation-specific behavior tree nodes, and
* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes.
* Navigation-specific behavior tree nodes, and
* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes.
This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing.
This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing.
## The bt_action_node Template and the Behavior Tree Engine
......@@ -58,14 +58,14 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
```
See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
## Navigation-Specific Behavior Tree Nodes
The nav2_behavior_tree package provides several navigation-specific nodes that are pre-registered and can be included in Behavior Trees.
| BT Node | Type | Description |
|----------|:-------------|------|
| Backup | Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. |
| Backup | Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. |
| ComputePathToPose | Action | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. |
| GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
......@@ -75,5 +75,6 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
| RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. |
| RecoveryNode | Control | The RecoveryNode is a control flow 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. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures.
| Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. |
| PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.|
For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/
// 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 <stdexcept>
#include <sstream>
#include <string>
#include "behaviortree_cpp_v3/control_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"
namespace nav2_behavior_tree
{
/** @brief Type of sequence node that re-ticks previous children when a child returns running
*
* Type of Control Node | Child Returns Failure | Child Returns Running
* ---------------------------------------------------------------------
* PipelineSequence | Restart | Tick All Previous Again
*
* Tick All Previous Again means every node up till this one will be reticked. Even
* if a previous node returns Running, the next node will be reticked.
*
* As an example, let's say this node has 3 children: A, B and C. At the start,
* they are all IDLE.
* | A | B | C |
* --------------------------------
* | IDLE | IDLE | IDLE |
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
* - PipelineSequence returns RUNNING and no other nodes are ticked.
* | SUCCESS | RUNNING | IDLE | - This time A returns SUCCESS so B gets ticked as well
* - PipelineSequence returns RUNNING and C is not ticked yet
* | RUNNING | SUCCESS | RUNNING | - A gets ticked and returns RUNNING, but since it had previously
* - returned SUCCESS, PipelineSequence continues on and ticks B.
* - Since B also returns SUCCESS, C gets ticked this time as well.
* | RUNNING | SUCCESS | SUCCESS | - A is still RUNNING, and B returns SUCCESS again. This time C
* - returned SUCCESS, ending the sequence. PipelineSequence
* - returns SUCCESS and halts A.
*
* If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
* and halted all children, ending the sequence.
*
* Usage in XML: <PipelineSequence>
*/
class PipelineSequence : public BT::ControlNode
{
public:
explicit PipelineSequence(const std::string & name);
PipelineSequence(const std::string & name, const BT::NodeConfiguration & config);
void halt() override;
static BT::PortsList providedPorts() {return {};}
protected:
BT::NodeStatus tick() override;
std::size_t last_child_ticked_ = 0;
};
PipelineSequence::PipelineSequence(const std::string & name)
: BT::ControlNode(name, {})
{
}
PipelineSequence::PipelineSequence(
const std::string & name,
const BT::NodeConfiguration & config)
: BT::ControlNode(name, config)
{
}
BT::NodeStatus PipelineSequence::tick()
{
for (std::size_t i = 0; i < children_nodes_.size(); ++i) {
auto status = children_nodes_[i]->executeTick();
switch (status) {
case BT::NodeStatus::FAILURE:
haltChildren(0);
last_child_ticked_ = 0; // reset
return status;
break;
case BT::NodeStatus::SUCCESS:
// do nothing and continue on to the next child. If it is the last child
// we'll exit the loop and hit the wrap-up code at the end of the method.
break;
case BT::NodeStatus::RUNNING:
if (i >= last_child_ticked_) {
last_child_ticked_ = i;
return status;
}
// else do nothing and continue on to the next child
break;
default:
std::stringstream error_msg;
error_msg << "Invalid node status. Received status " << status <<
"from child " << children_nodes_[i]->name();
throw std::runtime_error(error_msg.str());
break;
}
}
// Wrap up.
haltChildren(0);
last_child_ticked_ = 0; // reset
return BT::NodeStatus::SUCCESS;
}
void PipelineSequence::halt()
{
BT::ControlNode::halt();
}
} // namespace nav2_behavior_tree
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::PipelineSequence>("PipelineSequence");
}
......@@ -63,6 +63,7 @@ bt_navigator:
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
......
......@@ -63,6 +63,7 @@ bt_navigator:
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
......
......@@ -63,6 +63,7 @@ bt_navigator:
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
......
......@@ -46,7 +46,21 @@ The BT Navigator package has two sample XML-based descriptions of BTs. These tr
</root>
```
Navigate with replanning is composed of the following custom decorator, condition and action nodes:
Navigate with replanning is composed of the following custom decorator, condition,
contro and action nodes:
#### Control Nodes
* PipelineSequence: This is a variant of a Sequence Node. When this node is ticked,
it will tick the first child till it succeeds. Then it will tick the first two
children till the second one succeeds. Then it will tick the first three till the
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.
* 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">
<br/>
#### Decorator Nodes
* RateController: A custom control flow node, which throttles down the tick rate. This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set. This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the `ComputePathToPose` and `GoalReached` node at 1 Hz.
......@@ -57,7 +71,6 @@ Navigate with replanning is composed of the following custom decorator, conditio
#### Action Nodes
* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree. The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path. Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_ids` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available.
The graphical version of this Behavior Tree:
<img src="./doc/simple_parallel.png" title="" width="65%" align="middle">
......@@ -65,17 +78,9 @@ The graphical version of this Behavior Tree:
The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc.
### Recovery Node
In this section, the recovery node is being introduced to the navigation package.
Recovery node 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">
<br/>
### Navigate with replanning and simple recovery actions
With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps and `spin`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps and `spin`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.
<img src="./doc/parallel_w_recovery.png" title="" width="95%" align="middle">
<br/>
......@@ -92,3 +97,8 @@ Scope-based failure handling: Utilizing Behavior Trees with a recovery node allo
## 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.
## Legend
Legend for the behavior tree diagrams:
![Legend](./doc/legend.png)
......@@ -4,14 +4,11 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<Fallback>
<GoalReached/>
<ComputePathToPose goal="{goal}" planner_id="GridBased"/>
</Fallback>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</RateController>
<FollowPath path="{path}" controller_id="FollowPath"/>
</Sequence>
<FollowPath path="{path}" controller_id="FollowPath"/>
</PipelineSequence>
</BehaviorTree>
</root>
......@@ -5,21 +5,18 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<Sequence name="NavigateWithReplanning">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<Fallback name="GoalReached">
<GoalReached/>
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</Fallback>
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</Sequence>
</PipelineSequence>
<SequenceStar name="RecoveryActions">
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
......
nav2_bt_navigator/doc/legend.png

7.11 KiB

nav2_bt_navigator/doc/parallel_w_recovery.png

65.9 KiB | W: | H:

nav2_bt_navigator/doc/parallel_w_recovery.png

88 KiB | W: | H:

nav2_bt_navigator/doc/parallel_w_recovery.png
nav2_bt_navigator/doc/parallel_w_recovery.png
nav2_bt_navigator/doc/parallel_w_recovery.png
nav2_bt_navigator/doc/parallel_w_recovery.png
  • 2-up
  • Swipe
  • Onion skin
nav2_bt_navigator/doc/simple_parallel.png

19.6 KiB | W: | H:

nav2_bt_navigator/doc/simple_parallel.png

25.6 KiB | W: | H:

nav2_bt_navigator/doc/simple_parallel.png
nav2_bt_navigator/doc/simple_parallel.png
nav2_bt_navigator/doc/simple_parallel.png
nav2_bt_navigator/doc/simple_parallel.png
  • 2-up
  • Swipe
  • Onion skin
#!/usr/bin/python3
# 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.
# This tool converts a behavior tree XML file to a PNG image. Run bt2img.py -h
# for instructions
import argparse
import xml.etree.ElementTree
import graphviz # pip3 install graphviz
control_nodes = [
"Fallback",
"Parallel",
"ReactiveFallback",
"ReactiveSequence",
"Sequence",
"SequenceStar",
"BlackboardCheckInt",
"BlackboardCheckDouble",
"BlackboardCheckString",
"ForceFailure",
"ForceSuccess",
"Inverter",
"Repeat",
"Subtree",
"Timeout",
"RateController",
"RecoveryNode",
"PipelineSequence",
]
action_nodes = [
"AlwaysFailure",
"AlwaysSuccess",
"SetBlackboard",
"ComputePathToPose",
"FollowPath",
"BackUp",
"Spin",
"Wait",
"ClearEntireCostmap",
"ReinitializeGlobalLocalization",
]
condition_nodes = [
"IsStuck",
"GoalReached",
"initialPoseReceived",
]
def main():
args = parse_command_line()
xml_tree = xml.etree.ElementTree.parse(args.behavior_tree)
behavior_tree = find_behavior_tree(xml_tree)
dot = convert2dot(behavior_tree)
if args.legend:
legend = make_legend()
legend.format = 'png'
legend.render(args.legend)
dot.format = 'png'
if args.save_dot:
print("Saving dot to " + str(args.save_dot))
args.save_dot.write(dot.source)
dot.render(args.image_out, view=args.display)
def parse_command_line():
parser = argparse.ArgumentParser(description='Convert a behavior tree XML file to an image')
parser.add_argument('--behavior_tree', required=True,
help='the behavior tree XML file to convert to an image')
parser.add_argument('--image_out', required=True,
help='The name of the output image file. Leave off the .png extension')
parser.add_argument('--display', action="store_true",
help='If specified, opens the image in the default viewer')
parser.add_argument('--save_dot', type=argparse.FileType('w'),
help='Saves the intermediate dot source to the specified file')
parser.add_argument('--legend',
help='Generate a legend image as well')
return parser.parse_args()
# An XML file can have multiple behavior trees defined in it in theory. We don't
# currently support that.
def find_behavior_tree(xml_tree):
trees = xml_tree.findall('BehaviorTree')
if (len(trees) == 0):
raise RuntimeError("No behavior trees were found in the XML file")
if (len(trees) > 1):
raise RuntimeError("This program only supports one behavior tree per file")
return trees[0]
# Generate a dot description of the root of the behavior tree.
def convert2dot(behavior_tree):
dot = graphviz.Digraph()
root = behavior_tree
parent_dot_name = str(hash(root))
dot.node(parent_dot_name, root.get('ID'), shape='box')
convert_subtree(dot, root, parent_dot_name)
return dot
# Recursive function. We add the children to the dot file, and then recursively
# call this function on the children. Nodes are given an ID that is the hash
# of the node to ensure each is unique.
def convert_subtree(dot, parent_node, parent_dot_name):
for node in list(parent_node):
label = make_label(node)
dot.node(str(hash(node)), label, color=node_color(node.tag), style='filled', shape='box')
dot_name = str(hash(node))
dot.edge(parent_dot_name, dot_name)
convert_subtree(dot, node, dot_name)
# The node label contains the:
# type, the name if provided, and the parameters.
def make_label(node):
label = '< <table border="0" cellspacing="0" cellpadding="0">'
label += '<tr><td align="text"><i>' + node.tag + '</i></td></tr>'
name = node.get('name')
if name:
label += '<tr><td align="text"><b>' + name + '</b></td></tr>'
for (param_name, value) in node.items():
label += '<tr><td align="left"><sub>' + param_name + '=' + value + '</sub></td></tr>'
label += '</table> >'
return label
def node_color(type):
if type in control_nodes:
return "chartreuse4"
if type in action_nodes:
return "cornflowerblue"
if type in condition_nodes:
return "yellow2"
#else it's unknown
return "grey"
# creates a legend which can be provided with the other images.
def make_legend():
legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'})
legend.attr(label='Legend')
legend.node('Unknown', shape='box', style='filled', color="grey")
legend.node('Action', 'Action Node', shape='box', style='filled', color="cornflowerblue")
legend.node('Condition', 'Condition Node', shape='box', style='filled', color="yellow2")
legend.node('Control', 'Control Node', shape='box', style='filled', color="chartreuse4")
return legend
if __name__ == '__main__':
main()
#!/bin/bash
# Run this from the root of the workspace to update these behavior_tree images
# in the doc directory of the nav2_bt_navigator package
navigation2/tools/bt2img.py \
--behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml \
--image_out navigation2/nav2_bt_navigator/doc/simple_parallel \
--legend navigation2/nav2_bt_navigator/doc/legend
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment