diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 0bc932ec8a6879f7cd32f6148fd7f100cb48fab8..972fa43a5fa8eba646811b913eb3957437b8cbe9 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -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)
diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
index ac9db1f0cf0408aab592fe8ca235499230f188d1..2d7e3971aefd017e0f26c5aecf56378bdcd379c2 100644
--- a/nav2_behavior_tree/README.md
+++ b/nav2_behavior_tree/README.md
@@ -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/
diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c7dcf705915f8db83c543f455985b28ffea1aeb
--- /dev/null
+++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp
@@ -0,0 +1,124 @@
+// 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");
+}
diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
index 458b1aa192412f58c6746407ad745a9774cc95b2..a90b2224640aaa3fca1b6f2fff5001c09bc99c28 100644
--- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
@@ -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:
diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
index 9a45958d089f09868c0e1d23ee6b6c789de302a2..f366129faf4500a88c10167a5cbf8df1e9587e71 100644
--- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
@@ -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:
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index f3a336b99847df5fca55f69f17f209d525f7a4e8..094c3db995e016c3178b8eb07165c600f5b7331f 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -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:
diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md
index 4c4e0c20f958d604480ce63186cc44570902697e..8f9d05338a0bc739917dc4b267e316241dfa0911 100644
--- a/nav2_bt_navigator/README.md
+++ b/nav2_bt_navigator/README.md
@@ -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:
+
+
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
index b0767dfe00f0aa8f25e40a0fad6310980efe4273..33c2c411ad0d02108b8451893e515a6e160da8e3 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
@@ -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>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index a646c61f87dbbb4fb3e571dddd5b69f4323be85e..28aee8b78aed6e75d226aac6f4a6817f2a62193f 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
@@ -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"/>
diff --git a/nav2_bt_navigator/doc/legend.png b/nav2_bt_navigator/doc/legend.png
new file mode 100644
index 0000000000000000000000000000000000000000..e2f5bd1d6df70f554b93b6beb015d19dc2cc08b3
Binary files /dev/null and b/nav2_bt_navigator/doc/legend.png differ
diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png
index 24b608276155c9ddf00052fdf6969747216a9290..3201ff340357025bdbf61e6b9d9895399fae7e07 100644
Binary files a/nav2_bt_navigator/doc/parallel_w_recovery.png and b/nav2_bt_navigator/doc/parallel_w_recovery.png differ
diff --git a/nav2_bt_navigator/doc/simple_parallel.png b/nav2_bt_navigator/doc/simple_parallel.png
index fb51da284584090fb9d860afdc4acf75e243c882..eb5f569acf52d21104275f286eff79421e672cad 100644
Binary files a/nav2_bt_navigator/doc/simple_parallel.png and b/nav2_bt_navigator/doc/simple_parallel.png differ
diff --git a/tools/bt2img.py b/tools/bt2img.py
new file mode 100755
index 0000000000000000000000000000000000000000..b6f828ea3b2752a511ef974278bd8cdc28d50969
--- /dev/null
+++ b/tools/bt2img.py
@@ -0,0 +1,157 @@
+#!/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()
diff --git a/tools/update_bt_diagrams.bash b/tools/update_bt_diagrams.bash
new file mode 100755
index 0000000000000000000000000000000000000000..15cfe088c9fd88a9cda8f871b51191b5f6165ffe
--- /dev/null
+++ b/tools/update_bt_diagrams.bash
@@ -0,0 +1,11 @@
+#!/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