add humble-navigation2
This commit is contained in:
X-lanni
Executable
+197
@@ -0,0 +1,197 @@
|
||||
#!/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",
|
||||
"RecoveryNode",
|
||||
"PipelineSequence",
|
||||
"RoundRobin",
|
||||
"Control",
|
||||
]
|
||||
action_nodes = [
|
||||
"AlwaysFailure",
|
||||
"AlwaysSuccess",
|
||||
"SetBlackboard",
|
||||
"ComputePathToPose",
|
||||
"FollowPath",
|
||||
"BackUp",
|
||||
"Spin",
|
||||
"Wait",
|
||||
"ClearEntireCostmap",
|
||||
"ReinitializeGlobalLocalization",
|
||||
"Action",
|
||||
]
|
||||
condition_nodes = [
|
||||
"IsStuck",
|
||||
"GoalReached",
|
||||
"initialPoseReceived",
|
||||
"GoalUpdated",
|
||||
"DistanceTraveled",
|
||||
"TimeExpired",
|
||||
"TransformAvailable",
|
||||
"Condition",
|
||||
]
|
||||
decorator_nodes = [
|
||||
"Decorator",
|
||||
"RateController",
|
||||
"DistanceController",
|
||||
"SpeedController",
|
||||
]
|
||||
subtree_nodes = [
|
||||
"SubTree",
|
||||
]
|
||||
|
||||
global xml_tree
|
||||
|
||||
def main():
|
||||
global xml_tree
|
||||
args = parse_command_line()
|
||||
xml_tree = xml.etree.ElementTree.parse(args.behavior_tree)
|
||||
root_tree_name = find_root_tree_name(xml_tree)
|
||||
behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
|
||||
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(f'Saving dot to {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()
|
||||
|
||||
def find_root_tree_name(xml_tree):
|
||||
return xml_tree.getroot().get('main_tree_to_execute')
|
||||
|
||||
def find_behavior_tree(xml_tree, tree_name):
|
||||
trees = xml_tree.findall('BehaviorTree')
|
||||
if len(trees) == 0:
|
||||
raise RuntimeError("No behavior trees were found in the XML file")
|
||||
|
||||
for tree in trees:
|
||||
if tree_name == tree.get('ID'):
|
||||
return tree
|
||||
|
||||
raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file')
|
||||
|
||||
# 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):
|
||||
if parent_node.tag == "SubTree":
|
||||
add_sub_tree(dot, parent_dot_name, parent_node)
|
||||
else:
|
||||
add_nodes(dot, parent_dot_name, parent_node)
|
||||
|
||||
def add_sub_tree(dot, parent_dot_name, parent_node):
|
||||
root_tree_name = parent_node.get('ID')
|
||||
dot.node(parent_dot_name, root_tree_name, shape='box')
|
||||
behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
|
||||
convert_subtree(dot, behavior_tree, parent_dot_name)
|
||||
|
||||
def add_nodes(dot, parent_dot_name, parent_node):
|
||||
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"
|
||||
if type in decorator_nodes:
|
||||
return "darkorange1"
|
||||
if type in subtree_nodes:
|
||||
return "darkorchid1"
|
||||
#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()
|
||||
Reference in New Issue
Block a user