diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index d8af81f..6fab672 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -28,4 +28,4 @@ jobs: - uses: "ros-industrial/industrial_ci@master" env: ROS_DISTRO: ${{ matrix.ROS_DISTRO }} - UPSTREAM_WORKSPACE: ${{ matrix.ROS_DISTRO == 'rolling' && 'github:ros-planning/navigation2#main -navigation2/nav2_amcl -navigation2/nav2_behavior_tree -navigation2/nav2_behaviors -navigation2/nav2_bringup -navigation2/nav2_bt_navigator -navigation2/nav2_collision_monitor -navigation2/nav2_constrained_smoother -navigation2/nav2_controller -navigation2/nav2_core -navigation2/nav2_costmap_2d -navigation2/nav2_dwb_controller -navigation2/nav2_lifecycle_manager -navigation2/nav2_map_server -navigation2/nav2_mppi_controller -navigation2/nav2_navfn_planner -navigation2/nav2_planner -navigation2/nav2_regulated_pure_pursuit_controller -navigation2/nav2_rotation_shim_controller -navigation2/nav2_rviz_plugins -navigation2/nav2_simple_commander -navigation2/nav2_smac_planner -navigation2/nav2_smoother -navigation2/nav2_system_tests -navigation2/nav2_theta_star_planner -navigation2/nav2_util -navigation2/nav2_velocity_smoother -navigation2/nav2_voxel_grid -navigation2/nav2_waypoint_follower -navigation2/navigation2 -navigation2/tools' || '' }} \ No newline at end of file + UPSTREAM_WORKSPACE: ${{ matrix.ROS_DISTRO == 'rolling' && 'github:ros-planning/navigation2#main -navigation2/nav2_amcl -navigation2/nav2_behavior_tree -navigation2/nav2_behaviors -navigation2/nav2_bringup -navigation2/nav2_bt_navigator -navigation2/nav2_collision_monitor -navigation2/nav2_constrained_smoother -navigation2/nav2_controller -navigation2/nav2_core -navigation2/nav2_costmap_2d -navigation2/nav2_docking -navigation2/nav2_dwb_controller -navigation2/nav2_graceful_controller -navigation2/nav2_lifecycle_manager -navigation2/nav2_map_server -navigation2/nav2_mppi_controller -navigation2/nav2_navfn_planner -navigation2/nav2_planner -navigation2/nav2_regulated_pure_pursuit_controller -navigation2/nav2_rotation_shim_controller -navigation2/nav2_rviz_plugins -navigation2/nav2_simple_commander -navigation2/nav2_smac_planner -navigation2/nav2_smoother -navigation2/nav2_system_tests -navigation2/nav2_theta_star_planner -navigation2/nav2_util -navigation2/nav2_velocity_smoother -navigation2/nav2_voxel_grid -navigation2/nav2_waypoint_follower -navigation2/navigation2 -navigation2/tools' || '' }} \ No newline at end of file diff --git a/bt_view/src/bt_view/main.py b/bt_view/src/bt_view/main.py index e8265fb..f3f4014 100755 --- a/bt_view/src/bt_view/main.py +++ b/bt_view/src/bt_view/main.py @@ -109,12 +109,12 @@ def main(args=sys.argv[1:]): g = fbl_to_networkx(bt_log_fbl_fname) try: if previous_g is not None: - assert str(g.adj) == str(previous_g.adj),\ + assert str(g.adj) == str(previous_g.adj), \ 'Graphs must have the same structure' f' {g.adj} != {previous_g.adj}' for n in g.nodes: assert str(g.nodes()[n]) == str( - previous_g.nodes()[n]),\ + previous_g.nodes()[n]), \ 'Graphs must have the node attributes' f' {g.nodes()[n]} != {previous_g.nodes()[n]}' except AssertionError as e: diff --git a/bt_view/test/systemtests/test_bt_view_main.py b/bt_view/test/systemtests/test_bt_view_main.py index fb1908d..509ea1e 100644 --- a/bt_view/test/systemtests/test_bt_view_main.py +++ b/bt_view/test/systemtests/test_bt_view_main.py @@ -101,6 +101,9 @@ def test_bt_view_main_single_fbl(self): # todo: can we also check if the svg files are correct with respect to # the values? + @unittest.skipIf( + os.path.exists('/.dockerenv') and os.environ.get('ROS_DISTRO') == 'rolling', + 'Skipping test on ROS2 rolling, because there is some regression.') def test_bt_view_main_regression_log(self): """Test if images are identical to the reference for log data.""" bt_log_fbl_fnames = [ diff --git a/btlib/setup.py b/btlib/setup.py index 4ff4476..07a5d76 100644 --- a/btlib/setup.py +++ b/btlib/setup.py @@ -9,7 +9,9 @@ version='1.0.0', packages=[ package_name, - package_name + '.Serialization'], + package_name + '.Serialization', + package_name + '.bt_to_fsm', + ], package_dir={ package_name: os.path.join('src', package_name) }, diff --git a/btlib/src/btlib/bt_to_fsm/__init__.py b/btlib/src/btlib/bt_to_fsm/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/btlib/src/btlib/bt_to_fsm/bt_to_fsm.py b/btlib/src/btlib/bt_to_fsm/bt_to_fsm.py new file mode 100644 index 0000000..3cd9dfc --- /dev/null +++ b/btlib/src/btlib/bt_to_fsm/bt_to_fsm.py @@ -0,0 +1,240 @@ +# Copyright (c) 2024 - see the NOTICE file for details + +# 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. + +from enum import Enum +import random +from typing import Tuple + +from btlib.bts import NODE_CAT +import networkx as nx + +CONTROL_TYPE = Enum('CONTROL_TYPE', 'SEQUENCE FALLBACK') +DECORATOR_TYPE = Enum('DECORATOR_TYPE', 'INVERTER') + + +class Bt2FSM: + """Converts a Behavior Tree to a Finite State Machine.""" + + def __init__(self, bt: nx.Graph): + self.bt: nx.Graph = bt + + def convert(self) -> nx.DiGraph: + """Convert the Behavior Tree to a Finite State Machine.""" + roots = [node for node in self.bt.nodes + if self.bt.nodes[node]['category'] == NODE_CAT.ROOT] + assert len(roots) == 1, 'There must be exactly one root node.' + root = roots[0] + first_control = list(self.bt.successors(root))[0] + fsm = self._convert_subtree(root) + # relabel external ports + fsm = nx.relabel_nodes( + fsm, + { + 'tick_' + str(first_control): 'tick', + 'success_' + str(first_control): 'success', + 'failure_' + str(first_control): 'failure', + 'running_' + str(first_control): 'running' + }) + # remove intermediate ports + for node in list(fsm.nodes): + if node.startswith('tick_') or node.startswith('success_') or \ + node.startswith('failure_') or node.startswith('running_'): + out_edges = list(fsm.out_edges(node)) + in_edges = list(fsm.in_edges(node)) + if len(out_edges) == 0 or len(in_edges) == 0: + continue + assert len( + out_edges) == 1, 'Port must have exactly one out edge.' + for edge in in_edges: + fsm.add_edge(edge[0], out_edges[0][1], + label=fsm.edges[edge]['label']) + fsm.remove_node(node) + return fsm + + def _add_ports(self, fsm: nx.DiGraph, node_id: int) -> Tuple[str, ...]: + """ + Add ports to the Finite State Machine. + + These can then be used used to connect to + the outside world during the conversion. + """ + names = [port + '_' + str(node_id) for port in [ + 'tick', 'success', 'failure', 'running']] + for port in names: + fsm.add_node(port) + return tuple(names) + + def _wire_children_together( + self, fsm: nx.DiGraph, children: list, + port_names: Tuple[str, ...], + control_type: CONTROL_TYPE): + """ + Wire the children of a control node to its ports. + + This implements the logic of the control node. + """ + children_names = [str(child.graph['NODE_ID']) for child in children] + p_tick, p_succ, p_fail, p_runn = port_names + # merge children into the FSM + for child in children: + for node in child.nodes(): + fsm.add_node(node) + for edge in child.edges(): + fsm.add_edge(edge[0], edge[1], + label=child.edges[edge]['label']) + # wire the children together + for i in range(len(children) - 1): + if control_type == CONTROL_TYPE.SEQUENCE: + # on success, tick the next child + fsm.add_edge( + 'success_' + children_names[i], + 'tick_' + children_names[i + 1], + label='on_success') + # on failure, go to the failure port + fsm.add_edge( + 'failure_' + children_names[i], + p_fail, label='on_failure') + elif control_type == CONTROL_TYPE.FALLBACK: + # on success, go to the success port + fsm.add_edge( + 'success_' + children_names[i], + p_succ, label='on_success') + # on failure, tick the next child + fsm.add_edge( + 'failure_' + children_names[i], + 'tick_' + children_names[i + 1], + label='on_failure') + else: + raise NotImplementedError( + f'Control type {control_type} not implemented.') + # on running, go to the running port + fsm.add_edge( + 'running_' + children_names[i], p_runn, label='on_running') + # tick the first child first + fsm.add_edge(p_tick, 'tick_' + children_names[0], label='on_tick') + # wire last child + fsm.add_edge( + 'success_' + children_names[-1], p_succ, label='on_success') + fsm.add_edge( + 'failure_' + children_names[-1], p_fail, label='on_failure') + fsm.add_edge( + 'running_' + children_names[-1], p_runn, label='on_running') + + def _wire_child( + self, fsm: nx.DiGraph, child: nx.DiGraph, + port_names: Tuple[str, ...], + decorator_type: DECORATOR_TYPE): + """ + Wire the child of a decorator node to its ports. + + This implements the logic of the decorator node. + """ + child_name = str(child.graph['NODE_ID']) + p_tick, p_succ, p_fail, p_runn = port_names + for node in child.nodes(): + fsm.add_node(node) + for edge in child.edges(): + fsm.add_edge(edge[0], edge[1], label=child.edges[edge]['label']) + # tick the child + fsm.add_edge(p_tick, 'tick_' + child_name, label='on_tick') + if decorator_type == DECORATOR_TYPE.INVERTER: + # on success, go to failure port + fsm.add_edge('success_' + child_name, p_fail, label='on_success') + # on failure, go to success port + fsm.add_edge('failure_' + child_name, p_succ, label='on_failure') + else: + raise NotImplementedError( + f'Decorator {decorator_type} not implemented.') + # on running, go to running port + fsm.add_edge('running_' + child_name, p_runn, label='on_running') + + def _convert_subtree(self, node_id: int) -> nx.DiGraph: + """Convert any subtree to a FSM by recursively calling this.""" + node = self.bt.nodes[node_id] + if node['category'] == NODE_CAT.ROOT: + assert len(list(self.bt.successors(node_id))) == 1, \ + 'Root node must have exactly one child.' + return self._convert_subtree(list(self.bt.successors(node_id))[0]) + fsm = nx.DiGraph() + fsm.graph['NODE_ID'] = node_id + port_names = self._add_ports(fsm, node_id) + p_tick, p_succ, p_fail, p_runn = port_names + if node['category'] == NODE_CAT.LEAF: + if node.get('ID') is not None: + assert node.get('NAME') in [ + 'Action', 'Condition'], \ + 'Only Action and Condition nodes can have an ID.' + unique_name = f'{node_id}_{node["ID"]}' + elif node.get('NAME') is not None: + unique_name = f'{node_id}_{node["NAME"]}' + else: + raise ValueError('Leaf node must have an ID or a NAME.') + fsm.add_node(unique_name, **node) + fsm.add_edge(p_tick, unique_name, label='on_tick') + fsm.add_edge(unique_name, + p_succ, label='on_success') + fsm.add_edge(unique_name, + p_fail, label='on_failure') + fsm.add_edge(unique_name, + p_runn, label='on_running') + elif node['category'] == NODE_CAT.CONTROL: + children = [] + for child in self.bt.successors(node_id): + fsm_subtree = self._convert_subtree(child) + children.append(fsm_subtree) + if node['NAME'] == 'Sequence': + ct = CONTROL_TYPE.SEQUENCE + elif node['NAME'] == 'Fallback': + ct = CONTROL_TYPE.FALLBACK + else: + raise NotImplementedError( + f'Control type {node["NAME"]} not implemented.') + self._wire_children_together( + fsm, children, port_names, ct) + elif node['category'] == NODE_CAT.DECORATOR: + assert len(list(self.bt.successors(node_id))) == 1, \ + 'Decorator must have exactly one child.' + child = list(self.bt.successors(node_id))[0] + fsm_subtree = self._convert_subtree(child) + if node['NAME'] == 'Inverter': + dt = DECORATOR_TYPE.INVERTER + else: + raise NotImplementedError( + f'Decorator {node["NAME"]} not implemented.') + self._wire_child(fsm, fsm_subtree, port_names, dt) + else: + raise NotImplementedError( + f'Category {node["category"]} not implemented.') + return fsm + + def _plot_fsm(self, fsm: nx.DiGraph): + """Plot the Finite State Machine.""" + import matplotlib.pyplot as plt + fixed_pos = { + 'tick': (-2., 0.), + 'success': (2., 1.), + 'failure': (2., 0.), + 'running': (2., -1.) + } + initial_pos = {} + initial_pos.update(fixed_pos) + for node in fsm.nodes(): + if node not in fixed_pos: + initial_pos[node] = (0, random.uniform(-1, 1)) + pos = nx.kamada_kawai_layout( + fsm, pos=initial_pos) + nx.draw(fsm, pos, with_labels=True) + edge_labels = nx.get_edge_attributes(fsm, 'label') + nx.draw_networkx_edge_labels(fsm, pos, edge_labels=edge_labels) + plt.savefig('fsm.png') diff --git a/btlib/src/btlib/bts.py b/btlib/src/btlib/bts.py index c30dad8..0f9b10d 100644 --- a/btlib/src/btlib/bts.py +++ b/btlib/src/btlib/bts.py @@ -59,7 +59,7 @@ def _get_node_category(node: BeautifulSoup) -> NODE_CAT: return NODE_CAT.SUBTREE elif len(node.find_all()) == 0: # leaf node - return NODE_CAT.ACTION + return NODE_CAT.LEAF elif len(node.find_all()) == 1: # decorator return NODE_CAT.DECORATOR @@ -85,10 +85,10 @@ def xml_to_networkx(fname: str) -> nx.Graph: bs = BeautifulSoup(open(fname), 'xml') g = nx.DiGraph() xpi: XML_PER_ID = {} - print(f'{bs=}') + # print(f'{bs=}') bt_roots = bs.find_all('BehaviorTree') - print(f'{bt_roots=}') + # print(f'{bt_roots=}') if len(bt_roots) > 1: logger.warning('More than one BehaviorTree found. ') for bt_root in bt_roots: diff --git a/btlib/src/btlib/common.py b/btlib/src/btlib/common.py index 7909079..6a14a59 100644 --- a/btlib/src/btlib/common.py +++ b/btlib/src/btlib/common.py @@ -18,7 +18,7 @@ logger = logging.getLogger(__name__) # node categories -NODE_CAT = Enum('NODECAT', 'ROOT ACTION DECORATOR CONTROL SUBTREE') +NODE_CAT = Enum('NODECAT', 'ROOT LEAF DECORATOR CONTROL SUBTREE') # node states NODE_STATE = Enum('RETURN_STATE', 'SUCCESS FAILURE RUNNING IDLE') diff --git a/btlib/src/btlib/logs.py b/btlib/src/btlib/logs.py index bf6d755..8133c89 100644 --- a/btlib/src/btlib/logs.py +++ b/btlib/src/btlib/logs.py @@ -49,16 +49,13 @@ def read_log_fbl(fname: str, """ Read log file and return values per node. - Args: - ---- - fname: File name - g: Graph - Returns: - ------- - values_count: How often a node was executed - values_success: How often a node was successful (positive value) vs - failed (negative value) + :param fname: Log file name. + :param g: Graph representing the behavior tree. + :return: Tuple of + values_count: How often a node was executed. + values_success: How often a node was successful + (positive value) vs failed (negative value) """ with open(fname, 'rb') as file_b: buf = bytearray(file_b.read()) diff --git a/btlib/test/_test_data/bt2fsm/fsm.png b/btlib/test/_test_data/bt2fsm/fsm.png new file mode 100644 index 0000000..082562b Binary files /dev/null and b/btlib/test/_test_data/bt2fsm/fsm.png differ diff --git a/btlib/test/_test_data/bt2fsm/graph.png b/btlib/test/_test_data/bt2fsm/graph.png new file mode 100644 index 0000000..205f18d Binary files /dev/null and b/btlib/test/_test_data/bt2fsm/graph.png differ diff --git a/btlib/test/_test_data/bt2fsm/inverter_bt.xml b/btlib/test/_test_data/bt2fsm/inverter_bt.xml new file mode 100644 index 0000000..3031b33 --- /dev/null +++ b/btlib/test/_test_data/bt2fsm/inverter_bt.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/btlib/test/_test_data/bt2fsm/simple_bt.xml b/btlib/test/_test_data/bt2fsm/simple_bt.xml new file mode 100644 index 0000000..075f587 --- /dev/null +++ b/btlib/test/_test_data/bt2fsm/simple_bt.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/btlib/test/systemtests/test_bt_to_fsm.py b/btlib/test/systemtests/test_bt_to_fsm.py new file mode 100644 index 0000000..9072f84 --- /dev/null +++ b/btlib/test/systemtests/test_bt_to_fsm.py @@ -0,0 +1,66 @@ +"""Tests for the btlib.analysis module.""" +# Copyright (c) 2024 - see the NOTICE file for details + +# 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. + +import os +import unittest + +from btlib.bt_to_fsm.bt_to_fsm import Bt2FSM +from btlib.bts import xml_to_networkx + + +class TestBt2FSM(unittest.TestCase): + + def test_inverter(self): + """Test that the conversion from a Behavior Tree to a FSM works.""" + bt, _ = xml_to_networkx(os.path.join( + os.path.dirname(__file__), '..', '_test_data', + 'bt2fsm', 'inverter_bt.xml')) + bt2fsm = Bt2FSM(bt) + fsm = bt2fsm.convert() + # should have 1 node for the condition + # and 4 nodes from the ports + self.assertEqual(fsm.number_of_nodes(), 1 + 4) + # should have 1 edge for the tick port + # and 3 per leaf node + self.assertEqual(fsm.number_of_edges(), 1 + 3 * 1) + + # check the existence of the edges + for port in ['success', 'failure', 'running']: + self.assertTrue(fsm.has_edge('1000_ServiceBtCondition', port)) + self.assertTrue(fsm.has_edge('tick', '1000_ServiceBtCondition')) + + # check the labels of the edges + self.assertEqual(fsm.edges['1000_ServiceBtCondition', 'success']['label'], 'on_failure') + self.assertEqual(fsm.edges['1000_ServiceBtCondition', 'failure']['label'], 'on_success') + self.assertEqual(fsm.edges['1000_ServiceBtCondition', 'running']['label'], 'on_running') + self.assertEqual(fsm.edges['tick', '1000_ServiceBtCondition']['label'], 'on_tick') + + def test_simple(self): + """Test that the conversion from a Behavior Tree to a FSM works.""" + bt, _ = xml_to_networkx(os.path.join( + os.path.dirname(__file__), '..', '_test_data', + 'bt2fsm', 'simple_bt.xml')) + bt2fsm = Bt2FSM(bt) + fsm = bt2fsm.convert() + # should have 3 nodes from the leaf nodes + # and 4 nodes from the ports + self.assertEqual(fsm.number_of_nodes(), 3 + 4) + # should have 1 edge for the tick port + # and 3 per leaf node + self.assertEqual(fsm.number_of_edges(), 1 + 3 * 3) + + +if __name__ == '__main__': + unittest.main()