From 406c842e3d04b16971c4e41f4ba9cb83d00d57b2 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 15:56:39 +0200 Subject: [PATCH 01/49] Add support to BT Input Ports Signed-off-by: Marco Lampacrescia Signed-off-by: Christian Henkel --- docs/source/howto.rst | 86 +++++- .../ros_helpers/ros_services.py | 9 +- .../scxml_helpers/top_level_interpreter.py | 26 +- .../properties.jani | 12 +- .../happy_clients.jani | 8 +- .../test/test_systemtest_scxml_to_jani.py | 2 +- .../src/scxml_converter/bt_converter.py | 105 +++---- .../scxml_converter/scxml_entries/__init__.py | 8 +- .../scxml_converter/scxml_entries/bt_utils.py | 144 +++++++++ .../scxml_entries/ros_utils.py | 288 ++++++++++++++++++ .../scxml_entries/scxml_base.py | 4 + .../scxml_converter/scxml_entries/scxml_bt.py | 144 +++++++++ .../scxml_entries/scxml_data.py | 124 +++++--- .../scxml_entries/scxml_data_model.py | 5 + .../scxml_entries/scxml_executable_entries.py | 151 ++++++--- .../scxml_entries/scxml_param.py | 79 ++--- .../scxml_entries/scxml_root.py | 94 ++++-- .../scxml_entries/scxml_ros_field.py | 44 +-- .../scxml_entries/scxml_ros_service.py | 192 +++++++----- .../scxml_entries/scxml_ros_timer.py | 26 +- .../scxml_entries/scxml_ros_topic.py | 236 ++++++++------ .../scxml_entries/scxml_state.py | 36 ++- .../scxml_entries/scxml_transition.py | 35 ++- .../scxml_converter/scxml_entries/utils.py | 244 ++------------- .../scxml_entries/xml_utils.py | 109 +++++++ scxml_converter/test/_test_data/.gitignore | 1 + .../add_int_srv_example/client_1.scxml | 34 +++ .../gt_plain_scxml/client_1.scxml | 21 ++ .../gt_plain_scxml/server.scxml | 16 + .../add_int_srv_example/server.scxml | 27 ++ .../battery_drainer.scxml | 33 ++ .../battery_manager.scxml | 4 +- .../bt.xml | 0 .../bt_topic_action.scxml | 2 +- .../bt_topic_condition.scxml | 4 +- .../gt_bt_scxml}/10000_TopicCondition.scxml | 6 +- .../gt_bt_scxml}/1001_TopicAction.scxml | 6 +- .../gt_bt_scxml}/bt.scxml | 0 .../gt_parsed_scxml}/battery_drainer.scxml | 8 +- .../gt_parsed_scxml/bt_topic_condition.scxml | 31 ++ .../gt_plain_scxml}/battery_drainer.scxml | 0 .../gt_plain_scxml}/battery_manager.scxml | 0 .../gt_plain_scxml}/bt_topic_action.scxml | 0 .../gt_plain_scxml}/bt_topic_condition.scxml | 0 .../_test_data/bt_ports_blackboard/bt.xml | 8 + .../bt_ports_blackboard/bt_get_number.scxml | 21 ++ .../bt_ports_blackboard/bt_topic_action.scxml | 34 +++ .../test/_test_data/bt_ports_only/bt.xml | 8 + .../bt_ports_only/bt_topic_action.scxml | 38 +++ .../gt_bt_scxml/1000_TopicAction.scxml | 15 + .../gt_bt_scxml/1001_TopicAction.scxml | 15 + .../bt_ports_only/gt_bt_scxml/bt.scxml | 35 +++ .../gt_parsed_scxml/bt_topic_action.scxml | 26 ++ .../gt_plain_scxml/bt_topic_action.scxml | 14 + .../invalid_xmls/battery_drainer.scxml | 0 .../invalid_xmls/bt_topic_action.scxml | 0 .../test/test_systemtest_scxml_entries.py | 137 +++++---- scxml_converter/test/test_systemtest_xml.py | 136 +++++---- 58 files changed, 2062 insertions(+), 829 deletions(-) create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/bt_utils.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_bt.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py create mode 100644 scxml_converter/test/_test_data/.gitignore create mode 100644 scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml create mode 100644 scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml create mode 100644 scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml create mode 100644 scxml_converter/test/_test_data/add_int_srv_example/server.scxml create mode 100644 scxml_converter/test/_test_data/battery_drainer_w_bt/battery_drainer.scxml rename scxml_converter/test/_test_data/{input_files => battery_drainer_w_bt}/battery_manager.scxml (81%) rename scxml_converter/test/_test_data/{input_files => battery_drainer_w_bt}/bt.xml (100%) rename scxml_converter/test/_test_data/{input_files => battery_drainer_w_bt}/bt_topic_action.scxml (93%) rename scxml_converter/test/_test_data/{input_files => battery_drainer_w_bt}/bt_topic_condition.scxml (86%) rename scxml_converter/test/_test_data/{expected_output_bt_and_plugins => battery_drainer_w_bt/gt_bt_scxml}/10000_TopicCondition.scxml (83%) rename scxml_converter/test/_test_data/{expected_output_bt_and_plugins => battery_drainer_w_bt/gt_bt_scxml}/1001_TopicAction.scxml (79%) rename scxml_converter/test/_test_data/{expected_output_bt_and_plugins => battery_drainer_w_bt/gt_bt_scxml}/bt.scxml (100%) rename scxml_converter/test/_test_data/{input_files => battery_drainer_w_bt/gt_parsed_scxml}/battery_drainer.scxml (78%) create mode 100644 scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml rename scxml_converter/test/_test_data/{expected_output_ros_to_scxml => battery_drainer_w_bt/gt_plain_scxml}/battery_drainer.scxml (100%) rename scxml_converter/test/_test_data/{expected_output_ros_to_scxml => battery_drainer_w_bt/gt_plain_scxml}/battery_manager.scxml (100%) rename scxml_converter/test/_test_data/{expected_output_ros_to_scxml => battery_drainer_w_bt/gt_plain_scxml}/bt_topic_action.scxml (100%) rename scxml_converter/test/_test_data/{expected_output_ros_to_scxml => battery_drainer_w_bt/gt_plain_scxml}/bt_topic_condition.scxml (100%) create mode 100644 scxml_converter/test/_test_data/bt_ports_blackboard/bt.xml create mode 100644 scxml_converter/test/_test_data/bt_ports_blackboard/bt_get_number.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_blackboard/bt_topic_action.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/bt.xml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml create mode 100644 scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml rename scxml_converter/test/_test_data/{input_files => }/invalid_xmls/battery_drainer.scxml (100%) rename scxml_converter/test/_test_data/{input_files => }/invalid_xmls/bt_topic_action.scxml (100%) diff --git a/docs/source/howto.rst b/docs/source/howto.rst index 0a78a288..26e0aace 100644 --- a/docs/source/howto.rst +++ b/docs/source/howto.rst @@ -104,11 +104,14 @@ ROS Topics are used to publish (via a ROS Publisher) and receive (via a ROS Subs .. code-block:: xml - + - + -Once created, subscribers and publishers can be referenced using the `topic` name, and can be used in the states to send messages and perform callbacks upon receiving messages: +The two declarations above will create a ROS subscriber called `bool_topic` that reads messages of type `std_msgs/Bool` from the topic `/topic1` and a ROS publisher called `int_topic` that writes messages of type `std_msgs/Int32` on the topic `/topic2`. +The `name` argument is optional, and if not provided, it will be set to the same value as the `topic` argument. + +Once created, subscribers and publishers can be referenced using their names (`bool_topic` and `int_topic`), and can be used in the states to send messages and perform callbacks upon receiving messages: .. code-block:: xml @@ -117,7 +120,7 @@ Once created, subscribers and publishers can be referenced using the `topic` nam - + @@ -125,11 +128,11 @@ Once created, subscribers and publishers can be referenced using the `topic` nam - + - + @@ -154,11 +157,11 @@ The declaration of a ROS Service server and the one of a client can be achieved .. code-block:: xml - + - + -Once created, servers and clients can be referenced using the `service_name` name, and can be used in the states of a SCXML model to provide and request services. +Once created, servers and clients can be referenced using the provided `name` (i.e. `the_srv` and `the_client`), and can be used in the states of a SCXML model to provide and request services. In the following, an exemplary client is provided: .. code-block:: xml @@ -169,16 +172,16 @@ In the following, an exemplary client is provided: - + - + To send a request, the `ros_service_send_request` can be used where any other executable content may be used. -After the server has processed the service, `ros_service_handle_response`, can be used similarly to a SCXML transition and is triggered by the server. +After the server has processed the service, `ros_service_handle_response`, can be used similarly to a SCXML transition and is triggered when a response from the server is received. The data of the request can be accessed with the `_res` field. And here, an example of a server: @@ -190,9 +193,9 @@ And here, an example of a server: - + - + @@ -215,7 +218,60 @@ TODO Creating a SCXML model of a BT plugin ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -TODO +SCXML models of BT plugins can be done similarly to the ones for ROS nodes. However, in BT plugins there are a few special functionalities that are provided: + +* :ref:`BT communication `: A set of special events that are used in each BT plugins for starting a BT Node and provide results. +* :ref:`BT Ports `: A special BT interface to parametrize a specific plugin instance. + + +.. _bt_communication: + +BT Communication +_________________ + +TODO: describe `bt_tick`, `bt_running`, `bt_success`, `bt_failure`. + + +.. _bt_ports: + +BT Ports +________ + +Additionally, when loading a BT plugin in the BT XML Tree, it is possible to configure a specific plugin instance by means of the BT ports. + +As in the case of ROS functionalities, BT Ports need to be declared before being used, to provide the port name and expected type. + +.. code-block:: xml + + + + +Once declared, it is possible to reference to the port in multiple SCXML entries. + +For example, we can use `my_string_port` to define the topic used by a ROS publisher. + +.. code-block:: xml + + + + + + + +Or we can use `start_value` to define the initial value of a variable. + +.. code-block:: xml + + + + + + + + + + +BT ports can also be linked to variables in the `BT Blackboard` by wrapping the variable name in curly braces in the BT xml file. However, this feature is not yet supported. .. _additional_params_howto: diff --git a/jani_generator/src/jani_generator/ros_helpers/ros_services.py b/jani_generator/src/jani_generator/ros_helpers/ros_services.py index 62616c0f..93722eef 100644 --- a/jani_generator/src/jani_generator/ros_helpers/ros_services.py +++ b/jani_generator/src/jani_generator/ros_helpers/ros_services.py @@ -24,11 +24,10 @@ ScxmlDataModel, ScxmlParam, ScxmlRoot, ScxmlSend, ScxmlState, ScxmlTransition) -from scxml_converter.scxml_entries.utils import ( - generate_srv_request_event, generate_srv_response_event, - generate_srv_server_request_event, generate_srv_server_response_event, - get_default_expression_for_type, get_srv_type_params, - sanitize_ros_interface_name) +from scxml_converter.scxml_entries.utils import get_default_expression_for_type +from scxml_converter.scxml_entries.ros_utils import ( + generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, + generate_srv_server_response_event, get_srv_type_params, sanitize_ros_interface_name) SRV_PREFIX = "srv_handler_" diff --git a/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py b/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py index f6215993..6ea24aff 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py +++ b/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py @@ -20,11 +20,10 @@ import json import os from dataclasses import dataclass, field -from typing import Any, Dict, List, Optional, Tuple, Union +from typing import List, Optional, Tuple from xml.etree import ElementTree as ET from as2fm_common.common import remove_namespace -from jani_generator.jani_entries import JaniModel from jani_generator.ros_helpers.ros_services import RosService, RosServices from jani_generator.ros_helpers.ros_timer import RosTimer from jani_generator.scxml_helpers.scxml_to_jani import \ @@ -129,34 +128,33 @@ def generate_plain_scxml_models_and_timers( """ Generate plain SCXML models and ROS timers from the full model dictionary. """ - # Convert behavior tree and plugins to ROS-scxml + # Load the skills and components scxml files (ROS-SCXML) scxml_files_to_convert: list = model.skills + model.components + ros_scxmls: List[ScxmlRoot] = [] + for fname in scxml_files_to_convert: + ros_scxmls.append(ScxmlRoot.from_scxml_file(fname)) + # Convert behavior tree and plugins to ROS-SCXML if model.bt is not None: - bt_out_dir = os.path.join(os.path.dirname(model.bt), "generated_bt_scxml") - os.makedirs(bt_out_dir, exist_ok=True) - expanded_bt_plugin_scxmls = bt_converter( - model.bt, model.plugins, bt_out_dir) - scxml_files_to_convert.extend(expanded_bt_plugin_scxmls) - - # Convert ROS-SCXML FSMs to plain SCXML + ros_scxmls.extend(bt_converter(model.bt, model.plugins)) + # Convert the loaded entries to plain SCXML plain_scxml_models = [] all_timers: List[RosTimer] = [] all_services: RosServices = {} - for fname in scxml_files_to_convert: + for scxml_entry in ros_scxmls: plain_scxml, ros_declarations = \ - ScxmlRoot.from_scxml_file(fname).to_plain_scxml_and_declarations() + scxml_entry.to_plain_scxml_and_declarations() # Handle ROS timers for timer_name, timer_rate in ros_declarations._timers.items(): assert timer_name not in all_timers, \ f"Timer {timer_name} already exists." all_timers.append(RosTimer(timer_name, timer_rate)) # Handle ROS Services - for service_name, service_type in ros_declarations._service_clients.items(): + for service_name, service_type in ros_declarations._service_clients.values(): if service_name not in all_services: all_services[service_name] = RosService() all_services[service_name].append_service_client( service_name, service_type, plain_scxml.get_name()) - for service_name, service_type in ros_declarations._service_servers.items(): + for service_name, service_type in ros_declarations._service_servers.values(): if service_name not in all_services: all_services[service_name] = RosService() all_services[service_name].set_service_server( diff --git a/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani b/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani index 9d450983..25029a3e 100644 --- a/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani +++ b/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani @@ -13,19 +13,19 @@ "op": "∧", "left": { "op": "<", - "left": "ros_topic./sender_a_counter.data", + "left": "ros_topic.sender_a_counter.data", "right": 100 }, "right": { "op": "∧", "left": { "op": "<", - "left": "ros_topic./sender_b_counter.data", + "left": "ros_topic.sender_b_counter.data", "right": 100 }, "right": { "op": "<", - "left": "ros_topic./receiver_counter.data", + "left": "ros_topic.receiver_counter.data", "right": 100 } } @@ -35,7 +35,7 @@ "op": "∧", "left": { "op": ">", - "left": "ros_topic./receiver_counter.data", + "left": "ros_topic.receiver_counter.data", "right": 48 }, "right": { @@ -43,8 +43,8 @@ "left": 50, "right": { "op": "+", - "left": "ros_topic./sender_a_counter.data", - "right": "ros_topic./sender_b_counter.data" + "left": "ros_topic.sender_a_counter.data", + "right": "ros_topic.sender_b_counter.data" } } } diff --git a/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani b/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani index 418cab82..382a0882 100644 --- a/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani @@ -13,13 +13,13 @@ "op": "∧", "left": { "op": "∧", - "left": "ros_topic./client_1_res.data", - "right": "ros_topic./client_1_res.valid" + "left": "ros_topic.client_1_res.data", + "right": "ros_topic.client_1_res.valid" }, "right": { "op": "∧", - "left": "ros_topic./client_2_res.data", - "right": "ros_topic./client_2_res.valid" + "left": "ros_topic.client_2_res.data", + "right": "ros_topic.client_2_res.valid" } } } diff --git a/jani_generator/test/test_systemtest_scxml_to_jani.py b/jani_generator/test/test_systemtest_scxml_to_jani.py index 45a454db..19fa0348 100644 --- a/jani_generator/test/test_systemtest_scxml_to_jani.py +++ b/jani_generator/test/test_systemtest_scxml_to_jani.py @@ -240,7 +240,7 @@ def test_with_main_fail(self): def test_with_w_bt_main_battery_depleted(self): """Here we expect the property to be *not* satisfied.""" # TODO: Improve properties under evaluation! - self._test_with_main('ros_example_w_bt', 'battery_depleted', False) + self._test_with_main('ros_example_w_bt', 'battery_depleted', False, True) def test_with_w_bt_main_battery_under_twenty(self): """Here we expect the property to be *not* satisfied.""" diff --git a/scxml_converter/src/scxml_converter/bt_converter.py b/scxml_converter/src/scxml_converter/bt_converter.py index e4e6b411..8ba83b9f 100644 --- a/scxml_converter/src/scxml_converter/bt_converter.py +++ b/scxml_converter/src/scxml_converter/bt_converter.py @@ -15,22 +15,21 @@ """ Convert Behavior Trees (BT xml) to SCXML. - - """ +from copy import deepcopy import os -import xml.etree.ElementTree as ET from enum import Enum, auto from typing import List +import re from btlib.bt_to_fsm.bt_to_fsm import Bt2FSM from btlib.bts import xml_to_networkx from btlib.common import NODE_CAT -from scxml_converter.scxml_entries import (RosRateCallback, RosTimeRate, - ScxmlRoot, ScxmlSend, ScxmlState, - ScxmlTransition) +from scxml_converter.scxml_entries import ( + RosRateCallback, RosTimeRate, ScxmlRoot, ScxmlSend, ScxmlState, ScxmlTransition, + RESERVED_BT_PORT_NAMES) class BT_EVENT_TYPE(Enum): @@ -56,34 +55,31 @@ def bt_event_name(node_id: str, event_type: BT_EVENT_TYPE) -> str: def bt_converter( bt_xml_path: str, bt_plugins_scxml_paths: List[str], - output_folder: str -): +) -> List[ScxmlRoot]: """ Convert a Behavior Tree (BT) in XML format to SCXML. Args: bt_xml_path: The path to the Behavior Tree in XML format. bt_plugins_scxml_paths: The paths to the SCXML files of BT plugins. - output_folder: The folder where the SCXML files will be saved. Returns: - A list of the generated SCXML files. + A list of the generated SCXML objects. """ - bt_graph, xpi = xml_to_networkx(bt_xml_path) - generated_files = [] + bt_graph, _ = xml_to_networkx(bt_xml_path) - bt_plugins_scxml = {} + bt_plugins_scxmls = {} for path in bt_plugins_scxml_paths: assert os.path.exists(path), f'SCXML must exist. {path} not found.' - with open(path, 'r', encoding='utf-8') as f: - content = f.read() - xml = ET.fromstring(content) - name = xml.attrib['name'] - assert name not in bt_plugins_scxml, \ - f'Plugin name must be unique. {name} already exists.' - bt_plugins_scxml[name] = content + bt_plugin_scxml = ScxmlRoot.from_scxml_file(path) + bt_plugin_name = bt_plugin_scxml.get_name() + assert bt_plugin_name not in bt_plugins_scxmls, \ + f'Plugin name must be unique. {bt_plugin_name} already exists.' + bt_plugins_scxmls[bt_plugin_name] = bt_plugin_scxml leaf_node_ids = [] + generated_scxmls: List[ScxmlRoot] = [] + # Generate the instances of the plugins used in the BT for node in bt_graph.nodes: assert 'category' in bt_graph.nodes[node], 'Node must have a category.' if bt_graph.nodes[node]['category'] == NODE_CAT.LEAF: @@ -91,45 +87,34 @@ def bt_converter( assert 'ID' in bt_graph.nodes[node], 'Leaf node must have a type.' node_type = bt_graph.nodes[node]['ID'] node_id = node - assert node_type in bt_plugins_scxml, \ + assert node_type in bt_plugins_scxmls, \ f'Leaf node must have a plugin. {node_type} not found.' instance_name = f'{node_id}_{node_type}' - output_fname = os.path.join( - output_folder, f'{instance_name}.scxml') - generated_files.append(output_fname) - this_plugin_content = bt_plugins_scxml[node_type] - event_names_to_replace = [ - f'bt_{t}' for t in [ - 'tick', 'success', 'failure', 'running']] - for event_name in event_names_to_replace: - declaration_old = f'event="{event_name}"' - new_event_name = bt_event_name( - node_id, BT_EVENT_TYPE.from_str(event_name)) - declaration_new = f'event="{new_event_name}"' - this_plugin_content = this_plugin_content.replace( - declaration_old, declaration_new) - # TODO: Replace arguments from the BT xml file. - # TODO: Change name to instance name - with open(output_fname, 'w', encoding='utf-8') as f: - f.write(this_plugin_content) + scxml_plugin_instance: ScxmlRoot = deepcopy(bt_plugins_scxmls[node_type]) + scxml_plugin_instance.set_name(instance_name) + scxml_plugin_instance.instantiate_bt_events(node_id) + bt_ports = [(p_name, p_value) for p_name, p_value in bt_graph.nodes[node].items() + if p_name not in RESERVED_BT_PORT_NAMES] + scxml_plugin_instance.set_bt_ports_values(bt_ports) + scxml_plugin_instance.update_bt_ports_values() + assert scxml_plugin_instance.check_validity(), \ + f"Error: SCXML plugin instance {instance_name} is not valid." + generated_scxmls.append(scxml_plugin_instance) + # Generate the BT SCXML fsm_graph = Bt2FSM(bt_graph).convert() - output_file_bt = os.path.join(output_folder, 'bt.scxml') - generated_files.append(output_file_bt) - - root_tag = ScxmlRoot("bt") + bt_scxml_root = ScxmlRoot("bt") + name_with_id_pattern = re.compile(r"[0-9]+_.+") for node in fsm_graph.nodes: state = ScxmlState(node) - if '_' in node: + node_id = None + if name_with_id_pattern.match(node): node_id = int(node.split('_')[0]) - else: - node_id = None - if node_id and node_id in leaf_node_ids: - state.append_on_entry(ScxmlSend( - bt_event_name(node_id, BT_EVENT_TYPE.TICK))) + if node_id in leaf_node_ids: + state.append_on_entry(ScxmlSend(bt_event_name(node_id, BT_EVENT_TYPE.TICK))) for edge in fsm_graph.edges(node): target = edge[1] transition = ScxmlTransition(target) - if node_id and node_id in leaf_node_ids: + if node_id is not None and node_id in leaf_node_ids: if 'label' not in fsm_graph.edges[edge]: continue label = fsm_graph.edges[edge]['label'] @@ -145,21 +130,17 @@ def bt_converter( transition.add_event(event_name) state.add_transition(transition) if node in ['success', 'failure', 'running']: - state.add_transition( - ScxmlTransition("wait_for_tick")) - root_tag.add_state(state) - + state.add_transition(ScxmlTransition("wait_for_tick")) + bt_scxml_root.add_state(state) + # TODO: Make BT rate configurable, e.g. from main.xml rtr = RosTimeRate("bt_tick", 1.0) - root_tag.add_ros_declaration(rtr) + bt_scxml_root.add_ros_declaration(rtr) wait_for_tick = ScxmlState("wait_for_tick") wait_for_tick.add_transition( RosRateCallback(rtr, "tick")) - root_tag.add_state(wait_for_tick, initial=True) - - assert root_tag.check_validity(), "Error: SCXML root tag is not valid." - - with open(output_file_bt, 'w', encoding='utf-8') as f: - f.write(root_tag.as_xml_string()) + bt_scxml_root.add_state(wait_for_tick, initial=True) + assert bt_scxml_root.check_validity(), "Error: SCXML root tag is not valid." + generated_scxmls.append(bt_scxml_root) - return generated_files + return generated_scxmls diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index c827fae1..124266b5 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -1,16 +1,20 @@ # isort: skip_file # Skipping file to avoid circular import problem from .scxml_base import ScxmlBase # noqa: F401 +from .bt_utils import RESERVED_BT_PORT_NAMES # noqa: F401 +from .scxml_bt import ( # noqa: F401 + BtInputPortDeclaration, BtOutputPortDeclaration, BtGetValueInputPort) # noqa: F401 from .scxml_param import ScxmlParam # noqa: F401 from .scxml_ros_field import RosField # noqa: F401 -from .utils import ScxmlRosDeclarationsContainer # noqa: F401 from .scxml_data import ScxmlData # noqa: F401 from .scxml_data_model import ScxmlDataModel # noqa: F401 +from .ros_utils import ScxmlRosDeclarationsContainer # noqa: F401 from .scxml_executable_entries import ScxmlAssign, ScxmlIf, ScxmlSend # noqa: F401 from .scxml_executable_entries import ScxmlExecutableEntry, ScxmlExecutionBody # noqa: F401 from .scxml_executable_entries import ( # noqa: F401 execution_body_from_xml, as_plain_execution_body, # noqa: F401 - execution_entry_from_xml, valid_execution_body) # noqa: F401 + execution_entry_from_xml, valid_execution_body, # noqa: F401 + valid_execution_body_entry_types, instantiate_exec_body_bt_events) # noqa: F401 from .scxml_transition import ScxmlTransition # noqa: F401 from .scxml_ros_topic import ( # noqa: F401 RosTopicPublisher, RosTopicSubscriber, RosTopicCallback, RosTopicPublish) # noqa: F401 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/bt_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/bt_utils.py new file mode 100644 index 00000000..31e1f95c --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/bt_utils.py @@ -0,0 +1,144 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +"""Collection of SCXML utilities related to BT functionalities.""" + +from typing import Dict, Tuple, Any, Type + +import re + +from scxml_converter.scxml_entries.utils import SCXML_DATA_STR_TO_TYPE + + +VALID_BT_INPUT_PORT_TYPES: Dict[str, Type] = SCXML_DATA_STR_TO_TYPE | {"string": str} +VALID_BT_OUTPUT_PORT_TYPES: Dict[str, Type] = SCXML_DATA_STR_TO_TYPE + +"""List of keys that are not going to be read as BT ports from the BT XML definition.""" +RESERVED_BT_PORT_NAMES = ['NAME', 'ID', 'category'] + + +def is_bt_event(event_name: str) -> bool: + """Given an event name, returns whether it is related to a BT event or not.""" + bt_events = [f"bt_{suffix}" for suffix in ["tick", "running", "success", "failure"]] + return event_name in bt_events + + +def replace_bt_event(event_name: str, instance_id: str) -> str: + """Given a BT event name, returns the same event including the BT node instance.""" + assert is_bt_event(event_name), "Error: BT event instantiation: invalid BT event name." + return f"bt_{instance_id}_{event_name.removeprefix('bt_')}" + + +def is_blackboard_reference(port_value: str) -> bool: + """ + Check if a port value is a reference to a Blackboard variable. + + We consider a string to reference Blackboard variable if it is enclosed in curly braces. + """ + return re.match(r"\{.+\}", port_value) is not None + + +class BtPortsHandler: + """Collector for declared BT ports and their assigned value.""" + @staticmethod + def check_port_name_allowed(port_name: str) -> None: + """Check if the port name is allowed.""" + assert port_name not in RESERVED_BT_PORT_NAMES, \ + f"Error: Port name {port_name} is reserved in BT" + + def __init__(self): + # For each port name, store the port type string and value. + self._in_ports: Dict[str, Tuple[str, str]] = {} + self._out_ports: Dict[str, Tuple[Type, str]] = {} + + def in_port_exists(self, port_name: str) -> bool: + """Check if an input port exists.""" + return port_name in self._in_ports + + def out_port_exists(self, port_name: str) -> bool: + """Check if an output port exists.""" + return port_name in self._out_ports + + def declare_in_port(self, port_name: str, port_type: str) -> None: + """Add an input port to the handler.""" + BtPortsHandler.check_port_name_allowed(port_name) + assert not self.in_port_exists(port_name), \ + f"Error: Input port {port_name} already declared as input port." + assert not self.out_port_exists(port_name), \ + f"Error: Input port {port_name} already declared as output port." + assert port_type in VALID_BT_INPUT_PORT_TYPES, \ + f"Error: Unsupported input port type {port_type}." + self._in_ports[port_name] = (port_type, None) + + def declare_out_port(self, port_name: str, port_type: str) -> None: + """Add an output port to the handler.""" + BtPortsHandler.check_port_name_allowed(port_name) + assert not self.out_port_exists(port_name), \ + f"Error: Output port {port_name} already declared as output port." + assert not self.in_port_exists(port_name), \ + f"Error: Output port {port_name} already declared as input port." + assert port_type in VALID_BT_OUTPUT_PORT_TYPES, \ + f"Error: Unsupported output port type {port_type}." + self._out_ports[port_name] = (port_type, None) + + def get_port_value(self, port_name: str) -> Any: + """Get the value of a port.""" + if self.in_port_exists(port_name): + return self.get_in_port_value(port_name) + elif self.out_port_exists(port_name): + return self.get_out_port_value(port_name) + else: + raise RuntimeError(f"Error: Port {port_name} is not declared.") + + def get_in_port_value(self, port_name: str) -> str: + """Get the value of an input port.""" + assert self.in_port_exists(port_name), \ + f"Error: Port {port_name} is not declared as input port." + port_value = self._in_ports[port_name][1] + assert port_value is not None, f"Error: Port {port_name} has no assigned value." + return port_value + + def get_out_port_value(self, port_name: str) -> str: + """Get the value of an output port.""" + raise NotImplementedError("Error: Output ports are not supported yet.") + + def set_port_value(self, port_name: str, port_value: Any) -> None: + """Set the value of a port.""" + if self.in_port_exists(port_name): + self._set_in_port_value(port_name, port_value) + elif self.out_port_exists(port_name): + self._set_out_port_value(port_name, port_value) + else: + # The 'name' port can be set even if undeclared, since it defines the node name in BT. + if port_name != "name": + raise RuntimeError(f"Error: Port {port_name} is not declared.") + + def _set_in_port_value(self, port_name: str, port_value: str): + """Set the value of an input port.""" + assert self.in_port_exists(port_name), \ + f"Error: Port {port_name} is not declared as input port." + assert self._in_ports[port_name][1] is None, \ + f"Error: Port {port_name} already has a value assigned." + port_type = self._in_ports[port_name][0] + # Ensure this is not a Blackboard variable reference: currently not supported + if is_blackboard_reference(port_value): + raise NotImplementedError( + f"Error: {port_value} assigns a Blackboard variable to {port_name}. " + "This is not yet supported.") + self._in_ports[port_name] = (port_type, port_value) + + def _set_out_port_value(self, port_name: str, port_value: Any): + """Set the value of an output port.""" + raise NotImplementedError("Error: Output ports are not supported yet.") diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py new file mode 100644 index 00000000..56cd6c16 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -0,0 +1,288 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +"""Collection of SCXML utilities related to ROS functionalities.""" + +from typing import Dict, List, Optional, Tuple + +from scxml_converter.scxml_entries.scxml_ros_field import RosField + +from scxml_converter.scxml_entries.utils import all_non_empty_strings + + +MSG_TYPE_SUBSTITUTIONS = { + "boolean": "bool", +} + +BASIC_FIELD_TYPES = ['boolean', + 'int8', 'int16', 'int32', 'int64', + 'float', 'double'] + +"""Container for the ROS interface (e.g. topic or service) name and the related type""" +RosInterfaceAndType = Tuple[str, str] + + +def is_ros_type_known(type_definition: str, ros_interface: str) -> bool: + """ + Check if python can import the provided type definition. + + :param type_definition: The type definition to check (e.g. std_msgs/Empty). + """ + if not (isinstance(type_definition, str) and type_definition.count("/") == 1): + return False + interface_ns, interface_type = type_definition.split("/") + if len(interface_ns) == 0 or len(interface_type) == 0: + return False + assert ros_interface in ["msg", "srv"], "Error: SCXML ROS declarations: unknown ROS interface." + try: + interface_importer = __import__(interface_ns + f'.{ros_interface}', fromlist=['']) + _ = getattr(interface_importer, interface_type) + except (ImportError, AttributeError): + print(f"Error: SCXML ROS declarations: topic type {type_definition} not found.") + return False + return True + + +def is_msg_type_known(topic_definition: str) -> bool: + """Check if python can import the provided topic definition.""" + return is_ros_type_known(topic_definition, "msg") + + +def is_srv_type_known(service_definition: str) -> bool: + """Check if python can import the provided service definition.""" + return is_ros_type_known(service_definition, "srv") + + +def get_srv_type_params(service_definition: str) -> Tuple[Dict[str, str], Dict[str, str]]: + """ + Get the data fields of a service request and response type as pairs of name and type objects. + """ + assert is_srv_type_known(service_definition), \ + f"Error: SCXML ROS declarations: service type {service_definition} not found." + interface_ns, interface_type = service_definition.split("/") + srv_module = __import__(interface_ns + '.srv', fromlist=['']) + srv_class = getattr(srv_module, interface_type) + + # TODO: Fields can be nested. Look AS2FM/scxml_converter/src/scxml_converter/scxml_converter.py + req = srv_class.Request.get_fields_and_field_types() + for key in req.keys(): + # TODO: Support nested fields + assert req[key] in BASIC_FIELD_TYPES, \ + f"Error: SCXML ROS declarations: service request type {req[key]} isn't a basic field." + req[key] = MSG_TYPE_SUBSTITUTIONS.get(req[key], req[key]) + + res = srv_class.Response.get_fields_and_field_types() + for key in res.keys(): + assert res[key] in BASIC_FIELD_TYPES, \ + "Error: SCXML ROS declarations: service response type contains non-basic fields." + res[key] = MSG_TYPE_SUBSTITUTIONS.get(res[key], res[key]) + + return req, res + + +def replace_ros_interface_expression(msg_expr: str) -> str: + """Convert a ROS interface expression (msg, req, res) to plain SCXML (event).""" + scxml_prefix = "_event." + # TODO: Use regex and ensure no other valid character exists before the initial underscore + for ros_prefix in ["_msg.", "_req.", "_res."]: + msg_expr = msg_expr.replace(ros_prefix, scxml_prefix) + return msg_expr + + +def sanitize_ros_interface_name(interface_name: str) -> str: + """Replace slashes in a ROS interface name.""" + assert isinstance(interface_name, str), \ + "Error: ROS interface sanitizer: interface name must be a string." + # Remove potential prepended slash + interface_name = interface_name.removeprefix("/") + assert len(interface_name) > 0, \ + "Error: ROS interface sanitizer: interface name must not be empty." + assert interface_name.count(" ") == 0, \ + "Error: ROS interface sanitizer: interface name must not contain spaces." + return interface_name.replace("/", "__") + + +def generate_srv_request_event(service_name: str, automaton_name: str) -> str: + """Generate the name of the event that triggers a service request.""" + return f"srv_{sanitize_ros_interface_name(service_name)}_req_client_{automaton_name}" + + +def generate_srv_response_event(service_name: str, automaton_name: str) -> str: + """Generate the name of the event that provides the service response.""" + return f"srv_{sanitize_ros_interface_name(service_name)}_response_client_{automaton_name}" + + +def generate_srv_server_request_event(service_name: str) -> str: + """Generate the name of the event that makes a service server start processing a request.""" + return f"srv_{sanitize_ros_interface_name(service_name)}_request" + + +def generate_srv_server_response_event(service_name: str) -> str: + """Generate the name of the event that makes a service server send a response.""" + return f"srv_{sanitize_ros_interface_name(service_name)}_response" + + +class ScxmlRosDeclarationsContainer: + """Object that contains a description of the ROS declarations in the SCXML root.""" + + def __init__(self, automaton_name: str): + """Constructor of container. + + :automaton_name: Name of the automaton these declarations belong to. + """ + self._automaton_name: str = automaton_name + # Dict of publishers and subscribers: topic name -> type + self._publishers: Dict[str, RosInterfaceAndType] = {} + self._subscribers: Dict[str, RosInterfaceAndType] = {} + self._service_servers: Dict[str, RosInterfaceAndType] = {} + self._service_clients: Dict[str, RosInterfaceAndType] = {} + self._timers: Dict[str, float] = {} + + def get_automaton_name(self) -> str: + """Get name of the automaton that these declarations are defined in.""" + return self._automaton_name + + def append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> None: + assert all_non_empty_strings(pub_name, topic_name, topic_type), \ + "Error: ROS declarations: publisher name, topic name and type must be strings." + assert pub_name not in self._publishers, \ + f"Error: ROS declarations: topic publisher {pub_name} already declared." + self._publishers[pub_name] = (topic_name, topic_type) + + def append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> None: + assert all_non_empty_strings(sub_name, topic_name, topic_type), \ + "Error: ROS declarations: subscriber name, topic name and type must be strings." + assert sub_name not in self._subscribers, \ + f"Error: ROS declarations: topic subscriber {sub_name} already declared." + self._subscribers[sub_name] = (topic_name, topic_type) + + def append_service_client(self, client_name: str, service_name: str, service_type: str) -> None: + assert all_non_empty_strings(client_name, service_name, service_type), \ + "Error: ROS declarations: client name, service name and type must be strings." + assert client_name not in self._service_clients, \ + f"Error: ROS declarations: service client {client_name} already declared." + self._service_clients[client_name] = (service_name, service_type) + + def append_service_server(self, server_name: str, service_name: str, service_type: str) -> None: + assert all_non_empty_strings(server_name, service_name, service_type), \ + "Error: ROS declarations: server name, service name and type must be strings." + assert server_name not in self._service_servers, \ + f"Error: ROS declarations: service server {server_name} already declared." + self._service_servers[server_name] = (service_name, service_type) + + def append_timer(self, timer_name: str, timer_rate: float) -> None: + assert isinstance(timer_name, str), "Error: ROS declarations: timer name must be a string." + assert isinstance(timer_rate, float) and timer_rate > 0, \ + "Error: ROS declarations: timer rate must be a positive number." + assert timer_name not in self._timers, \ + f"Error: ROS declarations: timer {timer_name} already declared." + self._timers[timer_name] = timer_rate + + def is_publisher_defined(self, pub_name: str) -> bool: + return pub_name in self._publishers + + def is_subscriber_defined(self, sub_name: str) -> bool: + return sub_name in self._subscribers + + def is_timer_defined(self, timer_name: str) -> bool: + return timer_name in self._timers + + def get_timers(self) -> Dict[str, float]: + return self._timers + + def get_publisher_info(self, pub_name: str) -> Tuple[str, str]: + """Provide a publisher topic name and type""" + pub_info = self._publishers.get(pub_name) + assert pub_info is not None, f"Error: SCXML ROS declarations: unknown publisher {pub_name}." + return pub_info + + def get_subscriber_info(self, sub_name: str) -> Tuple[str, str]: + """Provide a subscriber topic name and type""" + sub_info = self._subscribers.get(sub_name) + assert sub_info is not None, \ + f"Error: SCXML ROS declarations: unknown subscriber {sub_name}." + return sub_info + + def get_service_server_info(self, server_name: str) -> Tuple[str, str]: + """Provide a server's service name and type""" + server_info = self._service_servers.get(server_name) + assert server_info is not None, \ + f"Error: SCXML ROS declarations: unknown service server {server_name}." + return server_info + + def get_service_client_info(self, client_name: str) -> Tuple[str, str]: + """Provide a client's service name and type""" + client_info = self._service_clients.get(client_name) + assert client_info is not None, \ + f"Error: SCXML ROS declarations: unknown service client {client_name}." + return client_info + + def is_service_client_defined(self, client_name: str) -> bool: + return client_name in self._service_clients + + def is_service_server_defined(self, server_name: str) -> bool: + return server_name in self._service_servers + + def get_service_client_type(self, client_name: str) -> Optional[str]: + client_definition = self._service_clients.get(client_name, None) + if client_definition is None: + return None + return client_definition[1] + + def get_service_server_type(self, server_name: str) -> Optional[str]: + server_definition = self._service_servers.get(server_name, None) + if server_definition is None: + return None + return server_definition[1] + + def check_valid_srv_req_fields(self, client_name: str, ros_fields: List[RosField]) -> bool: + """Check if the provided fields match the service request type.""" + req_type = self.get_service_client_type(client_name) + if req_type is None: + print(f"Error: SCXML ROS declarations: unknown service client {client_name}.") + return False + req_fields, _ = get_srv_type_params(req_type) + for ros_field in ros_fields: + if ros_field.get_name() not in req_fields: + print("Error: SCXML ROS declarations: " + f"unknown field {ros_field.get_name()} in service request.") + return False + req_fields.pop(ros_field.get_name()) + if len(req_fields) > 0: + print("Error: SCXML ROS declarations: missing fields in service request.") + for req_field in req_fields.keys(): + print(f"\t-{req_field}.") + return False + return True + + def check_valid_srv_res_fields(self, server_name: str, ros_fields: List[RosField]) -> bool: + """Check if the provided fields match the service response type.""" + res_type = self.get_service_server_type(server_name) + if res_type is None: + print(f"Error: SCXML ROS declarations: unknown service server {server_name}.") + return False + _, res_fields = get_srv_type_params(res_type) + for ros_field in ros_fields: + if ros_field.get_name() not in res_fields: + print("Error: SCXML ROS declarations: " + f"unknown field {ros_field.get_name()} in service response.") + return False + res_fields.pop(ros_field.get_name()) + if len(res_fields) > 0: + print("Error: SCXML ROS declarations: missing fields in service response.") + for res_field in res_fields.keys(): + print(f"\t-{res_field}.") + return False + return True diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_base.py index 8102baed..4e098c59 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_base.py @@ -37,6 +37,10 @@ def check_validity(self) -> bool: """Check if the object is valid.""" raise NotImplementedError + def update_bt_ports_values(self, bt_ports_handler): + """Update the values of potential entries making use of BT ports.""" + raise NotImplementedError + def as_plain_scxml(self, ros_declarations) -> "ScxmlBase": """Convert the object to its plain SCXML version.""" raise NotImplementedError diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_bt.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_bt.py new file mode 100644 index 00000000..6dad9892 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_bt.py @@ -0,0 +1,144 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +""" +SCXML entries related to Behavior Trees. +""" + +from typing import Union +from xml.etree import ElementTree as ET +from scxml_converter.scxml_entries import ScxmlBase +from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument +from scxml_converter.scxml_entries.utils import is_non_empty_string + + +class BtInputPortDeclaration(ScxmlBase): + """ + Declare an input port in a bt plugin. + """ + + @staticmethod + def get_tag_name() -> str: + return "bt_declare_port_in" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "BtInputPortDeclaration": + assert_xml_tag_ok(BtInputPortDeclaration, xml_tree) + key_str = get_xml_argument(BtInputPortDeclaration, xml_tree, "key") + type_str = get_xml_argument(BtInputPortDeclaration, xml_tree, "type") + return BtInputPortDeclaration(key_str, type_str) + + def __init__(self, key_str: str, type_str: str): + self._key = key_str + self._type = type_str + + def check_validity(self) -> bool: + return is_non_empty_string(BtInputPortDeclaration, "key", self._key) and \ + is_non_empty_string(BtInputPortDeclaration, "type", self._type) + + def get_key_name(self) -> str: + return self._key + + def get_key_type(self) -> str: + return self._type + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML BT Ports declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML BT Input Port: invalid parameters." + xml_bt_in_port = ET.Element( + BtInputPortDeclaration.get_tag_name(), {"key": self._key, "type": self._type}) + return xml_bt_in_port + + +class BtOutputPortDeclaration(ScxmlBase): + """ + Declare an input port in a bt plugin. + """ + + @staticmethod + def get_tag_name() -> str: + return "bt_declare_port_out" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "BtOutputPortDeclaration": + assert_xml_tag_ok(BtOutputPortDeclaration, xml_tree) + key_str = get_xml_argument(BtOutputPortDeclaration, xml_tree, "key") + type_str = get_xml_argument(BtOutputPortDeclaration, xml_tree, "type") + return BtOutputPortDeclaration(key_str, type_str) + + def __init__(self, key_str: str, type_str: str): + self._key = key_str + self._type = type_str + + def check_validity(self) -> bool: + return is_non_empty_string(BtOutputPortDeclaration, "key", self._key) and \ + is_non_empty_string(BtOutputPortDeclaration, "type", self._type) + + def get_key_name(self) -> str: + return self._key + + def get_key_type(self) -> str: + return self._type + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML BT Ports declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML BT Input Port: invalid parameters." + xml_bt_in_port = ET.Element( + BtOutputPortDeclaration.get_tag_name(), {"key": self._key, "type": self._type}) + return xml_bt_in_port + + +class BtGetValueInputPort(ScxmlBase): + """ + Get the value of an input port in a bt plugin. + """ + + @staticmethod + def get_tag_name() -> str: + return "bt_get_input" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "BtGetValueInputPort": + assert_xml_tag_ok(BtGetValueInputPort, xml_tree) + key_str = get_xml_argument(BtGetValueInputPort, xml_tree, "key") + return BtGetValueInputPort(key_str) + + def __init__(self, key_str: str): + self._key = key_str + + def check_validity(self) -> bool: + return is_non_empty_string(BtGetValueInputPort, "key", self._key) + + def get_key_name(self) -> str: + return self._key + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML BT Ports declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML BT Input Port: invalid parameters." + xml_bt_in_port = ET.Element( + BtGetValueInputPort.get_tag_name(), {"key": self._key}) + return xml_bt_in_port + + +BtPortDeclarations = Union[BtInputPortDeclaration, BtOutputPortDeclaration] diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py index e00dee73..9f96f02a 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py @@ -17,11 +17,44 @@ Container for a single variable definition in SCXML. In XML, it has the tag `data`. """ -from typing import Any +from typing import Any, Union, Optional from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ScxmlBase -from scxml_converter.scxml_entries.utils import SCXML_DATA_STR_TO_TYPE +from scxml_converter.scxml_entries import (ScxmlBase, BtGetValueInputPort) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import SCXML_DATA_STR_TO_TYPE, is_non_empty_string + + +ValidExpr = Union[BtGetValueInputPort, str, int, float] + + +def get_valid_entry_data_type( + value: Optional[Union[str, int, float]], data_type: str) -> Optional[Any]: + """ + Convert a value to the provided data type. Raise if impossible. + """ + if value is None: + return None + assert data_type in SCXML_DATA_STR_TO_TYPE, \ + f"Error: SCXML conversion of data entry: Unknown data type {data_type}." + if isinstance(value, str): + assert len(value) > 0, "Error: SCXML conversion of data bounds: Empty string." + return SCXML_DATA_STR_TO_TYPE[data_type](value) + assert isinstance(value, SCXML_DATA_STR_TO_TYPE[data_type]), \ + f"Error: SCXML conversion of data entry: Expected {data_type}, but got {type(value)}." + return value + + +def valid_bound(bound_value: Any) -> bool: + """Check if a bound is invalid.""" + if bound_value is None: + return True + if isinstance(bound_value, str): + return len(bound_value) > 0 + return isinstance(bound_value, (int, float)) class ScxmlData(ScxmlBase): @@ -34,21 +67,20 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "ScxmlData": """Create a ScxmlData object from an XML tree.""" - assert xml_tree.tag == ScxmlData.get_tag_name(), \ - f"Error: SCXML data: XML tag name is not {ScxmlData.get_tag_name()}." - data_id = xml_tree.attrib.get("id") - assert data_id is not None, "Error: SCXML data: 'id' not found." - data_expr = xml_tree.attrib.get("expr") - assert data_expr is not None, "Error: SCXML data: 'expr' not found." - data_type = xml_tree.attrib.get("type") - assert data_type is not None, "Error: SCXML data: 'type' not found." - lower_bound = xml_tree.attrib.get("lower_bound_incl", None) - upper_bound = xml_tree.attrib.get("upper_bound_incl", None) + assert_xml_tag_ok(ScxmlData, xml_tree) + data_id = get_xml_argument(ScxmlData, xml_tree, "id") + data_type = get_xml_argument(ScxmlData, xml_tree, "type") + data_expr = read_value_from_xml_arg_or_child(ScxmlData, xml_tree, "expr", + (BtGetValueInputPort, str)) + lower_bound = read_value_from_xml_arg_or_child(ScxmlData, xml_tree, "lower_bound_incl", + (BtGetValueInputPort, str), True) + upper_bound = read_value_from_xml_arg_or_child(ScxmlData, xml_tree, "upper_bound_incl", + (BtGetValueInputPort, str), True) return ScxmlData(data_id, data_expr, data_type, lower_bound, upper_bound) def __init__( - self, id_ : str, expr: str, data_type: str, - lower_bound: Any = None, upper_bound: Any = None): + self, id_: str, expr: ValidExpr, data_type: str, + lower_bound: Optional[ValidExpr] = None, upper_bound: Optional[ValidExpr] = None): self._id = id_ self._expr = expr self._data_type = data_type @@ -65,37 +97,23 @@ def get_expr(self) -> str: return self._expr def check_validity(self) -> bool: - validity = True - # ID - if not (isinstance(self._id, str) and len(self._id) > 0): - print(f"Error: SCXML data: 'id' {self._id} is not valid.") - validity = False - # Expression - if not (isinstance(self._expr, str) and len(self._expr) > 0): - print(f"Error: SCXML data: 'expr' {self._expr} is not valid.") - validity = False - # Data type - if not (isinstance(self._data_type, str) and self._data_type in SCXML_DATA_STR_TO_TYPE): - print(f"Error: SCXML data: 'type' {self._data_type} is not valid.") - validity = False - type_of_data = SCXML_DATA_STR_TO_TYPE[self._data_type] - # Lower bound - if self._lower_bound is not None: - if not isinstance(self._lower_bound, type_of_data): - print(f"Error: SCXML data: 'lower_bound_incl' type {self._lower_bound} is invalid.") - validity = False - # Upper bound - if self._upper_bound is not None: - if not isinstance(self._upper_bound, type_of_data): - print(f"Error: SCXML data: 'upper_bound_incl' type {self._upper_bound} is invalid.") - validity = False - # Check if lower bound is smaller than upper bound - if validity and self._upper_bound is not None and self._lower_bound is not None: - if self._lower_bound >= self._upper_bound: - print(f"Error: SCXML data: 'lower_bound_incl' {self._lower_bound} is not smaller " - f"than 'upper_bound_incl' {self._upper_bound}.") - validity = False - return validity + valid_id = is_non_empty_string(ScxmlData, "id", self._id) + valid_expr = is_non_empty_string(ScxmlData, "expr", self._expr) + valid_type = is_non_empty_string(ScxmlData, "type", self._data_type) and \ + self._data_type in SCXML_DATA_STR_TO_TYPE + if not (valid_bound(self._lower_bound) and valid_bound(self._upper_bound)): + print("Error: SCXML data: invalid lower_bound_incl or upper_bound_incl. " + f"lower_bound_incl: {self._lower_bound}, upper_bound_incl: {self._upper_bound}") + return False + lower_bound = get_valid_entry_data_type(self._lower_bound, self._data_type) + upper_bound = get_valid_entry_data_type(self._upper_bound, self._data_type) + valid_bounds = True + if lower_bound is not None and upper_bound is not None: + valid_bounds = lower_bound <= upper_bound + if not valid_bounds: + print(f"Error: SCXML data: 'lower_bound_incl' {lower_bound} is not smaller " + f"than 'upper_bound_incl' {upper_bound}.") + return valid_id and valid_expr and valid_type and valid_bounds def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid data object." @@ -106,6 +124,14 @@ def as_xml(self) -> ET.Element: if self._upper_bound is not None: xml_data.set("upper_bound_incl", str(self._upper_bound)) return xml_data - - def as_plain_scxml(self, ros_declarations): - raise NotImplementedError("Error: SCXML data: as_plain_scxml not implemented.") + + def as_plain_scxml(self, _): + raise RuntimeError("Error: SCXML data: unexpected call to as_plain_scxml.") + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + if isinstance(self._expr, BtGetValueInputPort): + self._expr = bt_ports_handler.get_in_port_value(self._expr.get_key_name()) + if isinstance(self._lower_bound, BtGetValueInputPort): + self._lower_bound = bt_ports_handler.get_in_port_value(self._lower_bound.get_key_name()) + if isinstance(self._upper_bound, BtGetValueInputPort): + self._upper_bound = bt_ports_handler.get_in_port_value(self._upper_bound.get_key_name()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py index e63db7a4..78c3ec1e 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py @@ -21,6 +21,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ScxmlBase, ScxmlData +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler class ScxmlDataModel(ScxmlBase): @@ -49,6 +50,10 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlDataModel": def get_data_entries(self) -> Optional[List[ScxmlData]]: return self._data_entries + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + for data_entry in self._data_entries: + data_entry.update_bt_ports_values(bt_ports_handler) + def check_validity(self) -> bool: valid_data_entries = True if self._data_entries is not None: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index e17ac00c..aae29039 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -20,10 +20,13 @@ from typing import List, Optional, Tuple, Union, get_args from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlParam, - ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.utils import \ - replace_ros_interface_expression +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlParam, ScxmlRosDeclarationsContainer, BtGetValueInputPort) +from scxml_converter.scxml_entries.ros_utils import replace_ros_interface_expression +from scxml_converter.scxml_entries.bt_utils import is_bt_event, replace_bt_event, BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string # Use delayed type evaluation: https://peps.python.org/pep-0484/#forward-references ScxmlExecutableEntry = Union['ScxmlAssign', 'ScxmlIf', 'ScxmlSend'] @@ -31,6 +34,29 @@ ConditionalExecutionBody = Tuple[str, ScxmlExecutionBody] +def instantiate_exec_body_bt_events( + exec_body: Optional[ScxmlExecutionBody], instance_id: str) -> None: + """ + Instantiate the behavior tree events in the execution body. + + :param exec_body: The execution body to instantiate the BT events in + :param instance_id: The instance ID of the BT node + """ + if exec_body is not None: + for entry in exec_body: + entry.instantiate_bt_events(instance_id) + + +def update_exec_body_bt_ports_values( + exec_body: Optional[ScxmlExecutionBody], bt_ports_handler: BtPortsHandler) -> None: + """ + Update the BT ports values in the execution body. + """ + if exec_body is not None: + for entry in exec_body: + entry.update_bt_ports_values(bt_ports_handler) + + class ScxmlIf(ScxmlBase): """This class represents SCXML conditionals.""" @@ -84,6 +110,17 @@ def get_else_execution(self) -> Optional[ScxmlExecutionBody]: """Get the else execution.""" return self._else_execution + def instantiate_bt_events(self, instance_id: str) -> None: + """Instantiate the behavior tree events in the If action, if available.""" + for _, exec_body in self._conditional_executions: + instantiate_exec_body_bt_events(exec_body, instance_id) + instantiate_exec_body_bt_events(self._else_execution, instance_id) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + for _, exec_body in self._conditional_executions: + update_exec_body_bt_ports_values(exec_body, bt_ports_handler) + update_exec_body_bt_ports_values(self._else_execution, bt_ports_handler) + def check_validity(self) -> bool: valid_conditional_executions = len(self._conditional_executions) > 0 if not valid_conditional_executions: @@ -152,12 +189,6 @@ def as_xml(self) -> ET.Element: class ScxmlSend(ScxmlBase): """This class represents a send action.""" - def __init__(self, event: str, params: Optional[List[ScxmlParam]] = None): - if params is None: - params = [] - self._event = event - self._params = params - @staticmethod def get_tag_name() -> str: return "send" @@ -174,6 +205,12 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlSend": params.append(ScxmlParam.from_xml_tree(param_xml)) return ScxmlSend(event, params) + def __init__(self, event: str, params: Optional[List[ScxmlParam]] = None): + if params is None: + params = [] + self._event = event + self._params = params + def get_event(self) -> str: """Get the event to send.""" return self._event @@ -182,6 +219,18 @@ def get_params(self) -> List[ScxmlParam]: """Get the parameters to send.""" return self._params + def instantiate_bt_events(self, instance_id: str) -> None: + """Instantiate the behavior tree events in the send action, if available.""" + # Make sure this method is executed only on ScxmlSend objects, and not on derived classes + if type(self) is ScxmlSend and is_bt_event(self._event): + # Those are expected to be only bt_success, bt_failure and bt_running + self._event = replace_bt_event(self._event, instance_id) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + for param in self._params: + param.update_bt_ports_values(bt_ports_handler) + def check_validity(self) -> bool: valid_event = isinstance(self._event, str) and len(self._event) > 0 valid_params = True @@ -217,10 +266,6 @@ def as_xml(self) -> ET.Element: class ScxmlAssign(ScxmlBase): """This class represents a variable assignment.""" - def __init__(self, location: str, expr: str): - self._location = location - self._expr = expr - @staticmethod def get_tag_name() -> str: return "assign" @@ -228,33 +273,40 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "ScxmlAssign": """Create a ScxmlAssign object from an XML tree.""" - assert xml_tree.tag == ScxmlAssign.get_tag_name(), \ - f"Error: SCXML assign: XML tag name is {xml_tree.tag} != {ScxmlAssign.get_tag_name()}." - location = xml_tree.attrib.get("location") - assert location is not None and len(location) > 0, \ - "Error: SCXML assign: location is not valid." - expr = xml_tree.attrib.get("expr") - assert expr is not None and len(expr) > 0, \ - "Error: SCXML assign: expr is not valid." + assert_xml_tag_ok(ScxmlAssign, xml_tree) + location = get_xml_argument(ScxmlAssign, xml_tree, "location") + expr = get_xml_argument(ScxmlAssign, xml_tree, "expr", none_allowed=True) + if expr is None: + expr = read_value_from_xml_child(xml_tree, "expr", (BtGetValueInputPort, str)) + assert expr is not None, "Error: SCXML assign: expr is not valid." return ScxmlAssign(location, expr) - + + def __init__(self, location: str, expr: Union[str, BtGetValueInputPort]): + self._location = location + self._expr = expr + print(f"ScxmlAssign: {location} = {expr}") + def get_location(self) -> str: """Get the location to assign.""" return self._location - def get_expr(self) -> str: + def get_expr(self) -> Union[str, BtGetValueInputPort]: """Get the expression to assign.""" return self._expr + def instantiate_bt_events(self, _) -> None: + """This functionality is not needed in this class.""" + return + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._expr, BtGetValueInputPort): + self._expr = bt_ports_handler.get_in_port_value(self._expr.get_key_name()) def check_validity(self) -> bool: # TODO: Check that the location to assign exists in the data-model - valid_location = isinstance(self._location, str) and len(self._location) > 0 - valid_expr = isinstance(self._expr, str) and len(self._expr) > 0 - if not valid_location: - print("Error: SCXML assign: location is not valid.") - if not valid_expr: - print("Error: SCXML assign: expr is not valid.") + valid_location = is_non_empty_string(ScxmlAssign, "location", self._location) + valid_expr = is_non_empty_string(ScxmlAssign, "expr", self._expr) return valid_location and valid_expr def check_valid_ros_instantiations(self, _) -> bool: @@ -279,6 +331,24 @@ def as_xml(self) -> ET.Element: for entry in get_args(ScxmlExecutableEntry)) +def valid_execution_body_entry_types(exec_body: ScxmlExecutionBody) -> bool: + """ + Check if the type of the entries in an execution body are valid. + + :param exec_body: The execution body to check + :return: True if all types of the body entries are the expected ones, False otherwise + """ + if not isinstance(exec_body, list): + print("Error: SCXML execution body: invalid type found: expected a list.") + return False + for entry in exec_body: + if not isinstance(entry, _ResolvedScxmlExecutableEntry): + print(f"Error: SCXML execution body: entry type {type(entry)} not in valid set." + f" {_ResolvedScxmlExecutableEntry}.") + return False + return True + + def valid_execution_body(execution_body: ScxmlExecutionBody) -> bool: """ Check if an execution body is valid. @@ -286,20 +356,13 @@ def valid_execution_body(execution_body: ScxmlExecutionBody) -> bool: :param execution_body: The execution body to check :return: True if the execution body is valid, False otherwise """ - valid = isinstance(execution_body, list) - if not valid: - print("Error: SCXML execution body: invalid type found: expected a list.") - for entry in execution_body: - if not isinstance(entry, _ResolvedScxmlExecutableEntry): - valid = False - print(f"Error: SCXML execution body: entry type {type(entry)} not in valid set " - f" {_ResolvedScxmlExecutableEntry}.") - break - if not entry.check_validity(): - valid = False - print("Error: SCXML execution body: invalid entry content found.") - break - return valid + if valid_execution_body_entry_types(execution_body): + for entry in execution_body: + if not entry.check_validity(): + print(f"Error: SCXML execution body: content of {entry.get_tag_name()} is invalid.") + return False + return True + return False def execution_entry_from_xml(xml_tree: ET.Element) -> ScxmlExecutableEntry: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py index c524cb66..11bd353a 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py @@ -17,21 +17,19 @@ Container for a single parameter, sent within an event. In XML, it has the tag `param`. """ -from typing import Optional +from typing import Optional, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ScxmlBase +from scxml_converter.scxml_entries import ScxmlBase, BtGetValueInputPort +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string class ScxmlParam(ScxmlBase): """This class represents a single parameter.""" - def __init__(self, name: str, *, expr: Optional[str] = None, location: Optional[str] = None): - # TODO: We might need types in ScxmlParams as well, for later converting them to JANI. - self._name = name - self._expr = expr - self._location = location - @staticmethod def get_tag_name() -> str: return "param" @@ -39,18 +37,30 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "ScxmlParam": """Create a ScxmlParam object from an XML tree.""" - assert xml_tree.tag == ScxmlParam.get_tag_name(), \ - f"Error: SCXML param: XML tag name is not {ScxmlParam.get_tag_name()}." - name = xml_tree.attrib.get("name") - assert name is not None and len(name) > 0, "Error: SCXML param: name is not valid." - expr = xml_tree.attrib.get("expr") - location = xml_tree.attrib.get("location") - assert not (expr is not None and location is not None), \ - "Error: SCXML param: expr and location are both set." - assert expr is not None or location is not None, \ - "Error: SCXML param: expr and location are both unset." + assert_xml_tag_ok(ScxmlParam, xml_tree) + name = get_xml_argument(ScxmlParam, xml_tree, "name") + expr = read_value_from_xml_arg_or_child(ScxmlParam, xml_tree, "expr", + (BtGetValueInputPort, str), True) + location = get_xml_argument(ScxmlParam, xml_tree, "location", none_allowed=True) return ScxmlParam(name, expr=expr, location=location) + def __init__(self, name: str, *, + expr: Optional[Union[BtGetValueInputPort, str]] = None, + location: Optional[str] = None): + """ + Initialize the SCXML Parameter object. + + The location entryu is kept for consistency, but using expr achieves the same result. + + :param name: The name of the parameter. + :param expr: The expression to assign to the parameter. Can come from a BT port. + :param location: The expression to assign to the parameter, if that's a data variable. + """ + # TODO: We might need types in ScxmlParams as well, for later converting them to JANI. + self._name = name + self._expr = expr + self._location = location + def get_name(self) -> str: return self._name @@ -60,26 +70,21 @@ def get_expr(self) -> Optional[str]: def get_location(self) -> Optional[str]: return self._location + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._expr, BtGetValueInputPort): + self._expr = bt_ports_handler.get_in_port_value(self._expr.get_key_name()) + def check_validity(self) -> bool: - valid_name = len(self._name) > 0 - if not valid_name: - print("Error: SCXML param: name is not valid") - valid_expr = isinstance(self._expr, str) and len(self._expr) > 0 and self._location is None - valid_location = isinstance(self._location, str) and len( - self._location) > 0 and self._expr is None - # Print possible errors - if self._expr is not None: - if not isinstance(self._expr, str) or len(self._expr) == 0: - print("Error: SCXML param: expr is not valid") - if self._location is not None: - if not isinstance(self._location, str) or len(self._location) == 0: - print("Error: SCXML param: location is not valid") - if self._expr is not None and self._location is not None: - print("Error: SCXML param: expr and location are both set") - if self._expr is None and self._location is None: - print("Error: SCXML param: expr and location are both unset") - - return valid_name and (valid_expr or valid_location) + valid_name = is_non_empty_string(ScxmlParam, "name", self._name) + valid_expr = False + if self._location is None: + valid_expr = is_non_empty_string(ScxmlParam, "expr", self._expr) + elif self._expr is None: + valid_expr = is_non_empty_string(ScxmlParam, "location", self._location) + else: + print("Error: SCXML param: expr and location are both set.") + return valid_name and valid_expr def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid param." diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 7dfd3d64..e061d4a7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -22,13 +22,15 @@ from typing import List, Optional, Tuple, get_args from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (RosServiceClient, RosServiceServer, - RosTimeRate, RosTopicPublisher, - RosTopicSubscriber, ScxmlBase, - ScxmlDataModel, - ScxmlRosDeclarations, - ScxmlRosDeclarationsContainer, - ScxmlState) +from scxml_converter.scxml_entries import ( + BtInputPortDeclaration, BtOutputPortDeclaration, RosServiceClient, RosServiceServer, + RosTimeRate, RosTopicPublisher, RosTopicSubscriber, ScxmlBase, ScxmlDataModel, + ScxmlRosDeclarations, ScxmlRosDeclarationsContainer, ScxmlState) + +from scxml_converter.scxml_entries.xml_utils import get_children_as_scxml + +from scxml_converter.scxml_entries.scxml_bt import BtPortDeclarations +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler class ScxmlRoot(ScxmlBase): @@ -53,18 +55,11 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": assert datamodel_elements is None or len(datamodel_elements) <= 1, \ f"Error: SCXML root: {len(datamodel_elements)} datamodels found, max 1 allowed." # ROS Declarations - ros_declarations: List[ScxmlRosDeclarations] = [] - for child in xml_tree: - if child.tag == RosTimeRate.get_tag_name(): - ros_declarations.append(RosTimeRate.from_xml_tree(child)) - elif child.tag == RosTopicSubscriber.get_tag_name(): - ros_declarations.append(RosTopicSubscriber.from_xml_tree(child)) - elif child.tag == RosTopicPublisher.get_tag_name(): - ros_declarations.append(RosTopicPublisher.from_xml_tree(child)) - elif child.tag == RosServiceServer.get_tag_name(): - ros_declarations.append(RosServiceServer.from_xml_tree(child)) - elif child.tag == RosServiceClient.get_tag_name(): - ros_declarations.append(RosServiceClient.from_xml_tree(child)) + ros_declarations: List[ScxmlRosDeclarations] = get_children_as_scxml( + xml_tree, get_args(ScxmlRosDeclarations)) + # BT Declarations + bt_port_declarations: List[BtPortDeclarations] = get_children_as_scxml( + xml_tree, get_args(BtPortDeclarations)) # States assert "initial" in xml_tree.attrib, \ "Error: SCXML root: 'initial' attribute not found in input xml." @@ -72,13 +67,16 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": state_elements = xml_tree.findall(ScxmlState.get_tag_name()) assert state_elements is not None and len(state_elements) > 0, \ "Error: SCXML root: no state found in input xml." - # Fill Data in the ScxmlRoot object + # --- Fill Data in the ScxmlRoot object scxml_root = ScxmlRoot(xml_tree.attrib["name"]) # Data Model if datamodel_elements is not None and len(datamodel_elements) > 0: scxml_root.set_data_model(ScxmlDataModel.from_xml_tree(datamodel_elements[0])) # ROS Declarations scxml_root._ros_declarations = ros_declarations + # BT Declarations + for bt_port_declaration in bt_port_declarations: + scxml_root.add_bt_port_declaration(bt_port_declaration) # States for state_element in state_elements: scxml_state = ScxmlState.from_xml_tree(state_element) @@ -109,11 +107,17 @@ def __init__(self, name: str): self._states: List[ScxmlState] = [] self._data_model: Optional[ScxmlDataModel] = None self._ros_declarations: List[ScxmlRosDeclarations] = [] + self._bt_ports_handler = BtPortsHandler() def get_name(self) -> str: """Get the name of the automaton represented by this SCXML model.""" return self._name + def set_name(self, name: str) -> None: + """Rename the automaton represented by this SCXML model.""" + assert isinstance(name, str) and len(name) > 0, "Error: SCXML root: invalid name." + self._name = name + def get_initial_state_id(self) -> str: """Get the ID of the initial state of the SCXML model.""" assert self._initial_state is not None, "Error: SCXML root: Initial state not set." @@ -131,6 +135,11 @@ def get_state_by_id(self, state_id: str) -> Optional[ScxmlState]: return state return None + def instantiate_bt_events(self, instance_id: str) -> None: + """Update all BT-related events to use the assigned instance ID.""" + for state in self._states: + state.instantiate_bt_events(instance_id) + def add_state(self, state: ScxmlState, *, initial: bool = False): """Append a state to the list of states. If initial is True, set it as the initial state.""" self._states.append(state) @@ -150,27 +159,62 @@ def add_ros_declaration(self, ros_declaration: ScxmlRosDeclarations): self._ros_declarations = [] self._ros_declarations.append(ros_declaration) + def add_bt_port_declaration(self, bt_port_decl: BtPortDeclarations): + """Add a BT port declaration to the handler.""" + if isinstance(bt_port_decl, BtInputPortDeclaration): + self._bt_ports_handler.declare_in_port( + bt_port_decl.get_key_name(), bt_port_decl.get_key_type()) + elif isinstance(bt_port_decl, BtOutputPortDeclaration): + self._bt_ports_handler.declare_out_port( + bt_port_decl.get_key_name(), bt_port_decl.get_key_type()) + else: + raise ValueError( + f"Error: SCXML root: invalid BT port declaration type {type(bt_port_decl)}.") + + def set_bt_port_value(self, port_name: str, port_value: str): + """Set the value of an input port.""" + self._bt_ports_handler.set_port_value(port_name, port_value) + + def set_bt_ports_values(self, ports_values: List[Tuple[str, str]]): + """Set the values of multiple input ports.""" + for port_name, port_value in ports_values: + self.set_bt_port_value(port_name, port_value) + + def update_bt_ports_values(self): + """Update the values of the declared BT ports in the SCXML object.""" + if self._data_model is not None: + self._data_model.update_bt_ports_values(self._bt_ports_handler) + for ros_decl_scxml in self._ros_declarations: + ros_decl_scxml.update_bt_ports_values(self._bt_ports_handler) + for state in self._states: + state.update_bt_ports_values(self._bt_ports_handler) + def _generate_ros_declarations_helper(self) -> Optional[ScxmlRosDeclarationsContainer]: """Generate a HelperRosDeclarations object from the existing ROS declarations.""" ros_decl_container = ScxmlRosDeclarationsContainer(self._name) if self._ros_declarations is not None: for ros_declaration in self._ros_declarations: - if not ros_declaration.check_validity(): + if not (ros_declaration.check_validity() and + ros_declaration.check_valid_instantiation()): return None if isinstance(ros_declaration, RosTimeRate): ros_decl_container.append_timer(ros_declaration.get_name(), ros_declaration.get_rate()) elif isinstance(ros_declaration, RosTopicSubscriber): - ros_decl_container.append_subscriber(ros_declaration.get_topic_name(), + ros_decl_container.append_subscriber(ros_declaration.get_name(), + ros_declaration.get_topic_name(), ros_declaration.get_topic_type()) elif isinstance(ros_declaration, RosTopicPublisher): - ros_decl_container.append_publisher(ros_declaration.get_topic_name(), + ros_decl_container.append_publisher(ros_declaration.get_name(), + ros_declaration.get_topic_name(), ros_declaration.get_topic_type()) elif isinstance(ros_declaration, RosServiceServer): - ros_decl_container.append_service_server(ros_declaration.get_service_name(), + ros_decl_container.append_service_server(ros_declaration.get_name(), + ros_declaration.get_service_name(), ros_declaration.get_service_type()) elif isinstance(ros_declaration, RosServiceClient): - ros_decl_container.append_service_client(ros_declaration.get_service_name(), + ros_decl_container.append_service_client(ros_declaration.get_name(), + ros_declaration.get_service_name(), ros_declaration.get_service_type()) else: raise ValueError("Error: SCXML root: invalid ROS declaration type.") diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py index 3d790fbb..5d83fdec 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py @@ -15,19 +15,20 @@ """Declaration of the ROS Field SCXML tag extension.""" +from typing import Optional, Union + from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ScxmlParam +from scxml_converter.scxml_entries import ScxmlParam, BtGetValueInputPort +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string class RosField(ScxmlParam): """Field of a ROS msg published in a topic.""" - def __init__(self, name: str, expr: str): - self._name = name - self._expr = expr - assert self.check_validity(), "Error: SCXML topic publish field: invalid parameters." - @staticmethod def get_tag_name() -> str: return "field" @@ -35,26 +36,29 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosField": """Create a RosField object from an XML tree.""" - assert xml_tree.tag == RosField.get_tag_name(), \ - f"Error: SCXML topic publish field: XML tag name is not {RosField.get_tag_name()}" - name = xml_tree.attrib.get("name") - expr = xml_tree.attrib.get("expr") - assert name is not None and expr is not None, \ - "Error: SCXML topic publish field: 'name' or 'expr' attribute not found in input xml." + assert_xml_tag_ok(RosField, xml_tree) + name = get_xml_argument(RosField, xml_tree, "name") + expr = read_value_from_xml_arg_or_child(RosField, xml_tree, "expr", + (BtGetValueInputPort, str)) return RosField(name, expr) + def __init__(self, name: str, expr: Optional[Union[BtGetValueInputPort, str]]): + self._name = name + self._expr = expr + assert self.check_validity(), "Error: SCXML topic publish field: invalid parameters." + def check_validity(self) -> bool: - valid_name = isinstance(self._name, str) and len(self._name) > 0 - valid_expr = isinstance(self._expr, str) and len(self._expr) > 0 - if not valid_name: - print("Error: SCXML topic publish field: name is not valid.") - if not valid_expr: - print("Error: SCXML topic publish field: expr is not valid.") + valid_name = is_non_empty_string(RosField, "name", self._name) + valid_expr = is_non_empty_string(RosField, "expr", self._expr) return valid_name and valid_expr + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._expr, BtGetValueInputPort): + self._expr = bt_ports_handler.get_in_port_value(self._expr.get_key_name()) + def as_plain_scxml(self, _) -> ScxmlParam: - from scxml_converter.scxml_entries.utils import \ - replace_ros_interface_expression + from scxml_converter.scxml_entries.ros_utils import replace_ros_interface_expression return ScxmlParam(self._name, expr=replace_ros_interface_expression(self._expr)) def as_xml(self) -> ET.Element: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 6246cbab..a1f65fbd 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -23,16 +23,18 @@ from typing import List, Optional, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (RosField, ScxmlBase, - ScxmlExecutionBody, ScxmlSend, - ScxmlTransition, - as_plain_execution_body, - execution_body_from_xml, - valid_execution_body) -from scxml_converter.scxml_entries.utils import ( +from scxml_converter.scxml_entries import ( + RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + as_plain_execution_body, execution_body_from_xml, valid_execution_body) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import ( ScxmlRosDeclarationsContainer, generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string class RosServiceServer(ScxmlBase): @@ -45,23 +47,35 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceServer": """Create a RosServiceServer object from an XML tree.""" - assert xml_tree.tag == RosServiceServer.get_tag_name(), \ - f"Error: SCXML Service Server: XML tag name is not '{RosServiceServer.get_tag_name()}'." - service_name = xml_tree.attrib.get("service_name") - service_type = xml_tree.attrib.get("type") - assert service_name is not None and service_type is not None, \ - "Error: SCXML Service Server: 'service_name' or 'type' cannot be found in input xml." - return RosServiceServer(service_name, service_type) - - def __init__(self, srv_name: str, srv_type: str) -> None: + assert_xml_tag_ok(RosServiceServer, xml_tree) + service_name = get_xml_argument( + RosServiceServer, xml_tree, "service_name", none_allowed=True) + service_type = get_xml_argument(RosServiceServer, xml_tree, "type") + service_alias = get_xml_argument( + RosServiceServer, xml_tree, "name", none_allowed=True) + if service_name is None: + service_name = read_value_from_xml_child(xml_tree, "service_name", + (BtGetValueInputPort, str)) + return RosServiceServer(service_name, service_type, service_alias) + + def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, + srv_alias: Optional[str] = None) -> None: """ Initialize a new RosServiceServer object. - :param srv_name: Topic used by the service. + :param srv_name: Service name used by the service for communication. :param srv_type: ROS type of the service. + :param srv_alias: Alias for the service server, for the handler to reference to it """ self._srv_name = srv_name self._srv_type = srv_type + self._srv_alias = srv_alias + assert isinstance(srv_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Server: invalid service name." + if self._srv_alias is None: + assert is_non_empty_string(RosServiceServer, "service_name", self._srv_name), \ + "Error: SCXML Service Server: an alias name is required for dynamic service names." + self._srv_alias = srv_name def get_service_name(self) -> str: """Get the name of the service.""" @@ -71,6 +85,10 @@ def get_service_type(self) -> str: """Get the type of the service.""" return self._srv_type + def get_name(self) -> str: + """Get the alias of the service server.""" + return self._srv_alias + def check_validity(self) -> bool: valid_name = isinstance(self._srv_name, str) and len(self._srv_name) > 0 valid_type = is_srv_type_known(self._srv_type) @@ -80,6 +98,14 @@ def check_validity(self) -> bool: print("Error: SCXML Service Server: service type is not valid.") return valid_name and valid_type + def check_valid_instantiation(self) -> bool: + """Check if the service server has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosServiceServer, "service_name", self._srv_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + pass + def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") @@ -88,7 +114,7 @@ def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Server: invalid parameters." xml_srv_server = ET.Element( RosServiceServer.get_tag_name(), - {"service_name": self._srv_name, "type": self._srv_type}) + {"name": self._srv_alias, "service_name": self._srv_name, "type": self._srv_type}) return xml_srv_server @@ -102,23 +128,35 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": """Create a RosServiceClient object from an XML tree.""" - assert xml_tree.tag == RosServiceClient.get_tag_name(), \ - f"Error: SCXML Service Client: XML tag name is not '{RosServiceClient.get_tag_name()}'." - service_name = xml_tree.attrib.get("service_name") - service_type = xml_tree.attrib.get("type") - assert service_name is not None and service_type is not None, \ - "Error: SCXML Service Client: 'service_name' or 'type' cannot be found in input xml." - return RosServiceClient(service_name, service_type) - - def __init__(self, srv_name: str, srv_type: str) -> None: + assert_xml_tag_ok(RosServiceClient, xml_tree) + service_name = get_xml_argument( + RosServiceClient, xml_tree, "service_name", none_allowed=True) + service_type = get_xml_argument(RosServiceClient, xml_tree, "type") + service_alias = get_xml_argument( + RosServiceClient, xml_tree, "name", none_allowed=True) + if service_name is None: + service_name = read_value_from_xml_child(xml_tree, "service_name", + (BtGetValueInputPort, str)) + return RosServiceClient(service_name, service_type, service_alias) + + def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, + srv_alias: Optional[str] = None) -> None: """ Initialize a new RosServiceClient object. :param srv_name: Topic used by the service. :param srv_type: ROS type of the service. + :param srv_alias: Alias for the service client, for the handler to reference to it """ self._srv_name = srv_name self._srv_type = srv_type + self._srv_alias = srv_alias + assert isinstance(srv_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Client: invalid service name." + if self._srv_alias is None: + assert is_non_empty_string(RosServiceClient, "service_name", self._srv_name), \ + "Error: SCXML Service Client: an alias name is required for dynamic service names." + self._srv_alias = srv_name def get_service_name(self) -> str: """Get the name of the service.""" @@ -128,6 +166,10 @@ def get_service_type(self) -> str: """Get the type of the service.""" return self._srv_type + def get_name(self) -> str: + """Get the alias of the service client.""" + return self._srv_alias + def check_validity(self) -> bool: valid_name = isinstance(self._srv_name, str) and len(self._srv_name) > 0 valid_type = is_srv_type_known(self._srv_type) @@ -137,6 +179,14 @@ def check_validity(self) -> bool: print("Error: SCXML Service Client: service type is not valid.") return valid_name and valid_type + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosServiceClient, "service_name", self._srv_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + pass + def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") @@ -145,7 +195,7 @@ def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Client: invalid parameters." xml_srv_server = ET.Element( RosServiceClient.get_tag_name(), - {"service_name": self._srv_name, "type": self._srv_type}) + {"name": self._srv_alias, "service_name": self._srv_name, "type": self._srv_type}) return xml_srv_server @@ -159,12 +209,12 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendRequest": """Create a RosServiceServer object from an XML tree.""" - assert xml_tree.tag == RosServiceSendRequest.get_tag_name(), \ - "Error: SCXML service request: XML tag name is not " + \ - RosServiceSendRequest.get_tag_name() - srv_name = xml_tree.attrib.get("service_name") - assert srv_name is not None, \ - "Error: SCXML service request: 'service_name' attribute not found in input xml." + assert_xml_tag_ok(RosServiceSendRequest, xml_tree) + srv_name = get_xml_argument(RosServiceSendRequest, xml_tree, "name", none_allowed=True) + if srv_name is None: + srv_name = get_xml_argument(RosServiceSendRequest, xml_tree, "service_name") + print("Warning: SCXML service request: 'service_name' xml arg. is deprecated. " + "Use 'name' instead.") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) @@ -180,7 +230,7 @@ def __init__(self, :param fields: List of fields to be sent in the request. """ if isinstance(service_decl, RosServiceClient): - self._srv_name = service_decl.get_service_name() + self._srv_name = service_decl.get_name() else: # Used for generating ROS entries from xml file assert isinstance(service_decl, str), \ @@ -219,15 +269,16 @@ def check_valid_ros_instantiations(self, def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service request: invalid ROS instantiations." - event_name = generate_srv_request_event( - self._srv_name, ros_declarations.get_automaton_name()) + automaton_name = ros_declarations.get_automaton_name() + srv_interface, _ = ros_declarations.get_service_client_info(self._srv_name) + event_name = generate_srv_request_event(srv_interface, automaton_name) event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, event_params) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Send Request: invalid parameters." xml_srv_request = ET.Element(RosServiceSendRequest.get_tag_name(), - {"service_name": self._srv_name}) + {"name": self._srv_name}) if self._fields is not None: for field in self._fields: xml_srv_request.append(field.as_xml()) @@ -244,14 +295,13 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleRequest": """Create a RosServiceServer object from an XML tree.""" - assert xml_tree.tag == RosServiceHandleRequest.get_tag_name(), \ - "Error: SCXML service request handler: XML tag name is not " +\ - RosServiceHandleRequest.get_tag_name() - srv_name = xml_tree.attrib.get("service_name") - target_name = xml_tree.attrib.get("target") - assert srv_name is not None and target_name is not None, \ - "Error: SCXML service request handler: 'service_name' or 'target' attribute not " \ - "found in input xml." + assert_xml_tag_ok(RosServiceHandleRequest, xml_tree) + srv_name = get_xml_argument(RosServiceHandleRequest, xml_tree, "name", none_allowed=True) + if srv_name is None: + srv_name = get_xml_argument(RosServiceHandleRequest, xml_tree, "service_name") + print("Warning: SCXML service request handler: 'service_name' xml arg. is deprecated. " + "Use 'name' instead.") + target_name = get_xml_argument(RosServiceHandleRequest, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) return RosServiceHandleRequest(srv_name, target_name, exec_body) @@ -265,7 +315,7 @@ def __init__(self, service_decl: Union[str, RosServiceServer], target: str, :param body: Execution body to be executed upon request, before transitioning to target. """ if isinstance(service_decl, RosServiceServer): - self._service_name = service_decl.get_service_name() + self._service_name = service_decl.get_name() else: # Used for generating ROS entries from xml file assert isinstance(service_decl, str), \ @@ -304,7 +354,8 @@ def check_valid_ros_instantiations(self, def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service request handler: invalid ROS instantiations." - event_name = generate_srv_server_request_event(self._service_name) + interface_name, _ = ros_declarations.get_service_server_info(self._service_name) + event_name = generate_srv_server_request_event(interface_name) target = self._target body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], None, body) @@ -312,7 +363,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Handle Request: invalid parameters." xml_srv_request = ET.Element(RosServiceHandleRequest.get_tag_name(), - {"service_name": self._service_name, "target": self._target}) + {"name": self._service_name, "target": self._target}) if self._body is not None: for body_elem in self._body: xml_srv_request.append(body_elem.as_xml()) @@ -329,12 +380,12 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendResponse": """Create a RosServiceServer object from an XML tree.""" - assert xml_tree.tag == RosServiceSendResponse.get_tag_name(), \ - "Error: SCXML service response: XML tag name is not " + \ - RosServiceSendResponse.get_tag_name() - srv_name = xml_tree.attrib.get("service_name") - assert srv_name is not None, \ - "Error: SCXML service response: 'service_name' attribute not found in input xml." + assert_xml_tag_ok(RosServiceSendResponse, xml_tree) + srv_name = get_xml_argument(RosServiceSendResponse, xml_tree, "name", none_allowed=True) + if srv_name is None: + srv_name = get_xml_argument(RosServiceSendResponse, xml_tree, "service_name") + print("Warning: SCXML service send response: 'service_name' xml arg. is deprecated. " + "Use 'name' instead.") fields: Optional[List[RosField]] = [] assert fields is not None, "Error: SCXML service response: fields is not valid." for field_xml in xml_tree: @@ -352,7 +403,7 @@ def __init__(self, service_name: Union[str, RosServiceServer], :param fields: List of fields to be sent in the response. """ if isinstance(service_name, RosServiceServer): - self._service_name = service_name.get_service_name() + self._service_name = service_name.get_name() else: # Used for generating ROS entries from xml file assert isinstance(service_name, str), \ @@ -389,14 +440,15 @@ def check_valid_ros_instantiations(self, def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service response: invalid ROS instantiations." - event_name = generate_srv_server_response_event(self._service_name) + interface_name, _ = ros_declarations.get_service_server_info(self._service_name) + event_name = generate_srv_server_response_event(interface_name) event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, event_params) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Send Response: invalid parameters." xml_srv_response = ET.Element(RosServiceSendResponse.get_tag_name(), - {"service_name": self._service_name}) + {"name": self._service_name}) if self._fields is not None: for field in self._fields: xml_srv_response.append(field.as_xml()) @@ -413,14 +465,13 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleResponse": """Create a RosServiceServer object from an XML tree.""" - assert xml_tree.tag == RosServiceHandleResponse.get_tag_name(), \ - "Error: SCXML service response handler: XML tag name is not " + \ - RosServiceHandleResponse.get_tag_name() - srv_name = xml_tree.attrib.get("service_name") - target_name = xml_tree.attrib.get("target") - assert srv_name is not None and target_name is not None, \ - "Error: SCXML service response handler: 'service_name' or 'target' attribute not " \ - "found in input xml." + assert_xml_tag_ok(RosServiceHandleResponse, xml_tree) + srv_name = get_xml_argument(RosServiceHandleResponse, xml_tree, "name", none_allowed=True) + if srv_name is None: + srv_name = get_xml_argument(RosServiceHandleResponse, xml_tree, "service_name") + print("Warning: SCXML service response handler: 'service_name' xml arg. is deprecated. " + "Use 'name' instead.") + target_name = get_xml_argument(RosServiceHandleResponse, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) return RosServiceHandleResponse(srv_name, target_name, exec_body) @@ -433,7 +484,7 @@ def __init__(self, service_decl: Union[str, RosServiceClient], target: str, :param type: ROS type of the service. """ if isinstance(service_decl, RosServiceClient): - self._service_name = service_decl.get_service_name() + self._service_name = service_decl.get_name() else: # Used for generating ROS entries from xml file assert isinstance(service_decl, str), \ @@ -472,8 +523,9 @@ def check_valid_ros_instantiations(self, def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service response handler: invalid ROS instantiations." - event_name = generate_srv_response_event( - self._service_name, ros_declarations.get_automaton_name()) + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_service_client_info(self._service_name) + event_name = generate_srv_response_event(interface_name, automaton_name) target = self._target body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], None, body) @@ -481,7 +533,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." xml_srv_response = ET.Element(RosServiceHandleResponse.get_tag_name(), - {"service_name": self._service_name, "target": self._target}) + {"name": self._service_name, "target": self._target}) if self._body is not None: for body_elem in self._body: xml_srv_response.append(body_elem.as_xml()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py index 9b10c7e4..93ab0e6c 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py @@ -18,21 +18,15 @@ from typing import Optional, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlExecutionBody, - ScxmlRosDeclarationsContainer, - ScxmlTransition, - as_plain_execution_body, - execution_body_from_xml, - valid_execution_body) +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlTransition, + as_plain_execution_body, execution_body_from_xml, valid_execution_body) +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler class RosTimeRate(ScxmlBase): """Object used in the SCXML root to declare a new timer with its related tick rate.""" - def __init__(self, name: str, rate_hz: float): - self._name = name - self._rate_hz = float(rate_hz) - @staticmethod def get_tag_name() -> str: return "ros_time_rate" @@ -52,6 +46,14 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTimeRate": raise ValueError("Error: SCXML rate timer: rate is not a number.") from e return RosTimeRate(timer_name, timer_rate) + def __init__(self, name: str, rate_hz: float): + self._name = name + self._rate_hz = float(rate_hz) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + pass + def check_validity(self) -> bool: valid_name = isinstance(self._name, str) and len(self._name) > 0 valid_rate = isinstance(self._rate_hz, float) and self._rate_hz > 0 @@ -61,6 +63,10 @@ def check_validity(self) -> bool: print("Error: SCXML rate timer: rate is not valid.") return valid_name and valid_rate + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return True + def get_name(self) -> str: return self._name diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index 5fa2beda..e7297b05 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -23,14 +23,15 @@ from typing import List, Optional, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (RosField, ScxmlBase, - ScxmlExecutionBody, ScxmlParam, - ScxmlRosDeclarationsContainer, - ScxmlSend, ScxmlTransition, - as_plain_execution_body, - execution_body_from_xml, - valid_execution_body) -from scxml_converter.scxml_entries.utils import is_msg_type_known +from scxml_converter.scxml_entries import ( + RosField, ScxmlBase, ScxmlExecutionBody, ScxmlParam, ScxmlRosDeclarationsContainer, ScxmlSend, + ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, + valid_execution_body) +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import is_msg_type_known, sanitize_ros_interface_name +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, get_children_as_scxml, read_value_from_xml_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string class RosTopicPublisher(ScxmlBase): @@ -43,33 +44,70 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosTopicPublisher": """Create a RosTopicPublisher object from an XML tree.""" - assert xml_tree.tag == RosTopicPublisher.get_tag_name(), \ - f"Error: SCXML topic publisher: XML tag name is not {RosTopicPublisher.get_tag_name()}" - topic_name = xml_tree.attrib.get("topic") - topic_type = xml_tree.attrib.get("type") - assert topic_name is not None and topic_type is not None, \ - "Error: SCXML topic publisher: 'topic' or 'type' attribute not found in input xml." - return RosTopicPublisher(topic_name, topic_type) - - def __init__(self, topic_name: str, topic_type: str) -> None: - self._topic_name = topic_name + assert_xml_tag_ok(RosTopicPublisher, xml_tree) + topic_name = get_xml_argument(RosTopicPublisher, xml_tree, "topic", none_allowed=True) + topic_type = get_xml_argument(RosTopicPublisher, xml_tree, "type") + pub_name = get_xml_argument(RosTopicPublisher, xml_tree, "name", none_allowed=True) + if topic_name is None: + topic_name = read_value_from_xml_child(xml_tree, "topic", (BtGetValueInputPort, str)) + assert topic_name is not None, "Error: SCXML topic publisher: topic name not found." + return RosTopicPublisher(topic_name, topic_type, pub_name) + + def __init__(self, + topic_name: Union[str, BtGetValueInputPort], topic_type: str, + pub_name: Optional[str] = None) -> None: + """ + Create a new ros_topic_publisher object instance. + + By default, its alias is the same as the topic name, if that is defined as a string. + If the topic is defined as a BtGetValueInputPort, an alias must be provided. + + :param topic_name: The name of the topic where messages are published. + :param topic_type: The type of the message to be published + :param pub_name: Alias used to reference the publisher in SCXML. + """ self._topic_type = topic_type + self._topic_name = topic_name + self._pub_name = pub_name + assert isinstance(self._topic_name, (str, BtGetValueInputPort)), \ + "Error: SCXML topic publisher: invalid topic name." + if self._pub_name is None: + assert is_non_empty_string(RosTopicPublisher, "topic", self._topic_name), \ + "Error: SCXML topic publisher: alias must be provided for dynamic topic names." + self._pub_name = self._topic_name def check_validity(self) -> bool: - valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 + valid_topic_name = isinstance(self._topic_name, BtGetValueInputPort) or \ + is_non_empty_string(RosTopicPublisher, "topic", self._topic_name) valid_type = is_msg_type_known(self._topic_type) - if not valid_name: - print("Error: SCXML topic subscriber: topic name is not valid.") + valid_alias = is_non_empty_string(RosTopicPublisher, "name", self._pub_name) if not valid_type: print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_name and valid_type + return valid_topic_name and valid_type and valid_alias + + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosTopicPublisher, "topic", self._topic_name) - def get_topic_name(self) -> str: + def get_topic_name(self) -> Union[str, BtGetValueInputPort]: + """Get the name of the topic where messages are published.""" return self._topic_name def get_topic_type(self) -> str: + """Get a string representation of the topic type.""" return self._topic_type + def get_name(self) -> str: + """Get the alias used to reference the publisher in SCXML.""" + return self._pub_name + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """ + Update the value of the BT ports used in the publisher, if any. + """ + if isinstance(self._topic_name, BtGetValueInputPort): + self._topic_name = bt_ports_handler.get_in_port_value(self._topic_name.get_key_name()) + def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") @@ -77,7 +115,8 @@ def as_plain_scxml(self, _) -> ScxmlBase: def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic subscriber: invalid parameters." xml_topic_publisher = ET.Element( - RosTopicPublisher.get_tag_name(), {"topic": self._topic_name, "type": self._topic_type}) + RosTopicPublisher.get_tag_name(), + {"name": self._pub_name, "topic": self._topic_name, "type": self._topic_type}) return xml_topic_publisher @@ -91,33 +130,53 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosTopicSubscriber": """Create a RosTopicSubscriber object from an XML tree.""" - assert xml_tree.tag == RosTopicSubscriber.get_tag_name(), \ - f"Error: SCXML topic subscribe: XML tag name is not {RosTopicSubscriber.get_tag_name()}" - topic_name = xml_tree.attrib.get("topic") - topic_type = xml_tree.attrib.get("type") - assert topic_name is not None and topic_type is not None, \ - "Error: SCXML topic subscriber: 'topic' or 'type' attribute not found in input xml." - return RosTopicSubscriber(topic_name, topic_type) - - def __init__(self, topic_name: str, topic_type: str) -> None: - self._topic_name = topic_name + assert_xml_tag_ok(RosTopicSubscriber, xml_tree) + topic_name = get_xml_argument(RosTopicSubscriber, xml_tree, "topic", none_allowed=True) + topic_type = get_xml_argument(RosTopicSubscriber, xml_tree, "type") + sub_name = get_xml_argument(RosTopicSubscriber, xml_tree, "name", none_allowed=True) + if topic_name is None: + topic_name = read_value_from_xml_child(xml_tree, "topic", (BtGetValueInputPort, str)) + assert topic_name is not None, "Error: SCXML topic subscriber: topic name not found." + return RosTopicSubscriber(topic_name, topic_type, sub_name) + + def __init__(self, topic_name: Union[str, BtGetValueInputPort], topic_type: str, + sub_name: Optional[str] = None) -> None: self._topic_type = topic_type + self._topic_name = topic_name + self._sub_name = sub_name + assert isinstance(self._topic_name, (str, BtGetValueInputPort)), \ + "Error: SCXML topic subscriber: invalid topic name." + if self._sub_name is None: + assert is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name), \ + "Error: SCXML topic subscriber: alias must be provided for dynamic topic names." + self._sub_name = self._topic_name def check_validity(self) -> bool: - valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 + valid_name = isinstance(self._topic_name, BtGetValueInputPort) or \ + is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name) valid_type = is_msg_type_known(self._topic_type) - if not valid_name: - print("Error: SCXML topic subscriber: topic name is not valid.") + valid_alias = is_non_empty_string(RosTopicSubscriber, "name", self._sub_name) if not valid_type: print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_name and valid_type + return valid_name and valid_type and valid_alias - def get_topic_name(self) -> str: + def check_valid_instantiation(self) -> bool: + """Check if the topic subscriber has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name) + + def get_topic_name(self) -> Union[str, BtGetValueInputPort]: return self._topic_name def get_topic_type(self) -> str: return self._topic_type + def get_name(self) -> str: + return self._sub_name + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + pass + def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") @@ -126,7 +185,7 @@ def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic subscriber: invalid parameters." xml_topic_subscriber = ET.Element( RosTopicSubscriber.get_tag_name(), - {"topic": self._topic_name, "type": self._topic_type}) + {"name": self._sub_name, "topic": self._topic_name, "type": self._topic_type}) return xml_topic_subscriber @@ -140,55 +199,52 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosTopicCallback": """Create a RosTopicCallback object from an XML tree.""" - assert xml_tree.tag == RosTopicCallback.get_tag_name(), \ - f"Error: SCXML topic callback: XML tag name is not {RosTopicCallback.get_tag_name()}" - topic_name = xml_tree.attrib.get("topic") - target = xml_tree.attrib.get("target") - assert topic_name is not None and target is not None, \ - "Error: SCXML topic callback: 'topic' or 'target' attribute not found in input xml." + assert_xml_tag_ok(RosTopicCallback, xml_tree) + sub_name = get_xml_argument(RosTopicCallback, xml_tree, "name", none_allowed=True) + if sub_name is None: + sub_name = get_xml_argument(RosTopicCallback, xml_tree, "topic") + print("Warning: SCXML topic callback: the 'topic' argument is deprecated. " + "Use 'name' instead.") + target = get_xml_argument(RosTopicCallback, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) - return RosTopicCallback(topic_name, target, exec_body) + return RosTopicCallback(sub_name, target, exec_body) def __init__( - self, topic: Union[RosTopicSubscriber, str], target: str, + self, topic_sub: Union[RosTopicSubscriber, str], target: str, body: Optional[ScxmlExecutionBody] = None): """ Create a new ros_topic_callback object instance. - :param topic: The RosTopicSubscriber instance triggering the callback, or its name + :param topic_sub: The RosTopicSubscriber instance triggering the callback, or its name :param target: The target state of the transition :param body: Execution body executed at the time the received message gets processed """ - if isinstance(topic, RosTopicSubscriber): - self._topic = topic.get_topic_name() + if isinstance(topic_sub, RosTopicSubscriber): + self._sub_name = topic_sub.get_name() else: # Used for generating ROS entries from xml file - assert isinstance(topic, str), "Error: SCXML topic callback: invalid topic type." - self._topic = topic + assert is_non_empty_string(RosTopicCallback, "name", topic_sub) + self._sub_name = topic_sub self._target = target self._body = body assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." def check_validity(self) -> bool: - valid_topic = isinstance(self._topic, str) and len(self._topic) > 0 - valid_target = isinstance(self._target, str) and len(self._target) > 0 + valid_sub_name = is_non_empty_string(RosTopicCallback, "name", self._sub_name) + valid_target = is_non_empty_string(RosTopicCallback, "target", self._target) valid_body = self._body is None or valid_execution_body(self._body) - if not valid_topic: - print("Error: SCXML topic callback: topic name is not valid.") - if not valid_target: - print("Error: SCXML topic callback: target is not valid.") if not valid_body: print("Error: SCXML topic callback: body is not valid.") - return valid_topic and valid_target and valid_body + return valid_sub_name and valid_target and valid_body def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared.""" assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ "Error: SCXML topic callback: invalid ROS declarations container." - topic_cb_declared = ros_declarations.is_subscriber_defined(self._topic) + topic_cb_declared = ros_declarations.is_subscriber_defined(self._sub_name) if not topic_cb_declared: - print(f"Error: SCXML topic callback: topic subscriber {self._topic} not declared.") + print(f"Error: SCXML topic callback: topic subscriber {self._sub_name} not declared.") return False valid_body = super().check_valid_ros_instantiations(ros_declarations) if not valid_body: @@ -198,7 +254,8 @@ def check_valid_ros_instantiations(self, def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML topic callback: invalid ROS instantiations." - event_name = "ros_topic." + self._topic + topic_name, _ = ros_declarations.get_subscriber_info(self._sub_name) + event_name = "ros_topic." + sanitize_ros_interface_name(topic_name) target = self._target body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], None, body) @@ -206,7 +263,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." xml_topic_callback = ET.Element( - "ros_topic_callback", {"topic": self._topic, "target": self._target}) + "ros_topic_callback", {"name": self._sub_name, "target": self._target}) if self._body is not None: for entry in self._body: xml_topic_callback.append(entry.as_xml()) @@ -223,47 +280,44 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> ScxmlSend: """Create a RosTopicPublish object from an XML tree.""" - assert xml_tree.tag == RosTopicPublish.get_tag_name(), \ - f"Error: SCXML topic publish: XML tag name is not {RosTopicPublish.get_tag_name()}" - topic_name = xml_tree.attrib.get("topic") - assert topic_name is not None, \ - "Error: SCXML topic publish: 'topic' attribute not found in input xml." - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosTopicPublish(topic_name, fields) - - def __init__(self, topic: Union[RosTopicPublisher, str], - fields: List[RosField] = None) -> None: + assert_xml_tag_ok(RosTopicPublish, xml_tree) + pub_name = get_xml_argument(RosTopicPublish, xml_tree, "name", none_allowed=True) + if pub_name is None: + pub_name = get_xml_argument(RosTopicSubscriber, xml_tree, "topic") + print("Warning: SCXML topic publisher: the 'topic' argument is deprecated. " + "Use 'name' instead.") + fields: List[RosField] = get_children_as_scxml(xml_tree, (RosField,)) + return RosTopicPublish(pub_name, fields) + + def __init__(self, topic_pub: Union[RosTopicPublisher, str], + fields: Optional[List[RosField]] = None) -> None: if fields is None: fields = [] - if isinstance(topic, RosTopicPublisher): - self._topic = topic.get_topic_name() + if isinstance(topic_pub, RosTopicPublisher): + self._pub_name = topic_pub.get_name() else: # Used for generating ROS entries from xml file - assert isinstance(topic, str), "Error: SCXML topic publish: invalid topic type." - self._topic = topic + assert is_non_empty_string(RosTopicPublish, "name", topic_pub) + self._pub_name = topic_pub self._fields = fields assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." def check_validity(self) -> bool: - valid_topic = isinstance(self._topic, str) and len(self._topic) > 0 + valid_pub_name = is_non_empty_string(RosTopicPublish, "name", self._pub_name) valid_fields = self._fields is None or \ all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_topic: - print("Error: SCXML topic publish: topic name is not valid.") if not valid_fields: print("Error: SCXML topic publish: fields are not valid.") - return valid_topic and valid_fields + return valid_pub_name and valid_fields def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared.""" assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ "Error: SCXML topic publish: invalid ROS declarations container." - topic_pub_declared = ros_declarations.is_publisher_defined(self._topic) + topic_pub_declared = ros_declarations.is_publisher_defined(self._pub_name) if not topic_pub_declared: - print(f"Error: SCXML topic publish: topic {self._topic} not declared.") + print(f"Error: SCXML topic publish: topic publisher {self._pub_name} not declared.") # TODO: Check for valid fields can be done here return topic_pub_declared @@ -277,17 +331,23 @@ def append_field(self, field: RosField) -> None: self._fields = [] self._fields.append(field) + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + for field in self._fields: + field.update_bt_ports_values(bt_ports_handler) + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML topic publish: invalid ROS instantiations." - event_name = "ros_topic." + self._topic + topic_name, _ = ros_declarations.get_publisher_info(self._pub_name) + event_name = "ros_topic." + sanitize_ros_interface_name(topic_name) params = None if self._fields is None else \ [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, params) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." - xml_topic_publish = ET.Element(RosTopicPublish.get_tag_name(), {"topic": self._topic}) + xml_topic_publish = ET.Element(RosTopicPublish.get_tag_name(), {"name": self._pub_name}) if self._fields is not None: for field in self._fields: xml_topic_publish.append(field.as_xml()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index ad1be9db..6fd2da19 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -17,17 +17,14 @@ A single state in SCXML. In XML, it has the tag `state`. """ -from typing import List, Optional, Sequence, Union +from typing import List, Sequence, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlExecutableEntry, - ScxmlExecutionBody, - ScxmlRosDeclarationsContainer, - ScxmlRosTransitions, - ScxmlTransition, - as_plain_execution_body, - execution_body_from_xml, - valid_execution_body) +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlExecutableEntry, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, + ScxmlTransition, as_plain_execution_body, execution_body_from_xml, valid_execution_body, + instantiate_exec_body_bt_events) +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler class ScxmlState(ScxmlBase): @@ -93,6 +90,22 @@ def get_body(self) -> List[ScxmlTransition]: """Return the transitions leaving the state.""" return self._body + def instantiate_bt_events(self, instance_id: str) -> None: + """Instantiate the BT events in all entries belonging to a state.""" + for transition in self._body: + transition.instantiate_bt_events(instance_id) + instantiate_exec_body_bt_events(self._on_entry, instance_id) + instantiate_exec_body_bt_events(self._on_exit, instance_id) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + for transition in self._body: + transition.update_bt_ports_values(bt_ports_handler) + for entry in self._on_entry: + entry.update_bt_ports_values(bt_ports_handler) + for entry in self._on_exit: + entry.update_bt_ports_values(bt_ports_handler) + @classmethod def _transitions_from_xml(cls, xml_tree: ET.Element) -> List[ScxmlTransition]: transitions: List[ScxmlTransition] = [] @@ -151,8 +164,9 @@ def check_valid_ros_instantiations(self, return valid_entry and valid_exit and valid_body @staticmethod - def _check_valid_ros_instantiations(body: Sequence[Union[ScxmlExecutableEntry, ScxmlTransition]], - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + def _check_valid_ros_instantiations( + body: Sequence[Union[ScxmlExecutableEntry, ScxmlTransition]], + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared in the body.""" return len(body) == 0 or \ all(entry.check_valid_ros_instantiations(ros_declarations) for entry in body) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py index 5b3fbc02..1bd4bf02 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py @@ -20,11 +20,12 @@ from typing import List, Optional from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlExecutableEntry, - ScxmlExecutionBody, - ScxmlRosDeclarationsContainer, - execution_body_from_xml, - valid_execution_body) +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlExecutableEntry, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, + execution_body_from_xml, valid_execution_body, valid_execution_body_entry_types, + instantiate_exec_body_bt_events) + +from scxml_converter.scxml_entries.bt_utils import is_bt_event, replace_bt_event, BtPortsHandler class ScxmlTransition(ScxmlBase): @@ -65,8 +66,8 @@ def __init__(self, f"Error SCXML transition: events must be a list of non-empty strings. Found {events}." assert condition is None or (isinstance(condition, str) and len(condition) > 0), \ "Error SCXML transition: condition must be a non-empty string." - assert body is None or valid_execution_body( - body), "Error SCXML transition: invalid body provided." + assert body is None or valid_execution_body_entry_types(body), \ + "Error SCXML transition: invalid body provided." self._target = target self._body = body self._events = events if events is not None else [] @@ -88,6 +89,22 @@ def get_executable_body(self) -> ScxmlExecutionBody: """Return the executable content of this transition.""" return self._body if self._body is not None else [] + def instantiate_bt_events(self, instance_id: str): + """Instantiate the BT events of this transition.""" + # Make sure to replace received events only for ScxmlTransition objects. + if type(self) is ScxmlTransition: + for event_id, event_str in enumerate(self._events): + # Those are expected to be only ticks + if is_bt_event(event_str): + self._events[event_id] = replace_bt_event(event_str, instance_id) + # The body of a transition is needs to be replaced on derived classes, too + instantiate_exec_body_bt_events(self._body, instance_id) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + for entry in self._body: + entry.update_bt_ports_values(bt_ports_handler) + def add_event(self, event: str): self._events.append(event) @@ -95,8 +112,8 @@ def append_body_executable_entry(self, exec_entry: ScxmlExecutableEntry): if self._body is None: self._body = [] self._body.append(exec_entry) - assert valid_execution_body(self._body), \ - "Error SCXML transition: invalid body after extension." + assert valid_execution_body_entry_types(self._body), \ + "Error SCXML transition: invalid body entry found after extension." def check_validity(self) -> bool: valid_target = isinstance(self._target, str) and len(self._target) > 0 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/utils.py b/scxml_converter/src/scxml_converter/scxml_entries/utils.py index 493ae98d..7e62e3bc 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/utils.py @@ -15,21 +15,13 @@ """Collection of various utilities for scxml entries.""" -from typing import Dict, List, Optional, Tuple +from typing import Dict, Type -from scxml_converter.scxml_entries.scxml_ros_field import RosField - -MSG_TYPE_SUBSTITUTIONS = { - "boolean": "bool", -} - -BASIC_FIELD_TYPES = ['boolean', - 'int8', 'int16', 'int32', 'int64', - 'float', 'double'] +from scxml_converter.scxml_entries import ScxmlBase # TODO: add lower and upper bounds depending on the n. of bits used. # TODO: add support to uint -SCXML_DATA_STR_TO_TYPE = { +SCXML_DATA_STR_TO_TYPE: Dict[str, Type] = { "bool": bool, "float32": float, "float64": float, @@ -40,227 +32,35 @@ } -def is_ros_type_known(type_definition: str, ros_interface: str) -> bool: +def all_non_empty_strings(*in_args) -> bool: """ - Check if python can import the provided type definition. + Check if all the arguments are non-empty strings. - :param type_definition: The type definition to check (e.g. std_msgs/Empty). + :param kwargs: The arguments to be checked. + :return: True if all the arguments are non-empty strings, False otherwise. """ - if not (isinstance(type_definition, str) and type_definition.count("/") == 1): - return False - interface_ns, interface_type = type_definition.split("/") - if len(interface_ns) == 0 or len(interface_type) == 0: - return False - assert ros_interface in ["msg", "srv"], "Error: SCXML ROS declarations: unknown ROS interface." - try: - interface_importer = __import__(interface_ns + f'.{ros_interface}', fromlist=['']) - _ = getattr(interface_importer, interface_type) - except (ImportError, AttributeError): - print(f"Error: SCXML ROS declarations: topic type {type_definition} not found.") - return False + for arg_value in in_args: + if not isinstance(arg_value, str) or len(arg_value) == 0: + return False return True -def is_msg_type_known(topic_definition: str) -> bool: - """Check if python can import the provided topic definition.""" - return is_ros_type_known(topic_definition, "msg") - - -def is_srv_type_known(service_definition: str) -> bool: - """Check if python can import the provided service definition.""" - return is_ros_type_known(service_definition, "srv") - - -def get_srv_type_params(service_definition: str) -> Tuple[Dict[str, str], Dict[str, str]]: - """ - Get the data fields of a service request and response type as pairs of name and type objects. +def is_non_empty_string(scxml_type: Type[ScxmlBase], arg_name: str, arg_value: str) -> bool: """ - assert is_srv_type_known(service_definition), \ - "Error: SCXML ROS declarations: service type not found." - interface_ns, interface_type = service_definition.split("/") - srv_module = __import__(interface_ns + '.srv', fromlist=['']) - srv_class = getattr(srv_module, interface_type) - - # TODO: Fields can be nested. Look AS2FM/scxml_converter/src/scxml_converter/scxml_converter.py - req = srv_class.Request.get_fields_and_field_types() - for key in req.keys(): - # TODO: Support nested fields - assert req[key] in BASIC_FIELD_TYPES, \ - f"Error: SCXML ROS declarations: service request type {req[key]} isn't a basic field." - req[key] = MSG_TYPE_SUBSTITUTIONS.get(req[key], req[key]) - - res = srv_class.Response.get_fields_and_field_types() - for key in res.keys(): - assert res[key] in BASIC_FIELD_TYPES, \ - "Error: SCXML ROS declarations: service response type contains non-basic fields." - res[key] = MSG_TYPE_SUBSTITUTIONS.get(res[key], res[key]) - - return req, res + Check if a string is non-empty. - -def replace_ros_interface_expression(msg_expr: str) -> str: - """Convert a ROS interface expression (msg, req, res) to plain SCXML (event).""" - scxml_prefix = "_event." - # TODO: Use regex and ensure no other valid character exists before the initial underscore - for ros_prefix in ["_msg.", "_req.", "_res."]: - msg_expr = msg_expr.replace(ros_prefix, scxml_prefix) - return msg_expr - - -def sanitize_ros_interface_name(interface_name: str) -> str: - """Replace slashes in a ROS interface name.""" - assert isinstance(interface_name, str), \ - "Error: ROS interface sanitizer: interface name must be a string." - # Remove potential prepended slash - interface_name = interface_name.removeprefix("/") - assert len(interface_name) > 0, \ - "Error: ROS interface sanitizer: interface name must not be empty." - assert interface_name.count(" ") == 0, \ - "Error: ROS interface sanitizer: interface name must not contain spaces." - return interface_name.replace("/", "__") + :param scxml_type: The scxml entry where this function is called, to write error msgs. + :param arg_name: The name of the argument, to write error msgs. + :param arg_value: The value of the argument to be checked. + :return: True if the string is non-empty, False otherwise. + """ + valid_str = isinstance(arg_value, str) and len(arg_value) > 0 + if not valid_str: + print(f"Error: SCXML conversion of {scxml_type.get_tag_name()}: " + f"Expected non-empty argument {arg_name}.") + return valid_str def get_default_expression_for_type(field_type: str) -> str: """Generate a default expression for a field type.""" return str(SCXML_DATA_STR_TO_TYPE[field_type]()) - - -def generate_srv_request_event(service_name: str, automaton_name: str) -> str: - """Generate the name of the event that triggers a service request.""" - return f"srv_{sanitize_ros_interface_name(service_name)}_req_client_{automaton_name}" - - -def generate_srv_response_event(service_name: str, automaton_name: str) -> str: - """Generate the name of the event that provides the service response.""" - return f"srv_{sanitize_ros_interface_name(service_name)}_response_client_{automaton_name}" - - -def generate_srv_server_request_event(service_name: str) -> str: - """Generate the name of the event that makes a service server start processing a request.""" - return f"srv_{sanitize_ros_interface_name(service_name)}_request" - - -def generate_srv_server_response_event(service_name: str) -> str: - """Generate the name of the event that makes a service server send a response.""" - return f"srv_{sanitize_ros_interface_name(service_name)}_response" - - -class ScxmlRosDeclarationsContainer: - """Object that contains a description of the ROS declarations in the SCXML root.""" - - def __init__(self, automaton_name: str): - """Constructor of container. - - :automaton_name: Name of the automaton these declarations belong to. - """ - self._automaton_name: str = automaton_name - # Dict of publishers and subscribers: topic name -> type - self._publishers: Dict[str, str] = {} - self._subscribers: Dict[str, str] = {} - self._service_servers: Dict[str, str] = {} - self._service_clients: Dict[str, str] = {} - self._timers: Dict[str, float] = {} - - def get_automaton_name(self) -> str: - """Get name of the automaton that these declarations are defined in.""" - return self._automaton_name - - def append_publisher(self, topic_name: str, topic_type: str) -> None: - assert isinstance(topic_name, str) and isinstance(topic_type, str), \ - "Error: ROS declarations: topic name and type must be strings." - assert topic_name not in self._publishers, \ - f"Error: ROS declarations: topic publisher {topic_name} already declared." - self._publishers[topic_name] = topic_type - - def append_subscriber(self, topic_name: str, topic_type: str) -> None: - assert isinstance(topic_name, str) and isinstance(topic_type, str), \ - "Error: ROS declarations: topic name and type must be strings." - assert topic_name not in self._subscribers, \ - f"Error: ROS declarations: topic subscriber {topic_name} already declared." - self._subscribers[topic_name] = topic_type - - def append_service_client(self, service_name: str, service_type: str) -> None: - assert isinstance(service_name, str) and isinstance(service_type, str), \ - "Error: ROS declarations: service name and type must be strings." - assert service_name not in self._service_clients, \ - f"Error: ROS declarations: service client {service_name} already declared." - self._service_clients[service_name] = service_type - - def append_service_server(self, service_name: str, service_type: str) -> None: - assert isinstance(service_name, str) and isinstance(service_type, str), \ - "Error: ROS declarations: service name and type must be strings." - assert service_name not in self._service_servers, \ - f"Error: ROS declarations: service server {service_name} already declared." - self._service_servers[service_name] = service_type - - def append_timer(self, timer_name: str, timer_rate: float) -> None: - assert isinstance(timer_name, str), "Error: ROS declarations: timer name must be a string." - assert isinstance(timer_rate, float) and timer_rate > 0, \ - "Error: ROS declarations: timer rate must be a positive number." - assert timer_name not in self._timers, \ - f"Error: ROS declarations: timer {timer_name} already declared." - self._timers[timer_name] = timer_rate - - def is_publisher_defined(self, topic_name: str) -> bool: - return topic_name in self._publishers - - def is_subscriber_defined(self, topic_name: str) -> bool: - return topic_name in self._subscribers - - def is_timer_defined(self, timer_name: str) -> bool: - return timer_name in self._timers - - def get_timers(self) -> Dict[str, float]: - return self._timers - - def is_service_client_defined(self, service_name: str) -> bool: - return service_name in self._service_clients - - def is_service_server_defined(self, service_name: str) -> bool: - return service_name in self._service_servers - - def get_service_client_type(self, service_name: str) -> Optional[str]: - return self._service_clients.get(service_name, None) - - def get_service_server_type(self, service_name: str) -> Optional[str]: - return self._service_servers.get(service_name, None) - - def check_valid_srv_req_fields(self, service_name: str, ros_fields: List[RosField]) -> bool: - """Check if the provided fields match the service request type.""" - req_type = self.get_service_client_type(service_name) - if req_type is None: - print(f"Error: SCXML ROS declarations: unknown service client {service_name}.") - return False - req_fields, _ = get_srv_type_params(req_type) - for ros_field in ros_fields: - if ros_field.get_name() not in req_fields: - print("Error: SCXML ROS declarations: " - f"unknown field {ros_field.get_name()} in service request.") - return False - req_fields.pop(ros_field.get_name()) - if len(req_fields) > 0: - print("Error: SCXML ROS declarations: missing fields in service request.") - for req_field in req_fields.keys(): - print(f"\t-{req_field}.") - return False - return True - - def check_valid_srv_res_fields(self, service_name: str, ros_fields: List[RosField]) -> bool: - """Check if the provided fields match the service response type.""" - res_type = self.get_service_server_type(service_name) - if res_type is None: - print(f"Error: SCXML ROS declarations: unknown service server {service_name}.") - return False - _, res_fields = get_srv_type_params(res_type) - for ros_field in ros_fields: - if ros_field.get_name() not in res_fields: - print("Error: SCXML ROS declarations: " - f"unknown field {ros_field.get_name()} in service response.") - return False - res_fields.pop(ros_field.get_name()) - if len(res_fields) > 0: - print("Error: SCXML ROS declarations: missing fields in service response.") - for res_field in res_fields.keys(): - print(f"\t-{res_field}.") - return False - return True diff --git a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py new file mode 100644 index 00000000..5b325379 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py @@ -0,0 +1,109 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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 typing import List, Optional, Tuple, Type, Union + +from scxml_converter.scxml_entries import ScxmlBase +from xml.etree.ElementTree import Element + + +def assert_xml_tag_ok(scxml_type: Type[ScxmlBase], xml_tree: Element): + """Ensures the xml_tree we are trying to parse has the expected name.""" + assert xml_tree.tag == scxml_type.get_tag_name(), \ + f"SCXML conversion: Expected tag {scxml_type.get_tag_name()}, but got {xml_tree.tag}" + + +def get_xml_argument(scxml_type: Type[ScxmlBase], xml_tree: Element, arg_name: str, *, + none_allowed=False, empty_allowed=False) -> Optional[str]: + """Load an argument from the xml tree's root tag.""" + arg_value = xml_tree.get(arg_name) + error_prefix = f"SCXML conversion of {scxml_type.get_tag_name()}" + if arg_value is None: + assert none_allowed, f"{error_prefix}: Expected argument {arg_name} in {xml_tree.tag}" + elif len(arg_value) == 0: + assert empty_allowed, \ + f"{error_prefix}: Expected non-empty argument {arg_name} in {xml_tree.tag}" + return arg_value + + +def get_children_as_scxml( + xml_tree: Element, scxml_types: Tuple[Type[ScxmlBase]]) -> List[ScxmlBase]: + """ + Load the children of the xml tree as scxml entries. + + :param xml_tree: The xml tree to read the children from. + :param scxml_types: The classes to read from the children. All others will be discarded. + :return: A list of scxml entries. + """ + scxml_list = [] + tag_to_type = {scxml_type.get_tag_name(): scxml_type for scxml_type in scxml_types} + for child in xml_tree: + if child.tag in tag_to_type: + scxml_list.append(tag_to_type[child.tag].from_xml_tree(child)) + return scxml_list + + +def read_value_from_xml_child( + xml_tree: Element, child_tag: str, valid_types: Tuple[Type[Union[ScxmlBase, str]]] + ) -> Optional[Union[str, ScxmlBase]]: + """ + Try to read the value of a child tag from the xml tree. If the child is not found, return None. + """ + xml_child = xml_tree.findall(child_tag) + if xml_child is None or len(xml_child) == 0: + print(f"Error reading from {xml_tree.tag}: Cannot find child '{child_tag}'.") + return None + if len(xml_child) > 1: + print(f"Error reading from {xml_tree.tag}: multiple children '{child_tag}', expected one.") + return None + n_tag_children = len(xml_child[0]) + if n_tag_children == 0 and str in valid_types: + # Try to read the text value + text_value = xml_child[0].text + if text_value is None or len(text_value) == 0: + print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has no text value.") + return None + return text_value + if n_tag_children > 1: + print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has multiple children.") + return None + # Remove string from valid types, if present + valid_types = tuple(t for t in valid_types if t != str) + scxml_entry = get_children_as_scxml(xml_child[0], valid_types) + if len(scxml_entry) == 0: + print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has no valid children.") + return None + return scxml_entry[0] + + +def read_value_from_xml_arg_or_child( + scxml_type: Type[ScxmlBase], xml_tree: Element, tag_name: str, + valid_types: Tuple[Type[Union[ScxmlBase, str]]], + none_allowed=False) -> Optional[Union[str, ScxmlBase]]: + """ + Read a value from an xml attribute or, if not found, the child tag with the same name. + + To read the value from the xml arguments, valid_types must include string. + """ + assert str in valid_types, \ + "Error: read_value_from_arg_or_child: valid_types must include str. " \ + "If strings are not expected, use 'read_value_from_xml_child'." + read_value = get_xml_argument(scxml_type, xml_tree, tag_name, none_allowed=True) + if read_value is None: + read_value = read_value_from_xml_child(xml_tree, tag_name, valid_types) + if not none_allowed: + assert read_value is not None, \ + f"Error: SCXML conversion of {scxml_type.get_tag_name()}: Missing argument {tag_name}." + return read_value diff --git a/scxml_converter/test/_test_data/.gitignore b/scxml_converter/test/_test_data/.gitignore new file mode 100644 index 00000000..5b0a2740 --- /dev/null +++ b/scxml_converter/test/_test_data/.gitignore @@ -0,0 +1 @@ +*/output \ No newline at end of file diff --git a/scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml b/scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml new file mode 100644 index 00000000..66d8237b --- /dev/null +++ b/scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml @@ -0,0 +1,34 @@ + + + + + + + + + + /adder + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml new file mode 100644 index 00000000..b5c3d0c4 --- /dev/null +++ b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml new file mode 100644 index 00000000..f0a5c913 --- /dev/null +++ b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/add_int_srv_example/server.scxml b/scxml_converter/test/_test_data/add_int_srv_example/server.scxml new file mode 100644 index 00000000..8200ad63 --- /dev/null +++ b/scxml_converter/test/_test_data/add_int_srv_example/server.scxml @@ -0,0 +1,27 @@ + + + + + + + + + + /adder + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/battery_drainer.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/battery_drainer.scxml new file mode 100644 index 00000000..fb85f977 --- /dev/null +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/battery_drainer.scxml @@ -0,0 +1,33 @@ + + + + + + + + + charge + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/input_files/battery_manager.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/battery_manager.scxml similarity index 81% rename from scxml_converter/test/_test_data/input_files/battery_manager.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/battery_manager.scxml index 1b092d32..e0291ef8 100644 --- a/scxml_converter/test/_test_data/input_files/battery_manager.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/battery_manager.scxml @@ -10,8 +10,8 @@ - - + + diff --git a/scxml_converter/test/_test_data/input_files/bt.xml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml similarity index 100% rename from scxml_converter/test/_test_data/input_files/bt.xml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml diff --git a/scxml_converter/test/_test_data/input_files/bt_topic_action.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml similarity index 93% rename from scxml_converter/test/_test_data/input_files/bt_topic_action.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml index ca15edbc..53736c4f 100644 --- a/scxml_converter/test/_test_data/input_files/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml @@ -13,7 +13,7 @@ - + diff --git a/scxml_converter/test/_test_data/input_files/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml similarity index 86% rename from scxml_converter/test/_test_data/input_files/bt_topic_condition.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml index 4fa248cd..cd91b1a0 100644 --- a/scxml_converter/test/_test_data/input_files/bt_topic_condition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml @@ -10,13 +10,13 @@ - + - + diff --git a/scxml_converter/test/_test_data/expected_output_bt_and_plugins/10000_TopicCondition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml similarity index 83% rename from scxml_converter/test/_test_data/expected_output_bt_and_plugins/10000_TopicCondition.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml index f458bfc9..7e07abd3 100644 --- a/scxml_converter/test/_test_data/expected_output_bt_and_plugins/10000_TopicCondition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml @@ -2,7 +2,7 @@ @@ -10,13 +10,13 @@ - + - + diff --git a/scxml_converter/test/_test_data/expected_output_bt_and_plugins/1001_TopicAction.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml similarity index 79% rename from scxml_converter/test/_test_data/expected_output_bt_and_plugins/1001_TopicAction.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml index b42be1e0..f76b81f0 100644 --- a/scxml_converter/test/_test_data/expected_output_bt_and_plugins/1001_TopicAction.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml @@ -2,18 +2,18 @@ - + - + diff --git a/scxml_converter/test/_test_data/expected_output_bt_and_plugins/bt.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml similarity index 100% rename from scxml_converter/test/_test_data/expected_output_bt_and_plugins/bt.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml diff --git a/scxml_converter/test/_test_data/input_files/battery_drainer.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/battery_drainer.scxml similarity index 78% rename from scxml_converter/test/_test_data/input_files/battery_drainer.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/battery_drainer.scxml index f38f1ff2..71315301 100644 --- a/scxml_converter/test/_test_data/input_files/battery_drainer.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/battery_drainer.scxml @@ -10,21 +10,21 @@ - + - + - + - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml new file mode 100644 index 00000000..cd91b1a0 --- /dev/null +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/expected_output_ros_to_scxml/battery_drainer.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml similarity index 100% rename from scxml_converter/test/_test_data/expected_output_ros_to_scxml/battery_drainer.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml diff --git a/scxml_converter/test/_test_data/expected_output_ros_to_scxml/battery_manager.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml similarity index 100% rename from scxml_converter/test/_test_data/expected_output_ros_to_scxml/battery_manager.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml diff --git a/scxml_converter/test/_test_data/expected_output_ros_to_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml similarity index 100% rename from scxml_converter/test/_test_data/expected_output_ros_to_scxml/bt_topic_action.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml diff --git a/scxml_converter/test/_test_data/expected_output_ros_to_scxml/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml similarity index 100% rename from scxml_converter/test/_test_data/expected_output_ros_to_scxml/bt_topic_condition.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml diff --git a/scxml_converter/test/_test_data/bt_ports_blackboard/bt.xml b/scxml_converter/test/_test_data/bt_ports_blackboard/bt.xml new file mode 100644 index 00000000..dae21f3f --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_blackboard/bt.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_blackboard/bt_get_number.scxml b/scxml_converter/test/_test_data/bt_ports_blackboard/bt_get_number.scxml new file mode 100644 index 00000000..2752d317 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_blackboard/bt_get_number.scxml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/bt_ports_blackboard/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_blackboard/bt_topic_action.scxml new file mode 100644 index 00000000..0f935e09 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_blackboard/bt_topic_action.scxml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/bt.xml b/scxml_converter/test/_test_data/bt_ports_only/bt.xml new file mode 100644 index 00000000..f690a4d4 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/bt.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml new file mode 100644 index 00000000..4dbce6c5 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml new file mode 100644 index 00000000..8012bf94 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml new file mode 100644 index 00000000..ffbcd88a --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml new file mode 100644 index 00000000..05cc4fb6 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml new file mode 100644 index 00000000..c29c0222 --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml new file mode 100644 index 00000000..7ca960ef --- /dev/null +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/input_files/invalid_xmls/battery_drainer.scxml b/scxml_converter/test/_test_data/invalid_xmls/battery_drainer.scxml similarity index 100% rename from scxml_converter/test/_test_data/input_files/invalid_xmls/battery_drainer.scxml rename to scxml_converter/test/_test_data/invalid_xmls/battery_drainer.scxml diff --git a/scxml_converter/test/_test_data/input_files/invalid_xmls/bt_topic_action.scxml b/scxml_converter/test/_test_data/invalid_xmls/bt_topic_action.scxml similarity index 100% rename from scxml_converter/test/_test_data/input_files/invalid_xmls/bt_topic_action.scxml rename to scxml_converter/test/_test_data/invalid_xmls/bt_topic_action.scxml diff --git a/scxml_converter/test/test_systemtest_scxml_entries.py b/scxml_converter/test/test_systemtest_scxml_entries.py index 2f79400a..c0fc9a02 100644 --- a/scxml_converter/test/test_systemtest_scxml_entries.py +++ b/scxml_converter/test/test_systemtest_scxml_entries.py @@ -17,37 +17,43 @@ from test_utils import canonicalize_xml, remove_empty_lines -from scxml_converter.scxml_entries import (RosField, RosRateCallback, - RosTimeRate, RosTopicCallback, - RosTopicPublish, RosTopicPublisher, - RosTopicSubscriber, ScxmlAssign, - ScxmlData, ScxmlDataModel, - ScxmlParam, ScxmlRoot, ScxmlSend, - ScxmlState, ScxmlTransition) +from scxml_converter.scxml_entries import ( + RosField, RosRateCallback, RosTimeRate, RosTopicCallback, RosTopicPublish, RosTopicPublisher, + RosTopicSubscriber, ScxmlAssign, ScxmlData, ScxmlDataModel, ScxmlParam, ScxmlRoot, ScxmlSend, + ScxmlState, ScxmlTransition, BtInputPortDeclaration, BtGetValueInputPort) + + +def _test_scxml_from_code(scxml_root: ScxmlRoot, ref_file_path: str): + # Check output xml + with open(ref_file_path, 'r', encoding='utf-8') as f_o: + expected_output = f_o.read() + test_output = scxml_root.as_xml_string() + test_xml_string = remove_empty_lines(canonicalize_xml(test_output)) + ref_xml_string = remove_empty_lines(canonicalize_xml(expected_output)) + assert test_xml_string == ref_xml_string + + +def _test_xml_parsing(xml_file_path: str, valid_xml: bool = True): + scxml_root = ScxmlRoot.from_scxml_file(xml_file_path) + # Check output xml + if valid_xml: + test_output = scxml_root.as_xml_string() + test_xml_string = remove_empty_lines(canonicalize_xml(test_output)) + ref_file_path = os.path.join(os.path.dirname(xml_file_path), 'gt_parsed_scxml', + os.path.basename(xml_file_path)) + with open(ref_file_path, 'r', encoding='utf-8') as f_o: + ref_xml_string = remove_empty_lines(canonicalize_xml(f_o.read())) + assert test_xml_string == ref_xml_string + # All the test scxml files we are using contain ROS declarations + assert not scxml_root.is_plain_scxml() + else: + assert not scxml_root.check_validity() def test_battery_drainer_from_code(): """ Test for scxml_entries generation and conversion to xml. - - It should support the following xml tree: - - scxml - - state - - onentry - - {executable content} - - onexit - - {executable content} - - transition - - {executable content} - - datamodel - - data - - Executable content consists of the following entries: - - send - - param - - if / elseif / else - - assign -""" + """ battery_drainer_scxml = ScxmlRoot("BatteryDrainer") battery_drainer_scxml.set_data_model(ScxmlDataModel([ ScxmlData("battery_percent", "100", "int16")])) @@ -60,17 +66,9 @@ def test_battery_drainer_from_code(): ScxmlTransition("use_battery", ["ros_topic.charge"], body=[ScxmlAssign("battery_percent", "100")])]) battery_drainer_scxml.add_state(use_battery_state, initial=True) - # Check output xml - ref_file = os.path.join(os.path.dirname(__file__), '_test_data', - 'expected_output_ros_to_scxml', 'battery_drainer.scxml') - assert os.path.exists(ref_file), f"Cannot find ref. file {ref_file}." - with open(ref_file, 'r', encoding='utf-8') as f_o: - expected_output = f_o.read() - test_output = battery_drainer_scxml.as_xml_string() - test_xml_string = remove_empty_lines(canonicalize_xml(test_output)) - ref_xml_string = remove_empty_lines(canonicalize_xml(expected_output)) - assert test_xml_string == ref_xml_string - assert battery_drainer_scxml.is_plain_scxml() + _test_scxml_from_code(battery_drainer_scxml, os.path.join( + os.path.dirname(__file__), '_test_data', 'battery_drainer_w_bt', + 'gt_plain_scxml', 'battery_drainer.scxml')) def test_battery_drainer_ros_from_code(): @@ -100,8 +98,8 @@ def test_battery_drainer_ros_from_code(): battery_drainer_scxml = ScxmlRoot("BatteryDrainer") battery_drainer_scxml.set_data_model(ScxmlDataModel([ ScxmlData("battery_percent", "100", "int16")])) - ros_topic_sub = RosTopicSubscriber("charge", "std_msgs/Empty") - ros_topic_pub = RosTopicPublisher("level", "std_msgs/Int32") + ros_topic_sub = RosTopicSubscriber("charge", "std_msgs/Empty", "sub") + ros_topic_pub = RosTopicPublisher("level", "std_msgs/Int32", "pub") ros_timer = RosTimeRate("my_timer", 1) battery_drainer_scxml.add_ros_declaration(ros_topic_sub) battery_drainer_scxml.add_ros_declaration(ros_topic_pub) @@ -116,53 +114,54 @@ def test_battery_drainer_ros_from_code(): use_battery_state.add_transition( RosTopicCallback(ros_topic_sub, "use_battery", [ScxmlAssign("battery_percent", "100")])) battery_drainer_scxml.add_state(use_battery_state, initial=True) - - # Check output xml - ref_file = os.path.join(os.path.dirname(__file__), '_test_data', - 'input_files', 'battery_drainer.scxml') - assert os.path.exists(ref_file), f"Cannot find ref. file {ref_file}." - with open(ref_file, 'r', encoding='utf-8') as f_o: - expected_output = f_o.read() - test_output = battery_drainer_scxml.as_xml_string() - test_xml_string = remove_empty_lines(canonicalize_xml(test_output)) - ref_xml_string = remove_empty_lines(canonicalize_xml(expected_output)) - assert test_xml_string == ref_xml_string - assert not battery_drainer_scxml.is_plain_scxml() + _test_scxml_from_code(battery_drainer_scxml, os.path.join( + os.path.dirname(__file__), '_test_data', 'battery_drainer_w_bt', + 'gt_parsed_scxml', 'battery_drainer.scxml')) -def _test_xml_parsing(xml_file_path: str, valid_xml: bool = True): - # TODO: Input path to scxml file from args - scxml_root = ScxmlRoot.from_scxml_file(xml_file_path) - # Check output xml - if valid_xml: - test_output = scxml_root.as_xml_string() - test_xml_string = remove_empty_lines(canonicalize_xml(test_output)) - with open(xml_file_path, 'r', encoding='utf-8') as f_o: - ref_xml_string = remove_empty_lines(canonicalize_xml(f_o.read())) - assert test_xml_string == ref_xml_string - # All the test scxml files we are using contain ROS declarations - assert not scxml_root.is_plain_scxml() - else: - assert not scxml_root.check_validity() +def test_bt_action_with_ports_from_code(): + """ + Test for generating scxml code containing BT Ports + """ + data_model = ScxmlDataModel([ScxmlData("number", "0", "int16")]) + topic_publisher = RosTopicPublisher(BtGetValueInputPort("name"), "std_msgs/Int16", "answer_pub") + init_state = ScxmlState("initial", body=[ + ScxmlTransition("initial", ["bt_tick"], None, [ + ScxmlAssign("number", BtGetValueInputPort("data")), + RosTopicPublish(topic_publisher, [RosField("data", "number")]) + ]) + ]) + scxml_root = ScxmlRoot("TopicAction") + scxml_root.set_data_model(data_model) + scxml_root.add_bt_port_declaration(BtInputPortDeclaration("name", "string")) + scxml_root.add_bt_port_declaration(BtInputPortDeclaration("data", "int16")) + scxml_root.add_ros_declaration(topic_publisher) + scxml_root.add_state(init_state, initial=True) + assert not scxml_root.check_validity(), "Currently, we handle unspecified BT entries as invalid" + scxml_root.set_bt_ports_values([("name", "/sys/add_srv"), ("data", "25")]) + scxml_root.update_bt_ports_values() + _test_scxml_from_code(scxml_root, os.path.join( + os.path.dirname(__file__), '_test_data', 'bt_ports_only', + 'gt_parsed_scxml', 'bt_topic_action.scxml')) def test_xml_parsing_battery_drainer(): _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', - 'input_files', 'battery_drainer.scxml')) + 'battery_drainer_w_bt', 'battery_drainer.scxml')) def test_xml_parsing_bt_topic_condition(): _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', - 'input_files', 'bt_topic_condition.scxml')) + 'battery_drainer_w_bt', 'bt_topic_condition.scxml')) def test_xml_parsing_invalid_battery_drainer_xml(): - _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', 'input_files', + _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', 'invalid_xmls', 'battery_drainer.scxml'), valid_xml=False) def test_xml_parsing_invalid_bt_topic_action_xml(): - _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', 'input_files', + _test_xml_parsing(os.path.join(os.path.dirname(__file__), '_test_data', 'invalid_xmls', 'bt_topic_action.scxml'), valid_xml=False) diff --git a/scxml_converter/test/test_systemtest_xml.py b/scxml_converter/test/test_systemtest_xml.py index 42634141..8dcaa234 100644 --- a/scxml_converter/test/test_systemtest_xml.py +++ b/scxml_converter/test/test_systemtest_xml.py @@ -15,18 +15,20 @@ import os +from typing import Dict, List, Tuple + from test_utils import canonicalize_xml, remove_empty_lines from scxml_converter.bt_converter import bt_converter from scxml_converter.scxml_entries import ScxmlRoot -def get_output_folder(): - return os.path.join(os.path.dirname(__file__), 'output') +def get_output_folder(test_folder: str): + return os.path.join(os.path.dirname(__file__), '_test_data', test_folder, 'output') -def clear_output_folder(): - output_folder = get_output_folder() +def clear_output_folder(test_folder: str): + output_folder = get_output_folder(test_folder) if os.path.exists(output_folder): for f in os.listdir(output_folder): os.remove(os.path.join(output_folder, f)) @@ -34,57 +36,89 @@ def clear_output_folder(): os.makedirs(output_folder) -def test_ros_scxml_to_plain_scxml(): +def bt_to_scxml_test( + test_folder: str, bt_file: str, bt_plugins: List[str], store_generated: bool = False): + """ + Test the conversion of a BT to SCXML. + + :param test_folder: The name of the folder with the files to evaluate. + :param bt_file: The name to the BT xml file. + :param bt_plugins: The names of the BT plugins scxml files. + :param store_generated: If True, the generated SCXML files are stored in the output folder. + """ + test_data_path = os.path.join(os.path.dirname(__file__), '_test_data', test_folder) + bt_file = os.path.join(test_data_path, bt_file) + plugin_files = [os.path.join(test_data_path, f) for f in bt_plugins] + scxml_objs = bt_converter(bt_file, plugin_files) + assert len(scxml_objs) == 3, \ + f"Expecting 3 scxml objects, found {len(scxml_objs)}." + if store_generated: + clear_output_folder(test_folder) + for scxml_obj in scxml_objs: + output_file = os.path.join( + get_output_folder(test_folder), f'{scxml_obj.get_name()}.scxml') + with open(output_file, 'w') as f_o: + f_o.write(scxml_obj.as_xml_string()) + for scxml_root in scxml_objs: + scxml_name = scxml_root.get_name() + gt_scxml_path = os.path.join(test_data_path, 'gt_bt_scxml', + f'{scxml_name}.scxml') + with open(gt_scxml_path, 'r', encoding='utf-8') as f_o: + gt_xml = remove_empty_lines(canonicalize_xml(f_o.read())) + scxml_xml = remove_empty_lines(canonicalize_xml(scxml_root.as_xml_string())) + + assert scxml_xml == gt_xml + + +def ros_to_plain_scxml_test(test_folder: str, + scxml_bt_ports: Dict[str, List[Tuple[str, str]]], + store_generated: bool = False): """Test the conversion of SCXML with ROS-specific macros to plain SCXML.""" - clear_output_folder() - scxml_files = [file for file in os.listdir( - os.path.join(os.path.dirname(__file__), '_test_data', 'input_files') - ) if file.endswith('.scxml')] + test_data_path = os.path.join(os.path.dirname(__file__), '_test_data', test_folder) + scxml_files = [file for file in os.listdir(test_data_path) if file.endswith('.scxml')] + if store_generated: + clear_output_folder(test_folder) for fname in scxml_files: - input_file = os.path.join(os.path.dirname(__file__), - '_test_data', 'input_files', fname) - output_file = os.path.join(os.path.dirname(__file__), - '_test_data', 'expected_output_ros_to_scxml', fname) + input_file = os.path.join(test_data_path, fname) + gt_file = os.path.join(test_data_path, 'gt_plain_scxml', fname) try: - scxml, _ = ScxmlRoot.from_scxml_file(input_file).to_plain_scxml_and_declarations() - scxml_str = scxml.as_xml_string() - with open(output_file, 'r', encoding='utf-8') as f_o: - expected_output = f_o.read() + scxml_obj = ScxmlRoot.from_scxml_file(input_file) + if fname in scxml_bt_ports: + scxml_obj.set_bt_ports_values(scxml_bt_ports[fname]) + scxml_obj.update_bt_ports_values() + plain_scxml, _ = scxml_obj.to_plain_scxml_and_declarations() + if store_generated: + output_file = os.path.join(get_output_folder(test_folder), fname) + with open(output_file, 'w') as f_o: + f_o.write(plain_scxml.as_xml_string()) + scxml_str = plain_scxml.as_xml_string() + with open(gt_file, 'r', encoding='utf-8') as f_o: + gt_output = f_o.read() assert remove_empty_lines(canonicalize_xml(scxml_str)) == \ - remove_empty_lines(canonicalize_xml(expected_output)) + remove_empty_lines(canonicalize_xml(gt_output)) except Exception as e: - clear_output_folder() print(f"Error in file {fname}:") raise e - clear_output_folder() - - -def test_bt_to_scxml(): - clear_output_folder() - input_file = os.path.join( - os.path.dirname(__file__), '_test_data', 'input_files', 'bt.xml') - output_file_bt = os.path.join(get_output_folder(), 'bt.scxml') - plugins = [os.path.join(os.path.dirname(__file__), - '_test_data', 'input_files', f) - for f in ['bt_topic_action.scxml', 'bt_topic_condition.scxml']] - bt_converter(input_file, plugins, get_output_folder()) - files = os.listdir(get_output_folder()) - assert len(files) == 3, \ - f"Expecting 3 files, found {len(files)}" - # 1 for the main BT and 2 for the plugins - assert os.path.exists(output_file_bt), \ - f"Expecting {output_file_bt} to exist, but it does not." - for fname in files: - with open(os.path.join(get_output_folder(), fname), 'r', encoding='utf-8') as f_o: - output = f_o.read() - with open(os.path.join( - os.path.dirname(__file__), '_test_data', 'expected_output_bt_and_plugins', fname - ), 'r', encoding='utf-8') as f_o: - expected_output = f_o.read() - assert remove_empty_lines(canonicalize_xml(output)) == \ - remove_empty_lines(canonicalize_xml(expected_output)) - clear_output_folder() - - -if __name__ == '__main__': - test_ros_scxml_to_plain_scxml() + + +def test_bt_to_scxml_battery_drainer(): + bt_to_scxml_test('battery_drainer_w_bt', 'bt.xml', + ['bt_topic_action.scxml', 'bt_topic_condition.scxml'], False) + + +def test_ros_to_plain_scxml_battery_drainer(): + ros_to_plain_scxml_test('battery_drainer_w_bt', {}, True) + + +def test_bt_to_scxml_bt_ports(): + bt_to_scxml_test('bt_ports_only', 'bt.xml', ['bt_topic_action.scxml'], False) + + +def test_ros_to_plain_scxml_bt_ports(): + ros_to_plain_scxml_test('bt_ports_only', + {'bt_topic_action.scxml': [('name', 'out'), ('data', '123')]}, + True) + + +def test_ros_to_plain_scxml_add_int_srv(): + ros_to_plain_scxml_test('add_int_srv_example', {}, True) From 71fcbd3d90a39ad5cc96cf975eed2e2e5090c43c Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 16:41:03 +0200 Subject: [PATCH 02/49] Support for Arrays on Jani entries and example for ROS actions in SCXML Signed-off-by: Christian Henkel Signed-off-by: Marco Lampacrescia --- .../convince_to_plain_jani.py | 29 +- .../jani_entries/jani_automaton.py | 11 +- .../jani_entries/jani_composition.py | 4 + .../jani_entries/jani_constant.py | 49 +- .../jani_convince_expression_expansion.py | 5 +- .../jani_generator/jani_entries/jani_edge.py | 2 +- .../jani_entries/jani_expression.py | 15 +- .../jani_entries/jani_expression_generator.py | 22 + .../jani_generator/jani_entries/jani_guard.py | 10 +- .../jani_generator/jani_entries/jani_model.py | 31 + .../jani_entries/jani_property.py | 10 +- .../jani_entries/jani_variable.py | 76 +- .../plain_jani_examples/example_arrays.jani | 2193 +++++++++++++++++ .../ros_fibonacci_action_example/.gitignore | 3 + .../client_1.scxml | 45 + .../client_2.scxml | 45 + .../happy_clients.jani | 33 + .../ros_fibonacci_action_example/main.xml | 15 + .../ros_fibonacci_action_example/server.scxml | 94 + .../server_execute.scxml | 57 + .../test/test_unittest_jani_model_loading.py | 34 + 21 files changed, 2736 insertions(+), 47 deletions(-) create mode 100644 jani_generator/test/_test_data/plain_jani_examples/example_arrays.jani create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/.gitignore create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/client_2.scxml create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/happy_clients.jani create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/main.xml create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml create mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml create mode 100644 jani_generator/test/test_unittest_jani_model_loading.py diff --git a/jani_generator/src/jani_generator/convince_jani_helpers/convince_to_plain_jani.py b/jani_generator/src/jani_generator/convince_jani_helpers/convince_to_plain_jani.py index 0b98ab73..e63d00eb 100644 --- a/jani_generator/src/jani_generator/convince_jani_helpers/convince_to_plain_jani.py +++ b/jani_generator/src/jani_generator/convince_jani_helpers/convince_to_plain_jani.py @@ -37,7 +37,7 @@ def to_deg(value: float) -> int: def __convince_env_model_to_jani(base_model: JaniModel, env_model: dict): - """Add the converted entries from the convince environment model to the provided JaniModel object.""" + """Add the converted entries from the convince environment model to the provided JaniModel.""" # Check if the base_model is a JaniModel instance assert isinstance(base_model, JaniModel), "The base_model should be a JaniModel instance" # Check if the env_model is a dictionary @@ -61,7 +61,8 @@ def __convince_env_model_to_jani(base_model: JaniModel, env_model: dict): # The robot pose should be stored using integers -> centimeters and degrees base_model.add_variable(f"robots.{robot_name}.pose.x_cm", int, to_cm(robot_pose["x"])) base_model.add_variable(f"robots.{robot_name}.pose.y_cm", int, to_cm(robot_pose["y"])) - base_model.add_variable(f"robots.{robot_name}.pose.theta_deg", int, to_deg(robot_pose["theta"])) + base_model.add_variable(f"robots.{robot_name}.pose.theta_deg", int, + to_deg(robot_pose["theta"])) base_model.add_variable(f"robots.{robot_name}.pose.x", float, transient=True) base_model.add_variable(f"robots.{robot_name}.pose.y", float, transient=True) base_model.add_variable(f"robots.{robot_name}.pose.theta", float, transient=True) @@ -69,17 +70,23 @@ def __convince_env_model_to_jani(base_model: JaniModel, env_model: dict): base_model.add_variable(f"robots.{robot_name}.goal.y", float, transient=True) base_model.add_variable(f"robots.{robot_name}.goal.theta", float, transient=True) robot_shape = robot["shape"] - base_model.add_constant(f"robots.{robot_name}.shape.radius", float, float(robot_shape["radius"])) - base_model.add_constant(f"robots.{robot_name}.shape.height", float, float(robot_shape["height"])) - base_model.add_constant(f"robots.{robot_name}.linear_velocity", float, float(robot["linear_velocity"])) - base_model.add_constant(f"robots.{robot_name}.angular_velocity", float, float(robot["angular_velocity"])) + base_model.add_constant(f"robots.{robot_name}.shape.radius", float, + float(robot_shape["radius"])) + base_model.add_constant(f"robots.{robot_name}.shape.height", float, + float(robot_shape["height"])) + base_model.add_constant(f"robots.{robot_name}.linear_velocity", float, + float(robot["linear_velocity"])) + base_model.add_constant(f"robots.{robot_name}.angular_velocity", float, + float(robot["angular_velocity"])) if "obstacles" in env_model: # Extract the obstacles from the env_model # TODO pass # TODO: Discuss the perception part - # TODO: Discuss the possibility of generating a base automata for each robot + standard edges (i.e. switch_on/off, drive, rotate) - # This would make sense, allowing the definition of default mobile robots without the need of defining how they drive. + # TODO: Discuss the possibility of generating a base automata for each robot + standard edges + # (i.e. switch_on/off, drive, rotate) + # This would make sense, allowing the definition of default mobile robots without the need of + # defining how they drive. # By the way, keeping this out for now! @@ -104,7 +111,8 @@ def __convince_properties_to_jani(base_model: JaniModel, properties: List[dict]) assert isinstance(base_model, JaniModel), "The base_model should be a JaniModel instance" for property_dict in properties: assert isinstance(property_dict, dict), "The properties list should contain dictionaries" - base_model.add_jani_property(JaniProperty(property_dict["name"], property_dict["expression"])) + base_model.add_jani_property(JaniProperty(property_dict["name"], + property_dict["expression"])) def convince_jani_parser(base_model: JaniModel, convince_jani_path: str): @@ -119,7 +127,8 @@ def convince_jani_parser(base_model: JaniModel, convince_jani_path: str): # ---- Metadata ---- base_model.set_name(convince_jani_json["name"]) # Make sure we are loading a convince-jani file - assert "features" in convince_jani_json and "convince_extensions" in convince_jani_json["features"], \ + assert "features" in convince_jani_json and \ + "convince_extensions" in convince_jani_json["features"], \ "The provided file is not a convince-jani file (missing feature entry)" # Extract the environment model from the convince-jani file # ---- Environment Model ---- diff --git a/jani_generator/src/jani_generator/jani_entries/jani_automaton.py b/jani_generator/src/jani_generator/jani_entries/jani_automaton.py index 55f112de..b8868276 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_automaton.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_automaton.py @@ -22,6 +22,10 @@ class JaniAutomaton: + @staticmethod + def from_dict(automaton_dict: dict) -> "JaniAutomaton": + return JaniAutomaton(automaton_dict=automaton_dict) + def __init__(self, *, automaton_dict: Optional[Dict[str, Any]] = None): self._locations: Set[str] = set() self._initial_locations: Set[str] = set() @@ -34,7 +38,7 @@ def __init__(self, *, automaton_dict: Optional[Dict[str, Any]] = None): self._name = automaton_dict["name"] self._generate_locations( automaton_dict["locations"], automaton_dict["initial-locations"]) - self._generate_variables(automaton_dict["variables"]) + self._generate_variables(automaton_dict.get("variables", [])) self._generate_edges(automaton_dict["edges"]) def get_name(self): @@ -86,7 +90,8 @@ def remove_empty_self_loop_edges(self): """Remove all self-loop edges from the automaton.""" self._edges = [edge for edge in self._edges if not edge.is_empty_self_loop()] - def _generate_locations(self, location_list: List[Dict[str, Any]], initial_locations: List[str]): + def _generate_locations(self, + location_list: List[Dict[str, Any]], initial_locations: List[str]): for location in location_list: self._locations.add(location["name"]) for init_location in initial_locations: @@ -100,7 +105,7 @@ def _generate_variables(self, variable_list: List[dict]): is_transient = False if "transient" in variable: is_transient = variable["transient"] - var_type = JaniVariable.jani_type_from_string(variable["type"]) + var_type = JaniVariable.python_type_from_json(variable["type"]) self._local_variables.update({variable["name"]: JaniVariable( variable["name"], var_type, init_expr, is_transient)}) diff --git a/jani_generator/src/jani_generator/jani_entries/jani_composition.py b/jani_generator/src/jani_generator/jani_entries/jani_composition.py index 75a5ebf4..51543515 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_composition.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_composition.py @@ -19,6 +19,10 @@ class JaniComposition: + @staticmethod + def from_dict(composition_dict: dict) -> "JaniComposition": + return JaniComposition(composition_dict=composition_dict) + def __init__(self, composition_dict: Optional[Dict[str, Any]] = None): if composition_dict is None: self._elements = [] diff --git a/jani_generator/src/jani_generator/jani_entries/jani_constant.py b/jani_generator/src/jani_generator/jani_entries/jani_constant.py index 7031ced0..a6bfaeec 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_constant.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_constant.py @@ -15,15 +15,38 @@ """A constant value expression.""" -from typing import Type, Union, get_args +from typing import Type, Optional, Union, get_args -from jani_generator.jani_entries import JaniExpression, JaniValue +from jani_generator.jani_entries import JaniExpression ValidTypes = Union[bool, int, float] class JaniConstant: - def __init__(self, c_name: str, c_type: Type, c_value: JaniExpression): + @staticmethod + def from_dict(constant_dict: dict) -> "JaniConstant": + constant_name = constant_dict["name"] + constant_type = JaniConstant.jani_type_from_string(constant_dict["type"]) + constant_value = constant_dict.get("value", None) + if constant_value is None: + return JaniConstant(constant_name, constant_type, None) + if isinstance(constant_value, str): + # Check if conversion from string to constant_type is possible + try: + const_value_cast = constant_type(constant_value) + return JaniConstant(constant_name, + constant_type, + JaniExpression(const_value_cast)) + except ValueError: + # If no conversion possible, raise an error (constant names are not supported) + raise ValueError( + f"Value {constant_value} for constant {constant_name} " + f"is not a valid value for type {constant_type}.") + return JaniConstant(constant_name, + constant_type, + JaniExpression(constant_value)) + + def __init__(self, c_name: str, c_type: Type, c_value: Optional[JaniExpression]): assert isinstance(c_value, JaniExpression), "Value should be a JaniExpression" assert c_type in get_args(ValidTypes), f"Type {c_type} not supported by Jani" self._name = c_name @@ -33,10 +56,12 @@ def __init__(self, c_name: str, c_type: Type, c_value: JaniExpression): def name(self) -> str: return self._name - def value(self) -> ValidTypes: - assert self._value is not None, "Value not set" + def value(self) -> Optional[ValidTypes]: + if self._value is None: + return None jani_value = self._value.value - assert jani_value is not None and jani_value.is_valid(), "The expression can't be evaluated to a constant value" + assert jani_value.is_valid(), \ + "The expression can't be evaluated to a constant value" return jani_value.value() @staticmethod @@ -61,8 +86,8 @@ def jani_type_to_string(c_type: Type[ValidTypes]) -> str: // Types // We cover only the most basic types at the moment. - // In the remainder of the specification, all requirements like "y must be of type x" are to be interpreted - // as "type x must be assignable from y's type". + // In the remainder of the specification, all requirements like "y must be of type x" are + // to be interpreted as "type x must be assignable from y's type". var BasicType = schema([ "bool", // assignable from bool "int", // numeric; assignable from int and bounded int @@ -81,8 +106,10 @@ def jani_type_to_string(c_type: Type[ValidTypes]) -> str: raise ValueError(f"Type {c_type} not supported by Jani") def as_dict(self): - return { + const_dict = { "name": self._name, - "type": JaniConstant.jani_type_to_string(self._type), - "value": self._value.as_dict() + "type": JaniConstant.jani_type_to_string(self._type) } + if self._value is not None: + const_dict["value"] = self._value.as_dict() + return const_dict diff --git a/jani_generator/src/jani_generator/jani_entries/jani_convince_expression_expansion.py b/jani_generator/src/jani_generator/jani_entries/jani_convince_expression_expansion.py index 9817dd6e..e062f15a 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_convince_expression_expansion.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_convince_expression_expansion.py @@ -59,6 +59,8 @@ "ite": "ite", "⇒": "⇒", "=>": "⇒", + "aa": "aa", + "ac": "ac", } @@ -391,7 +393,8 @@ def __substitute_expression_op(expression: JaniExpression) -> JaniExpression: def expand_expression( - expression: Union[JaniExpression, JaniValue], jani_constants: Dict[str, JaniConstant]) -> JaniExpression: + expression: Union[JaniExpression, JaniValue], + jani_constants: Dict[str, JaniConstant]) -> JaniExpression: # Given a CONVINCE JaniExpression, expand it to a plain JaniExpression assert isinstance(expression, JaniExpression), \ f"The expression should be a JaniExpression instance, found {type(expression)} instead." diff --git a/jani_generator/src/jani_generator/jani_entries/jani_edge.py b/jani_generator/src/jani_generator/jani_entries/jani_edge.py index e50a67f0..c3bc6f6a 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_edge.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_edge.py @@ -31,7 +31,7 @@ def __init__(self, edge_dict: dict): self.action = edge_dict["action"] self.guard = None if "guard" in edge_dict: - self.guard = JaniGuard(edge_dict["guard"]) + self.guard = JaniGuard.from_dict(edge_dict["guard"]) self.destinations = [] for dest in edge_dict["destinations"]: jani_destination = { diff --git a/jani_generator/src/jani_generator/jani_entries/jani_expression.py b/jani_generator/src/jani_generator/jani_entries/jani_expression.py index 0e161ccd..a15cfdea 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_expression.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_expression.py @@ -49,8 +49,8 @@ def __init__(self, expression: Union[SupportedExp, 'JaniExpression', JaniValue]) elif isinstance(expression, JaniValue): self.value = expression else: - if (not isinstance(expression, SupportedExp)): # type: ignore - raise RuntimeError(f"Unexpected expression type: {type(expression)} should be a dict or a base type.") + assert isinstance(expression, SupportedExp), \ + f"Unexpected expression type: {type(expression)} should be a dict or a base type." if isinstance(expression, str): # If it is a reference to a constant or variable, we do not need to expand further self.identifier = expression @@ -100,6 +100,17 @@ def _get_operands(self, expression_dict: dict): "if": JaniExpression(expression_dict["if"]), "then": JaniExpression(expression_dict["then"]), "else": JaniExpression(expression_dict["else"])} + # Array-specific expressions + if (self.op == "ac"): + return { + "var": JaniExpression(expression_dict["var"]), + "length": JaniExpression(expression_dict["length"]), + "exp": JaniExpression(expression_dict["exp"])} + if (self.op == "aa"): + return { + "exp": JaniExpression(expression_dict["exp"]), + "index": JaniExpression(expression_dict["index"])} + # Convince specific expressions if (self.op in ("norm2d")): return { "x": JaniExpression(expression_dict["x"]), diff --git a/jani_generator/src/jani_generator/jani_entries/jani_expression_generator.py b/jani_generator/src/jani_generator/jani_entries/jani_expression_generator.py index 5ce6e68c..347f5f68 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_expression_generator.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_expression_generator.py @@ -112,3 +112,25 @@ def or_operator(left, right) -> JaniExpression: # if operator def if_operator(condition, true_value, false_value) -> JaniExpression: return JaniExpression({"op": "ite", "if": condition, "then": true_value, "else": false_value}) + + +# array operators +def array_create_operator(var, length, exp) -> JaniExpression: + """ + Generate an array initialization expression + + :param var: The name of an int variable, used to iterate over the array indexes + :param length: The length of the array + :param exp: The expression to initialize the array with, based on the variable in var + """ + return JaniExpression({"op": "ac", "var": var, "length": length, "exp": exp}) + + +def array_access_operator(exp, index) -> JaniExpression: + """ + Generate an array access expression + + :param exp: The array variable to access + :param index: The index to access on exp + """ + return JaniExpression({"op": "aa", "exp": exp, "index": index}) diff --git a/jani_generator/src/jani_generator/jani_entries/jani_guard.py b/jani_generator/src/jani_generator/jani_entries/jani_guard.py index cf5c4d49..cb6ce9cb 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_guard.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_guard.py @@ -24,10 +24,18 @@ class JaniGuard: + @staticmethod + def from_dict(guard_dict: dict) -> "JaniGuard": + assert isinstance(guard_dict, dict), f"Unexpected type {type(guard_dict)} for guard_dict." + assert "exp" in guard_dict, "Missing 'exp' key in guard_dict." + return JaniGuard(JaniExpression(guard_dict["exp"])) + def __init__(self, expression: Optional[JaniExpression]): + assert expression is None or isinstance(expression, JaniExpression), \ + f"Unexpected expression type: {type(expression)} should be a JaniExpression." self.expression = expression - def as_dict(self, constants: Optional[dict] = None): + def as_dict(self, _: Optional[dict] = None): d = {} if self.expression: exp = self.expression.as_dict() diff --git a/jani_generator/src/jani_generator/jani_entries/jani_model.py b/jani_generator/src/jani_generator/jani_entries/jani_model.py index 8dfc5e59..aaf29fc2 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_model.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_model.py @@ -32,9 +32,27 @@ class JaniModel: Class representing a complete Jani Model, containing all necessary information to generate a plain Jani file. """ + @staticmethod + def from_dict(model_dict: dict) -> "JaniModel": + model = JaniModel() + model.set_name(model_dict["name"]) + for feature in model_dict.get("features", []): + model.add_feature(feature) + for variable_dict in model_dict["variables"]: + model.add_jani_variable(JaniVariable.from_dict(variable_dict)) + for constant_dict in model_dict["constants"]: + model.add_jani_constant(JaniConstant.from_dict(constant_dict)) + for automaton_dict in model_dict["automata"]: + model.add_jani_automaton(JaniAutomaton.from_dict(automaton_dict)) + model.add_system_sync(JaniComposition.from_dict(model_dict["system"])) + for property_dict in model_dict["properties"]: + model.add_jani_property(JaniProperty.from_dict(property_dict)) + return model + def __init__(self): self._name = "" self._type = "mdp" + self._features: List[str] = [] self._variables: Dict[str, JaniVariable] = {} self._constants: Dict[str, JaniConstant] = {} self._automata: List[JaniAutomaton] = [] @@ -48,6 +66,13 @@ def set_name(self, name: str): def get_name(self): return self._name + def add_feature(self, feature: str): + assert feature in ["arrays", "trigonometric-functions"], f"Unknown Jani feature {feature}" + self._features.append(feature) + + def get_features(self) -> List[str]: + return self._features + def add_jani_variable(self, variable: JaniVariable): self._variables.update({variable.name(): variable}) @@ -82,6 +107,12 @@ def add_jani_automaton(self, automaton: JaniAutomaton): def get_automata(self) -> List[JaniAutomaton]: return self._automata + def get_constants(self) -> Dict[str, JaniConstant]: + return self._constants + + def get_variables(self) -> Dict[str, JaniVariable]: + return self._variables + def get_automaton(self, automaton_name: str) -> Optional[JaniAutomaton]: for automaton in self._automata: if automaton._name == automaton_name: diff --git a/jani_generator/src/jani_generator/jani_entries/jani_property.py b/jani_generator/src/jani_generator/jani_entries/jani_property.py index 1a0e3d86..e500b46d 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_property.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_property.py @@ -29,14 +29,16 @@ class FilterProperty: """All Property operators must occur in a FilterProperty object.""" def __init__(self, property_filter_exp: Dict[str, Any]): assert isinstance(property_filter_exp, dict), "Unexpected FilterProperty initialization" - assert "op" in property_filter_exp and property_filter_exp["op"] == "filter", "Unexpected FilterProperty initialization" + assert "op" in property_filter_exp and property_filter_exp["op"] == "filter", \ + "Unexpected FilterProperty initialization" self._fun = property_filter_exp["fun"] raw_states = property_filter_exp["states"] assert isinstance(raw_states, dict) and raw_states["op"] == "initial" self._process_values(property_filter_exp["values"]) def _process_values(self, prop_values: Dict[str, Any]) -> None: - self._values: Union[ProbabilityProperty, RewardProperty, NumPathsProperty] = ProbabilityProperty(prop_values) + self._values: Union[ProbabilityProperty, RewardProperty, NumPathsProperty] = \ + ProbabilityProperty(prop_values) if self._values.is_valid(): return self._values = RewardProperty(prop_values) @@ -153,6 +155,10 @@ def as_dict(self, constants: Dict[str, JaniConstant]): class JaniProperty: + @staticmethod + def from_dict(property_dict: dict) -> "JaniProperty": + return JaniProperty(property_dict["name"], property_dict["expression"]) + def __init__(self, name, expression): self._name = name # TODO: For now copy as it is. Later we might expand it to support more functionalities diff --git a/jani_generator/src/jani_generator/jani_entries/jani_variable.py b/jani_generator/src/jani_generator/jani_entries/jani_variable.py index 93ec8e86..4a27d9f6 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_variable.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_variable.py @@ -17,14 +17,42 @@ Variables in Jani """ -from typing import Optional, Union, get_args +from typing import List, Optional, Union, Type, get_args from as2fm_common.common import ValidTypes from jani_generator.jani_entries import JaniExpression, JaniValue class JaniVariable: - def __init__(self, v_name: str, v_type: ValidTypes, + @staticmethod + def from_dict(variable_dict: dict) -> "JaniVariable": + variable_name = variable_dict["name"] + initial_value = variable_dict.get("initial-value", None) + variable_type: type = JaniVariable.python_type_from_json(variable_dict["type"]) + if initial_value is None: + return JaniVariable(variable_name, + variable_type, + None, + variable_dict.get("transient", False)) + if isinstance(initial_value, str): + # Check if conversion from string to variable_type is possible + try: + init_value_cast = variable_type(initial_value) + return JaniVariable(variable_name, + variable_type, + JaniExpression(init_value_cast), + variable_dict.get("transient", False)) + except ValueError: + # If no conversion possible, raise an error (variable names are not supported) + raise ValueError( + f"Initial value {initial_value} for variable {variable_name} " + f"is not a valid value for type {variable_type}.") + return JaniVariable(variable_name, + variable_type, + JaniExpression(initial_value), + variable_dict.get("transient", False)) + + def __init__(self, v_name: str, v_type: Type[ValidTypes], init_value: Optional[Union[JaniExpression, JaniValue]] = None, v_transient: bool = False): assert init_value is None or isinstance(init_value, (JaniExpression, JaniValue)), \ @@ -43,10 +71,12 @@ def __init__(self, v_name: str, v_type: ValidTypes, self._init_expr = JaniExpression(False) elif self._type == float: self._init_expr = JaniExpression(0.0) + else: + raise ValueError(f"Type {self._type} needs an initial value") assert v_type in get_args(ValidTypes), f"Type {v_type} not supported by Jani" - if not self._transient and self._type == float: + if not self._transient and self._type in (float, List[float]): print(f"Warning: Variable {self._name} is not transient and has type float." - "This is not supported by STORM yet.") + "This is not supported by STORM.") def name(self): """Get name.""" @@ -60,7 +90,7 @@ def as_dict(self): """Return the variable as a dictionary.""" d = { "name": self._name, - "type": JaniVariable.jani_type_to_string(self._type), + "type": JaniVariable.python_type_to_json(self._type), "transient": self._transient } if self._init_expr is not None: @@ -68,21 +98,31 @@ def as_dict(self): return d @staticmethod - def jani_type_from_string(str_type: str) -> ValidTypes: + def python_type_from_json(json_type: Union[str, dict]) -> ValidTypes: """ - Translate a (Jani) type string to a Python type. + Translate a (Jani) type string or dict to a Python type. """ - if str_type == "bool": - return bool - elif str_type == "int": - return int - elif str_type == "real": - return float - else: - raise ValueError(f"Type {str_type} not supported by Jani") + if isinstance(json_type, str): + if json_type == "bool": + return bool + elif json_type == "int": + return int + elif json_type == "real": + return float + else: + raise ValueError(f"Type {json_type} not supported by Jani") + elif isinstance(json_type, dict): + assert "kind" in json_type, "Type dict should contain a 'kind' key" + if json_type["kind"] == "array": + assert "base" in json_type, "Array type should contain a 'base' key" + if json_type["base"] == "int": + return List[int] + if json_type["base"] == "real": + return List[float] + raise ValueError(f"Type {json_type} not supported by Jani") @staticmethod - def jani_type_to_string(v_type: ValidTypes) -> str: + def python_type_to_json(v_type: Type[ValidTypes]) -> Union[str, dict]: """ Translate a Python type to the name of the type in Jani. @@ -104,5 +144,9 @@ def jani_type_to_string(v_type: ValidTypes) -> str: return "int" elif v_type == float: return "real" + elif v_type == List[int]: + return {"kind": "array", "base": "int"} + elif v_type == List[float]: + return {"kind": "array", "base": "real"} else: raise ValueError(f"Type {v_type} not supported by Jani") diff --git a/jani_generator/test/_test_data/plain_jani_examples/example_arrays.jani b/jani_generator/test/_test_data/plain_jani_examples/example_arrays.jani new file mode 100644 index 00000000..441fcf2d --- /dev/null +++ b/jani_generator/test/_test_data/plain_jani_examples/example_arrays.jani @@ -0,0 +1,2193 @@ +{ + "jani-version": 1, + "name": "example_arrays", + "type": "mdp", + "features": [ + "arrays" + ], + "metadata": { + "description": "Autogenerated with CONVINCE toolchain" + }, + "variables": [ + {"name": "array_idx", + "type": "int", + "initial-value": 0 + }, + { + "name": "are_array_ok", + "type": { + "kind": "array", + "base": "int" + }, + "transient": false, + "initial-value": { + "op": "ac", + "var": "array_idx", + "length": 2, + "exp": 100 + } + }, + { + "name": "ros_topic.level.data", + "type": "int", + "transient": false, + "initial-value": 0 + }, + { + "name": "ros_topic.level.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "ros_topic.charge.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "ros_topic.alarm.data", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "ros_topic.alarm.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "bt_1000_tick.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "bt_1000_success.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "bt_1000_failure.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "bt_1001_tick.valid", + "type": "bool", + "transient": false, + "initial-value": false + }, + { + "name": "bt_1001_success.valid", + "type": "bool", + "transient": false, + "initial-value": false + } + ], + "constants": [], + "actions": [ + { + "name": "bt_1000_failure_on_receive" + }, + { + "name": "bt_1000_failure_on_send" + }, + { + "name": "bt_1000_success_on_receive" + }, + { + "name": "bt_1000_success_on_send" + }, + { + "name": "bt_1000_tick_on_receive" + }, + { + "name": "bt_1000_tick_on_send" + }, + { + "name": "bt_1001_success_on_receive" + }, + { + "name": "bt_1001_success_on_send" + }, + { + "name": "bt_1001_tick_on_receive" + }, + { + "name": "bt_1001_tick_on_send" + }, + { + "name": "check_battery-first-exec-check_battery-a449c803" + }, + { + "name": "failure-wait_for_tick-6568ac14" + }, + { + "name": "global_timer_action_0" + }, + { + "name": "initial-initial-5eebea38" + }, + { + "name": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-else" + }, + { + "name": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-last_msg" + }, + { + "name": "ros_time_rate.bt_tick_on_receive" + }, + { + "name": "ros_time_rate.my_timer_on_receive" + }, + { + "name": "ros_topic.alarm_on_receive" + }, + { + "name": "ros_topic.alarm_on_send" + }, + { + "name": "ros_topic.charge_on_receive" + }, + { + "name": "ros_topic.charge_on_send" + }, + { + "name": "ros_topic.level_on_receive" + }, + { + "name": "ros_topic.level_on_send" + }, + { + "name": "running-wait_for_tick-89afabc1" + }, + { + "name": "success-wait_for_tick-c305aa83" + }, + { + "name": "tick-1000_TopicCondition-8aab702a" + }, + { + "name": "use_battery-first-exec-use_battery-766fa6e4" + } + ], + "automata": [ + { + "name": "BatteryDrainer", + "locations": [ + { + "name": "use_battery" + }, + { + "name": "use_battery-1-59a11059" + }, + { + "name": "use_battery-1-c00ac01e" + }, + { + "name": "use_battery-first-exec" + }, + { + "name": "use_battery-first-exec-0-766fa6e4" + } + ], + "initial-locations": [ + "use_battery-first-exec" + ], + "edges": [ + { + "location": "use_battery", + "destinations": [ + { + "location": "use_battery-1-c00ac01e", + "assignments": [ + { + "ref": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "value": { + "op": "-", + "left": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "right": 1 + }, + "index": 0 + } + ] + } + ], + "action": "ros_time_rate.my_timer_on_receive", + "guard": { + "exp": { + "op": ">", + "left": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "right": 0 + } + } + }, + { + "location": "use_battery-1-c00ac01e", + "destinations": [ + { + "location": "use_battery", + "assignments": [ + { + "ref": "ros_topic.level.data", + "value": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "index": 0 + }, + { + "ref": "ros_topic.level.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.level_on_send" + }, + { + "location": "use_battery", + "destinations": [ + { + "location": "use_battery-1-59a11059", + "assignments": [ + { + "ref": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "value": 100, + "index": 0 + } + ] + } + ], + "action": "ros_topic.charge_on_receive" + }, + { + "location": "use_battery-1-59a11059", + "destinations": [ + { + "location": "use_battery", + "assignments": [ + { + "ref": "ros_topic.level.data", + "value": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "index": 0 + }, + { + "ref": "ros_topic.level.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.level_on_send" + }, + { + "location": "use_battery", + "destinations": [ + { + "location": "use_battery", + "assignments": [] + } + ], + "action": "ros_time_rate.my_timer_on_receive", + "guard": { + "exp": { + "op": "∧", + "left": true, + "right": { + "op": "¬", + "exp": { + "op": ">", + "left": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "right": 0 + } + } + } + } + }, + { + "location": "use_battery-first-exec", + "destinations": [ + { + "location": "use_battery-first-exec-0-766fa6e4", + "assignments": [] + } + ], + "action": "use_battery-first-exec-use_battery-766fa6e4" + }, + { + "location": "use_battery-first-exec-0-766fa6e4", + "destinations": [ + { + "location": "use_battery", + "assignments": [ + { + "ref": "ros_topic.level.data", + "value": { + "op": "aa", + "exp": "are_array_ok", + "index": 1 + }, + "index": 0 + }, + { + "ref": "ros_topic.level.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.level_on_send" + } + ] + }, + { + "name": "BatteryManager", + "locations": [ + { + "name": "check_battery" + }, + { + "name": "check_battery-1-f1b4fdd4" + }, + { + "name": "check_battery-first-exec" + }, + { + "name": "check_battery-first-exec-0-a449c803" + } + ], + "initial-locations": [ + "check_battery-first-exec" + ], + "edges": [ + { + "location": "check_battery", + "destinations": [ + { + "location": "check_battery-1-f1b4fdd4", + "assignments": [ + { + "ref": "battery_alarm", + "value": { + "op": "<", + "left": "ros_topic.level.data", + "right": 30 + }, + "index": 0 + } + ] + } + ], + "action": "ros_topic.level_on_receive" + }, + { + "location": "check_battery-1-f1b4fdd4", + "destinations": [ + { + "location": "check_battery", + "assignments": [ + { + "ref": "ros_topic.alarm.data", + "value": "battery_alarm", + "index": 0 + }, + { + "ref": "ros_topic.alarm.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.alarm_on_send" + }, + { + "location": "check_battery-first-exec", + "destinations": [ + { + "location": "check_battery-first-exec-0-a449c803", + "assignments": [] + } + ], + "action": "check_battery-first-exec-check_battery-a449c803" + }, + { + "location": "check_battery-first-exec-0-a449c803", + "destinations": [ + { + "location": "check_battery", + "assignments": [ + { + "ref": "ros_topic.alarm.data", + "value": "battery_alarm", + "index": 0 + }, + { + "ref": "ros_topic.alarm.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.alarm_on_send" + } + ], + "variables": [ + { + "name": "battery_alarm", + "type": "bool", + "transient": false, + "initial-value": false + } + ] + }, + { + "name": "TopicCondition", + "locations": [ + { + "name": "initial" + }, + { + "name": "initial_0_after_if" + }, + { + "name": "initial_0_before_if" + }, + { + "name": "initial_0_before_if-0-5eebea38-860ebf50-else" + }, + { + "name": "initial_0_before_if-0-5eebea38-860ebf50-last_msg" + } + ], + "initial-locations": [ + "initial" + ], + "edges": [ + { + "location": "initial", + "destinations": [ + { + "location": "initial", + "assignments": [ + { + "ref": "last_msg", + "value": "ros_topic.alarm.data", + "index": 0 + } + ] + } + ], + "action": "ros_topic.alarm_on_receive" + }, + { + "location": "initial", + "destinations": [ + { + "location": "initial_0_before_if", + "assignments": [] + } + ], + "action": "bt_1000_tick_on_receive" + }, + { + "location": "initial_0_before_if", + "destinations": [ + { + "location": "initial_0_before_if-0-5eebea38-860ebf50-last_msg", + "assignments": [] + } + ], + "action": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-last_msg", + "guard": { + "exp": "last_msg" + } + }, + { + "location": "initial_0_before_if-0-5eebea38-860ebf50-last_msg", + "destinations": [ + { + "location": "initial_0_after_if", + "assignments": [ + { + "ref": "bt_1000_success.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "bt_1000_success_on_send" + }, + { + "location": "initial_0_before_if", + "destinations": [ + { + "location": "initial_0_before_if-0-5eebea38-860ebf50-else", + "assignments": [] + } + ], + "action": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-else", + "guard": { + "exp": { + "op": "∧", + "left": true, + "right": { + "op": "¬", + "exp": "last_msg" + } + } + } + }, + { + "location": "initial_0_before_if-0-5eebea38-860ebf50-else", + "destinations": [ + { + "location": "initial_0_after_if", + "assignments": [ + { + "ref": "bt_1000_failure.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "bt_1000_failure_on_send" + }, + { + "location": "initial_0_after_if", + "destinations": [ + { + "location": "initial", + "assignments": [] + } + ], + "action": "initial-initial-5eebea38" + } + ], + "variables": [ + { + "name": "last_msg", + "type": "bool", + "transient": false, + "initial-value": false + } + ] + }, + { + "name": "TopicAction", + "locations": [ + { + "name": "initial" + }, + { + "name": "initial-0-dd921629" + }, + { + "name": "initial-1-dd921629" + } + ], + "initial-locations": [ + "initial" + ], + "edges": [ + { + "location": "initial", + "destinations": [ + { + "location": "initial-0-dd921629", + "assignments": [] + } + ], + "action": "bt_1001_tick_on_receive" + }, + { + "location": "initial-0-dd921629", + "destinations": [ + { + "location": "initial-1-dd921629", + "assignments": [ + { + "ref": "ros_topic.charge.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "ros_topic.charge_on_send" + }, + { + "location": "initial-1-dd921629", + "destinations": [ + { + "location": "initial", + "assignments": [ + { + "ref": "bt_1001_success.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "bt_1001_success_on_send" + } + ] + }, + { + "name": "bt", + "locations": [ + { + "name": "1000_TopicCondition" + }, + { + "name": "1000_TopicCondition-0-28dfb6ec" + }, + { + "name": "1001_TopicAction" + }, + { + "name": "failure" + }, + { + "name": "running" + }, + { + "name": "success" + }, + { + "name": "tick" + }, + { + "name": "tick-0-8aab702a" + }, + { + "name": "wait_for_tick" + } + ], + "initial-locations": [ + "wait_for_tick" + ], + "edges": [ + { + "location": "tick", + "destinations": [ + { + "location": "tick-0-8aab702a", + "assignments": [] + } + ], + "action": "tick-1000_TopicCondition-8aab702a" + }, + { + "location": "tick-0-8aab702a", + "destinations": [ + { + "location": "1000_TopicCondition", + "assignments": [ + { + "ref": "bt_1000_tick.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "bt_1000_tick_on_send" + }, + { + "location": "success", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "success-wait_for_tick-c305aa83" + }, + { + "location": "failure", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "failure-wait_for_tick-6568ac14" + }, + { + "location": "running", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "running-wait_for_tick-89afabc1" + }, + { + "location": "1000_TopicCondition", + "destinations": [ + { + "location": "failure", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "1000_TopicCondition", + "destinations": [ + { + "location": "1000_TopicCondition-0-28dfb6ec", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "1000_TopicCondition-0-28dfb6ec", + "destinations": [ + { + "location": "1001_TopicAction", + "assignments": [ + { + "ref": "bt_1001_tick.valid", + "value": true, + "index": 0 + } + ] + } + ], + "action": "bt_1001_tick_on_send" + }, + { + "location": "1001_TopicAction", + "destinations": [ + { + "location": "success", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "wait_for_tick", + "destinations": [ + { + "location": "tick", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "tick", + "destinations": [ + { + "location": "tick", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "tick", + "destinations": [ + { + "location": "tick", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "tick", + "destinations": [ + { + "location": "tick", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "tick", + "destinations": [ + { + "location": "tick", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "success", + "destinations": [ + { + "location": "success", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "success", + "destinations": [ + { + "location": "success", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "success", + "destinations": [ + { + "location": "success", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "success", + "destinations": [ + { + "location": "success", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "failure", + "destinations": [ + { + "location": "failure", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "failure", + "destinations": [ + { + "location": "failure", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "failure", + "destinations": [ + { + "location": "failure", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "failure", + "destinations": [ + { + "location": "failure", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "running", + "destinations": [ + { + "location": "running", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "running", + "destinations": [ + { + "location": "running", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "running", + "destinations": [ + { + "location": "running", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "running", + "destinations": [ + { + "location": "running", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "1000_TopicCondition", + "destinations": [ + { + "location": "1000_TopicCondition", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "1000_TopicCondition", + "destinations": [ + { + "location": "1000_TopicCondition", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "1001_TopicAction", + "destinations": [ + { + "location": "1001_TopicAction", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "1001_TopicAction", + "destinations": [ + { + "location": "1001_TopicAction", + "assignments": [] + } + ], + "action": "ros_time_rate.bt_tick_on_receive" + }, + { + "location": "1001_TopicAction", + "destinations": [ + { + "location": "1001_TopicAction", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + }, + { + "location": "wait_for_tick", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + }, + { + "location": "wait_for_tick", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + }, + { + "location": "wait_for_tick", + "destinations": [ + { + "location": "wait_for_tick", + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + } + ] + }, + { + "name": "global_timer", + "locations": [ + { + "name": "loc" + } + ], + "initial-locations": [ + "loc" + ], + "edges": [ + { + "location": "loc", + "destinations": [ + { + "location": "loc", + "assignments": [ + { + "ref": "t", + "value": { + "op": "+", + "left": "t", + "right": 1 + }, + "index": 0 + }, + { + "ref": "my_timer_needed", + "value": { + "op": "=", + "left": { + "op": "%", + "left": "t", + "right": 1 + }, + "right": 0 + }, + "index": 1 + }, + { + "ref": "bt_tick_needed", + "value": { + "op": "=", + "left": { + "op": "%", + "left": "t", + "right": 1 + }, + "right": 0 + }, + "index": 2 + } + ] + } + ], + "action": "global_timer_action_0", + "guard": { + "exp": { + "op": "∧", + "left": { + "op": "∧", + "left": { + "op": "<", + "left": "t", + "right": 100 + }, + "right": { + "op": "¬", + "exp": "my_timer_needed" + } + }, + "right": { + "op": "¬", + "exp": "bt_tick_needed" + } + } + } + }, + { + "location": "loc", + "destinations": [ + { + "location": "loc", + "assignments": [ + { + "ref": "my_timer_needed", + "value": false, + "index": 0 + } + ] + } + ], + "action": "ros_time_rate.my_timer_on_receive", + "guard": { + "exp": "my_timer_needed" + } + }, + { + "location": "loc", + "destinations": [ + { + "location": "loc", + "assignments": [ + { + "ref": "bt_tick_needed", + "value": false, + "index": 0 + } + ] + } + ], + "action": "ros_time_rate.bt_tick_on_receive", + "guard": { + "exp": "bt_tick_needed" + } + } + ], + "variables": [ + { + "name": "t", + "type": "int", + "transient": false, + "initial-value": 0 + }, + { + "name": "my_timer_needed", + "type": "bool", + "transient": false, + "initial-value": true + }, + { + "name": "bt_tick_needed", + "type": "bool", + "transient": false, + "initial-value": true + } + ] + }, + { + "name": "ros_topic.level", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.level_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.level_on_receive" + } + ] + }, + { + "name": "ros_topic.charge", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.charge_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.charge_on_receive" + } + ] + }, + { + "name": "ros_topic.alarm", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.alarm_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "ros_topic.alarm_on_receive" + } + ] + }, + { + "name": "bt_1000_tick", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_tick_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_tick_on_receive" + } + ] + }, + { + "name": "bt_1000_success", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_success_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_success_on_receive" + } + ] + }, + { + "name": "bt_1000_failure", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_failure_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1000_failure_on_receive" + } + ] + }, + { + "name": "bt_1001_tick", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1001_tick_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1001_tick_on_receive" + } + ] + }, + { + "name": "bt_1001_success", + "locations": [ + { + "name": "received" + }, + { + "name": "waiting" + } + ], + "initial-locations": [ + "waiting" + ], + "edges": [ + { + "location": "waiting", + "destinations": [ + { + "location": "received", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1001_success_on_send" + }, + { + "location": "received", + "destinations": [ + { + "location": "waiting", + "probability": { + "exp": 1.0 + }, + "assignments": [] + } + ], + "action": "bt_1001_success_on_receive" + } + ] + } + ], + "system": { + "elements": [ + { + "automaton": "BatteryDrainer" + }, + { + "automaton": "BatteryManager" + }, + { + "automaton": "TopicCondition" + }, + { + "automaton": "TopicAction" + }, + { + "automaton": "bt" + }, + { + "automaton": "global_timer" + }, + { + "automaton": "ros_topic.level" + }, + { + "automaton": "ros_topic.charge" + }, + { + "automaton": "ros_topic.alarm" + }, + { + "automaton": "bt_1000_tick" + }, + { + "automaton": "bt_1000_success" + }, + { + "automaton": "bt_1000_failure" + }, + { + "automaton": "bt_1001_tick" + }, + { + "automaton": "bt_1001_success" + } + ], + "syncs": [ + { + "result": "bt_1000_failure_on_receive", + "synchronise": [ + null, + null, + null, + null, + "bt_1000_failure_on_receive", + null, + null, + null, + null, + null, + null, + "bt_1000_failure_on_receive", + null, + null + ] + }, + { + "result": "bt_1000_failure_on_send", + "synchronise": [ + null, + null, + "bt_1000_failure_on_send", + null, + null, + null, + null, + null, + null, + null, + null, + "bt_1000_failure_on_send", + null, + null + ] + }, + { + "result": "bt_1000_success_on_receive", + "synchronise": [ + null, + null, + null, + null, + "bt_1000_success_on_receive", + null, + null, + null, + null, + null, + "bt_1000_success_on_receive", + null, + null, + null + ] + }, + { + "result": "bt_1000_success_on_send", + "synchronise": [ + null, + null, + "bt_1000_success_on_send", + null, + null, + null, + null, + null, + null, + null, + "bt_1000_success_on_send", + null, + null, + null + ] + }, + { + "result": "bt_1000_tick_on_receive", + "synchronise": [ + null, + null, + "bt_1000_tick_on_receive", + null, + null, + null, + null, + null, + null, + "bt_1000_tick_on_receive", + null, + null, + null, + null + ] + }, + { + "result": "bt_1000_tick_on_send", + "synchronise": [ + null, + null, + null, + null, + "bt_1000_tick_on_send", + null, + null, + null, + null, + "bt_1000_tick_on_send", + null, + null, + null, + null + ] + }, + { + "result": "bt_1001_success_on_receive", + "synchronise": [ + null, + null, + null, + null, + "bt_1001_success_on_receive", + null, + null, + null, + null, + null, + null, + null, + null, + "bt_1001_success_on_receive" + ] + }, + { + "result": "bt_1001_success_on_send", + "synchronise": [ + null, + null, + null, + "bt_1001_success_on_send", + null, + null, + null, + null, + null, + null, + null, + null, + null, + "bt_1001_success_on_send" + ] + }, + { + "result": "bt_1001_tick_on_receive", + "synchronise": [ + null, + null, + null, + "bt_1001_tick_on_receive", + null, + null, + null, + null, + null, + null, + null, + null, + "bt_1001_tick_on_receive", + null + ] + }, + { + "result": "bt_1001_tick_on_send", + "synchronise": [ + null, + null, + null, + null, + "bt_1001_tick_on_send", + null, + null, + null, + null, + null, + null, + null, + "bt_1001_tick_on_send", + null + ] + }, + { + "result": "check_battery-first-exec-check_battery-a449c803", + "synchronise": [ + null, + "check_battery-first-exec-check_battery-a449c803", + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "failure-wait_for_tick-6568ac14", + "synchronise": [ + null, + null, + null, + null, + "failure-wait_for_tick-6568ac14", + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "global_timer_action_0", + "synchronise": [ + null, + null, + null, + null, + null, + "global_timer_action_0", + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "initial-initial-5eebea38", + "synchronise": [ + null, + null, + "initial-initial-5eebea38", + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-else", + "synchronise": [ + null, + null, + "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-else", + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-last_msg", + "synchronise": [ + null, + null, + "initial_0_before_if-initial_0_after_if-5eebea38-860ebf50-last_msg", + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_time_rate.bt_tick_on_receive", + "synchronise": [ + null, + null, + null, + null, + "ros_time_rate.bt_tick_on_receive", + "ros_time_rate.bt_tick_on_receive", + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_time_rate.my_timer_on_receive", + "synchronise": [ + "ros_time_rate.my_timer_on_receive", + null, + null, + null, + null, + "ros_time_rate.my_timer_on_receive", + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.alarm_on_receive", + "synchronise": [ + null, + null, + "ros_topic.alarm_on_receive", + null, + null, + null, + null, + null, + "ros_topic.alarm_on_receive", + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.alarm_on_send", + "synchronise": [ + null, + "ros_topic.alarm_on_send", + null, + null, + null, + null, + null, + null, + "ros_topic.alarm_on_send", + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.charge_on_receive", + "synchronise": [ + "ros_topic.charge_on_receive", + null, + null, + null, + null, + null, + null, + "ros_topic.charge_on_receive", + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.charge_on_send", + "synchronise": [ + null, + null, + null, + "ros_topic.charge_on_send", + null, + null, + null, + "ros_topic.charge_on_send", + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.level_on_receive", + "synchronise": [ + null, + "ros_topic.level_on_receive", + null, + null, + null, + null, + "ros_topic.level_on_receive", + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "ros_topic.level_on_send", + "synchronise": [ + "ros_topic.level_on_send", + null, + null, + null, + null, + null, + "ros_topic.level_on_send", + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "running-wait_for_tick-89afabc1", + "synchronise": [ + null, + null, + null, + null, + "running-wait_for_tick-89afabc1", + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "success-wait_for_tick-c305aa83", + "synchronise": [ + null, + null, + null, + null, + "success-wait_for_tick-c305aa83", + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "tick-1000_TopicCondition-8aab702a", + "synchronise": [ + null, + null, + null, + null, + "tick-1000_TopicCondition-8aab702a", + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + }, + { + "result": "use_battery-first-exec-use_battery-766fa6e4", + "synchronise": [ + "use_battery-first-exec-use_battery-766fa6e4", + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null, + null + ] + } + ] + }, + "properties": [ + { + "name": "battery_depleted", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "left": { + "op": "≤", + "left": "ros_topic.level.data", + "right": 0 + }, + "op": "∧", + "right": "ros_topic.level.valid" + } + } + }, + "states": { + "op": "initial" + } + } + }, + { + "name": "battery_below_20", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "left": { + "op": "<", + "left": "ros_topic.level.data", + "right": 20 + }, + "op": "∧", + "right": "ros_topic.level.valid" + } + } + }, + "states": { + "op": "initial" + } + } + }, + { + "name": "battery_alarm_on", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "op": "∧", + "left": "ros_topic.alarm.data", + "right": "ros_topic.charge.valid" + } + } + }, + "states": { + "op": "initial" + } + } + } + ] +} \ No newline at end of file diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/.gitignore b/jani_generator/test/_test_data/ros_fibonacci_action_example/.gitignore new file mode 100644 index 00000000..b941f4e4 --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/.gitignore @@ -0,0 +1,3 @@ +generated_bt_scxml +generated_plain_scxml +main.jani \ No newline at end of file diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml new file mode 100644 index 00000000..9640ac5a --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_2.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_2.scxml new file mode 100644 index 00000000..c4691bbd --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_2.scxml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/happy_clients.jani b/jani_generator/test/_test_data/ros_fibonacci_action_example/happy_clients.jani new file mode 100644 index 00000000..418cab82 --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/happy_clients.jani @@ -0,0 +1,33 @@ +{ + "properties": [ + { + "name": "happy_clients", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "op": "F", + "exp": { + "op": "∧", + "left": { + "op": "∧", + "left": "ros_topic./client_1_res.data", + "right": "ros_topic./client_1_res.valid" + }, + "right": { + "op": "∧", + "left": "ros_topic./client_2_res.data", + "right": "ros_topic./client_2_res.valid" + } + } + } + }, + "states": { + "op": "initial" + } + } + } + ] +} \ No newline at end of file diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/main.xml b/jani_generator/test/_test_data/ros_fibonacci_action_example/main.xml new file mode 100644 index 00000000..36074c02 --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/main.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml new file mode 100644 index 00000000..77aefb71 --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml new file mode 100644 index 00000000..3b40e27d --- /dev/null +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/test_unittest_jani_model_loading.py b/jani_generator/test/test_unittest_jani_model_loading.py new file mode 100644 index 00000000..b9436fcf --- /dev/null +++ b/jani_generator/test/test_unittest_jani_model_loading.py @@ -0,0 +1,34 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +""""Test the SCXML data conversion""" + +import os +import json +from jani_generator.jani_entries import JaniModel + + +def test_jani_file_loading(): + jani_file = os.path.join(os.path.dirname(__file__), + '_test_data', 'plain_jani_examples', 'example_arrays.jani') + with open(jani_file, "r", encoding='utf-8') as file: + convince_jani_json = json.load(file) + jani_model = JaniModel.from_dict(convince_jani_json) + assert isinstance(jani_model, JaniModel) + assert jani_model.get_name() == "example_arrays" + assert "arrays" in jani_model.get_features() + assert len(jani_model.get_variables()) == 12 + assert len(jani_model.get_constants()) == 0 + assert len(jani_model.get_automata()) == 14 From ff9777db613069d5ba7dc631c8c11cf1cd0292a9 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 17:53:08 +0200 Subject: [PATCH 03/49] Reduce code duplication and introduce some action interfaces Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 209 +++++++++++++----- 1 file changed, 152 insertions(+), 57 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 56cd6c16..35b75545 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -15,7 +15,7 @@ """Collection of SCXML utilities related to ROS functionalities.""" -from typing import Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Tuple, Type from scxml_converter.scxml_entries.scxml_ros_field import RosField @@ -30,6 +30,14 @@ 'int8', 'int16', 'int32', 'int64', 'float', 'double'] +"""List of prefixes for ROS-related event data in SCXML.""" +ROS_EVENT_PREFIXES = [ + "_msg.", # Topic-related + "_req.", "_res.", # Service-related + "_goal.", "_feedback.", "_result." # Action-related +] + + """Container for the ROS interface (e.g. topic or service) name and the related type""" RosInterfaceAndType = Tuple[str, str] @@ -65,9 +73,44 @@ def is_srv_type_known(service_definition: str) -> bool: return is_ros_type_known(service_definition, "srv") +def is_action_type_known(action_definition: str) -> bool: + """Check if python can import the provided action definition.""" + return is_ros_type_known(action_definition, "action") + + +def extract_params_from_ros_type(ros_interface_type: Type[Any]) -> Dict[str, str]: + """ + Extract the data fields of a ROS message type as pairs of name and type objects. + """ + fields = ros_interface_type.get_fields_and_field_types() + for key in fields.keys(): + assert fields[key] in BASIC_FIELD_TYPES, \ + f"Error: SCXML ROS declarations: {ros_interface_type} {key} field is " \ + f"of type {fields[key]}, that is not supported." + fields[key] = MSG_TYPE_SUBSTITUTIONS.get(fields[key], fields[key]) + return fields + + +def check_all_fields_known(ros_fields: List[RosField], field_types: Dict[str, str]) -> bool: + """ + Check that all fields from ros_fields are in field_types, and that no field is missing. + """ + for ros_field in ros_fields: + if ros_field.get_name() not in field_types: + print(f"Error: SCXML ROS declarations: unknown field {ros_field.get_name()}.") + return False + field_types.pop(ros_field.get_name()) + if len(field_types) > 0: + print("Error: SCXML ROS declarations: there are missing fields:") + for field_key in field_types.keys(): + print(f"\t-{field_key}.") + return False + return True + + def get_srv_type_params(service_definition: str) -> Tuple[Dict[str, str], Dict[str, str]]: """ - Get the data fields of a service request and response type as pairs of name and type objects. + Get the fields of a service request and response as pairs of name and type objects. """ assert is_srv_type_known(service_definition), \ f"Error: SCXML ROS declarations: service type {service_definition} not found." @@ -76,27 +119,33 @@ def get_srv_type_params(service_definition: str) -> Tuple[Dict[str, str], Dict[s srv_class = getattr(srv_module, interface_type) # TODO: Fields can be nested. Look AS2FM/scxml_converter/src/scxml_converter/scxml_converter.py - req = srv_class.Request.get_fields_and_field_types() - for key in req.keys(): - # TODO: Support nested fields - assert req[key] in BASIC_FIELD_TYPES, \ - f"Error: SCXML ROS declarations: service request type {req[key]} isn't a basic field." - req[key] = MSG_TYPE_SUBSTITUTIONS.get(req[key], req[key]) + req_fields = extract_params_from_ros_type(srv_class.Request) + res_fields = extract_params_from_ros_type(srv_class.Response) - res = srv_class.Response.get_fields_and_field_types() - for key in res.keys(): - assert res[key] in BASIC_FIELD_TYPES, \ - "Error: SCXML ROS declarations: service response type contains non-basic fields." - res[key] = MSG_TYPE_SUBSTITUTIONS.get(res[key], res[key]) + return req_fields, res_fields - return req, res + +def get_action_type_params(action_definition: str + ) -> Tuple[Dict[str, str], Dict[str, str], Dict[str, str]]: + """ + Get the fields of an action goal, feedback and result as pairs of name and type objects. + """ + assert is_action_type_known(action_definition), \ + f"Error: SCXML ROS declarations: action type {action_definition} not found." + interface_ns, interface_type = action_definition.split("/") + action_module = __import__(interface_ns + '.action', fromlist=['']) + action_class = getattr(action_module, interface_type) + action_goal_fields = extract_params_from_ros_type(action_class.Goal) + action_feedback_fields = extract_params_from_ros_type(action_class.Feedback) + action_result_fields = extract_params_from_ros_type(action_class.Result) + return action_goal_fields, action_feedback_fields, action_result_fields def replace_ros_interface_expression(msg_expr: str) -> str: - """Convert a ROS interface expression (msg, req, res) to plain SCXML (event).""" + """Convert all ROS interface expressions (in ROS_EVENT_PREFIXES) to plain SCXML events.""" scxml_prefix = "_event." # TODO: Use regex and ensure no other valid character exists before the initial underscore - for ros_prefix in ["_msg.", "_req.", "_res."]: + for ros_prefix in ROS_EVENT_PREFIXES: msg_expr = msg_expr.replace(ros_prefix, scxml_prefix) return msg_expr @@ -134,6 +183,38 @@ def generate_srv_server_response_event(service_name: str) -> str: return f"srv_{sanitize_ros_interface_name(service_name)}_response" +def generate_action_goal_event(action_name: str, automaton_name: str) -> str: + """Generate the name of the event that sends an action goal from a client to the server.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_client_{automaton_name}" + + +def generate_action_goal_handle_event(action_name: str) -> str: + """Generate the name of the event that triggers an action goal handling in the server.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_handle" + + +def generate_action_feedback_event(action_name: str) -> str: + """Generate the name of the event that sends a feedback from the action server.""" + return f"action_{sanitize_ros_interface_name(action_name)}_feedback" + + +def generate_action_feedback_handle_event(action_name: str, automaton_name: str) -> str: + """Generate the name of the event that handles a feedback in an action client.""" + return f"action_{sanitize_ros_interface_name(action_name)}_" \ + f"feedback_handle_client_{automaton_name}" + + +def generate_action_result_event(action_name: str) -> str: + """Generate the name of the event that sends a result from the action server.""" + return f"action_{sanitize_ros_interface_name(action_name)}_result" + + +def generate_action_result_handle_event(action_name: str, automaton_name: str) -> str: + """Generate the name of the event that handles a result in an action client.""" + return f"action_{sanitize_ros_interface_name(action_name)}_" \ + f"result_handle_client_{automaton_name}" + + class ScxmlRosDeclarationsContainer: """Object that contains a description of the ROS declarations in the SCXML root.""" @@ -143,11 +224,17 @@ def __init__(self, automaton_name: str): :automaton_name: Name of the automaton these declarations belong to. """ self._automaton_name: str = automaton_name - # Dict of publishers and subscribers: topic name -> type + # Dictionaries relating an interface ref. name to the comm. channel name and data type + # ROS Topics self._publishers: Dict[str, RosInterfaceAndType] = {} self._subscribers: Dict[str, RosInterfaceAndType] = {} + # ROS Services self._service_servers: Dict[str, RosInterfaceAndType] = {} self._service_clients: Dict[str, RosInterfaceAndType] = {} + # ROS Actions + self._action_servers: Dict[str, RosInterfaceAndType] = {} + self._action_clients: Dict[str, RosInterfaceAndType] = {} + # ROS Timers self._timers: Dict[str, float] = {} def get_automaton_name(self) -> str: @@ -155,6 +242,13 @@ def get_automaton_name(self) -> str: return self._automaton_name def append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> None: + """ + Add a publisher to the container. + + :param pub_name: Name of the publisher (alias, user-defined). + :param topic_name: Name of the topic to publish to. + :param topic_type: Type of the message to publish. + """ assert all_non_empty_strings(pub_name, topic_name, topic_type), \ "Error: ROS declarations: publisher name, topic name and type must be strings." assert pub_name not in self._publishers, \ @@ -162,6 +256,13 @@ def append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> N self._publishers[pub_name] = (topic_name, topic_type) def append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> None: + """ + Add a subscriber to the container. + + :param sub_name: Name of the subscriber (alias, user-defined). + :param topic_name: Name of the topic to subscribe to. + :param topic_type: Type of the message to subscribe to. + """ assert all_non_empty_strings(sub_name, topic_name, topic_type), \ "Error: ROS declarations: subscriber name, topic name and type must be strings." assert sub_name not in self._subscribers, \ @@ -169,6 +270,13 @@ def append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> self._subscribers[sub_name] = (topic_name, topic_type) def append_service_client(self, client_name: str, service_name: str, service_type: str) -> None: + """ + Add a service client to the container. + + :param client_name: Name of the service client (alias, user-defined). + :param service_name: Name of the service to call. + :param service_type: Type of data used in the service communication. + """ assert all_non_empty_strings(client_name, service_name, service_type), \ "Error: ROS declarations: client name, service name and type must be strings." assert client_name not in self._service_clients, \ @@ -176,12 +284,33 @@ def append_service_client(self, client_name: str, service_name: str, service_typ self._service_clients[client_name] = (service_name, service_type) def append_service_server(self, server_name: str, service_name: str, service_type: str) -> None: + """ + Add a service server to the container. + + :param server_name: Name of the service server (alias, user-defined). + :param service_name: Name of the provided service (what the client needs to call). + :param service_type: Type of data used in the service communication. + """ assert all_non_empty_strings(server_name, service_name, service_type), \ "Error: ROS declarations: server name, service name and type must be strings." assert server_name not in self._service_servers, \ f"Error: ROS declarations: service server {server_name} already declared." self._service_servers[server_name] = (service_name, service_type) + def append_action_client(self, client_name: str, action_name: str, action_type: str) -> None: + assert all_non_empty_strings(client_name, action_name, action_type), \ + "Error: ROS declarations: client name, action name and type must be strings." + assert client_name not in self._action_clients, \ + f"Error: ROS declarations: action client {client_name} already declared." + self._action_clients[client_name] = (action_name, action_type) + + def append_action_server(self, server_name: str, action_name: str, action_type: str) -> None: + assert all_non_empty_strings(server_name, action_name, action_type), \ + "Error: ROS declarations: server name, action name and type must be strings." + assert server_name not in self._action_servers, \ + f"Error: ROS declarations: action server {server_name} already declared." + self._action_servers[server_name] = (action_name, action_type) + def append_timer(self, timer_name: str, timer_rate: float) -> None: assert isinstance(timer_name, str), "Error: ROS declarations: timer name must be a string." assert isinstance(timer_rate, float) and timer_rate > 0, \ @@ -235,54 +364,20 @@ def is_service_client_defined(self, client_name: str) -> bool: def is_service_server_defined(self, server_name: str) -> bool: return server_name in self._service_servers - def get_service_client_type(self, client_name: str) -> Optional[str]: - client_definition = self._service_clients.get(client_name, None) - if client_definition is None: - return None - return client_definition[1] - - def get_service_server_type(self, server_name: str) -> Optional[str]: - server_definition = self._service_servers.get(server_name, None) - if server_definition is None: - return None - return server_definition[1] - def check_valid_srv_req_fields(self, client_name: str, ros_fields: List[RosField]) -> bool: """Check if the provided fields match the service request type.""" - req_type = self.get_service_client_type(client_name) - if req_type is None: - print(f"Error: SCXML ROS declarations: unknown service client {client_name}.") - return False + _, req_type = self.get_service_client_info(client_name) req_fields, _ = get_srv_type_params(req_type) - for ros_field in ros_fields: - if ros_field.get_name() not in req_fields: - print("Error: SCXML ROS declarations: " - f"unknown field {ros_field.get_name()} in service request.") - return False - req_fields.pop(ros_field.get_name()) - if len(req_fields) > 0: - print("Error: SCXML ROS declarations: missing fields in service request.") - for req_field in req_fields.keys(): - print(f"\t-{req_field}.") + if not check_all_fields_known(ros_fields, req_fields): + print(f"Error: SCXML ROS declarations: Srv request {client_name} has invalid fields.") return False return True def check_valid_srv_res_fields(self, server_name: str, ros_fields: List[RosField]) -> bool: """Check if the provided fields match the service response type.""" - res_type = self.get_service_server_type(server_name) - if res_type is None: - print(f"Error: SCXML ROS declarations: unknown service server {server_name}.") - return False + _, res_type = self.get_service_server_info(server_name) _, res_fields = get_srv_type_params(res_type) - for ros_field in ros_fields: - if ros_field.get_name() not in res_fields: - print("Error: SCXML ROS declarations: " - f"unknown field {ros_field.get_name()} in service response.") - return False - res_fields.pop(ros_field.get_name()) - if len(res_fields) > 0: - print("Error: SCXML ROS declarations: missing fields in service response.") - for res_field in res_fields.keys(): - print(f"\t-{res_field}.") + if not check_all_fields_known(ros_fields, res_fields): + print(f"Error: SCXML ROS declarations: Srv response {server_name} has invalid fields.") return False return True From 97255b4350fb3f5f696048182964e103e7adf5cf Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 18:02:15 +0200 Subject: [PATCH 04/49] Integrate action functionalities in RosDeclarationsContainer Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 70 ++++++++++++++++--- 1 file changed, 60 insertions(+), 10 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 35b75545..ea2c54ef 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -15,7 +15,7 @@ """Collection of SCXML utilities related to ROS functionalities.""" -from typing import Any, Dict, List, Optional, Tuple, Type +from typing import Any, Dict, List, Tuple, Type from scxml_converter.scxml_entries.scxml_ros_field import RosField @@ -325,12 +325,21 @@ def is_publisher_defined(self, pub_name: str) -> bool: def is_subscriber_defined(self, sub_name: str) -> bool: return sub_name in self._subscribers + def is_service_client_defined(self, client_name: str) -> bool: + return client_name in self._service_clients + + def is_service_server_defined(self, server_name: str) -> bool: + return server_name in self._service_servers + + def is_action_client_defined(self, client_name: str) -> bool: + return client_name in self._action_clients + + def is_action_server_defined(self, server_name: str) -> bool: + return server_name in self._action_servers + def is_timer_defined(self, timer_name: str) -> bool: return timer_name in self._timers - def get_timers(self) -> Dict[str, float]: - return self._timers - def get_publisher_info(self, pub_name: str) -> Tuple[str, str]: """Provide a publisher topic name and type""" pub_info = self._publishers.get(pub_name) @@ -358,16 +367,27 @@ def get_service_client_info(self, client_name: str) -> Tuple[str, str]: f"Error: SCXML ROS declarations: unknown service client {client_name}." return client_info - def is_service_client_defined(self, client_name: str) -> bool: - return client_name in self._service_clients + def get_action_server_info(self, server_name: str) -> Tuple[str, str]: + """Given an action server name, provide the related action name and type.""" + server_info = self._action_servers.get(server_name) + assert server_info is not None, \ + f"Error: SCXML ROS declarations: unknown action server {server_name}." + return server_info - def is_service_server_defined(self, server_name: str) -> bool: - return server_name in self._service_servers + def get_action_client_info(self, client_name: str) -> Tuple[str, str]: + """Given an action client name, provide the related action name and type.""" + client_info = self._action_clients.get(client_name) + assert client_info is not None, \ + f"Error: SCXML ROS declarations: unknown action client {client_name}." + return client_info + + def get_timers(self) -> Dict[str, float]: + return self._timers def check_valid_srv_req_fields(self, client_name: str, ros_fields: List[RosField]) -> bool: """Check if the provided fields match the service request type.""" - _, req_type = self.get_service_client_info(client_name) - req_fields, _ = get_srv_type_params(req_type) + _, service_type = self.get_service_client_info(client_name) + req_fields, _ = get_srv_type_params(service_type) if not check_all_fields_known(ros_fields, req_fields): print(f"Error: SCXML ROS declarations: Srv request {client_name} has invalid fields.") return False @@ -381,3 +401,33 @@ def check_valid_srv_res_fields(self, server_name: str, ros_fields: List[RosField print(f"Error: SCXML ROS declarations: Srv response {server_name} has invalid fields.") return False return True + + def check_valid_action_goal_fields(self, client_name: str, ros_fields: List[RosField]) -> bool: + """Check if the provided fields match the action goal type.""" + _, action_type = self.get_action_client_info(client_name) + goal_fields, _, _ = get_action_type_params(action_type) + if not check_all_fields_known(ros_fields, goal_fields): + print(f"Error: SCXML ROS declarations: Action goal {client_name} has invalid fields.") + return False + return True + + def check_valid_action_feedback_fields( + self, server_name: str, ros_fields: List[RosField]) -> bool: + """Check if the provided fields match the action feedback type.""" + _, action_type = self.get_action_server_info(server_name) + _, feedback_fields, _ = get_action_type_params(action_type) + if not check_all_fields_known(ros_fields, feedback_fields): + print(f"Error: SCXML ROS declarations: Action feedback {server_name} " + "has invalid fields.") + return False + return True + + def check_valid_action_result_fields( + self, server_name: str, ros_fields: List[RosField]) -> bool: + """Check if the provided fields match the action result type.""" + _, action_type = self.get_action_server_info(server_name) + _, _, result_fields = get_action_type_params(action_type) + if not check_all_fields_known(ros_fields, result_fields): + print(f"Error: SCXML ROS declarations: Action result {server_name} has invalid fields.") + return False + return True From eac137260e101534e63790769cf737180dc78e30 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 18:12:23 +0200 Subject: [PATCH 05/49] Handle empty jani guard Signed-off-by: Marco Lampacrescia --- jani_generator/src/jani_generator/jani_entries/jani_guard.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jani_generator/src/jani_generator/jani_entries/jani_guard.py b/jani_generator/src/jani_generator/jani_entries/jani_guard.py index cb6ce9cb..343c9681 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_guard.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_guard.py @@ -25,7 +25,9 @@ class JaniGuard: @staticmethod - def from_dict(guard_dict: dict) -> "JaniGuard": + def from_dict(guard_dict: Optional[dict]) -> "JaniGuard": + if guard_dict is None: + return None assert isinstance(guard_dict, dict), f"Unexpected type {type(guard_dict)} for guard_dict." assert "exp" in guard_dict, "Missing 'exp' key in guard_dict." return JaniGuard(JaniExpression(guard_dict["exp"])) From cb24981c1fbd56d28a41a78f626467005164c50d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 19 Aug 2024 19:38:32 +0200 Subject: [PATCH 06/49] Fix bugs related to JaniEntries Signed-off-by: Marco Lampacrescia --- .../jani_generator/jani_entries/jani_edge.py | 2 +- .../jani_generator/jani_entries/jani_guard.py | 41 ++++++++++++------- .../jani_generator/ros_helpers/ros_timer.py | 8 ++-- .../scxml_helpers/scxml_tags.py | 2 +- 4 files changed, 31 insertions(+), 22 deletions(-) diff --git a/jani_generator/src/jani_generator/jani_entries/jani_edge.py b/jani_generator/src/jani_generator/jani_entries/jani_edge.py index c3bc6f6a..e50a67f0 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_edge.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_edge.py @@ -31,7 +31,7 @@ def __init__(self, edge_dict: dict): self.action = edge_dict["action"] self.guard = None if "guard" in edge_dict: - self.guard = JaniGuard.from_dict(edge_dict["guard"]) + self.guard = JaniGuard(edge_dict["guard"]) self.destinations = [] for dest in edge_dict["destinations"]: jani_destination = { diff --git a/jani_generator/src/jani_generator/jani_entries/jani_guard.py b/jani_generator/src/jani_generator/jani_entries/jani_guard.py index 343c9681..03f564f3 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_guard.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_guard.py @@ -18,29 +18,40 @@ """ -from typing import Optional +from typing import Optional, Union from jani_generator.jani_entries.jani_expression import JaniExpression class JaniGuard: - @staticmethod - def from_dict(guard_dict: Optional[dict]) -> "JaniGuard": - if guard_dict is None: - return None - assert isinstance(guard_dict, dict), f"Unexpected type {type(guard_dict)} for guard_dict." - assert "exp" in guard_dict, "Missing 'exp' key in guard_dict." - return JaniGuard(JaniExpression(guard_dict["exp"])) - - def __init__(self, expression: Optional[JaniExpression]): - assert expression is None or isinstance(expression, JaniExpression), \ - f"Unexpected expression type: {type(expression)} should be a JaniExpression." - self.expression = expression + + def __init__(self, guard_exp: Optional[Union['JaniGuard', JaniExpression, dict]]): + """ + Construct a new JaniGuard object. + + It checks against an expression that can be provided as a dict (with the 'exp' key) or a + JaniExpression variable. + + :param guard_exp: The expression that must hold for the guard to be ok. + """ + if guard_exp is None or isinstance(guard_exp, JaniExpression): + self._expression = guard_exp + elif isinstance(guard_exp, JaniGuard): + self._expression = guard_exp._expression + elif isinstance(guard_exp, dict): + assert "exp" in guard_exp, "Expected guard expression to be in the 'exp' dict entry" + self._expression = JaniExpression(guard_exp["exp"]) + else: + raise ValueError(f"Unexpected guard_exp type {type(guard_exp)}. " + "Should be None, JaniExpression or Dict.") + + def get_expression(self) -> Optional[JaniExpression]: + return self._expression def as_dict(self, _: Optional[dict] = None): d = {} - if self.expression: - exp = self.expression.as_dict() + if self._expression: + exp = self._expression.as_dict() if (isinstance(exp, dict) and list(exp.keys()) == ['exp']): d['exp'] = exp['exp'] else: diff --git a/jani_generator/src/jani_generator/ros_helpers/ros_timer.py b/jani_generator/src/jani_generator/ros_helpers/ros_timer.py index 84c6da6c..d662ea97 100644 --- a/jani_generator/src/jani_generator/ros_helpers/ros_timer.py +++ b/jani_generator/src/jani_generator/ros_helpers/ros_timer.py @@ -95,12 +95,10 @@ def make_global_timer_automaton(timers: List[RosTimer], try: max_time = _convert_time_between_units( max_time_ns, "ns", global_timer_period_unit) - except AssertionError as exc: - # TODO what to do with exc? + except AssertionError: raise ValueError( - f"Max time {max_time_ns} cannot be converted to " +\ - f"{global_timer_period_unit}. The max_time must have a unit " +\ - "that is the same or larger than the smallest timer period.") + f"Max time {max_time_ns} cannot be converted to {global_timer_period_unit}. " + "The max_time must have a unit that is greater or equal to the smallest timer period.") # Automaton LOC_NAME = "loc" diff --git a/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py b/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py index a8586954..5e978d65 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py +++ b/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py @@ -116,7 +116,7 @@ def _append_scxml_body_to_jani_automaton(jani_automaton: JaniAutomaton, events_h new_edges.append(JaniEdge({ "location": source, "action": trigger_event_action, - "guard": guard.expression if guard is not None else None, + "guard": JaniGuard(guard), "destinations": [{ "location": None, "assignments": [] From eccf7582889c3d98c4ea12dd8b966a689b3d23d4 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 08:33:51 +0200 Subject: [PATCH 07/49] Missing supported types update Signed-off-by: Marco Lampacrescia --- as2fm_common/src/as2fm_common/common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/as2fm_common/src/as2fm_common/common.py b/as2fm_common/src/as2fm_common/common.py index f6e2d3f2..d2b79744 100644 --- a/as2fm_common/src/as2fm_common/common.py +++ b/as2fm_common/src/as2fm_common/common.py @@ -17,9 +17,9 @@ Common functionalities used throughout the toolchain. """ -from typing import Union +from typing import List, Union -ValidTypes = Union[bool, int, float] +ValidTypes = Union[bool, int, float, List[int], List[float]] """We define the basic types that are supported by the Jani language: // Types From 928b1c8f88de793cd0a2196a51b767912280b661 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 10:22:35 +0200 Subject: [PATCH 08/49] Some refinements in SCXML Ros Service definiion and fibonacci client xml example update Signed-off-by: Marco Lampacrescia --- as2fm_common/src/as2fm_common/common.py | 25 +++++--- .../client_1.scxml | 11 ++-- .../scxml_entries/scxml_ros_service.py | 58 +++++++++---------- 3 files changed, 50 insertions(+), 44 deletions(-) diff --git a/as2fm_common/src/as2fm_common/common.py b/as2fm_common/src/as2fm_common/common.py index d2b79744..b6f7a2a3 100644 --- a/as2fm_common/src/as2fm_common/common.py +++ b/as2fm_common/src/as2fm_common/common.py @@ -19,20 +19,25 @@ from typing import List, Union -ValidTypes = Union[bool, int, float, List[int], List[float]] -"""We define the basic types that are supported by the Jani language: +""" +Set of basic types that are supported by the Jani language. -// Types -// We cover only the most basic types at the moment. -// In the remainder of the specification, all requirements like "y must be of type x" are to be interpreted -// as "type x must be assignable from y's type". +Basic types (from Jani docs): +Types +We cover only the most basic types at the moment. +In the remainder of the specification, all requirements like "y must be of type x" are to be +interpreted as "type x must be assignable from y's type". var BasicType = schema([ "bool", // assignable from bool "int", // numeric; assignable from int and bounded int "real" // numeric; assignable from all numeric types ]); src https://docs.google.com/document/d/\ - 1BDQIzPBtscxJFFlDUEPIo8ivKHgXT8_X6hz5quq7jK0/edit""" + 1BDQIzPBtscxJFFlDUEPIo8ivKHgXT8_X6hz5quq7jK0/edit + +Additionally, we support the array types from the array extension. +""" +ValidTypes = Union[bool, int, float, List[int], List[float]] def ros_type_name_to_python_type(type_str: str) -> type: @@ -51,6 +56,10 @@ def ros_type_name_to_python_type(type_str: str) -> type: return int if type_str in ['float32', 'float64']: return float + if type_str in ['sequence', 'sequence']: + return List[int] + if type_str in ['sequence', 'sequence']: + return List[float] raise NotImplementedError(f"Type {type_str} not supported.") @@ -67,4 +76,4 @@ def remove_namespace(tag: str) -> str: tag_wo_ns = tag.split('}')[-1] else: tag_wo_ns = tag - return tag_wo_ns \ No newline at end of file + return tag_wo_ns diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml index 9640ac5a..cb006341 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml @@ -14,28 +14,29 @@ - + - + - + - + - + + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index a1f65fbd..234262f0 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -33,7 +33,7 @@ generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_child) + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -48,14 +48,11 @@ def get_tag_name() -> str: def from_xml_tree(xml_tree: ET.Element) -> "RosServiceServer": """Create a RosServiceServer object from an XML tree.""" assert_xml_tag_ok(RosServiceServer, xml_tree) - service_name = get_xml_argument( - RosServiceServer, xml_tree, "service_name", none_allowed=True) + service_name = read_value_from_xml_arg_or_child(RosServiceServer, xml_tree, "service_name" + (BtGetValueInputPort, str)) service_type = get_xml_argument(RosServiceServer, xml_tree, "type") service_alias = get_xml_argument( RosServiceServer, xml_tree, "name", none_allowed=True) - if service_name is None: - service_name = read_value_from_xml_child(xml_tree, "service_name", - (BtGetValueInputPort, str)) return RosServiceServer(service_name, service_type, service_alias) def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, @@ -63,7 +60,7 @@ def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, """ Initialize a new RosServiceServer object. - :param srv_name: Service name used by the service for communication. + :param srv_name: Service name used for communication. :param srv_type: ROS type of the service. :param srv_alias: Alias for the service server, for the handler to reference to it """ @@ -90,13 +87,13 @@ def get_name(self) -> str: return self._srv_alias def check_validity(self) -> bool: - valid_name = isinstance(self._srv_name, str) and len(self._srv_name) > 0 - valid_type = is_srv_type_known(self._srv_type) - if not valid_name: - print("Error: SCXML Service Server: service name is not valid.") - if not valid_type: + valid_alias = is_non_empty_string(RosServiceServer, "name", self._srv_alias) + valid_srv_name = isinstance(self._srv_name, BtGetValueInputPort) or \ + is_non_empty_string(RosServiceServer, "service_name", self._srv_name) + valid_srv_type = is_srv_type_known(self._srv_type) + if not valid_srv_type: print("Error: SCXML Service Server: service type is not valid.") - return valid_name and valid_type + return valid_alias and valid_srv_name and valid_srv_type def check_valid_instantiation(self) -> bool: """Check if the service server has undefined entries (i.e. from BT ports).""" @@ -104,7 +101,8 @@ def check_valid_instantiation(self) -> bool: def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: """Update the values of potential entries making use of BT ports.""" - pass + if isinstance(self._srv_name, BtGetValueInputPort): + self._srv_name = bt_ports_handler.get_in_port_value(self._srv_name.get_key_name()) def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot @@ -129,14 +127,11 @@ def get_tag_name() -> str: def from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": """Create a RosServiceClient object from an XML tree.""" assert_xml_tag_ok(RosServiceClient, xml_tree) - service_name = get_xml_argument( - RosServiceClient, xml_tree, "service_name", none_allowed=True) + service_name = read_value_from_xml_arg_or_child(RosServiceClient, xml_tree, "service_name", + (BtGetValueInputPort, str)) service_type = get_xml_argument(RosServiceClient, xml_tree, "type") service_alias = get_xml_argument( RosServiceClient, xml_tree, "name", none_allowed=True) - if service_name is None: - service_name = read_value_from_xml_child(xml_tree, "service_name", - (BtGetValueInputPort, str)) return RosServiceClient(service_name, service_type, service_alias) def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, @@ -144,7 +139,7 @@ def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, """ Initialize a new RosServiceClient object. - :param srv_name: Topic used by the service. + :param srv_name: Service name used for communication. :param srv_type: ROS type of the service. :param srv_alias: Alias for the service client, for the handler to reference to it """ @@ -171,13 +166,13 @@ def get_name(self) -> str: return self._srv_alias def check_validity(self) -> bool: - valid_name = isinstance(self._srv_name, str) and len(self._srv_name) > 0 - valid_type = is_srv_type_known(self._srv_type) - if not valid_name: - print("Error: SCXML Service Client: service name is not valid.") - if not valid_type: + valid_alias = is_non_empty_string(RosServiceClient, "name", self._srv_alias) + valid_srv_name = isinstance(self._srv_name, BtGetValueInputPort) or \ + is_non_empty_string(RosServiceClient, "service_name", self._srv_name) + valid_srv_type = is_srv_type_known(self._srv_type) + if not valid_srv_type: print("Error: SCXML Service Client: service type is not valid.") - return valid_name and valid_type + return valid_alias and valid_srv_name and valid_srv_type def check_valid_instantiation(self) -> bool: """Check if the topic publisher has undefined entries (i.e. from BT ports).""" @@ -185,7 +180,8 @@ def check_valid_instantiation(self) -> bool: def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: """Update the values of potential entries making use of BT ports.""" - pass + if isinstance(self._srv_name, BtGetValueInputPort): + self._srv_name = bt_ports_handler.get_in_port_value(self._srv_name.get_key_name()) def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot @@ -208,7 +204,7 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendRequest": - """Create a RosServiceServer object from an XML tree.""" + """Create a RosServiceSendRequest object from an XML tree.""" assert_xml_tag_ok(RosServiceSendRequest, xml_tree) srv_name = get_xml_argument(RosServiceSendRequest, xml_tree, "name", none_allowed=True) if srv_name is None: @@ -294,7 +290,7 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleRequest": - """Create a RosServiceServer object from an XML tree.""" + """Create a RosServiceHandleRequest object from an XML tree.""" assert_xml_tag_ok(RosServiceHandleRequest, xml_tree) srv_name = get_xml_argument(RosServiceHandleRequest, xml_tree, "name", none_allowed=True) if srv_name is None: @@ -379,7 +375,7 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendResponse": - """Create a RosServiceServer object from an XML tree.""" + """Create a RosServiceSendResponse object from an XML tree.""" assert_xml_tag_ok(RosServiceSendResponse, xml_tree) srv_name = get_xml_argument(RosServiceSendResponse, xml_tree, "name", none_allowed=True) if srv_name is None: @@ -464,7 +460,7 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleResponse": - """Create a RosServiceServer object from an XML tree.""" + """Create a RosServiceHandleResponse object from an XML tree.""" assert_xml_tag_ok(RosServiceHandleResponse, xml_tree) srv_name = get_xml_argument(RosServiceHandleResponse, xml_tree, "name", none_allowed=True) if srv_name is None: From e84d4c673a8afc6f725fceb8f8e02201a2c089fd Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 11:37:30 +0200 Subject: [PATCH 09/49] First action client scxml implementation Signed-off-by: Marco Lampacrescia --- .../client_1.scxml | 2 +- .../scxml_entries/ros_utils.py | 24 +- .../scxml_entries/scxml_ros_action_client.py | 423 ++++++++++++++++++ 3 files changed, 446 insertions(+), 3 deletions(-) create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml index cb006341..0fc88cd2 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/client_1.scxml @@ -22,7 +22,7 @@ - + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index ea2c54ef..dddef13e 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -183,9 +183,19 @@ def generate_srv_server_response_event(service_name: str) -> str: return f"srv_{sanitize_ros_interface_name(service_name)}_response" -def generate_action_goal_event(action_name: str, automaton_name: str) -> str: +def generate_action_goal_req_event(action_name: str, client_name: str) -> str: """Generate the name of the event that sends an action goal from a client to the server.""" - return f"action_{sanitize_ros_interface_name(action_name)}_goal_client_{automaton_name}" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_req_client_{client_name}" + + +def generate_action_goal_accepted_event(action_name: str, client_name: str) -> str: + """Generate the name of the event that reports goal acceptance to a client.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_accept_client_{client_name}" + + +def generate_action_goal_rejected_event(action_name: str, client_name: str) -> str: + """Generate the name of the event that reports goal rejection to a client.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_reject_client_{client_name}" def generate_action_goal_handle_event(action_name: str) -> str: @@ -193,6 +203,16 @@ def generate_action_goal_handle_event(action_name: str) -> str: return f"action_{sanitize_ros_interface_name(action_name)}_goal_handle" +def generate_action_goal_handle_accepted_event(action_name: str) -> str: + """Generate the name of the event sent from the server in case of goal acceptance.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_accepted" + + +def generate_action_goal_handle_rejected_event(action_name: str) -> str: + """Generate the name of the event sent from the server in case of goal rejection.""" + return f"action_{sanitize_ros_interface_name(action_name)}_goal_rejected" + + def generate_action_feedback_event(action_name: str) -> str: """Generate the name of the event that sends a feedback from the action server.""" return f"action_{sanitize_ros_interface_name(action_name)}_feedback" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py new file mode 100644 index 00000000..9f21c3ba --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -0,0 +1,423 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +""" +Declaration of SCXML tags related to ROS Action Clients. + +Based loosely on https://design.ros2.org/articles/actions.html +""" + +from typing import List, Optional, Union +from xml.etree import ElementTree as ET + +from scxml_converter.scxml_entries import ( + RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + as_plain_execution_body, execution_body_from_xml, valid_execution_body, + ScxmlRosDeclarationsContainer) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import ( + is_action_type_known, generate_action_goal_req_event, generate_action_goal_accepted_event, + generate_action_goal_rejected_event, generate_action_feedback_handle_event, + generate_action_result_handle_event) +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string + + +class RosActionClient(ScxmlBase): + """Object used in SCXML root to declare a new action client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_client" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": + """Create a RosActionClient object from an XML tree.""" + assert_xml_tag_ok(RosActionClient, xml_tree) + action_alias = get_xml_argument( + RosActionClient, xml_tree, "name", none_allowed=True) + action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", + (BtGetValueInputPort, str)) + action_type = get_xml_argument(RosActionClient, xml_tree, "type") + return RosActionClient(action_name, action_type, action_alias) + + def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, + action_alias: Optional[str] = None) -> None: + """ + Initialize a new RosActionClient object. + + :param action_name: Comm. interface used by the action. + :param action_type: ROS type of the service. + :param action_alias: Alias for the service client, for the handler to reference to it + """ + self._action_name = action_name + self._action_type = action_type + self._action_alias = action_alias + assert isinstance(action_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Client: invalid service name." + if self._action_alias is None: + assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ + "Error: SCXML Action Client: an alias name is required for dynamic action names." + self._action_alias = action_name + + def get_action_name(self) -> str: + """Get the name of the action.""" + return self._action_name + + def get_action_type(self) -> str: + """Get the type of the action.""" + return self._action_type + + def get_name(self) -> str: + """Get the alias of the action client.""" + return self._action_alias + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) + valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ + is_non_empty_string(RosActionClient, "action_name", self._action_name) + valid_action_type = is_action_type_known(self._action_type) + if not valid_action_type: + print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosActionClient, "action_name", self._action_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._action_name, BtGetValueInputPort): + self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." + xml_action_server = ET.Element( + RosActionClient.get_tag_name(), + {"name": self._action_alias, + "action_name": self._action_name, + "type": self._action_type}) + return xml_action_server + + +class RosActionSendGoal(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request + + +class RosActionHandleGoalResponse(ScxmlTransition): + """ + SCXML object representing the handler of an action response upon a goal request. + + A server might accept or refuse a goal request, based on its internal state. + This handler is meant to handle both acceptance or refusal of a request. + Translating this to plain-SCXML, it results to two conditional transitions. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_goal_response" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalResponse": + """Create a RosServiceServer object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleGoalResponse, xml_tree) + action_name = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "name") + accept_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "accept") + reject_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "reject") + return RosActionHandleGoalResponse(action_name, accept_target, reject_target) + + def __init__(self, action_client: Union[str, RosActionClient], + accept_target: str, reject_target: str) -> None: + """ + Initialize a new RosActionHandleGoalResponse object. + + :param action_client: Action client used by this handler, or its name. + :param accept_target: State to transition to, in case of goal acceptance. + :param reject_target: State to transition to, in case of goal refusal. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleGoalResponse, "name", action_client) + self._client_name = action_client + self._accept_target = accept_target + self._reject_target = reject_target + assert self.check_validity(), "Error: SCXML RosActionHandleGoalResponse: invalid params." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleGoalResponse, "name", self._client_name) + valid_accept = is_non_empty_string(RosActionHandleGoalResponse, "accept", + self._accept_target) + valid_reject = is_non_empty_string(RosActionHandleGoalResponse, "reject", + self._reject_target) + return valid_name and valid_accept and valid_reject + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML Service Handle Response: invalid ROS declarations container." + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + return True + + def as_plain_scxml(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> List[ScxmlTransition]: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + accept_event = generate_action_goal_accepted_event(interface_name, automaton_name) + reject_event = generate_action_goal_rejected_event(interface_name, automaton_name) + accept_transition = ScxmlTransition(self._accept_target, [accept_event]) + reject_transition = ScxmlTransition(self._reject_target, [reject_event]) + return [accept_transition, reject_transition] + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." + return ET.Element(RosActionHandleGoalResponse.get_tag_name(), + {"name": self._client_name, + "accept": self._accept_target, "reject": self._reject_target}) + + +class RosActionHandleFeedback(ScxmlTransition): + """SCXML object representing the handler of an action feedback.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_feedback" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleFeedback": + """Create a RosActionHandleFeedback object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleFeedback, xml_tree) + client_name = get_xml_argument(RosActionHandleFeedback, xml_tree, "name") + target_name = get_xml_argument(RosActionHandleFeedback, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleFeedback(client_name, target_name, exec_body) + + def __init__(self, action_client: Union[str, RosActionClient], target: str, + body: Optional[ScxmlExecutionBody] = None) -> None: + """ + Initialize a new RosActionHandleFeedback object. + + :param action_client: Action client used by this handler, or its name. + :param target: Target state to transition to after the feedback is received. + :param body: Execution body to be executed upon feedback reception (before transition). + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleFeedback, "name", action_client) + self._client_name = action_client + self._target = target + self._body = body + assert self.check_validity(), "Error: SCXML RosActionHandleFeedback: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleFeedback, "name", self._client_name) + valid_target = is_non_empty_string(RosActionHandleFeedback, "target", self._target) + valid_body = self._body is None or valid_execution_body(self._body) + if not valid_body: + print("Error: SCXML RosActionHandleFeedback: body is not valid.") + return valid_name and valid_target and valid_body + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML RosActionHandleFeedback: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML RosActionHandleFeedback: " + f"action client {self._client_name} not declared.") + return False + if not super().check_valid_ros_instantiations(ros_declarations): + print("Error: SCXML RosActionHandleFeedback: invalid ROS instantiations in exec body.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_feedback_handle_event(interface_name, automaton_name) + target = self._target + body = as_plain_execution_body(self._body, ros_declarations) + return ScxmlTransition(target, [event_name], None, body) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML RosActionHandleFeedback: invalid parameters." + xml_handle_feedback = ET.Element(RosActionHandleFeedback.get_tag_name(), + {"name": self._client_name, "target": self._target}) + if self._body is not None: + for body_elem in self._body: + xml_handle_feedback.append(body_elem.as_xml()) + return xml_handle_feedback + + +class RosActionHandleResult(ScxmlTransition): + """SCXML object representing the handler of am action result for a service client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_result" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": + """Create a RosActionHandleResult object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleResult, xml_tree) + client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") + target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleResult(client_name, target_name, exec_body) + + def __init__(self, action_client: Union[str, RosActionClient], target: str, + body: Optional[ScxmlExecutionBody] = None) -> None: + """ + Initialize a new RosActionHandleResult object. + + :param action_client: Action client used by this handler, or its name. + :param target: Target state to transition to after the feedback is received. + :param body: Execution body to be executed upon feedback reception (before transition). + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleResult, "name", action_client) + self._client_name = action_client + self._target = target + self._body = body + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) + valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) + valid_body = self._body is None or valid_execution_body(self._body) + if not valid_body: + print("Error: SCXML RosActionHandleResult: body is not valid.") + return valid_name and valid_target and valid_body + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML RosActionHandleResult: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML RosActionHandleResult: " + f"action client {self._client_name} not declared.") + return False + if not super().check_valid_ros_instantiations(ros_declarations): + print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_result_handle_event(interface_name, automaton_name) + target = self._target + body = as_plain_execution_body(self._body, ros_declarations) + return ScxmlTransition(target, [event_name], None, body) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), + {"name": self._client_name, "target": self._target}) + if self._body is not None: + for body_elem in self._body: + xml_handle_feedback.append(body_elem.as_xml()) + return xml_handle_feedback From c72e89860a256614d6340d3a64b2f55ba0f58b71 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 12:21:47 +0200 Subject: [PATCH 10/49] Fix missing comma Signed-off-by: Marco Lampacrescia --- .../scxml_converter/scxml_entries/scxml_ros_service.py | 2 +- .../src/scxml_converter/scxml_entries/xml_utils.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 234262f0..41e0da49 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -48,7 +48,7 @@ def get_tag_name() -> str: def from_xml_tree(xml_tree: ET.Element) -> "RosServiceServer": """Create a RosServiceServer object from an XML tree.""" assert_xml_tag_ok(RosServiceServer, xml_tree) - service_name = read_value_from_xml_arg_or_child(RosServiceServer, xml_tree, "service_name" + service_name = read_value_from_xml_arg_or_child(RosServiceServer, xml_tree, "service_name", (BtGetValueInputPort, str)) service_type = get_xml_argument(RosServiceServer, xml_tree, "type") service_alias = get_xml_argument( diff --git a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py index 5b325379..9700cbcb 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py @@ -63,27 +63,27 @@ def read_value_from_xml_child( """ xml_child = xml_tree.findall(child_tag) if xml_child is None or len(xml_child) == 0: - print(f"Error reading from {xml_tree.tag}: Cannot find child '{child_tag}'.") + print(f"Warn: reading from {xml_tree.tag}: Cannot find child '{child_tag}'.") return None if len(xml_child) > 1: - print(f"Error reading from {xml_tree.tag}: multiple children '{child_tag}', expected one.") + print(f"Warn: reading from {xml_tree.tag}: multiple children '{child_tag}', expected one.") return None n_tag_children = len(xml_child[0]) if n_tag_children == 0 and str in valid_types: # Try to read the text value text_value = xml_child[0].text if text_value is None or len(text_value) == 0: - print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has no text value.") + print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has no text value.") return None return text_value if n_tag_children > 1: - print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has multiple children.") + print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has multiple children.") return None # Remove string from valid types, if present valid_types = tuple(t for t in valid_types if t != str) scxml_entry = get_children_as_scxml(xml_child[0], valid_types) if len(scxml_entry) == 0: - print(f"Error reading from {xml_tree.tag}: Child '{child_tag}' has no valid children.") + print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has no valid children.") return None return scxml_entry[0] From 3c7ced25ccfb116a017e96e32c8308e4fb05e4a9 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 13:06:20 +0200 Subject: [PATCH 11/49] Define first batch of SCXML classes for action server Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 730 ++++++++++++++++++ 1 file changed, 730 insertions(+) create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py new file mode 100644 index 00000000..8d384ec4 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -0,0 +1,730 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +""" +Declaration of SCXML tags related to ROS Action Clients. + +Based loosely on https://design.ros2.org/articles/actions.html +""" + +from typing import List, Optional, Union +from xml.etree import ElementTree as ET + +from scxml_converter.scxml_entries import ( + RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + as_plain_execution_body, execution_body_from_xml, valid_execution_body, + ScxmlRosDeclarationsContainer) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import ( + is_action_type_known) +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string + + +class RosActionServer(ScxmlBase): + """Object used in SCXML root to declare a new action client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_client" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": + """Create a RosActionClient object from an XML tree.""" + assert_xml_tag_ok(RosActionClient, xml_tree) + action_alias = get_xml_argument( + RosActionClient, xml_tree, "name", none_allowed=True) + action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", + (BtGetValueInputPort, str)) + action_type = get_xml_argument(RosActionClient, xml_tree, "type") + return RosActionClient(action_name, action_type, action_alias) + + def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, + action_alias: Optional[str] = None) -> None: + """ + Initialize a new RosActionClient object. + + :param action_name: Comm. interface used by the action. + :param action_type: ROS type of the service. + :param action_alias: Alias for the service client, for the handler to reference to it + """ + self._action_name = action_name + self._action_type = action_type + self._action_alias = action_alias + assert isinstance(action_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Client: invalid service name." + if self._action_alias is None: + assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ + "Error: SCXML Action Client: an alias name is required for dynamic action names." + self._action_alias = action_name + + def get_action_name(self) -> str: + """Get the name of the action.""" + return self._action_name + + def get_action_type(self) -> str: + """Get the type of the action.""" + return self._action_type + + def get_name(self) -> str: + """Get the alias of the action client.""" + return self._action_alias + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) + valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ + is_non_empty_string(RosActionClient, "action_name", self._action_name) + valid_action_type = is_action_type_known(self._action_type) + if not valid_action_type: + print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosActionClient, "action_name", self._action_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._action_name, BtGetValueInputPort): + self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." + xml_action_server = ET.Element( + RosActionClient.get_tag_name(), + {"name": self._action_alias, + "action_name": self._action_name, + "type": self._action_type}) + return xml_action_server + + +class RosActionThread(ScxmlBase): + """Object used in SCXML root to declare a new action client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_client" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": + """Create a RosActionClient object from an XML tree.""" + assert_xml_tag_ok(RosActionClient, xml_tree) + action_alias = get_xml_argument( + RosActionClient, xml_tree, "name", none_allowed=True) + action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", + (BtGetValueInputPort, str)) + action_type = get_xml_argument(RosActionClient, xml_tree, "type") + return RosActionClient(action_name, action_type, action_alias) + + def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, + action_alias: Optional[str] = None) -> None: + """ + Initialize a new RosActionClient object. + + :param action_name: Comm. interface used by the action. + :param action_type: ROS type of the service. + :param action_alias: Alias for the service client, for the handler to reference to it + """ + self._action_name = action_name + self._action_type = action_type + self._action_alias = action_alias + assert isinstance(action_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Client: invalid service name." + if self._action_alias is None: + assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ + "Error: SCXML Action Client: an alias name is required for dynamic action names." + self._action_alias = action_name + + def get_action_name(self) -> str: + """Get the name of the action.""" + return self._action_name + + def get_action_type(self) -> str: + """Get the type of the action.""" + return self._action_type + + def get_name(self) -> str: + """Get the alias of the action client.""" + return self._action_alias + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) + valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ + is_non_empty_string(RosActionClient, "action_name", self._action_name) + valid_action_type = is_action_type_known(self._action_type) + if not valid_action_type: + print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosActionClient, "action_name", self._action_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._action_name, BtGetValueInputPort): + self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." + xml_action_server = ET.Element( + RosActionClient.get_tag_name(), + {"name": self._action_alias, + "action_name": self._action_name, + "type": self._action_type}) + return xml_action_server + + +class RosActionHandleGoalRequest(ScxmlTransition): + """ + SCXML object representing the handler of an action response upon a goal request. + + A server might accept or refuse a goal request, based on its internal state. + This handler is meant to handle both acceptance or refusal of a request. + Translating this to plain-SCXML, it results to two conditional transitions. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_goal_response" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalResponse": + """Create a RosServiceServer object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleGoalResponse, xml_tree) + action_name = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "name") + accept_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "accept") + decline_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "decline") + return RosActionHandleGoalResponse(action_name, accept_target, decline_target) + + def __init__(self, action_client: Union[str, RosActionClient], + accept_target: str, decline_target: str) -> None: + """ + Initialize a new RosActionHandleGoalResponse object. + + :param action_client: Action client used by this handler, or its name. + :param accept_target: State to transition to, in case of goal acceptance. + :param decline_target: State to transition to, in case of goal refusal. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleGoalResponse, "name", action_client) + self._client_name = action_client + self._accept_target = accept_target + self._decline_target = decline_target + assert self.check_validity(), "Error: SCXML RosActionHandleGoalResponse: invalid params." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleGoalResponse, "name", self._client_name) + valid_accept = is_non_empty_string(RosActionHandleGoalResponse, "accept", + self._accept_target) + valid_decline = is_non_empty_string(RosActionHandleGoalResponse, "decline", + self._decline_target) + return valid_name and valid_accept and valid_decline + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML Service Handle Response: invalid ROS declarations container." + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + accept_event = generate_action_goal_accepted_event(interface_name, automaton_name) + decline_event = generate_action_goal_declined_event(interface_name, automaton_name) + accept_transition = ScxmlTransition(self._accept_target, [accept_event]) + decline_transition = ScxmlTransition(self._decline_target, [decline_event]) + return [accept_transition, decline_transition] + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." + return ET.Element(RosActionHandleGoalResponse.get_tag_name(), + {"name": self._client_name, + "accept": self._accept_target, "decline": self._decline_target}) + + +class RosActionAcceptGoal(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request + + +class RosActionRejectGoal(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request + + +class RosActionStartThread(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request + + +class RosActionHandleThreadStart(ScxmlTransition): + """SCXML object representing the handler of am action result for a service client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_result" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": + """Create a RosActionHandleResult object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleResult, xml_tree) + client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") + target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleResult(client_name, target_name, exec_body) + + def __init__(self, action_client: Union[str, RosActionClient], target: str, + body: Optional[ScxmlExecutionBody] = None) -> None: + """ + Initialize a new RosActionHandleResult object. + + :param action_client: Action client used by this handler, or its name. + :param target: Target state to transition to after the feedback is received. + :param body: Execution body to be executed upon feedback reception (before transition). + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleResult, "name", action_client) + self._client_name = action_client + self._target = target + self._body = body + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) + valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) + valid_body = self._body is None or valid_execution_body(self._body) + if not valid_body: + print("Error: SCXML RosActionHandleResult: body is not valid.") + return valid_name and valid_target and valid_body + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML RosActionHandleResult: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML RosActionHandleResult: " + f"action client {self._client_name} not declared.") + return False + if not super().check_valid_ros_instantiations(ros_declarations): + print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_result_handle_event(interface_name, automaton_name) + target = self._target + body = as_plain_execution_body(self._body, ros_declarations) + return ScxmlTransition(target, [event_name], None, body) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), + {"name": self._client_name, "target": self._target}) + if self._body is not None: + for body_elem in self._body: + xml_handle_feedback.append(body_elem.as_xml()) + return xml_handle_feedback + + +class RosActionSendFeedback(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request + + +class RosActionSendResult(ScxmlSend): + """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_send_goal" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + """Create a RosActionSendGoal object from an XML tree.""" + assert_xml_tag_ok(RosActionSendGoal, xml_tree) + action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + fields: List[RosField] = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + return RosActionSendGoal(action_name, fields) + + def __init__(self, action_client: Union[str, RosActionClient], + fields: List[RosField] = None) -> None: + """ + Initialize a new RosActionSendGoal object. + + :param action_client: The ActionClient object used by the sender, or its name. + :param fields: List of fields to be sent in the goal request. + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionSendGoal, "name", action_client) + self._client_name = action_client + if fields is None: + fields = [] + self._fields = fields + assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) + valid_fields = all([isinstance(field, RosField) and field.check_validity() + for field in self._fields]) + if not valid_fields: + print("Error: SCXML service request: fields are not valid.") + return valid_name and valid_fields + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ros instantiations have been declared.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML action goal request: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML action goal request: " + f"action client {self._client_name} not declared.") + return False + if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): + print("Error: SCXML action goal request: invalid fields in request.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML action goal request: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + action_interface, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_goal_req_event(action_interface, automaton_name) + event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, event_params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." + xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { + "name": self._client_name}) + if self._fields is not None: + for field in self._fields: + xml_goal_request.append(field.as_xml()) + return xml_goal_request From c416b516448543d18c565d57844fe2dc9fdc7951 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 13:55:53 +0200 Subject: [PATCH 12/49] Implement ActionServer declaration. Prepare RosDeclaration base class to reduce code duplication Signed-off-by: Marco Lampacrescia --- .../ros_fibonacci_action_example/server.scxml | 4 +- .../scxml_entries/ros_utils.py | 74 +++++++++++++++- .../scxml_entries/scxml_ros_action_client.py | 3 +- .../scxml_entries/scxml_ros_action_server.py | 86 ++++--------------- 4 files changed, 94 insertions(+), 73 deletions(-) diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml index 77aefb71..28cf0d13 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -22,10 +22,10 @@ - + - + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index dddef13e..91ef8c75 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -15,11 +15,15 @@ """Collection of SCXML utilities related to ROS functionalities.""" -from typing import Any, Dict, List, Tuple, Type +from typing import Any, Dict, List, Optional, Union, Tuple, Type + +from scxml_converter.scxml_entries import (ScxmlBase, BtGetValueInputPort) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.scxml_ros_field import RosField -from scxml_converter.scxml_entries.utils import all_non_empty_strings +from scxml_converter.scxml_entries.utils import all_non_empty_strings, is_non_empty_string MSG_TYPE_SUBSTITUTIONS = { @@ -38,6 +42,72 @@ ] +class RosDeclaration(ScxmlBase): + """Base class for ROS declarations in SCXML.""" + + @classmethod + def get_tag_name(cls) -> str: + raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") + + def __init__(self, interface_name: Union[str, BtGetValueInputPort], interface_type: str, + interface_alias: Optional[str] = None): + """ + Constructor of ROS declaration. + + :param interface_name: Comm. interface used by the declared ROS interface. + :param interface_type: ROS type used for communication. + :param interface_alias: Alias for the defined interface, used for ref. by the the handlers + """ + self._interface_name = interface_name + self._interface_type = interface_type + self._interface_alias = interface_alias + assert isinstance(interface_name, (str, BtGetValueInputPort)), \ + f"Error: SCXML {self.get_tag_name()}: " \ + f"invalid type of interface_name {type(interface_name)}." + if self._interface_alias is None: + assert is_non_empty_string(self.__class__, "interface_name", self._interface_name), \ + f"Error: SCXML {self.get_tag_name()}: " \ + "an alias name is required for dynamic ROS interfaces." + self._interface_alias = interface_name + + def get_interface_name(self) -> str: + """Get the name of the ROS comm. interface.""" + return self._interface_name + + def get_interface_type(self) -> str: + """Get the ROS type used for communication.""" + return self._interface_type + + def get_name(self) -> str: + """Get the alias name of the ROS interface.""" + return self._interface_alias + + def check_valid_interface_type(self) -> bool: + return NotImplementedError( + f"{self.__class__.__name__} doesn't implement check_valid_interface_type.") + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(self.__class__, "name", self._interface_alias) + valid_action_name = isinstance(self._interface_name, BtGetValueInputPort) or \ + is_non_empty_string(self.__class__, "interface_name", self._interface_name) + valid_action_type = self.check_valid_interface_type() + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the interface name is still undefined (i.e. from BT ports).""" + return is_non_empty_string(self.__class__, "interface_name", self._interface_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._interface_name, BtGetValueInputPort): + self._interface_name = \ + bt_ports_handler.get_in_port_value(self._interface_name.get_key_name()) + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError(f"Error: SCXML {self.__class__} cannot be converted to plain SCXML.") + + """Container for the ROS interface (e.g. topic or service) name and the related type""" RosInterfaceAndType = Tuple[str, str] diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 9f21c3ba..6bba2803 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -48,8 +48,7 @@ def get_tag_name() -> str: def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": """Create a RosActionClient object from an XML tree.""" assert_xml_tag_ok(RosActionClient, xml_tree) - action_alias = get_xml_argument( - RosActionClient, xml_tree, "name", none_allowed=True) + action_alias = get_xml_argument(RosActionClient, xml_tree, "name", none_allowed=True) action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", (BtGetValueInputPort, str)) action_type = get_xml_argument(RosActionClient, xml_tree, "type") diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 8d384ec4..330495e9 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -29,13 +29,13 @@ from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - is_action_type_known) + RosDeclaration, is_action_type_known) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosActionServer(ScxmlBase): +class RosActionServer(RosDeclaration): """Object used in SCXML root to declare a new action client.""" @staticmethod @@ -43,76 +43,28 @@ def get_tag_name() -> str: return "ros_action_client" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": - """Create a RosActionClient object from an XML tree.""" - assert_xml_tag_ok(RosActionClient, xml_tree) - action_alias = get_xml_argument( - RosActionClient, xml_tree, "name", none_allowed=True) - action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", + def from_xml_tree(xml_tree: ET.Element) -> "RosActionServer": + """Create a RosActionServer object from an XML tree.""" + assert_xml_tag_ok(RosActionServer, xml_tree) + action_alias = get_xml_argument(RosActionServer, xml_tree, "name", none_allowed=True) + action_name = read_value_from_xml_arg_or_child(RosActionServer, xml_tree, "action_name", (BtGetValueInputPort, str)) - action_type = get_xml_argument(RosActionClient, xml_tree, "type") - return RosActionClient(action_name, action_type, action_alias) - - def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, - action_alias: Optional[str] = None) -> None: - """ - Initialize a new RosActionClient object. - - :param action_name: Comm. interface used by the action. - :param action_type: ROS type of the service. - :param action_alias: Alias for the service client, for the handler to reference to it - """ - self._action_name = action_name - self._action_type = action_type - self._action_alias = action_alias - assert isinstance(action_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Client: invalid service name." - if self._action_alias is None: - assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ - "Error: SCXML Action Client: an alias name is required for dynamic action names." - self._action_alias = action_name - - def get_action_name(self) -> str: - """Get the name of the action.""" - return self._action_name + action_type = get_xml_argument(RosActionServer, xml_tree, "type") + return RosActionServer(action_name, action_type, action_alias) - def get_action_type(self) -> str: - """Get the type of the action.""" - return self._action_type - - def get_name(self) -> str: - """Get the alias of the action client.""" - return self._action_alias - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) - valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ - is_non_empty_string(RosActionClient, "action_name", self._action_name) - valid_action_type = is_action_type_known(self._action_type) - if not valid_action_type: - print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") - return valid_alias and valid_action_name and valid_action_type - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosActionClient, "action_name", self._action_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._action_name, BtGetValueInputPort): - self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_action_type_known(self._interface_type): + print(f"Error: SCXML RosActionServer: invalid action type {self._interface_type}.") + return False + return True def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." + assert self.check_validity(), "Error: SCXML RosActionServer: invalid parameters." xml_action_server = ET.Element( - RosActionClient.get_tag_name(), - {"name": self._action_alias, - "action_name": self._action_name, - "type": self._action_type}) + RosActionServer.get_tag_name(), + {"name": self._interface_alias, + "action_name": self._interface_name, + "type": self._interface_type}) return xml_action_server From d189df3dcd62aebaf9aaf44b2a79ee199a95daab Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 14:49:47 +0200 Subject: [PATCH 13/49] Reduce code duplication in ROS declarations Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_executable_entries.py | 2 + .../scxml_entries/scxml_root.py | 16 +- .../scxml_entries/scxml_ros_action_client.py | 74 ++------ .../scxml_entries/scxml_ros_service.py | 168 ++++-------------- .../scxml_entries/scxml_ros_topic.py | 132 +++----------- 5 files changed, 81 insertions(+), 311 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index aae29039..952f324b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -249,6 +249,8 @@ def check_valid_ros_instantiations(self, _) -> bool: return True def append_param(self, param: ScxmlParam) -> None: + assert self.__class__ is ScxmlSend, \ + f"Error: SCXML send: cannot append param to derived class {self.__class__}." assert isinstance(param, ScxmlParam), "Error: SCXML send: invalid param." self._params.append(param) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index e061d4a7..6373f0e9 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -202,20 +202,20 @@ def _generate_ros_declarations_helper(self) -> Optional[ScxmlRosDeclarationsCont ros_declaration.get_rate()) elif isinstance(ros_declaration, RosTopicSubscriber): ros_decl_container.append_subscriber(ros_declaration.get_name(), - ros_declaration.get_topic_name(), - ros_declaration.get_topic_type()) + ros_declaration.get_interface_name(), + ros_declaration.get_interface_type()) elif isinstance(ros_declaration, RosTopicPublisher): ros_decl_container.append_publisher(ros_declaration.get_name(), - ros_declaration.get_topic_name(), - ros_declaration.get_topic_type()) + ros_declaration.get_interface_name(), + ros_declaration.get_interface_type()) elif isinstance(ros_declaration, RosServiceServer): ros_decl_container.append_service_server(ros_declaration.get_name(), - ros_declaration.get_service_name(), - ros_declaration.get_service_type()) + ros_declaration.get_interface_name(), + ros_declaration.get_interface_type()) elif isinstance(ros_declaration, RosServiceClient): ros_decl_container.append_service_client(ros_declaration.get_name(), - ros_declaration.get_service_name(), - ros_declaration.get_service_type()) + ros_declaration.get_interface_name(), + ros_declaration.get_interface_type()) else: raise ValueError("Error: SCXML root: invalid ROS declaration type.") return ros_decl_container diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 6bba2803..c9cf4fdb 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -23,21 +23,20 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + RosField, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - is_action_type_known, generate_action_goal_req_event, generate_action_goal_accepted_event, - generate_action_goal_rejected_event, generate_action_feedback_handle_event, - generate_action_result_handle_event) + RosDeclaration, is_action_type_known, generate_action_goal_req_event, + generate_action_goal_accepted_event, generate_action_goal_rejected_event, + generate_action_feedback_handle_event, generate_action_result_handle_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosActionClient(ScxmlBase): +class RosActionClient(RosDeclaration): """Object used in SCXML root to declare a new action client.""" @staticmethod @@ -54,66 +53,19 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": action_type = get_xml_argument(RosActionClient, xml_tree, "type") return RosActionClient(action_name, action_type, action_alias) - def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, - action_alias: Optional[str] = None) -> None: - """ - Initialize a new RosActionClient object. - - :param action_name: Comm. interface used by the action. - :param action_type: ROS type of the service. - :param action_alias: Alias for the service client, for the handler to reference to it - """ - self._action_name = action_name - self._action_type = action_type - self._action_alias = action_alias - assert isinstance(action_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Client: invalid service name." - if self._action_alias is None: - assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ - "Error: SCXML Action Client: an alias name is required for dynamic action names." - self._action_alias = action_name - - def get_action_name(self) -> str: - """Get the name of the action.""" - return self._action_name - - def get_action_type(self) -> str: - """Get the type of the action.""" - return self._action_type - - def get_name(self) -> str: - """Get the alias of the action client.""" - return self._action_alias - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) - valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ - is_non_empty_string(RosActionClient, "action_name", self._action_name) - valid_action_type = is_action_type_known(self._action_type) - if not valid_action_type: - print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") - return valid_alias and valid_action_name and valid_action_type - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosActionClient, "action_name", self._action_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._action_name, BtGetValueInputPort): - self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_action_type_known(self._interface_type): + print(f"Error: SCXML RosActionServer: invalid action type {self._interface_type}.") + return False + return True def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." xml_action_server = ET.Element( RosActionClient.get_tag_name(), - {"name": self._action_alias, - "action_name": self._action_name, - "type": self._action_type}) + {"name": self._interface_alias, + "action_name": self._interface_name, + "type": self._interface_type}) return xml_action_server diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 41e0da49..66f96316 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -24,12 +24,12 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, - as_plain_execution_body, execution_body_from_xml, valid_execution_body) + RosField, ScxmlRosDeclarationsContainer, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, + BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body) from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - ScxmlRosDeclarationsContainer, generate_srv_request_event, + RosDeclaration, generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) from scxml_converter.scxml_entries.xml_utils import ( @@ -37,7 +37,7 @@ from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosServiceServer(ScxmlBase): +class RosServiceServer(RosDeclaration): """Object used in SCXML root to declare a new service server.""" @staticmethod @@ -55,68 +55,22 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceServer": RosServiceServer, xml_tree, "name", none_allowed=True) return RosServiceServer(service_name, service_type, service_alias) - def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, - srv_alias: Optional[str] = None) -> None: - """ - Initialize a new RosServiceServer object. - - :param srv_name: Service name used for communication. - :param srv_type: ROS type of the service. - :param srv_alias: Alias for the service server, for the handler to reference to it - """ - self._srv_name = srv_name - self._srv_type = srv_type - self._srv_alias = srv_alias - assert isinstance(srv_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Server: invalid service name." - if self._srv_alias is None: - assert is_non_empty_string(RosServiceServer, "service_name", self._srv_name), \ - "Error: SCXML Service Server: an alias name is required for dynamic service names." - self._srv_alias = srv_name - - def get_service_name(self) -> str: - """Get the name of the service.""" - return self._srv_name - - def get_service_type(self) -> str: - """Get the type of the service.""" - return self._srv_type - - def get_name(self) -> str: - """Get the alias of the service server.""" - return self._srv_alias - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosServiceServer, "name", self._srv_alias) - valid_srv_name = isinstance(self._srv_name, BtGetValueInputPort) or \ - is_non_empty_string(RosServiceServer, "service_name", self._srv_name) - valid_srv_type = is_srv_type_known(self._srv_type) - if not valid_srv_type: - print("Error: SCXML Service Server: service type is not valid.") - return valid_alias and valid_srv_name and valid_srv_type - - def check_valid_instantiation(self) -> bool: - """Check if the service server has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosServiceServer, "service_name", self._srv_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._srv_name, BtGetValueInputPort): - self._srv_name = bt_ports_handler.get_in_port_value(self._srv_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_srv_type_known(self._interface_type): + print("Error: SCXML RosServiceServer: service type is not valid.") + return False + return True def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Server: invalid parameters." + assert self.check_validity(), "Error: SCXML RosServiceServer: invalid parameters." xml_srv_server = ET.Element( RosServiceServer.get_tag_name(), - {"name": self._srv_alias, "service_name": self._srv_name, "type": self._srv_type}) + {"name": self._interface_alias, + "service_name": self._interface_name, "type": self._interface_type}) return xml_srv_server -class RosServiceClient(ScxmlBase): +class RosServiceClient(RosDeclaration): """Object used in SCXML root to declare a new service client.""" @staticmethod @@ -134,64 +88,18 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": RosServiceClient, xml_tree, "name", none_allowed=True) return RosServiceClient(service_name, service_type, service_alias) - def __init__(self, srv_name: Union[str, BtGetValueInputPort], srv_type: str, - srv_alias: Optional[str] = None) -> None: - """ - Initialize a new RosServiceClient object. - - :param srv_name: Service name used for communication. - :param srv_type: ROS type of the service. - :param srv_alias: Alias for the service client, for the handler to reference to it - """ - self._srv_name = srv_name - self._srv_type = srv_type - self._srv_alias = srv_alias - assert isinstance(srv_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Client: invalid service name." - if self._srv_alias is None: - assert is_non_empty_string(RosServiceClient, "service_name", self._srv_name), \ - "Error: SCXML Service Client: an alias name is required for dynamic service names." - self._srv_alias = srv_name - - def get_service_name(self) -> str: - """Get the name of the service.""" - return self._srv_name - - def get_service_type(self) -> str: - """Get the type of the service.""" - return self._srv_type - - def get_name(self) -> str: - """Get the alias of the service client.""" - return self._srv_alias - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosServiceClient, "name", self._srv_alias) - valid_srv_name = isinstance(self._srv_name, BtGetValueInputPort) or \ - is_non_empty_string(RosServiceClient, "service_name", self._srv_name) - valid_srv_type = is_srv_type_known(self._srv_type) - if not valid_srv_type: - print("Error: SCXML Service Client: service type is not valid.") - return valid_alias and valid_srv_name and valid_srv_type - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosServiceClient, "service_name", self._srv_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._srv_name, BtGetValueInputPort): - self._srv_name = bt_ports_handler.get_in_port_value(self._srv_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_srv_type_known(self._interface_type): + print("Error: SCXML RosServiceClient: service type is not valid.") + return False + return True def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Client: invalid parameters." + assert self.check_validity(), "Error: SCXML RosServiceClient: invalid parameters." xml_srv_server = ET.Element( RosServiceClient.get_tag_name(), - {"name": self._srv_alias, "service_name": self._srv_name, "type": self._srv_type}) + {"name": self._interface_alias, + "service_name": self._interface_name, "type": self._interface_type}) return xml_srv_server @@ -238,11 +146,9 @@ def __init__(self, assert self.check_validity(), "Error: SCXML Service Send Request: invalid parameters." def check_validity(self) -> bool: - valid_name = isinstance(self._srv_name, str) and len(self._srv_name) > 0 + valid_name = is_non_empty_string(RosServiceSendRequest, "name", self._srv_name) valid_fields = self._fields is None or \ all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_name: - print("Error: SCXML service request: service name is not valid.") if not valid_fields: print("Error: SCXML service request: fields are not valid.") return valid_name and valid_fields @@ -262,6 +168,11 @@ def check_valid_ros_instantiations(self, return False return True + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + for field in self._fields: + field.update_bt_ports_values(bt_ports_handler) + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service request: invalid ROS instantiations." @@ -322,13 +233,9 @@ def __init__(self, service_decl: Union[str, RosServiceServer], target: str, assert self.check_validity(), "Error: SCXML Service Handle Request: invalid parameters." def check_validity(self) -> bool: - valid_name = isinstance(self._service_name, str) and len(self._service_name) > 0 - valid_target = isinstance(self._target, str) and len(self._target) > 0 + valid_name = is_non_empty_string(RosServiceHandleRequest, "name", self._service_name) + valid_target = is_non_empty_string(RosServiceHandleRequest, "target", self._target) valid_body = self._body is None or valid_execution_body(self._body) - if not valid_name: - print("Error: SCXML Service Handle Request: service name is not valid.") - if not valid_target: - print("Error: SCXML Service Handle Request: target is not valid.") if not valid_body: print("Error: SCXML Service Handle Request: body is not valid.") return valid_name and valid_target and valid_body @@ -409,11 +316,9 @@ def __init__(self, service_name: Union[str, RosServiceServer], assert self.check_validity(), "Error: SCXML Service Send Response: invalid parameters." def check_validity(self) -> bool: - valid_name = isinstance(self._service_name, str) and len(self._service_name) > 0 + valid_name = is_non_empty_string(RosServiceSendResponse, "name", self._service_name) valid_fields = self._fields is None or \ all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_name: - print("Error: SCXML service response: service name is not valid.") if not valid_fields: print("Error: SCXML service response: fields are not valid.") return valid_name and valid_fields @@ -433,6 +338,11 @@ def check_valid_ros_instantiations(self, return False return True + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + for field in self._fields: + field.update_bt_ports_values(bt_ports_handler) + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML service response: invalid ROS instantiations." @@ -491,13 +401,9 @@ def __init__(self, service_decl: Union[str, RosServiceClient], target: str, assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." def check_validity(self) -> bool: - valid_name = isinstance(self._service_name, str) and len(self._service_name) > 0 - valid_target = isinstance(self._target, str) and len(self._target) > 0 + valid_name = is_non_empty_string(RosServiceHandleResponse, "name", self._service_name) + valid_target = is_non_empty_string(RosServiceHandleResponse, "target", self._target) valid_body = self._body is None or valid_execution_body(self._body) - if not valid_name: - print("Error: SCXML Service Handle Response: service name is not valid.") - if not valid_target: - print("Error: SCXML Service Handle Response: target is not valid.") if not valid_body: print("Error: SCXML Service Handle Response: body is not valid.") return valid_name and valid_target and valid_body diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index e7297b05..3ce6023c 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -24,17 +24,18 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlBase, ScxmlExecutionBody, ScxmlParam, ScxmlRosDeclarationsContainer, ScxmlSend, + RosField, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlSend, ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body) from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import is_msg_type_known, sanitize_ros_interface_name +from scxml_converter.scxml_entries.ros_utils import ( + RosDeclaration, is_msg_type_known, sanitize_ros_interface_name) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml, read_value_from_xml_child) from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosTopicPublisher(ScxmlBase): +class RosTopicPublisher(RosDeclaration): """Object used in SCXML root to declare a new topic publisher.""" @staticmethod @@ -53,74 +54,22 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTopicPublisher": assert topic_name is not None, "Error: SCXML topic publisher: topic name not found." return RosTopicPublisher(topic_name, topic_type, pub_name) - def __init__(self, - topic_name: Union[str, BtGetValueInputPort], topic_type: str, - pub_name: Optional[str] = None) -> None: - """ - Create a new ros_topic_publisher object instance. - - By default, its alias is the same as the topic name, if that is defined as a string. - If the topic is defined as a BtGetValueInputPort, an alias must be provided. - - :param topic_name: The name of the topic where messages are published. - :param topic_type: The type of the message to be published - :param pub_name: Alias used to reference the publisher in SCXML. - """ - self._topic_type = topic_type - self._topic_name = topic_name - self._pub_name = pub_name - assert isinstance(self._topic_name, (str, BtGetValueInputPort)), \ - "Error: SCXML topic publisher: invalid topic name." - if self._pub_name is None: - assert is_non_empty_string(RosTopicPublisher, "topic", self._topic_name), \ - "Error: SCXML topic publisher: alias must be provided for dynamic topic names." - self._pub_name = self._topic_name - - def check_validity(self) -> bool: - valid_topic_name = isinstance(self._topic_name, BtGetValueInputPort) or \ - is_non_empty_string(RosTopicPublisher, "topic", self._topic_name) - valid_type = is_msg_type_known(self._topic_type) - valid_alias = is_non_empty_string(RosTopicPublisher, "name", self._pub_name) - if not valid_type: - print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_topic_name and valid_type and valid_alias - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosTopicPublisher, "topic", self._topic_name) - - def get_topic_name(self) -> Union[str, BtGetValueInputPort]: - """Get the name of the topic where messages are published.""" - return self._topic_name - - def get_topic_type(self) -> str: - """Get a string representation of the topic type.""" - return self._topic_type - - def get_name(self) -> str: - """Get the alias used to reference the publisher in SCXML.""" - return self._pub_name - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """ - Update the value of the BT ports used in the publisher, if any. - """ - if isinstance(self._topic_name, BtGetValueInputPort): - self._topic_name = bt_ports_handler.get_in_port_value(self._topic_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_msg_type_known(self._interface_type): + print(f"Error: SCXML RosTopicPublisher: invalid msg type {self._interface_type}.") + return False + return True def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML topic subscriber: invalid parameters." + assert self.check_validity(), "Error: RosTopicPublisher: invalid parameters." xml_topic_publisher = ET.Element( RosTopicPublisher.get_tag_name(), - {"name": self._pub_name, "topic": self._topic_name, "type": self._topic_type}) + {"name": self._interface_alias, + "topic": self._interface_name, "type": self._interface_type}) return xml_topic_publisher -class RosTopicSubscriber(ScxmlBase): +class RosTopicSubscriber(RosDeclaration): """Object used in SCXML root to declare a new topic subscriber.""" @staticmethod @@ -139,53 +88,18 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTopicSubscriber": assert topic_name is not None, "Error: SCXML topic subscriber: topic name not found." return RosTopicSubscriber(topic_name, topic_type, sub_name) - def __init__(self, topic_name: Union[str, BtGetValueInputPort], topic_type: str, - sub_name: Optional[str] = None) -> None: - self._topic_type = topic_type - self._topic_name = topic_name - self._sub_name = sub_name - assert isinstance(self._topic_name, (str, BtGetValueInputPort)), \ - "Error: SCXML topic subscriber: invalid topic name." - if self._sub_name is None: - assert is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name), \ - "Error: SCXML topic subscriber: alias must be provided for dynamic topic names." - self._sub_name = self._topic_name - - def check_validity(self) -> bool: - valid_name = isinstance(self._topic_name, BtGetValueInputPort) or \ - is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name) - valid_type = is_msg_type_known(self._topic_type) - valid_alias = is_non_empty_string(RosTopicSubscriber, "name", self._sub_name) - if not valid_type: - print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_name and valid_type and valid_alias - - def check_valid_instantiation(self) -> bool: - """Check if the topic subscriber has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosTopicSubscriber, "topic", self._topic_name) - - def get_topic_name(self) -> Union[str, BtGetValueInputPort]: - return self._topic_name - - def get_topic_type(self) -> str: - return self._topic_type - - def get_name(self) -> str: - return self._sub_name - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - pass - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") + def check_valid_interface_type(self) -> bool: + if not is_msg_type_known(self._interface_type): + print(f"Error: SCXML RosTopicSubscriber: invalid msg type {self._interface_type}.") + return False + return True def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML topic subscriber: invalid parameters." + assert self.check_validity(), "Error: SCXML RosTopicSubscriber: invalid parameters." xml_topic_subscriber = ET.Element( RosTopicSubscriber.get_tag_name(), - {"name": self._sub_name, "topic": self._topic_name, "type": self._topic_type}) + {"name": self._interface_alias, + "topic": self._interface_name, "type": self._interface_type}) return xml_topic_subscriber @@ -321,10 +235,6 @@ def check_valid_ros_instantiations(self, # TODO: Check for valid fields can be done here return topic_pub_declared - def append_param(self, param: ScxmlParam) -> None: - raise RuntimeError( - "Error: SCXML topic publish: cannot append scxml params, use append_field instead.") - def append_field(self, field: RosField) -> None: assert isinstance(field, RosField), "Error: SCXML topic publish: invalid field." if self._fields is None: From 5855ef7eb8093e87b8b677a066bbc46a6aad6a52 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 15:10:24 +0200 Subject: [PATCH 14/49] Move action server thread to separated file Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 156 -------------- .../scxml_ros_action_server_thread.py | 191 ++++++++++++++++++ 2 files changed, 191 insertions(+), 156 deletions(-) create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 330495e9..03baecc2 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -68,87 +68,6 @@ def as_xml(self) -> ET.Element: return xml_action_server -class RosActionThread(ScxmlBase): - """Object used in SCXML root to declare a new action client.""" - - @staticmethod - def get_tag_name() -> str: - return "ros_action_client" - - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": - """Create a RosActionClient object from an XML tree.""" - assert_xml_tag_ok(RosActionClient, xml_tree) - action_alias = get_xml_argument( - RosActionClient, xml_tree, "name", none_allowed=True) - action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", - (BtGetValueInputPort, str)) - action_type = get_xml_argument(RosActionClient, xml_tree, "type") - return RosActionClient(action_name, action_type, action_alias) - - def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, - action_alias: Optional[str] = None) -> None: - """ - Initialize a new RosActionClient object. - - :param action_name: Comm. interface used by the action. - :param action_type: ROS type of the service. - :param action_alias: Alias for the service client, for the handler to reference to it - """ - self._action_name = action_name - self._action_type = action_type - self._action_alias = action_alias - assert isinstance(action_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Client: invalid service name." - if self._action_alias is None: - assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ - "Error: SCXML Action Client: an alias name is required for dynamic action names." - self._action_alias = action_name - - def get_action_name(self) -> str: - """Get the name of the action.""" - return self._action_name - - def get_action_type(self) -> str: - """Get the type of the action.""" - return self._action_type - - def get_name(self) -> str: - """Get the alias of the action client.""" - return self._action_alias - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) - valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ - is_non_empty_string(RosActionClient, "action_name", self._action_name) - valid_action_type = is_action_type_known(self._action_type) - if not valid_action_type: - print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") - return valid_alias and valid_action_name and valid_action_type - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosActionClient, "action_name", self._action_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._action_name, BtGetValueInputPort): - self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") - - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." - xml_action_server = ET.Element( - RosActionClient.get_tag_name(), - {"name": self._action_alias, - "action_name": self._action_name, - "type": self._action_type}) - return xml_action_server - - class RosActionHandleGoalRequest(ScxmlTransition): """ SCXML object representing the handler of an action response upon a goal request. @@ -455,81 +374,6 @@ def as_xml(self) -> ET.Element: return xml_goal_request -class RosActionHandleThreadStart(ScxmlTransition): - """SCXML object representing the handler of am action result for a service client.""" - - @staticmethod - def get_tag_name() -> str: - return "ros_action_handle_result" - - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": - """Create a RosActionHandleResult object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleResult, xml_tree) - client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") - target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleResult(client_name, target_name, exec_body) - - def __init__(self, action_client: Union[str, RosActionClient], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: - """ - Initialize a new RosActionHandleResult object. - - :param action_client: Action client used by this handler, or its name. - :param target: Target state to transition to after the feedback is received. - :param body: Execution body to be executed upon feedback reception (before transition). - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionHandleResult, "name", action_client) - self._client_name = action_client - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) - valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML RosActionHandleResult: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML RosActionHandleResult: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML RosActionHandleResult: " - f"action client {self._client_name} not declared.") - return False - if not super().check_valid_ros_instantiations(ros_declarations): - print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") - return False - return True - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_result_handle_event(interface_name, automaton_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) - - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." - xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), - {"name": self._client_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_handle_feedback.append(body_elem.as_xml()) - return xml_handle_feedback - - class RosActionSendFeedback(ScxmlSend): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py new file mode 100644 index 00000000..0e398ec6 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -0,0 +1,191 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +""" +Declaration of SCXML tags related to ROS Action Clients. + +Based loosely on https://design.ros2.org/articles/actions.html +""" + +from typing import List, Optional, Union +from xml.etree import ElementTree as ET + +from scxml_converter.scxml_entries import ( + RosField, ScxmlRoot, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + as_plain_execution_body, execution_body_from_xml, valid_execution_body, + ScxmlRosDeclarationsContainer) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import ( + is_action_type_known) +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.utils import is_non_empty_string + + +class RosActionThread(ScxmlRoot): + """ + SCXML declaration of a set of threads for executing the action server code. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_client" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionThread": + """Create a RosActionThread object from an XML tree.""" + assert_xml_tag_ok(RosActionThread, xml_tree) + action_alias = get_xml_argument(RosActionThread, xml_tree, "name", none_allowed=True) + action_name = read_value_from_xml_arg_or_child(RosActionThread, xml_tree, "action_name", + (BtGetValueInputPort, str)) + action_type = get_xml_argument(RosActionClient, xml_tree, "type") + return RosActionClient(action_name, action_type, action_alias) + + def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, + action_alias: Optional[str] = None) -> None: + """ + Initialize a new RosActionClient object. + + :param action_name: Comm. interface used by the action. + :param action_type: ROS type of the service. + :param action_alias: Alias for the service client, for the handler to reference to it + """ + self._action_name = action_name + self._action_type = action_type + self._action_alias = action_alias + assert isinstance(action_name, (str, BtGetValueInputPort)), \ + "Error: SCXML Service Client: invalid service name." + if self._action_alias is None: + assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ + "Error: SCXML Action Client: an alias name is required for dynamic action names." + self._action_alias = action_name + + def get_action_name(self) -> str: + """Get the name of the action.""" + return self._action_name + + def get_action_type(self) -> str: + """Get the type of the action.""" + return self._action_type + + def get_name(self) -> str: + """Get the alias of the action client.""" + return self._action_alias + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) + valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ + is_non_empty_string(RosActionClient, "action_name", self._action_name) + valid_action_type = is_action_type_known(self._action_type) + if not valid_action_type: + print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + return is_non_empty_string(RosActionClient, "action_name", self._action_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._action_name, BtGetValueInputPort): + self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlRoot: + raise NotImplementedError("Error: This should return a ScxmlRoot.") + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." + xml_action_server = ET.Element( + RosActionClient.get_tag_name(), + {"name": self._action_alias, + "action_name": self._action_name, + "type": self._action_type}) + return xml_action_server + + +class RosActionHandleThreadStart(ScxmlTransition): + """SCXML object representing the handler of am action result for a service client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_result" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": + """Create a RosActionHandleResult object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleResult, xml_tree) + client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") + target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleResult(client_name, target_name, exec_body) + + def __init__(self, action_client: Union[str, RosActionClient], target: str, + body: Optional[ScxmlExecutionBody] = None) -> None: + """ + Initialize a new RosActionHandleResult object. + + :param action_client: Action client used by this handler, or its name. + :param target: Target state to transition to after the feedback is received. + :param body: Execution body to be executed upon feedback reception (before transition). + """ + if isinstance(action_client, RosActionClient): + self._client_name = action_client.get_name() + else: + assert is_non_empty_string(RosActionHandleResult, "name", action_client) + self._client_name = action_client + self._target = target + self._body = body + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) + valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) + valid_body = self._body is None or valid_execution_body(self._body) + if not valid_body: + print("Error: SCXML RosActionHandleResult: body is not valid.") + return valid_name and valid_target and valid_body + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + "Error: SCXML RosActionHandleResult: invalid ROS declarations container." + if not ros_declarations.is_action_client_defined(self._client_name): + print("Error: SCXML RosActionHandleResult: " + f"action client {self._client_name} not declared.") + return False + if not super().check_valid_ros_instantiations(ros_declarations): + print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML service response handler: invalid ROS instantiations." + automaton_name = ros_declarations.get_automaton_name() + interface_name, _ = ros_declarations.get_action_client_info(self._client_name) + event_name = generate_action_result_handle_event(interface_name, automaton_name) + target = self._target + body = as_plain_execution_body(self._body, ros_declarations) + return ScxmlTransition(target, [event_name], None, body) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), + {"name": self._client_name, "target": self._target}) + if self._body is not None: + for body_elem in self._body: + xml_handle_feedback.append(body_elem.as_xml()) + return xml_handle_feedback From a4a1a0ce8c9e47ca8a4576b6b1afd8978bf53481 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 15:19:25 +0200 Subject: [PATCH 15/49] Merge action server and its thread in the same xml file Signed-off-by: Marco Lampacrescia --- .../ros_fibonacci_action_example/server.scxml | 65 +++++++++++++++++-- .../server_execute.scxml | 57 ---------------- 2 files changed, 58 insertions(+), 64 deletions(-) delete mode 100644 jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml index 28cf0d13..eaec707c 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -6,6 +6,59 @@ model_src="" xmlns="http://www.w3.org/2005/07/scxml"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22,13 +75,11 @@ - - - + @@ -52,18 +103,18 @@ - + - + - + @@ -76,7 +127,7 @@ - + diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml deleted file mode 100644 index 3b40e27d..00000000 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/server_execute.scxml +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 0e044e3f0fafc8a21206f6c4123d00814ce9167f Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 16:39:35 +0200 Subject: [PATCH 16/49] Introduce RosCallback base class and integrate in topic callback Signed-off-by: Marco Lampacrescia --- .../properties.jani | 12 +- .../happy_clients.jani | 8 +- .../ros_example/battery_depleted.jani | 8 +- .../ros_example_w_bt/battery_properties.jani | 16 +- .../scxml_entries/ros_utils.py | 79 +-------- .../scxml_entries/scxml_ros_action_client.py | 3 +- .../scxml_entries/scxml_ros_action_server.py | 7 +- .../scxml_entries/scxml_ros_base.py | 167 ++++++++++++++++++ .../scxml_entries/scxml_ros_service.py | 5 +- .../scxml_entries/scxml_ros_topic.py | 75 ++------ .../scxml_entries/scxml_state.py | 11 +- .../gt_plain_scxml/client_1.scxml | 2 +- .../gt_plain_scxml/battery_drainer.scxml | 4 +- .../gt_plain_scxml/battery_manager.scxml | 4 +- .../gt_plain_scxml/bt_topic_action.scxml | 2 +- .../gt_plain_scxml/bt_topic_condition.scxml | 2 +- .../gt_plain_scxml/bt_topic_action.scxml | 2 +- .../test/test_systemtest_scxml_entries.py | 4 +- 18 files changed, 240 insertions(+), 171 deletions(-) create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py diff --git a/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani b/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani index 25029a3e..4239746b 100644 --- a/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani +++ b/jani_generator/test/_test_data/multiple_senders_same_event/properties.jani @@ -13,19 +13,19 @@ "op": "∧", "left": { "op": "<", - "left": "ros_topic.sender_a_counter.data", + "left": "topic_sender_a_counter_msg.data", "right": 100 }, "right": { "op": "∧", "left": { "op": "<", - "left": "ros_topic.sender_b_counter.data", + "left": "topic_sender_b_counter_msg.data", "right": 100 }, "right": { "op": "<", - "left": "ros_topic.receiver_counter.data", + "left": "topic_receiver_counter_msg.data", "right": 100 } } @@ -35,7 +35,7 @@ "op": "∧", "left": { "op": ">", - "left": "ros_topic.receiver_counter.data", + "left": "topic_receiver_counter_msg.data", "right": 48 }, "right": { @@ -43,8 +43,8 @@ "left": 50, "right": { "op": "+", - "left": "ros_topic.sender_a_counter.data", - "right": "ros_topic.sender_b_counter.data" + "left": "topic_sender_a_counter_msg.data", + "right": "topic_sender_b_counter_msg.data" } } } diff --git a/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani b/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani index 382a0882..52f43069 100644 --- a/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani @@ -13,13 +13,13 @@ "op": "∧", "left": { "op": "∧", - "left": "ros_topic.client_1_res.data", - "right": "ros_topic.client_1_res.valid" + "left": "topic_client_1_res_msg.data", + "right": "topic_client_1_res_msg.valid" }, "right": { "op": "∧", - "left": "ros_topic.client_2_res.data", - "right": "ros_topic.client_2_res.valid" + "left": "topic_client_2_res_msg.data", + "right": "topic_client_2_res_msg.valid" } } } diff --git a/jani_generator/test/_test_data/ros_example/battery_depleted.jani b/jani_generator/test/_test_data/ros_example/battery_depleted.jani index 7438d694..b4035ff1 100644 --- a/jani_generator/test/_test_data/ros_example/battery_depleted.jani +++ b/jani_generator/test/_test_data/ros_example/battery_depleted.jani @@ -13,11 +13,11 @@ "right": { "left": { "op": "≤", - "left": "ros_topic.level.data", + "left": "topic_level_msg.data", "right": 0 }, "op": "∧", - "right": "ros_topic.level.valid" + "right": "topic_level_msg.valid" } } }, @@ -39,11 +39,11 @@ "right": { "left": { "op": "≤", - "left": "ros_topic.level.data", + "left": "topic_level_msg.data", "right": -1 }, "op": "∧", - "right": "ros_topic.level.valid" + "right": "topic_level_msg.valid" } } }, diff --git a/jani_generator/test/_test_data/ros_example_w_bt/battery_properties.jani b/jani_generator/test/_test_data/ros_example_w_bt/battery_properties.jani index a8093a4f..c2d07091 100644 --- a/jani_generator/test/_test_data/ros_example_w_bt/battery_properties.jani +++ b/jani_generator/test/_test_data/ros_example_w_bt/battery_properties.jani @@ -13,11 +13,11 @@ "right": { "left": { "op": "≤", - "left": "ros_topic.level.data", + "left": "topic_level_msg.data", "right": 0 }, "op": "∧", - "right": "ros_topic.level.valid" + "right": "topic_level_msg.valid" } } }, @@ -39,11 +39,11 @@ "right": { "left": { "op": "<", - "left": "ros_topic.level.data", + "left": "topic_level_msg.data", "right": 20 }, "op": "∧", - "right": "ros_topic.level.valid" + "right": "topic_level_msg.valid" } } }, @@ -64,8 +64,8 @@ "op": "U", "right": { "op": "∧", - "left": "ros_topic.alarm.data", - "right": "ros_topic.charge.valid" + "left": "topic_alarm_msg.data", + "right": "topic_charge_msg.valid" } } }, @@ -90,11 +90,11 @@ "right": { "left": { "op": "=", - "left": "ros_topic.level.data", + "left": "topic_level_msg.data", "right": 100 }, "op": "∧", - "right": "ros_topic.level.valid" + "right": "topic_level_msg.valid" } } }, diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 91ef8c75..fe3d7ac6 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -15,15 +15,11 @@ """Collection of SCXML utilities related to ROS functionalities.""" -from typing import Any, Dict, List, Optional, Union, Tuple, Type - -from scxml_converter.scxml_entries import (ScxmlBase, BtGetValueInputPort) - -from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from typing import Any, Dict, List, Tuple, Type from scxml_converter.scxml_entries.scxml_ros_field import RosField -from scxml_converter.scxml_entries.utils import all_non_empty_strings, is_non_empty_string +from scxml_converter.scxml_entries.utils import all_non_empty_strings MSG_TYPE_SUBSTITUTIONS = { @@ -42,72 +38,6 @@ ] -class RosDeclaration(ScxmlBase): - """Base class for ROS declarations in SCXML.""" - - @classmethod - def get_tag_name(cls) -> str: - raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") - - def __init__(self, interface_name: Union[str, BtGetValueInputPort], interface_type: str, - interface_alias: Optional[str] = None): - """ - Constructor of ROS declaration. - - :param interface_name: Comm. interface used by the declared ROS interface. - :param interface_type: ROS type used for communication. - :param interface_alias: Alias for the defined interface, used for ref. by the the handlers - """ - self._interface_name = interface_name - self._interface_type = interface_type - self._interface_alias = interface_alias - assert isinstance(interface_name, (str, BtGetValueInputPort)), \ - f"Error: SCXML {self.get_tag_name()}: " \ - f"invalid type of interface_name {type(interface_name)}." - if self._interface_alias is None: - assert is_non_empty_string(self.__class__, "interface_name", self._interface_name), \ - f"Error: SCXML {self.get_tag_name()}: " \ - "an alias name is required for dynamic ROS interfaces." - self._interface_alias = interface_name - - def get_interface_name(self) -> str: - """Get the name of the ROS comm. interface.""" - return self._interface_name - - def get_interface_type(self) -> str: - """Get the ROS type used for communication.""" - return self._interface_type - - def get_name(self) -> str: - """Get the alias name of the ROS interface.""" - return self._interface_alias - - def check_valid_interface_type(self) -> bool: - return NotImplementedError( - f"{self.__class__.__name__} doesn't implement check_valid_interface_type.") - - def check_validity(self) -> bool: - valid_alias = is_non_empty_string(self.__class__, "name", self._interface_alias) - valid_action_name = isinstance(self._interface_name, BtGetValueInputPort) or \ - is_non_empty_string(self.__class__, "interface_name", self._interface_name) - valid_action_type = self.check_valid_interface_type() - return valid_alias and valid_action_name and valid_action_type - - def check_valid_instantiation(self) -> bool: - """Check if the interface name is still undefined (i.e. from BT ports).""" - return is_non_empty_string(self.__class__, "interface_name", self._interface_name) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._interface_name, BtGetValueInputPort): - self._interface_name = \ - bt_ports_handler.get_in_port_value(self._interface_name.get_key_name()) - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError(f"Error: SCXML {self.__class__} cannot be converted to plain SCXML.") - - """Container for the ROS interface (e.g. topic or service) name and the related type""" RosInterfaceAndType = Tuple[str, str] @@ -233,6 +163,11 @@ def sanitize_ros_interface_name(interface_name: str) -> str: return interface_name.replace("/", "__") +def generate_topic_event(topic_name: str) -> str: + """Generate the name of the event that triggers a message reception from a topic.""" + return f"topic_{sanitize_ros_interface_name(topic_name)}_msg" + + def generate_srv_request_event(service_name: str, automaton_name: str) -> str: """Generate the name of the event that triggers a service request.""" return f"srv_{sanitize_ros_interface_name(service_name)}_req_client_{automaton_name}" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index c9cf4fdb..3d136636 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -26,9 +26,10 @@ RosField, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration from scxml_converter.scxml_entries.ros_utils import ( - RosDeclaration, is_action_type_known, generate_action_goal_req_event, + is_action_type_known, generate_action_goal_req_event, generate_action_goal_accepted_event, generate_action_goal_rejected_event, generate_action_feedback_handle_event, generate_action_result_handle_event) from scxml_converter.scxml_entries.xml_utils import ( diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 03baecc2..004afff7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -27,9 +27,10 @@ as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration + from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import ( - RosDeclaration, is_action_type_known) +from scxml_converter.scxml_entries.ros_utils import (is_action_type_known) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -79,7 +80,7 @@ class RosActionHandleGoalRequest(ScxmlTransition): @staticmethod def get_tag_name() -> str: - return "ros_action_handle_goal_response" + return "ros_action_handle_goal" @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalResponse": diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py new file mode 100644 index 00000000..a67085ee --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -0,0 +1,167 @@ +# Copyright (c) 2024 - for information on the respective copyright owner +# see the NOTICE file + +# 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. + +"""Collection of SCXML ROS Base classes to derive from.""" + +from typing import Optional, Union, Type + +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlTransition, ScxmlExecutionBody, BtGetValueInputPort, + ScxmlRosDeclarationsContainer, as_plain_execution_body, valid_execution_body) + +from scxml_converter.scxml_entries.bt_utils import BtPortsHandler + +from scxml_converter.scxml_entries.utils import is_non_empty_string + + +class RosDeclaration(ScxmlBase): + """Base class for ROS declarations in SCXML.""" + + @classmethod + def get_tag_name(cls) -> str: + raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") + + def __init__(self, interface_name: Union[str, BtGetValueInputPort], interface_type: str, + interface_alias: Optional[str] = None): + """ + Constructor of ROS declaration. + + :param interface_name: Comm. interface used by the declared ROS interface. + :param interface_type: ROS type used for communication. + :param interface_alias: Alias for the defined interface, used for ref. by the the handlers + """ + self._interface_name = interface_name + self._interface_type = interface_type + self._interface_alias = interface_alias + assert isinstance(interface_name, (str, BtGetValueInputPort)), \ + f"Error: SCXML {self.get_tag_name()}: " \ + f"invalid type of interface_name {type(interface_name)}." + if self._interface_alias is None: + assert is_non_empty_string(self.__class__, "interface_name", self._interface_name), \ + f"Error: SCXML {self.get_tag_name()}: " \ + "an alias name is required for dynamic ROS interfaces." + self._interface_alias = interface_name + + def get_interface_name(self) -> str: + """Get the name of the ROS comm. interface.""" + return self._interface_name + + def get_interface_type(self) -> str: + """Get the ROS type used for communication.""" + return self._interface_type + + def get_name(self) -> str: + """Get the alias name of the ROS interface.""" + return self._interface_alias + + def check_valid_interface_type(self) -> bool: + return NotImplementedError( + f"{self.__class__.__name__} doesn't implement check_valid_interface_type.") + + def check_validity(self) -> bool: + valid_alias = is_non_empty_string(self.__class__, "name", self._interface_alias) + valid_action_name = isinstance(self._interface_name, BtGetValueInputPort) or \ + is_non_empty_string(self.__class__, "interface_name", self._interface_name) + valid_action_type = self.check_valid_interface_type() + return valid_alias and valid_action_name and valid_action_type + + def check_valid_instantiation(self) -> bool: + """Check if the interface name is still undefined (i.e. from BT ports).""" + return is_non_empty_string(self.__class__, "interface_name", self._interface_name) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + """Update the values of potential entries making use of BT ports.""" + if isinstance(self._interface_name, BtGetValueInputPort): + self._interface_name = \ + bt_ports_handler.get_in_port_value(self._interface_name.get_key_name()) + + def as_plain_scxml(self, _) -> ScxmlBase: + # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot + raise RuntimeError(f"Error: SCXML {self.__class__} cannot be converted to plain SCXML.") + + +class RosCallback(ScxmlTransition): + """Base class for ROS callbacks in SCXML.""" + + @classmethod + def get_tag_name(cls) -> str: + """XML tag name for the ROS callback type.""" + raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") + + @classmethod + def get_declaration_type(cls) -> Type[RosDeclaration]: + """ + Get the type of ROS declaration related to the callback. + + Examples: RosSubscriber, RosPublisher, ... + """ + raise NotImplementedError(f"{cls.__name__} doesn't implement get_declaration_type.") + + def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str, + exec_body: Optional[ScxmlExecutionBody] = None) -> None: + """ + Constructor of ROS callback. + + :param interface_decl: ROS interface declaration to be used in the callback, or its name. + :param target_state: Name of the state to transition to after the callback. + :param exec_body: Executable body of the callback. + """ + if isinstance(interface_decl, self.get_declaration_type()): + self._interface_name = interface_decl.get_name() + else: + assert is_non_empty_string(self.__class__, "name", interface_decl) + self._interface_name = interface_decl + self._target = target_state + self._body = exec_body + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(self.__class__, "name", self._interface_name) + valid_target = is_non_empty_string(self.__class__, "target", self._target) + valid_body = self._body is None or valid_execution_body(self._body) + if not valid_body: + print(f"Error: SCXML {self.__class__}: invalid entries in executable body.") + return valid_name and valid_target and valid_body + + def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ROS interface used in the callback exists.""" + raise NotImplementedError( + f"{self.__class__.__name__} doesn't implement check_valid_interface.") + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + """Translate the ROS interface name to a plain scxml event.""" + raise NotImplementedError( + f"{self.__class__.__name__} doesn't implement get_plain_scxml_event.") + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ROS entries in the callback are correctly defined.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + f"Error: SCXML {self.__class__}: invalid type of ROS declarations container." + if not self.check_valid_interface(ros_declarations): + print(f"Error: SCXML {self.__class__}: undefined ROS interface {self._interface_name}.") + return False + valid_body = super().check_valid_ros_instantiations(ros_declarations) + if not valid_body: + print(f"Error: SCXML {self.__class__}: body has invalid ROS instantiations.") + return valid_body + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlBase: + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML topic callback: invalid ROS instantiations." + event_name = self.get_plain_scxml_event(ros_declarations) + target = self._target + body = as_plain_execution_body(self._body, ros_declarations) + return ScxmlTransition(target, [event_name], None, body) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 66f96316..5663fc28 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -27,10 +27,11 @@ RosField, ScxmlRosDeclarationsContainer, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body) +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration + from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - RosDeclaration, generate_srv_request_event, - generate_srv_response_event, generate_srv_server_request_event, + generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index 3ce6023c..66861136 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -20,16 +20,16 @@ https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html """ -from typing import List, Optional, Union +from typing import List, Optional, Union, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlSend, - ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, - valid_execution_body) + RosField, ScxmlRosDeclarationsContainer, ScxmlSend, BtGetValueInputPort, + execution_body_from_xml) +from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosDeclaration + from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import ( - RosDeclaration, is_msg_type_known, sanitize_ros_interface_name) +from scxml_converter.scxml_entries.ros_utils import (is_msg_type_known, generate_topic_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml, read_value_from_xml_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -103,13 +103,17 @@ def as_xml(self) -> ET.Element: return xml_topic_subscriber -class RosTopicCallback(ScxmlTransition): +class RosTopicCallback(RosCallback): """Object representing a transition to perform when a new ROS msg is received.""" @staticmethod def get_tag_name() -> str: return "ros_topic_callback" + @staticmethod + def get_declaration_type() -> Type[RosDeclaration]: + return RosTopicSubscriber + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosTopicCallback": """Create a RosTopicCallback object from an XML tree.""" @@ -123,61 +127,16 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTopicCallback": exec_body = execution_body_from_xml(xml_tree) return RosTopicCallback(sub_name, target, exec_body) - def __init__( - self, topic_sub: Union[RosTopicSubscriber, str], target: str, - body: Optional[ScxmlExecutionBody] = None): - """ - Create a new ros_topic_callback object instance. - - :param topic_sub: The RosTopicSubscriber instance triggering the callback, or its name - :param target: The target state of the transition - :param body: Execution body executed at the time the received message gets processed - """ - if isinstance(topic_sub, RosTopicSubscriber): - self._sub_name = topic_sub.get_name() - else: - # Used for generating ROS entries from xml file - assert is_non_empty_string(RosTopicCallback, "name", topic_sub) - self._sub_name = topic_sub - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." - - def check_validity(self) -> bool: - valid_sub_name = is_non_empty_string(RosTopicCallback, "name", self._sub_name) - valid_target = is_non_empty_string(RosTopicCallback, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML topic callback: body is not valid.") - return valid_sub_name and valid_target and valid_body + def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_subscriber_defined(self._interface_name) - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML topic callback: invalid ROS declarations container." - topic_cb_declared = ros_declarations.is_subscriber_defined(self._sub_name) - if not topic_cb_declared: - print(f"Error: SCXML topic callback: topic subscriber {self._sub_name} not declared.") - return False - valid_body = super().check_valid_ros_instantiations(ros_declarations) - if not valid_body: - print("Error: SCXML topic callback: body has invalid ROS instantiations.") - return valid_body - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML topic callback: invalid ROS instantiations." - topic_name, _ = ros_declarations.get_subscriber_info(self._sub_name) - event_name = "ros_topic." + sanitize_ros_interface_name(topic_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_topic_event(ros_declarations.get_subscriber_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." xml_topic_callback = ET.Element( - "ros_topic_callback", {"name": self._sub_name, "target": self._target}) + "ros_topic_callback", {"name": self._interface_name, "target": self._target}) if self._body is not None: for entry in self._body: xml_topic_callback.append(entry.as_xml()) @@ -250,7 +209,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx assert self.check_valid_ros_instantiations(ros_declarations), \ "Error: SCXML topic publish: invalid ROS instantiations." topic_name, _ = ros_declarations.get_publisher_info(self._pub_name) - event_name = "ros_topic." + sanitize_ros_interface_name(topic_name) + event_name = generate_topic_event(topic_name) params = None if self._fields is None else \ [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, params) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index 6fd2da19..1e8aec1b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -106,10 +106,15 @@ def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: for entry in self._on_exit: entry.update_bt_ports_values(bt_ports_handler) - @classmethod - def _transitions_from_xml(cls, xml_tree: ET.Element) -> List[ScxmlTransition]: + @staticmethod + def _transitions_from_xml(xml_tree: ET.Element) -> List[ScxmlTransition]: + from scxml_converter.scxml_entries.scxml_ros_base import RosCallback transitions: List[ScxmlTransition] = [] - tag_to_cls = {cls.get_tag_name(): cls for cls in ScxmlTransition.__subclasses__()} + tag_to_cls = {cls.get_tag_name(): cls + for cls in ScxmlTransition.__subclasses__() + if cls != RosCallback} + # TODO: Temporary workaroud, to fix once all ROS callbacks are implemented + tag_to_cls.update({cls.get_tag_name(): cls for cls in RosCallback.__subclasses__()}) tag_to_cls.update({ScxmlTransition.get_tag_name(): ScxmlTransition}) for child in xml_tree: if child.tag in tag_to_cls: diff --git a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml index b5c3d0c4..fca9af5c 100644 --- a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml +++ b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml @@ -12,7 +12,7 @@ - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml index 3ce31988..9144e2f8 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_drainer.scxml @@ -12,14 +12,14 @@ - + - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml index 9ce00239..b91434e3 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/battery_manager.scxml @@ -7,8 +7,8 @@ xmlns="http://www.w3.org/2005/07/scxml"> - - + + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml index afc8a495..13bbf09f 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml @@ -2,7 +2,7 @@ - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml index e81ee036..aec642b6 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml @@ -5,7 +5,7 @@ - + diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml index 7ca960ef..753d90f4 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml @@ -6,7 +6,7 @@ - + diff --git a/scxml_converter/test/test_systemtest_scxml_entries.py b/scxml_converter/test/test_systemtest_scxml_entries.py index c0fc9a02..a136fa17 100644 --- a/scxml_converter/test/test_systemtest_scxml_entries.py +++ b/scxml_converter/test/test_systemtest_scxml_entries.py @@ -59,11 +59,11 @@ def test_battery_drainer_from_code(): ScxmlData("battery_percent", "100", "int16")])) use_battery_state = ScxmlState( "use_battery", - on_entry=[ScxmlSend("ros_topic.level", + on_entry=[ScxmlSend("topic_level_msg", [ScxmlParam("data", expr="battery_percent")])], body=[ScxmlTransition("use_battery", ["ros_time_rate.my_timer"], body=[ScxmlAssign("battery_percent", "battery_percent - 1")]), - ScxmlTransition("use_battery", ["ros_topic.charge"], + ScxmlTransition("use_battery", ["topic_charge_msg"], body=[ScxmlAssign("battery_percent", "100")])]) battery_drainer_scxml.add_state(use_battery_state, initial=True) _test_scxml_from_code(battery_drainer_scxml, os.path.join( From 93eb0049aaec682706efca3d557ecebcd60a500e Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 17:08:39 +0200 Subject: [PATCH 17/49] Action server goal handle implemented Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 93 ++++++------------- .../scxml_entries/scxml_ros_topic.py | 6 +- 2 files changed, 32 insertions(+), 67 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 004afff7..f9a9d937 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -19,7 +19,7 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Union +from typing import List, Optional, Union, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( @@ -27,10 +27,12 @@ as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import (is_action_type_known) +from scxml_converter.scxml_entries.ros_utils import ( + is_action_type_known, generate_action_goal_handle_event, + generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -69,13 +71,11 @@ def as_xml(self) -> ET.Element: return xml_action_server -class RosActionHandleGoalRequest(ScxmlTransition): +class RosActionHandleGoalRequest(RosCallback): """ - SCXML object representing the handler of an action response upon a goal request. + SCXML object representing the handler for a goal request. - A server might accept or refuse a goal request, based on its internal state. - This handler is meant to handle both acceptance or refusal of a request. - Translating this to plain-SCXML, it results to two conditional transitions. + A server receives the request, containing the action goal fields and the client ID. """ @staticmethod @@ -83,68 +83,33 @@ def get_tag_name() -> str: return "ros_action_handle_goal" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalResponse": - """Create a RosServiceServer object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleGoalResponse, xml_tree) - action_name = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "name") - accept_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "accept") - decline_target = get_xml_argument(RosActionHandleGoalResponse, xml_tree, "decline") - return RosActionHandleGoalResponse(action_name, accept_target, decline_target) + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer - def __init__(self, action_client: Union[str, RosActionClient], - accept_target: str, decline_target: str) -> None: - """ - Initialize a new RosActionHandleGoalResponse object. - - :param action_client: Action client used by this handler, or its name. - :param accept_target: State to transition to, in case of goal acceptance. - :param decline_target: State to transition to, in case of goal refusal. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionHandleGoalResponse, "name", action_client) - self._client_name = action_client - self._accept_target = accept_target - self._decline_target = decline_target - assert self.check_validity(), "Error: SCXML RosActionHandleGoalResponse: invalid params." + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalRequest": + """Create a RosActionHandleGoalRequest object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleGoalRequest, xml_tree) + server_name = get_xml_argument(RosActionHandleGoalRequest, xml_tree, "name") + target_name = get_xml_argument(RosActionHandleGoalRequest, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleGoalRequest(server_name, target_name, exec_body) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionHandleGoalResponse, "name", self._client_name) - valid_accept = is_non_empty_string(RosActionHandleGoalResponse, "accept", - self._accept_target) - valid_decline = is_non_empty_string(RosActionHandleGoalResponse, "decline", - self._decline_target) - return valid_name and valid_accept and valid_decline + def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML Service Handle Response: invalid ROS declarations container." - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - return True - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_action_client_info(self._client_name) - accept_event = generate_action_goal_accepted_event(interface_name, automaton_name) - decline_event = generate_action_goal_declined_event(interface_name, automaton_name) - accept_transition = ScxmlTransition(self._accept_target, [accept_event]) - decline_transition = ScxmlTransition(self._decline_target, [decline_event]) - return [accept_transition, decline_transition] + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_goal_handle_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." - return ET.Element(RosActionHandleGoalResponse.get_tag_name(), - {"name": self._client_name, - "accept": self._accept_target, "decline": self._decline_target}) + xml_goal_handler = ET.Element(RosActionHandleGoalRequest.get_tag_name(), + {"name": self._interface_name, "target": self._target}) + if self._body is not None: + for entry in self._body: + xml_goal_handler.append(entry.as_xml()) + return xml_goal_handler class RosActionAcceptGoal(ScxmlSend): diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index 66861136..d3dced4c 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -111,7 +111,7 @@ def get_tag_name() -> str: return "ros_topic_callback" @staticmethod - def get_declaration_type() -> Type[RosDeclaration]: + def get_declaration_type() -> Type[RosTopicSubscriber]: return RosTopicSubscriber @staticmethod @@ -135,8 +135,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." - xml_topic_callback = ET.Element( - "ros_topic_callback", {"name": self._interface_name, "target": self._target}) + xml_topic_callback = ET.Element(RosTopicCallback.get_tag_name(), + {"name": self._interface_name, "target": self._target}) if self._body is not None: for entry in self._body: xml_topic_callback.append(entry.as_xml()) From fa2e1d0518240a5f1f77fa4ed0bde1ea0836f745 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 17:33:55 +0200 Subject: [PATCH 18/49] Base class for ROS interface trigger Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_base.py | 105 ++++++++++++++++-- .../scxml_entries/scxml_ros_topic.py | 75 +++---------- 2 files changed, 116 insertions(+), 64 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index a67085ee..75c64259 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -15,10 +15,10 @@ """Collection of SCXML ROS Base classes to derive from.""" -from typing import Optional, Union, Type +from typing import Optional, List, Union, Type from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlTransition, ScxmlExecutionBody, BtGetValueInputPort, + ScxmlBase, ScxmlTransition, ScxmlSend, ScxmlExecutionBody, RosField, BtGetValueInputPort, ScxmlRosDeclarationsContainer, as_plain_execution_body, valid_execution_body) from scxml_converter.scxml_entries.bt_utils import BtPortsHandler @@ -135,10 +135,10 @@ def check_validity(self) -> bool: print(f"Error: SCXML {self.__class__}: invalid entries in executable body.") return valid_name and valid_target and valid_body - def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ROS interface used in the callback exists.""" raise NotImplementedError( - f"{self.__class__.__name__} doesn't implement check_valid_interface.") + f"{self.__class__.__name__} doesn't implement check_interface_defined.") def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: """Translate the ROS interface name to a plain scxml event.""" @@ -150,18 +150,109 @@ def check_valid_ros_instantiations(self, """Check if the ROS entries in the callback are correctly defined.""" assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ f"Error: SCXML {self.__class__}: invalid type of ROS declarations container." - if not self.check_valid_interface(ros_declarations): + if not self.check_interface_defined(ros_declarations): print(f"Error: SCXML {self.__class__}: undefined ROS interface {self._interface_name}.") return False valid_body = super().check_valid_ros_instantiations(ros_declarations) if not valid_body: - print(f"Error: SCXML {self.__class__}: body has invalid ROS instantiations.") + print(f"Error: SCXML {self.__class__}: " + f"body of {self._interface_name} has invalid ROS instantiations.") return valid_body def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlBase: assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML topic callback: invalid ROS instantiations." + f"Error: SCXML {self.__class__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) target = self._target body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], None, body) + + +class RosTrigger(ScxmlSend): + """Base class for ROS triggers in SCXML.""" + + @classmethod + def get_tag_name(cls) -> str: + """XML tag name for the ROS trigger type.""" + raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") + + @classmethod + def get_declaration_type(cls) -> Type[RosDeclaration]: + """ + Get the type of ROS declaration related to the trigger. + + Examples: RosServiceClient, RosActionClient, ... + """ + raise NotImplementedError(f"{cls.__name__} doesn't implement get_declaration_type.") + + def __init__(self, interface_decl: Union[str, RosDeclaration], + fields: Optional[List[RosField]] = None) -> None: + """ + Constructor of a generic ROS trigger. + + :param interface_decl: ROS interface declaration to be used in the trigger, or its name. + :param fields: Name of fields that are sent together with the trigger. + """ + if isinstance(interface_decl, self.get_declaration_type()): + self._interface_name = interface_decl.get_name() + else: + assert is_non_empty_string(self.__class__, "name", interface_decl) + self._interface_name = interface_decl + if fields is None: + fields = [] + self._fields: List[RosField] = fields + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + + def append_field(self, field: RosField) -> None: + assert isinstance(field, RosField), "Error: SCXML topic publish: invalid field." + if self._fields is None: + self._fields = [] + self._fields.append(field) + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): + """Update the values of potential entries making use of BT ports.""" + for field in self._fields: + field.update_bt_ports_values(bt_ports_handler) + + def check_validity(self) -> bool: + valid_name = is_non_empty_string(self.__class__, "name", self._interface_name) + valid_fields = all(isinstance(field, RosField) for field in self._fields) + if not valid_fields: + print(f"Error: SCXML {self.__class__}: " + f"invalid entries in fields of {self._interface_name}.") + return valid_name and valid_fields + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ROS interface used in the trigger exists.""" + raise NotImplementedError( + f"{self.__class__.__name__} doesn't implement check_interface_defined.") + + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if all fields are assigned, given the ROS interface definition.""" + raise NotImplementedError( + f"{self.__class__.__name__} doesn't implement check_fields_validity.") + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + """Translate the ROS interface name to a plain scxml event.""" + raise NotImplementedError( + f"{self.__class__.__name__} doesn't implement get_plain_scxml_event.") + + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the ROS entries in the trigger are correctly defined.""" + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ + f"Error: SCXML {self.__class__}: invalid type of ROS declarations container." + if not self.check_interface_defined(ros_declarations): + print(f"Error: SCXML {self.__class__}: undefined ROS interface {self._interface_name}.") + return False + if not self.check_fields_validity(ros_declarations): + print(f"Error: SCXML {self.__class__}: invalid fields for {self._interface_name}.") + return False + return True + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlBase: + assert self.check_valid_ros_instantiations(ros_declarations), \ + f"Error: SCXML {self.__class__}: invalid ROS instantiations." + event_name = self.get_plain_scxml_event(ros_declarations) + params = [field.as_plain_scxml(ros_declarations) for field in self._fields] + return ScxmlSend(event_name, params) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index d3dced4c..5c0602c5 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -20,19 +20,17 @@ https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html """ -from typing import List, Optional, Union, Type +from typing import List, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( RosField, ScxmlRosDeclarationsContainer, ScxmlSend, BtGetValueInputPort, execution_body_from_xml) -from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosDeclaration +from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosTrigger, RosDeclaration -from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import (is_msg_type_known, generate_topic_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml, read_value_from_xml_child) -from scxml_converter.scxml_entries.utils import is_non_empty_string class RosTopicPublisher(RosDeclaration): @@ -127,7 +125,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTopicCallback": exec_body = execution_body_from_xml(xml_tree) return RosTopicCallback(sub_name, target, exec_body) - def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_subscriber_defined(self._interface_name) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: @@ -143,13 +141,17 @@ def as_xml(self) -> ET.Element: return xml_topic_callback -class RosTopicPublish(ScxmlSend): +class RosTopicPublish(RosTrigger): """Object representing the shipping of a ROS msg through a topic.""" @staticmethod def get_tag_name() -> str: return "ros_topic_publish" + @staticmethod + def get_declaration_type() -> Type[RosTopicPublisher]: + return RosTopicPublisher + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> ScxmlSend: """Create a RosTopicPublish object from an XML tree.""" @@ -162,61 +164,20 @@ def from_xml_tree(xml_tree: ET.Element) -> ScxmlSend: fields: List[RosField] = get_children_as_scxml(xml_tree, (RosField,)) return RosTopicPublish(pub_name, fields) - def __init__(self, topic_pub: Union[RosTopicPublisher, str], - fields: Optional[List[RosField]] = None) -> None: - if fields is None: - fields = [] - if isinstance(topic_pub, RosTopicPublisher): - self._pub_name = topic_pub.get_name() - else: - # Used for generating ROS entries from xml file - assert is_non_empty_string(RosTopicPublish, "name", topic_pub) - self._pub_name = topic_pub - self._fields = fields - assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_publisher_defined(self._interface_name) - def check_validity(self) -> bool: - valid_pub_name = is_non_empty_string(RosTopicPublish, "name", self._pub_name) - valid_fields = self._fields is None or \ - all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_fields: - print("Error: SCXML topic publish: fields are not valid.") - return valid_pub_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML topic publish: invalid ROS declarations container." - topic_pub_declared = ros_declarations.is_publisher_defined(self._pub_name) - if not topic_pub_declared: - print(f"Error: SCXML topic publish: topic publisher {self._pub_name} not declared.") - # TODO: Check for valid fields can be done here - return topic_pub_declared - - def append_field(self, field: RosField) -> None: - assert isinstance(field, RosField), "Error: SCXML topic publish: invalid field." - if self._fields is None: - self._fields = [] - self._fields.append(field) - - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): - """Update the values of potential entries making use of BT ports.""" - for field in self._fields: - field.update_bt_ports_values(bt_ports_handler) - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML topic publish: invalid ROS instantiations." - topic_name, _ = ros_declarations.get_publisher_info(self._pub_name) - event_name = generate_topic_event(topic_name) - params = None if self._fields is None else \ - [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, params) + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + # TODO: CHeck fields for topics, too + return True + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_topic_event(ros_declarations.get_publisher_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." - xml_topic_publish = ET.Element(RosTopicPublish.get_tag_name(), {"name": self._pub_name}) + xml_topic_publish = ET.Element(RosTopicPublish.get_tag_name(), + {"name": self._interface_name}) if self._fields is not None: for field in self._fields: xml_topic_publish.append(field.as_xml()) From dbd0f4ee14153cca973a19363998d54314f8daca Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Tue, 20 Aug 2024 17:56:53 +0200 Subject: [PATCH 19/49] Implement accept and reject action server responses Signed-off-by: Marco Lampacrescia --- .../ros_fibonacci_action_example/server.scxml | 10 +- .../scxml_entries/scxml_ros_action_server.py | 177 ++++++------------ 2 files changed, 64 insertions(+), 123 deletions(-) diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml index eaec707c..48887805 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -77,6 +77,7 @@ + @@ -102,8 +103,9 @@ - - + + + @@ -114,7 +116,9 @@ - + + + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index f9a9d937..d707ae16 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -27,7 +27,7 @@ as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( @@ -75,7 +75,8 @@ class RosActionHandleGoalRequest(RosCallback): """ SCXML object representing the handler for a goal request. - A server receives the request, containing the action goal fields and the client ID. + A server receives the request, containing the action goal fields and the goal_id. + The goal_id is set from the action handler, based on the client. """ @staticmethod @@ -95,7 +96,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalRequest": exec_body = execution_body_from_xml(xml_tree) return RosActionHandleGoalRequest(server_name, target_name, exec_body) - def check_valid_interface(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: @@ -112,156 +113,92 @@ def as_xml(self) -> ET.Element: return xml_goal_handler -class RosActionAcceptGoal(ScxmlSend): - """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" +class RosActionAcceptGoal(RosTrigger): + """ + Object representing the SCXML ROS Event sent from the server when an action Goal is accepted. + """ @staticmethod def get_tag_name() -> str: - return "ros_action_send_goal" + return "ros_action_accept_goal" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionAcceptGoal": """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + assert_xml_tag_ok(RosActionAcceptGoal, xml_tree) + action_name = get_xml_argument(RosActionAcceptGoal, xml_tree, "name") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) - - def __init__(self, action_client: Union[str, RosActionClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosActionSendGoal object. + return RosActionAcceptGoal(action_name, fields) - :param action_client: The ActionClient object used by the sender, or its name. - :param fields: List of fields to be sent in the goal request. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") - return False - return True + def check_fields_validity(self, _) -> bool: + # When accepting the goal, we send only the goal_id of the accepted goal + return len(self._fields) == 1 and self._fields[0].get_name() == "goal_id" - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_goal_handle_accepted_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) - if self._fields is not None: - for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + assert self.check_fields_validity(None), "Error: SCXML action goal Request: invalid fields." + xml_goal_accepted = ET.Element(RosActionAcceptGoal.get_tag_name(), + {"name": self._interface_name}) + xml_goal_accepted.append(self._fields[0].as_xml()) + return xml_goal_accepted -class RosActionRejectGoal(ScxmlSend): +class RosActionRejectGoal(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" + """ + Object representing the SCXML ROS Event sent from the server when an action Goal is accepted. + """ + @staticmethod def get_tag_name() -> str: - return "ros_action_send_goal" + return "ros_action_reject_goal" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionRejectGoal": """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + assert_xml_tag_ok(RosActionRejectGoal, xml_tree) + action_name = get_xml_argument(RosActionRejectGoal, xml_tree, "name") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) - - def __init__(self, action_client: Union[str, RosActionClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosActionSendGoal object. + return RosActionRejectGoal(action_name, fields) - :param action_client: The ActionClient object used by the sender, or its name. - :param fields: List of fields to be sent in the goal request. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") - return False - return True + def check_fields_validity(self, _) -> bool: + # When accepting the goal, we send only the goal_id of the accepted goal + return len(self._fields) == 1 and self._fields[0].get_name() == "goal_id" - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_goal_handle_accepted_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) - if self._fields is not None: - for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + assert self.check_fields_validity(None), "Error: SCXML action goal Request: invalid fields." + xml_goal_accepted = ET.Element(RosActionRejectGoal.get_tag_name(), + {"name": self._interface_name}) + xml_goal_accepted.append(self._fields[0].as_xml()) + return xml_goal_accepted class RosActionStartThread(ScxmlSend): From a8a3f764899a73563bdd018340aecd0bad66fa44 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Wed, 21 Aug 2024 10:25:21 +0200 Subject: [PATCH 20/49] RosActionStartThread Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 5 + .../scxml_entries/scxml_ros_action_server.py | 97 ++++++++----------- 2 files changed, 48 insertions(+), 54 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index fe3d7ac6..41004295 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -218,6 +218,11 @@ def generate_action_goal_handle_rejected_event(action_name: str) -> str: return f"action_{sanitize_ros_interface_name(action_name)}_goal_rejected" +def generate_action_thread_execution_start_event(action_name: str, thread_id: str) -> str: + """Generate the name of the event that triggers the start of an action thread execution.""" + return f"action_{sanitize_ros_interface_name(action_name)}_thread_{thread_id}_start" + + def generate_action_feedback_event(action_name: str) -> str: """Generate the name of the event that sends a feedback from the action server.""" return f"action_{sanitize_ros_interface_name(action_name)}_feedback" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index d707ae16..b08f73bd 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -32,7 +32,8 @@ from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_handle_event, - generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event) + generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, + generate_action_thread_execution_start_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -157,10 +158,9 @@ def as_xml(self) -> ET.Element: class RosActionRejectGoal(RosTrigger): - """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" """ - Object representing the SCXML ROS Event sent from the server when an action Goal is accepted. + Object representing the SCXML ROS Event sent from the server when an action Goal is rejected. """ @staticmethod @@ -189,7 +189,7 @@ def check_fields_validity(self, _) -> bool: return len(self._fields) == 1 and self._fields[0].get_name() == "goal_id" def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: - return generate_action_goal_handle_accepted_event( + return generate_action_goal_handle_rejected_event( ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: @@ -201,80 +201,69 @@ def as_xml(self) -> ET.Element: return xml_goal_accepted -class RosActionStartThread(ScxmlSend): - """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" +class RosActionStartThread(RosTrigger): + """ + Object representing the request, from an action server, to start a new execute thread instance. + """ @staticmethod def get_tag_name() -> str: - return "ros_action_send_goal" + return "ros_action_start_thread" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionStartThread": + """Create a RosActionStartThread object from an XML tree.""" + assert_xml_tag_ok(RosActionStartThread, xml_tree) + action_name = get_xml_argument(RosActionStartThread, xml_tree, "name") + thread_id = get_xml_argument(RosActionStartThread, xml_tree, "thread_id") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) + return RosActionStartThread(action_name, thread_id, fields) - def __init__(self, action_client: Union[str, RosActionClient], + def __init__(self, action_name: Union[str, RosActionServer], thread_id: str, fields: List[RosField] = None) -> None: """ - Initialize a new RosActionSendGoal object. + Initialize a new RosActionStartThread object. - :param action_client: The ActionClient object used by the sender, or its name. + :param action_name: The ActionServer object used by the sender, or its name. + :param thread_id: The ID of the new thread instance. :param fields: List of fields to be sent in the goal request. """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." + self._thread_id = thread_id + super().__init__(action_name, fields) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the action server has been declared.""" + return ros_declarations.is_action_server_defined(self._interface_name) - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the goal_id and the request fields have been defined.""" + if not any([field.get_name() == "goal_id" for field in self._fields]): + print(f"Error: SCXML {self.__class__}: goal_id not defined.") return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") + goal_fields = [field for field in self._fields if field.get_name() != "goal_id"] + if not ros_declarations.check_valid_action_goal_fields(self._interface_name, goal_fields): + print(f"Error: SCXML {self.__class__}: invalid fields in goal request.") return False return True - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_execution_start_event( + ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_thread_start_req = ET.Element(RosActionStartThread.get_tag_name(), { + "name": self._interface_name, "thread_id": self._thread_id}) if self._fields is not None: for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + xml_thread_start_req.append(field.as_xml()) + return xml_thread_start_req class RosActionSendFeedback(ScxmlSend): From eb98056b38b4c2f17814cc61f557cd0e563557c6 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Wed, 21 Aug 2024 16:29:18 +0200 Subject: [PATCH 21/49] Finish entries for Action server Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 46 ++++- .../scxml_entries/scxml_ros_action_server.py | 189 ++++++------------ 2 files changed, 97 insertions(+), 138 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 41004295..fa75835d 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -432,20 +432,43 @@ def check_valid_srv_res_fields(self, server_name: str, ros_fields: List[RosField return False return True - def check_valid_action_goal_fields(self, client_name: str, ros_fields: List[RosField]) -> bool: - """Check if the provided fields match the action goal type.""" + def check_valid_action_goal_fields( + self, client_name: str, ros_fields: List[RosField], has_goal_id: bool = False) -> bool: + """ + Check if the provided fields match with the action type's goal entries. + + :param client_name: Name of the action client. + :param ros_fields: List of fields to check. + :param has_goal_id: Whether the goal_id shall be included among the fields. + """ _, action_type = self.get_action_client_info(client_name) goal_fields, _, _ = get_action_type_params(action_type) + # We use the goal ID as a reserved field for the action. Make sure it is available. + assert "goal_id" not in goal_fields, \ + f"Error: SCXML ROS declarations: action {action_type} goal has the 'goal_id' field." + if has_goal_id: + goal_fields["goal_id"] = "int32" if not check_all_fields_known(ros_fields, goal_fields): print(f"Error: SCXML ROS declarations: Action goal {client_name} has invalid fields.") return False return True def check_valid_action_feedback_fields( - self, server_name: str, ros_fields: List[RosField]) -> bool: - """Check if the provided fields match the action feedback type.""" + self, server_name: str, ros_fields: List[RosField], has_goal_id: bool = False) -> bool: + """ + Check if the provided fields match with the action type's feedback entries. + + :param client_name: Name of the action client. + :param ros_fields: List of fields to check. + :param has_goal_id: Whether the goal_id shall be included among the fields. + """ _, action_type = self.get_action_server_info(server_name) _, feedback_fields, _ = get_action_type_params(action_type) + # We use the goal ID as a reserved field for the action. Make sure it is available. + assert "goal_id" not in feedback_fields, \ + f"Error: SCXML ROS declarations: action {action_type} feedback has the 'goal_id' field." + if has_goal_id: + feedback_fields["goal_id"] = "int32" if not check_all_fields_known(ros_fields, feedback_fields): print(f"Error: SCXML ROS declarations: Action feedback {server_name} " "has invalid fields.") @@ -453,10 +476,21 @@ def check_valid_action_feedback_fields( return True def check_valid_action_result_fields( - self, server_name: str, ros_fields: List[RosField]) -> bool: - """Check if the provided fields match the action result type.""" + self, server_name: str, ros_fields: List[RosField], has_goal_id: bool = False) -> bool: + """ + Check if the provided fields match with the action type's result entries. + + :param client_name: Name of the action client. + :param ros_fields: List of fields to check. + :param has_goal_id: Whether the goal_id shall be included among the fields. + """ _, action_type = self.get_action_server_info(server_name) _, _, result_fields = get_action_type_params(action_type) + # We use the goal ID as a reserved field for the action. Make sure it is available. + assert "goal_id" not in result_fields, \ + f"Error: SCXML ROS declarations: action {action_type} feedback has the 'goal_id' field." + if has_goal_id: + result_fields["goal_id"] = "int32" if not check_all_fields_known(ros_fields, result_fields): print(f"Error: SCXML ROS declarations: Action result {server_name} has invalid fields.") return False diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index b08f73bd..7b368f30 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -19,24 +19,20 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Union, Type +from typing import List, Union, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlBase, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, - as_plain_execution_body, execution_body_from_xml, valid_execution_body, - ScxmlRosDeclarationsContainer) + RosField, BtGetValueInputPort, ScxmlRosDeclarationsContainer, execution_body_from_xml) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger -from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_handle_event, generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, generate_action_thread_execution_start_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) -from scxml_converter.scxml_entries.utils import is_non_empty_string class RosActionServer(RosDeclaration): @@ -243,12 +239,10 @@ def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContaine def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the goal_id and the request fields have been defined.""" - if not any([field.get_name() == "goal_id" for field in self._fields]): - print(f"Error: SCXML {self.__class__}: goal_id not defined.") - return False - goal_fields = [field for field in self._fields if field.get_name() != "goal_id"] - if not ros_declarations.check_valid_action_goal_fields(self._interface_name, goal_fields): - print(f"Error: SCXML {self.__class__}: invalid fields in goal request.") + if not ros_declarations.check_valid_action_goal_fields(self._interface_name, self._fields, + has_goal_id=True): + print(f"Error: SCXML {self.__class__}: " + f"invalid fields in goal request {self._interface_name}.") return False return True @@ -258,161 +252,92 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_xml(self) -> ET.Element: assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." - xml_thread_start_req = ET.Element(RosActionStartThread.get_tag_name(), { - "name": self._interface_name, "thread_id": self._thread_id}) + xml_thread_start_req = ET.Element(RosActionStartThread.get_tag_name(), + {"name": self._interface_name, + "thread_id": self._thread_id}) if self._fields is not None: for field in self._fields: xml_thread_start_req.append(field.as_xml()) return xml_thread_start_req -class RosActionSendFeedback(ScxmlSend): +class RosActionSendFeedback(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" @staticmethod def get_tag_name() -> str: - return "ros_action_send_goal" + return "ros_action_feedback" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendFeedback": + """Create a RosActionSendFeedback object from an XML tree.""" + assert_xml_tag_ok(RosActionSendFeedback, xml_tree) + action_name = get_xml_argument(RosActionSendFeedback, xml_tree, "name") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) + return RosActionSendFeedback(action_name, fields) - def __init__(self, action_client: Union[str, RosActionClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosActionSendGoal object. - - :param action_client: The ActionClient object used by the sender, or its name. - :param fields: List of fields to be sent in the goal request. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") - return False - return True + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the goal_id and the request fields have been defined.""" + if not ros_declarations.check_valid_action_feedback_fields(self._interface_name, + self._fields, has_goal_id=True): + print(f"Error: SCXML {self.__class__}: " + f"invalid fields in feedback request {self._interface_name}.") def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_action_feedback = ET.Element(RosActionSendFeedback.get_tag_name(), + {"name": self._interface_name}) if self._fields is not None: for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + xml_action_feedback.append(field.as_xml()) + return xml_action_feedback -class RosActionSendResult(ScxmlSend): +class RosActionSendResult(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" @staticmethod def get_tag_name() -> str: - return "ros_action_send_goal" + return "ros_action_succeed" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendResult": + """Create a RosActionSendResult object from an XML tree.""" + assert_xml_tag_ok(RosActionSendResult, xml_tree) + action_name = get_xml_argument(RosActionSendResult, xml_tree, "name") fields: List[RosField] = [] for field_xml in xml_tree: fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) + return RosActionSendResult(action_name, fields) - def __init__(self, action_client: Union[str, RosActionClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosActionSendGoal object. - - :param action_client: The ActionClient object used by the sender, or its name. - :param fields: List of fields to be sent in the goal request. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") - return False - return True + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the goal_id and the request fields have been defined.""" + if not ros_declarations.check_valid_action_result_fields(self._interface_name, + self._fields, has_goal_id=True): + print(f"Error: SCXML {self.__class__}: " + f"invalid fields in result request {self._interface_name}.") def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_action_result = ET.Element(RosActionSendResult.get_tag_name(), + {"name": self._interface_name}) if self._fields is not None: for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + xml_action_result.append(field.as_xml()) + return xml_action_result From 9b23b35053a9ba0bb18e2569a3b421a763b7dbff Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Wed, 21 Aug 2024 17:35:00 +0200 Subject: [PATCH 22/49] Start looking into the server thread functionalities Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_root.py | 38 +++++++++---------- .../scxml_ros_action_server_thread.py | 15 ++++++-- 2 files changed, 28 insertions(+), 25 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 6373f0e9..a778bdfd 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -27,7 +27,8 @@ RosTimeRate, RosTopicPublisher, RosTopicSubscriber, ScxmlBase, ScxmlDataModel, ScxmlRosDeclarations, ScxmlRosDeclarationsContainer, ScxmlState) -from scxml_converter.scxml_entries.xml_utils import get_children_as_scxml +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_children_as_scxml, get_xml_argument) from scxml_converter.scxml_entries.scxml_bt import BtPortDeclarations from scxml_converter.scxml_entries.bt_utils import BtPortsHandler @@ -44,15 +45,15 @@ def get_tag_name() -> str: def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": """Create a ScxmlRoot object from an XML tree.""" # --- Get the ElementTree objects - assert xml_tree.tag == ScxmlRoot.get_tag_name(), \ - f"Error: SCXML root: XML root tag {xml_tree.tag} is not {ScxmlRoot.get_tag_name()}." - assert "name" in xml_tree.attrib, \ - "Error: SCXML root: 'name' attribute not found in input xml." - assert "version" in xml_tree.attrib and xml_tree.attrib["version"] == "1.0", \ - "Error: SCXML root: 'version' attribute not found or invalid in input xml." + assert_xml_tag_ok(ScxmlRoot, xml_tree) + scxml_name = get_xml_argument(ScxmlRoot, xml_tree, "name") + scxml_version = get_xml_argument(ScxmlRoot, xml_tree, "version") + assert scxml_version == "1.0", \ + f"Error: SCXML root: expected version 1.0, found {scxml_version}." + scxml_init_state = get_xml_argument(ScxmlRoot, xml_tree, "initial") # Data Model - datamodel_elements = xml_tree.findall(ScxmlDataModel.get_tag_name()) - assert datamodel_elements is None or len(datamodel_elements) <= 1, \ + datamodel_elements = get_children_as_scxml(xml_tree, (ScxmlDataModel,)) + assert len(datamodel_elements) <= 1, \ f"Error: SCXML root: {len(datamodel_elements)} datamodels found, max 1 allowed." # ROS Declarations ros_declarations: List[ScxmlRosDeclarations] = get_children_as_scxml( @@ -61,26 +62,21 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": bt_port_declarations: List[BtPortDeclarations] = get_children_as_scxml( xml_tree, get_args(BtPortDeclarations)) # States - assert "initial" in xml_tree.attrib, \ - "Error: SCXML root: 'initial' attribute not found in input xml." - initial_state = xml_tree.attrib["initial"] - state_elements = xml_tree.findall(ScxmlState.get_tag_name()) - assert state_elements is not None and len(state_elements) > 0, \ - "Error: SCXML root: no state found in input xml." + scxml_states: List[ScxmlState] = get_children_as_scxml(xml_tree, (ScxmlState,)) + assert len(scxml_states) > 0, "Error: SCXML root: no state found in input xml." # --- Fill Data in the ScxmlRoot object - scxml_root = ScxmlRoot(xml_tree.attrib["name"]) + scxml_root = ScxmlRoot(scxml_name) # Data Model - if datamodel_elements is not None and len(datamodel_elements) > 0: - scxml_root.set_data_model(ScxmlDataModel.from_xml_tree(datamodel_elements[0])) + if len(datamodel_elements) > 0: + scxml_root.set_data_model(datamodel_elements[0]) # ROS Declarations scxml_root._ros_declarations = ros_declarations # BT Declarations for bt_port_declaration in bt_port_declarations: scxml_root.add_bt_port_declaration(bt_port_declaration) # States - for state_element in state_elements: - scxml_state = ScxmlState.from_xml_tree(state_element) - is_initial = scxml_state.get_id() == initial_state + for scxml_state in scxml_states: + is_initial = scxml_state.get_id() == scxml_init_state scxml_root.add_state(scxml_state, initial=is_initial) return scxml_root diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 0e398ec6..190879c5 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -23,7 +23,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlRoot, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, + ScxmlRoot, ScxmlDataModel, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body, ScxmlRosDeclarationsContainer) @@ -31,7 +31,7 @@ from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known) from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -42,13 +42,20 @@ class RosActionThread(ScxmlRoot): @staticmethod def get_tag_name() -> str: - return "ros_action_client" + return "ros_action_thread" @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosActionThread": """Create a RosActionThread object from an XML tree.""" assert_xml_tag_ok(RosActionThread, xml_tree) - action_alias = get_xml_argument(RosActionThread, xml_tree, "name", none_allowed=True) + action_alias = get_xml_argument(RosActionThread, xml_tree, "name") + n_threads = get_xml_argument(RosActionThread, xml_tree, "n_threads") + initial_state = get_xml_argument(RosActionThread, xml_tree, "initial_state") + datamodel = get_children_as_scxml(xml_tree, (ScxmlDataModel,)) + ros_declarations: List[ScxmlRosDeclarations] = get_children_as_scxml( + xml_tree, get_args(ScxmlRosDeclarations)) + # TODO: Append the action server to the ROS declarations in the thread, somehow + action_name = read_value_from_xml_arg_or_child(RosActionThread, xml_tree, "action_name", (BtGetValueInputPort, str)) action_type = get_xml_argument(RosActionClient, xml_tree, "type") From 920912d2b41264c74817fe1e81ffb3e167aa775d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Wed, 21 Aug 2024 19:09:45 +0200 Subject: [PATCH 23/49] Finish designing preliminary interface of RosActionThread Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_root.py | 8 +- .../scxml_ros_action_server_thread.py | 151 ++++++++++-------- 2 files changed, 90 insertions(+), 69 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index a778bdfd..b1edbf31 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -27,11 +27,11 @@ RosTimeRate, RosTopicPublisher, RosTopicSubscriber, ScxmlBase, ScxmlDataModel, ScxmlRosDeclarations, ScxmlRosDeclarationsContainer, ScxmlState) -from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_children_as_scxml, get_xml_argument) - from scxml_converter.scxml_entries.scxml_bt import BtPortDeclarations from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_children_as_scxml, get_xml_argument) +from scxml_converter.scxml_entries.utils import is_non_empty_string class ScxmlRoot(ScxmlBase): @@ -111,7 +111,7 @@ def get_name(self) -> str: def set_name(self, name: str) -> None: """Rename the automaton represented by this SCXML model.""" - assert isinstance(name, str) and len(name) > 0, "Error: SCXML root: invalid name." + assert is_non_empty_string(ScxmlRoot, "name", name) self._name = name def get_initial_state_id(self) -> str: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 190879c5..89e539a4 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -19,19 +19,18 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Union +from typing import List, Optional, Tuple, Union, get_args from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlRoot, ScxmlDataModel, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, - as_plain_execution_body, execution_body_from_xml, valid_execution_body, - ScxmlRosDeclarationsContainer) + ScxmlRoot, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlTransition, + ScxmlRosDeclarationsContainer, as_plain_execution_body, + execution_body_from_xml, valid_execution_body) +from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import ( - is_action_type_known) from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child, get_children_as_scxml) + assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -50,77 +49,99 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosActionThread": assert_xml_tag_ok(RosActionThread, xml_tree) action_alias = get_xml_argument(RosActionThread, xml_tree, "name") n_threads = get_xml_argument(RosActionThread, xml_tree, "n_threads") + n_threads = int(n_threads) + assert n_threads > 0, f"Error: SCXML Action Thread: invalid n. of threads ({n_threads})." initial_state = get_xml_argument(RosActionThread, xml_tree, "initial_state") datamodel = get_children_as_scxml(xml_tree, (ScxmlDataModel,)) - ros_declarations: List[ScxmlRosDeclarations] = get_children_as_scxml( - xml_tree, get_args(ScxmlRosDeclarations)) - # TODO: Append the action server to the ROS declarations in the thread, somehow + # ros declarations and bt ports are expected to be defined in the parent tag (scxml_root) + scxml_states: List[ScxmlState] = get_children_as_scxml(xml_tree, (ScxmlState,)) + assert len(datamodel) <= 1, "Error: SCXML Action Thread: multiple datamodels." + assert scxml_states > 0, "Error: SCXML Action Thread: no states defined." + # The non-plain SCXML Action thread has the same name as the action + scxml_action_thread = RosActionThread(action_alias, n_threads) + # Fill the thread with the states and datamodel + if len(datamodel) == 1: + scxml_action_thread.set_data_model(datamodel[0]) + for scxml_state in scxml_states: + is_initial = scxml_state.get_id() == initial_state + scxml_action_thread.add_state(scxml_state, initial=is_initial) + return scxml_action_thread - action_name = read_value_from_xml_arg_or_child(RosActionThread, xml_tree, "action_name", - (BtGetValueInputPort, str)) - action_type = get_xml_argument(RosActionClient, xml_tree, "type") - return RosActionClient(action_name, action_type, action_alias) + @staticmethod + def from_scxml_file(_): + raise RuntimeError("Error: Cannot load a RosActionThread directly from SCXML file.") - def __init__(self, action_name: Union[str, BtGetValueInputPort], action_type: str, - action_alias: Optional[str] = None) -> None: + def __init__(self, action_server: Union[str, RosActionServer], n_threads: int) -> None: """ - Initialize a new RosActionClient object. + Initialize a new RosActionThread object. - :param action_name: Comm. interface used by the action. - :param action_type: ROS type of the service. - :param action_alias: Alias for the service client, for the handler to reference to it + :param action_server: ActionServer declaration, or its alias name. + :param n_threads: Max. n. of parallel action requests that can be handled. """ - self._action_name = action_name - self._action_type = action_type - self._action_alias = action_alias - assert isinstance(action_name, (str, BtGetValueInputPort)), \ - "Error: SCXML Service Client: invalid service name." - if self._action_alias is None: - assert is_non_empty_string(RosActionClient, "action_name", self._action_name), \ - "Error: SCXML Action Client: an alias name is required for dynamic action names." - self._action_alias = action_name - - def get_action_name(self) -> str: - """Get the name of the action.""" - return self._action_name - - def get_action_type(self) -> str: - """Get the type of the action.""" - return self._action_type - - def get_name(self) -> str: - """Get the alias of the action client.""" - return self._action_alias + if isinstance(action_server, RosActionServer): + action_name = action_server.get_name() + else: + assert is_non_empty_string(RosActionThread, "name", action_server) + action_name = action_server + self._n_threads = n_threads + super().__init__(action_name) + del self._ros_declarations # The ROS declarations should be externally provided + del self._bt_ports_handler # The BT ports should be externally provided + + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: + # TODO + pass def check_validity(self) -> bool: - valid_alias = is_non_empty_string(RosActionClient, "name", self._action_alias) - valid_action_name = isinstance(self._action_name, BtGetValueInputPort) or \ - is_non_empty_string(RosActionClient, "action_name", self._action_name) - valid_action_type = is_action_type_known(self._action_type) - if not valid_action_type: - print(f"Error: SCXML Action Client: action type {self._action_type} is not valid.") - return valid_alias and valid_action_name and valid_action_type - - def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" - return is_non_empty_string(RosActionClient, "action_name", self._action_name) + # TODO + pass - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - """Update the values of potential entries making use of BT ports.""" - if isinstance(self._action_name, BtGetValueInputPort): - self._action_name = bt_ports_handler.get_in_port_value(self._action_name.get_key_name()) + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + # TODO + pass - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlRoot: - raise NotImplementedError("Error: This should return a ScxmlRoot.") + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> List[ScxmlRoot]: + """Convert the ROS-specific entries to be plain SCXML""" + # TODO + pass def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." - xml_action_server = ET.Element( - RosActionClient.get_tag_name(), - {"name": self._action_alias, - "action_name": self._action_name, - "type": self._action_type}) - return xml_action_server + assert self.check_validity(), "SCXML: found invalid state object." + # TODO + pass + + # Disable a bunch of unneeded methods + def instantiate_bt_events(self, _) -> None: + raise RuntimeError("Error: SCXML Action Thread: deleted method 'instantiate_bt_events'.") + + def add_ros_declaration(self, _): + raise RuntimeError("Error: SCXML Action Thread: deleted method 'add_ros_declaration'.") + + def add_bt_port_declaration(self, _): + raise RuntimeError("Error: SCXML Action Thread: deleted method 'add_bt_port_declaration'.") + + def set_bt_port_value(self, _, __): + raise RuntimeError("Error: SCXML Action Thread: deleted method 'set_bt_port_values'.") + + def set_bt_ports_values(self, _): + raise RuntimeError("Error: SCXML Action Thread: deleted method 'set_bt_ports_values'.") + + def _generate_ros_declarations_helper(self): + raise RuntimeError("Error: SCXML Action Thread: deleted method " + "'_generate_ros_declarations_helper'.") + + def _check_valid_ros_declarations(self): + raise RuntimeError( + "Error: SCXML Action Thread: deleted method '_check_valid_ros_declarations': " + "use 'check_valid_ros_instantiations' instead (no underscore).") + + def is_plain_scxml(self): + raise RuntimeError("Error: SCXML Action Thread: deleted method 'is_plain_scxml'.") + + def to_plain_scxml_and_declarations(self): + raise RuntimeError("Error: SCXML Action Thread: deleted method " + "'to_plain_scxml_and_declarations'.") class RosActionHandleThreadStart(ScxmlTransition): From 0817a1fba41bce4e6596a33fd74cddf271723147 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 08:56:48 +0200 Subject: [PATCH 24/49] Avoid inheriting from ScxmlRoot Signed-off-by: Marco Lampacrescia --- .../scxml_ros_action_server_thread.py | 73 ++++++++----------- 1 file changed, 29 insertions(+), 44 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 89e539a4..dc658139 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -23,7 +23,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlRoot, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlTransition, + ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlTransition, ScxmlRosDeclarationsContainer, as_plain_execution_body, execution_body_from_xml, valid_execution_body) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer @@ -34,7 +34,7 @@ from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosActionThread(ScxmlRoot): +class RosActionThread(ScxmlBase): """ SCXML declaration of a set of threads for executing the action server code. """ @@ -78,15 +78,27 @@ def __init__(self, action_server: Union[str, RosActionServer], n_threads: int) - :param action_server: ActionServer declaration, or its alias name. :param n_threads: Max. n. of parallel action requests that can be handled. """ + self._name: str = "" if isinstance(action_server, RosActionServer): - action_name = action_server.get_name() + self._name = action_server.get_name() else: assert is_non_empty_string(RosActionThread, "name", action_server) - action_name = action_server + self._name = action_server self._n_threads = n_threads - super().__init__(action_name) - del self._ros_declarations # The ROS declarations should be externally provided - del self._bt_ports_handler # The BT ports should be externally provided + self._initial_state: Optional[str] = None + self._datamodel: Optional[ScxmlDataModel] = None + self._states: List[Tuple[ScxmlState, bool]] = [] + + def add_state(self, state: ScxmlState, *, initial: bool = False): + """Append a state to the list of states. If initial is True, set it as the initial state.""" + self._states.append(state) + if initial: + assert self._initial_state is None, "Error: SCXML root: Initial state already set" + self._initial_state = state.get_id() + + def set_data_model(self, data_model: ScxmlDataModel): + assert self._data_model is None, "Data model already set" + self._data_model = data_model def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: # TODO @@ -96,53 +108,26 @@ def check_validity(self) -> bool: # TODO pass - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer + ) -> bool: # TODO pass - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> List[ScxmlRoot]: - """Convert the ROS-specific entries to be plain SCXML""" + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> List[ScxmlBase]: + """ + Convert the ROS-specific entries to be plain SCXML. + + This returns a list of ScxmlRoot objects, using ScxmlBase to avoid circular dependencies. + """ + from scxml_converter.scxml_entries import ScxmlRoot # TODO - pass + return [ScxmlRoot("name")] def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid state object." # TODO pass - # Disable a bunch of unneeded methods - def instantiate_bt_events(self, _) -> None: - raise RuntimeError("Error: SCXML Action Thread: deleted method 'instantiate_bt_events'.") - - def add_ros_declaration(self, _): - raise RuntimeError("Error: SCXML Action Thread: deleted method 'add_ros_declaration'.") - - def add_bt_port_declaration(self, _): - raise RuntimeError("Error: SCXML Action Thread: deleted method 'add_bt_port_declaration'.") - - def set_bt_port_value(self, _, __): - raise RuntimeError("Error: SCXML Action Thread: deleted method 'set_bt_port_values'.") - - def set_bt_ports_values(self, _): - raise RuntimeError("Error: SCXML Action Thread: deleted method 'set_bt_ports_values'.") - - def _generate_ros_declarations_helper(self): - raise RuntimeError("Error: SCXML Action Thread: deleted method " - "'_generate_ros_declarations_helper'.") - - def _check_valid_ros_declarations(self): - raise RuntimeError( - "Error: SCXML Action Thread: deleted method '_check_valid_ros_declarations': " - "use 'check_valid_ros_instantiations' instead (no underscore).") - - def is_plain_scxml(self): - raise RuntimeError("Error: SCXML Action Thread: deleted method 'is_plain_scxml'.") - - def to_plain_scxml_and_declarations(self): - raise RuntimeError("Error: SCXML Action Thread: deleted method " - "'to_plain_scxml_and_declarations'.") - class RosActionHandleThreadStart(ScxmlTransition): """SCXML object representing the handler of am action result for a service client.""" From 93a2ec5df9622593b2568fc088428b3477ce9965 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 10:10:32 +0200 Subject: [PATCH 25/49] Thread start and cancel and rework interface of action thread Signed-off-by: Marco Lampacrescia --- .../ros_fibonacci_action_example/server.scxml | 4 +- .../scxml_ros_action_server_thread.py | 136 ++++++++++-------- 2 files changed, 77 insertions(+), 63 deletions(-) diff --git a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml index 48887805..8d4fdb1b 100644 --- a/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/jani_generator/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -26,9 +26,9 @@ - + - + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index dc658139..1a4e6ad0 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -19,16 +19,17 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Tuple, Union, get_args +from typing import List, Optional, Tuple, Type, Union from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlTransition, - ScxmlRosDeclarationsContainer, as_plain_execution_body, - execution_body_from_xml, valid_execution_body) + ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer, + execution_body_from_xml) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer +from scxml_converter.scxml_entries.scxml_ros_base import RosCallback from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import generate_action_thread_execution_start_event from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -129,76 +130,89 @@ def as_xml(self) -> ET.Element: pass -class RosActionHandleThreadStart(ScxmlTransition): - """SCXML object representing the handler of am action result for a service client.""" +class RosActionHandleThreadStart(RosCallback): + """ + SCXML object receiving a trigger from the action server to start a thread. + + The selection of the thread is encoded in the event name. + The thread ID is set from the parent, via a dedicated method. + """ @staticmethod def get_tag_name() -> str: - return "ros_action_handle_result" + return "ros_action_thread_start" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": - """Create a RosActionHandleResult object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleResult, xml_tree) - client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") - target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleThreadStart": + """Create a RosActionHandleThreadStart object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleThreadStart, xml_tree) + server_alias = get_xml_argument(RosActionHandleThreadStart, xml_tree, "name") + target_state = get_xml_argument(RosActionHandleThreadStart, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleResult(client_name, target_name, exec_body) + return RosActionHandleThreadStart(server_alias, target_state, exec_body) - def __init__(self, action_client: Union[str, RosActionClient], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: + def __init__(self, server_alias: Union[str, RosActionServer], target_state: str, + exec_body: Optional[ScxmlExecutionBody] = None) -> None: """ Initialize a new RosActionHandleResult object. - :param action_client: Action client used by this handler, or its name. - :param target: Target state to transition to after the feedback is received. - :param body: Execution body to be executed upon feedback reception (before transition). + :param server_alias: Action Server used by this handler, or its name. + :param target_state: Target state to transition to after the start trigger is received. + :param exec_body: Execution body to be executed upon thread start (before transition). """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionHandleResult, "name", action_client) - self._client_name = action_client - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." + super().__init__(server_alias, target_state, exec_body) + # The thread ID depends on the plain scxml instance, so it is set later + self._thread_id: Optional[str] = None - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) - valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML RosActionHandleResult: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML RosActionHandleResult: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML RosActionHandleResult: " - f"action client {self._client_name} not declared.") - return False - if not super().check_valid_ros_instantiations(ros_declarations): - print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") - return False - return True - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_result_handle_event(interface_name, automaton_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + """Check if the action server has been declared.""" + return ros_declarations.is_action_server_defined(self._interface_name) + + def set_thread_id(self, thread_id: str) -> None: + """Set the thread ID for this handler.""" + assert self._thread_id is None, f"Error: SCXML {self.__class__}: thread ID set." + is_non_empty_string(self.__class__, "thread_id", thread_id) + self._thread_id = thread_id + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_execution_start_event( + ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." - xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), - {"name": self._client_name, "target": self._target}) + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_thread_start = ET.Element(self.get_tag_name(), + {"name": self._interface_name, "target": self._target}) if self._body is not None: for body_elem in self._body: - xml_handle_feedback.append(body_elem.as_xml()) - return xml_handle_feedback + xml_thread_start.append(body_elem.as_xml()) + return xml_thread_start + + +class RosActionHandleThreadCancel(RosActionHandleThreadStart): + """ + SCXML object receiving a trigger from the action server to stop a thread. + + The selection of the thread is encoded in the event name. + The thread ID is set from the parent, via a dedicated method. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_thread_cancel" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleThreadCancel": + """Create a RosActionHandleThreadCancel object from an XML tree.""" + assert_xml_tag_ok(RosActionHandleThreadCancel, xml_tree) + server_alias = get_xml_argument(RosActionHandleThreadCancel, xml_tree, "name") + target_state = get_xml_argument(RosActionHandleThreadCancel, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return RosActionHandleThreadCancel(server_alias, target_state, exec_body) + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_execution_start_event( + ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) From 94093a5717aaf79cd69561d9e7bb1406f76b3805 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 11:15:10 +0200 Subject: [PATCH 26/49] Finish action thread SCXML class Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 5 ++ .../scxml_ros_action_server_thread.py | 83 ++++++++++++++----- 2 files changed, 68 insertions(+), 20 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index fa75835d..df09e95b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -223,6 +223,11 @@ def generate_action_thread_execution_start_event(action_name: str, thread_id: st return f"action_{sanitize_ros_interface_name(action_name)}_thread_{thread_id}_start" +def generate_action_thread_execution_cancel_event(action_name: str, thread_id: str) -> str: + """Generate the name of the event that triggers the start of an action thread execution.""" + return f"action_{sanitize_ros_interface_name(action_name)}_thread_{thread_id}_cancel" + + def generate_action_feedback_event(action_name: str) -> str: """Generate the name of the event that sends a feedback from the action server.""" return f"action_{sanitize_ros_interface_name(action_name)}_feedback" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 1a4e6ad0..7a246f0f 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -19,7 +19,7 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Tuple, Type, Union +from typing import List, Optional, Type, Union from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( @@ -29,7 +29,8 @@ from scxml_converter.scxml_entries.scxml_ros_base import RosCallback from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.ros_utils import generate_action_thread_execution_start_event +from scxml_converter.scxml_entries.ros_utils import ( + generate_action_thread_execution_start_event, generate_action_thread_execution_cancel_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -85,10 +86,10 @@ def __init__(self, action_server: Union[str, RosActionServer], n_threads: int) - else: assert is_non_empty_string(RosActionThread, "name", action_server) self._name = action_server - self._n_threads = n_threads + self._n_threads: int = n_threads self._initial_state: Optional[str] = None self._datamodel: Optional[ScxmlDataModel] = None - self._states: List[Tuple[ScxmlState, bool]] = [] + self._states: List[ScxmlState] = [] def add_state(self, state: ScxmlState, *, initial: bool = False): """Append a state to the list of states. If initial is True, set it as the initial state.""" @@ -102,17 +103,42 @@ def set_data_model(self, data_model: ScxmlDataModel): self._data_model = data_model def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - # TODO - pass + if self._datamodel is not None: + self._datamodel.update_bt_ports_values(bt_ports_handler) + for state in self._states: + state.update_bt_ports_values(bt_ports_handler) def check_validity(self) -> bool: - # TODO - pass - - def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer - ) -> bool: - # TODO - pass + valid_name = is_non_empty_string(RosActionThread, "name", self._name) + valid_n_threads = isinstance(self._n_threads, int) and self._n_threads > 0 + valid_initial_state = self._initial_state is not None + valid_datamodel = self._datamodel is None or self._datamodel.check_validity() + valid_states = all(isinstance(state, ScxmlState) and state.check_validity() + for state in self._states) + if not valid_name: + return False + if not valid_n_threads: + print("Error: SCXML RosActionThread: " + f"{self._name} has invalid n_threads ({self._n_threads}).") + if not valid_initial_state: + print(f"Error: SCXML RosActionThread: {self._name} has no initial state.") + if not valid_datamodel: + print(f"Error: SCXML RosActionThread: {self._name} nas an invalid datamodel.") + if not valid_states: + print(f"Error: SCXML RosActionThread: {self._name} has invalid states.") + return valid_n_threads and valid_initial_state and valid_datamodel and valid_states + + def check_valid_ros_instantiations(self, ros_decls: ScxmlRosDeclarationsContainer) -> bool: + assert isinstance(ros_decls, ScxmlRosDeclarationsContainer), \ + "Error: SCXML RosActionThread: Invalid ROS declarations container." + if not ros_decls.is_action_server_defined(self._name): + print(f"Error: SCXML RosActionThread: undeclared thread action server '{self._name}'.") + return False + if not all(state.check_valid_ros_instantiations(ros_decls) for state in self._states): + print("Error: SCXML RosActionThread: " + f"invalid ROS instantiation for states in thread '{self._name}'.") + return False + return True def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> List[ScxmlBase]: """ @@ -121,8 +147,22 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Lis This returns a list of ScxmlRoot objects, using ScxmlBase to avoid circular dependencies. """ from scxml_converter.scxml_entries import ScxmlRoot - # TODO - return [ScxmlRoot("name")] + thread_instances: List[ScxmlRoot] = [] + action_name = ros_declarations.get_action_server_info(self._name)[0] + for thread_idx in range(self._n_threads): + thread_name = f"{action_name}_thread_{thread_idx}" + plain_thread_instance = ScxmlRoot(thread_name) + plain_thread_instance.set_data_model(self._datamodel) + for state in self._states: + initial_state = state.get_id() == self._initial_state + state.set_thread_id(thread_idx) + plain_thread_instance.add_state(state.as_plain_scxml(ros_declarations), + initial=initial_state) + assert plain_thread_instance.is_plain_scxml(), \ + "Error: SCXML RosActionThread: " \ + f"failed to generate a plain-SCXML instance from thread '{self._name}'" + thread_instances.append(plain_thread_instance) + return [thread_instances] def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid state object." @@ -166,19 +206,21 @@ def __init__(self, server_alias: Union[str, RosActionServer], target_state: str, """ super().__init__(server_alias, target_state, exec_body) # The thread ID depends on the plain scxml instance, so it is set later - self._thread_id: Optional[str] = None + self._thread_id: Optional[int] = None def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the action server has been declared.""" return ros_declarations.is_action_server_defined(self._interface_name) - def set_thread_id(self, thread_id: str) -> None: + def set_thread_id(self, thread_id: int) -> None: """Set the thread ID for this handler.""" - assert self._thread_id is None, f"Error: SCXML {self.__class__}: thread ID set." - is_non_empty_string(self.__class__, "thread_id", thread_id) + # The thread ID is expected to be overwritten every time a new thread is generated. + assert isinstance(thread_id, int) and thread_id >= 0, \ + f"Error: SCXML {self.__class__}: invalid thread ID ({thread_id})." self._thread_id = thread_id def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." return generate_action_thread_execution_start_event( ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) @@ -214,5 +256,6 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleThreadCancel": return RosActionHandleThreadCancel(server_alias, target_state, exec_body) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: - return generate_action_thread_execution_start_event( + assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." + return generate_action_thread_execution_cancel_event( ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) From 6ee13878534bc3e8cd91352c258104162fae85c8 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 11:32:41 +0200 Subject: [PATCH 27/49] Add missing function to scxml state Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_state.py | 33 ++++++++++++++----- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index 1e8aec1b..61555aec 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -34,15 +34,6 @@ class ScxmlState(ScxmlBase): def get_tag_name() -> str: return "state" - def __init__(self, id_: str, *, - on_entry: ScxmlExecutionBody = None, - on_exit: ScxmlExecutionBody = None, - body: List[ScxmlTransition] = None): - self._id = id_ - self._on_entry = on_entry if on_entry is not None else [] - self._on_exit = on_exit if on_exit is not None else [] - self._body: List[ScxmlTransition] = body if body is not None else [] - @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "ScxmlState": """Create a ScxmlState object from an XML tree.""" @@ -77,6 +68,23 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlState": scxml_state.add_transition(body_entry) return scxml_state + def __init__(self, state_id: str, *, + on_entry: ScxmlExecutionBody = None, + on_exit: ScxmlExecutionBody = None, + body: List[ScxmlTransition] = None): + """ + Initialize a new ScxmlState object. + + :param state_id: The id of the state, unique in the ScxmlRoot object. + :param on_entry: The executable entries to be executed on entry. + :param on_exit: The executable entries to be executed on exit. + :param body: The transitions leaving the state. + """ + self._id: str = state_id + self._on_entry: ScxmlExecutionBody = on_entry if on_entry is not None else [] + self._on_exit: ScxmlExecutionBody = on_exit if on_exit is not None else [] + self._body: List[ScxmlTransition] = body if body is not None else [] + def get_id(self) -> str: return self._id @@ -90,6 +98,13 @@ def get_body(self) -> List[ScxmlTransition]: """Return the transitions leaving the state.""" return self._body + def set_thread_id(self, thread_idx: int): + """Assign the thread ID to the thread-specific transitions in the body.""" + for transition in self._body: + # Assign the thread only to thread specific transitions + if hasattr(transition, 'set_thread_id'): + transition.set_thread_id(thread_idx) + def instantiate_bt_events(self, instance_id: str) -> None: """Instantiate the BT events in all entries belonging to a state.""" for transition in self._body: From 218642d7619dc3f4f1ebf5acfd6288ffe6a79cb5 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 12:42:33 +0200 Subject: [PATCH 28/49] Reduce code duplication in scxml ros services Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 11 +- .../scxml_entries/scxml_ros_service.py | 287 ++++-------------- 2 files changed, 69 insertions(+), 229 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 7b368f30..2e702c09 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -30,7 +30,8 @@ from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_handle_event, generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, - generate_action_thread_execution_start_event) + generate_action_thread_execution_start_event, generate_action_feedback_event, + generate_action_result_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) @@ -292,6 +293,10 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) print(f"Error: SCXML {self.__class__}: " f"invalid fields in feedback request {self._interface_name}.") + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_feedback_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) + def as_xml(self) -> ET.Element: assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." xml_action_feedback = ET.Element(RosActionSendFeedback.get_tag_name(), @@ -333,6 +338,10 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) print(f"Error: SCXML {self.__class__}: " f"invalid fields in result request {self._interface_name}.") + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_result_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) + def as_xml(self) -> ET.Element: assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." xml_action_result = ET.Element(RosActionSendResult.get_tag_name(), diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 5663fc28..49893722 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -20,22 +20,19 @@ https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Services/Understanding-ROS2-Services.html """ -from typing import List, Optional, Union +from typing import List, Optional, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlRosDeclarationsContainer, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, - BtGetValueInputPort, as_plain_execution_body, execution_body_from_xml, valid_execution_body) + RosField, ScxmlRosDeclarationsContainer, BtGetValueInputPort, execution_body_from_xml) -from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger -from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) -from scxml_converter.scxml_entries.utils import is_non_empty_string class RosServiceServer(RosDeclaration): @@ -104,13 +101,17 @@ def as_xml(self) -> ET.Element: return xml_srv_server -class RosServiceSendRequest(ScxmlSend): +class RosServiceSendRequest(RosTrigger): """Object representing a ROS service request (from the client side) in SCXML.""" @staticmethod def get_tag_name() -> str: return "ros_service_send_request" + @staticmethod + def get_declaration_type() -> Type[RosServiceClient]: + return RosServiceClient + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendRequest": """Create a RosServiceSendRequest object from an XML tree.""" @@ -125,81 +126,37 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendRequest": fields.append(RosField.from_xml_tree(field_xml)) return RosServiceSendRequest(srv_name, fields) - def __init__(self, - service_decl: Union[str, RosServiceClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosServiceSendRequest object. - - :param service_decl: Name of the service of Scxml obj. of Service Client declaration. - :param fields: List of fields to be sent in the request. - """ - if isinstance(service_decl, RosServiceClient): - self._srv_name = service_decl.get_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(service_decl, str), \ - "Error: SCXML Service Send Request: invalid service name." - self._srv_name = service_decl - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Service Send Request: invalid parameters." + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_service_client_defined(self._interface_name) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosServiceSendRequest, "name", self._srv_name) - valid_fields = self._fields is None or \ - all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML service request: invalid ROS declarations container." - srv_client_declared = ros_declarations.is_service_client_defined(self._srv_name) - if not srv_client_declared: - print(f"Error: SCXML service request: srv client {self._srv_name} not declared.") - return False - valid_fields = ros_declarations.check_valid_srv_req_fields(self._srv_name, self._fields) - if not valid_fields: - print("Error: SCXML service request: invalid fields in request.") - return False - return True + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.check_valid_srv_req_fields(self._interface_name, self._fields) - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): - """Update the values of potential entries making use of BT ports.""" - for field in self._fields: - field.update_bt_ports_values(bt_ports_handler) - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - srv_interface, _ = ros_declarations.get_service_client_info(self._srv_name) - event_name = generate_srv_request_event(srv_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_srv_request_event( + ros_declarations.get_service_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Send Request: invalid parameters." xml_srv_request = ET.Element(RosServiceSendRequest.get_tag_name(), - {"name": self._srv_name}) - if self._fields is not None: - for field in self._fields: - xml_srv_request.append(field.as_xml()) + {"name": self._interface_name}) + for field in self._fields: + xml_srv_request.append(field.as_xml()) return xml_srv_request -class RosServiceHandleRequest(ScxmlTransition): +class RosServiceHandleRequest(RosCallback): """SCXML object representing a ROS service callback on the server, acting upon a request.""" @staticmethod def get_tag_name() -> str: return "ros_service_handle_request" + @staticmethod + def get_declaration_type() -> Type[RosServiceServer]: + return RosServiceServer + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleRequest": """Create a RosServiceHandleRequest object from an XML tree.""" @@ -213,74 +170,33 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleRequest": exec_body = execution_body_from_xml(xml_tree) return RosServiceHandleRequest(srv_name, target_name, exec_body) - def __init__(self, service_decl: Union[str, RosServiceServer], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: - """ - Initialize a new RosServiceHandleRequest object. - - :param service_decl: The service server declaration, or its name. - :param target: Target state after the request has been received. - :param body: Execution body to be executed upon request, before transitioning to target. - """ - if isinstance(service_decl, RosServiceServer): - self._service_name = service_decl.get_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(service_decl, str), \ - "Error: SCXML Service Handle Request: invalid service name." - self._service_name = service_decl - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML Service Handle Request: invalid parameters." + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_service_server_defined(self._interface_name) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosServiceHandleRequest, "name", self._service_name) - valid_target = is_non_empty_string(RosServiceHandleRequest, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML Service Handle Request: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML service request handler: invalid ROS declarations container." - srv_server_declared = ros_declarations.is_service_server_defined(self._service_name) - if not srv_server_declared: - print("Error: SCXML service request handler: " - f"srv server {self._service_name} not declared.") - return False - valid_body = super().check_valid_ros_instantiations(ros_declarations) - if not valid_body: - print("Error: SCXML service request handler: body has invalid ROS instantiations.") - return valid_body - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service request handler: invalid ROS instantiations." - interface_name, _ = ros_declarations.get_service_server_info(self._service_name) - event_name = generate_srv_server_request_event(interface_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_srv_server_request_event( + ros_declarations.get_service_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Handle Request: invalid parameters." xml_srv_request = ET.Element(RosServiceHandleRequest.get_tag_name(), - {"name": self._service_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_srv_request.append(body_elem.as_xml()) + {"name": self._interface_name, "target": self._target}) + for body_elem in self._body: + xml_srv_request.append(body_elem.as_xml()) return xml_srv_request -class RosServiceSendResponse(ScxmlSend): +class RosServiceSendResponse(RosTrigger): """SCXML object representing the response from a service server.""" @staticmethod def get_tag_name() -> str: return "ros_service_send_response" + @staticmethod + def get_declaration_type() -> Type[RosServiceServer]: + return RosServiceServer + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendResponse": """Create a RosServiceSendResponse object from an XML tree.""" @@ -298,77 +214,36 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceSendResponse": fields = None return RosServiceSendResponse(srv_name, fields) - def __init__(self, service_name: Union[str, RosServiceServer], - fields: Optional[List[RosField]]) -> None: - """ - Initialize a new RosServiceClient object. - - :param service_name: Topic used by the service. - :param fields: List of fields to be sent in the response. - """ - if isinstance(service_name, RosServiceServer): - self._service_name = service_name.get_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(service_name, str), \ - "Error: SCXML Service Send Response: invalid service name." - self._service_name = service_name - self._fields = fields if fields is not None else [] - assert self.check_validity(), "Error: SCXML Service Send Response: invalid parameters." + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_service_server_defined(self._interface_name) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosServiceSendResponse, "name", self._service_name) - valid_fields = self._fields is None or \ - all([isinstance(field, RosField) and field.check_validity() for field in self._fields]) - if not valid_fields: - print("Error: SCXML service response: fields are not valid.") - return valid_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML service response: invalid ROS declarations container." - srv_declared = ros_declarations.is_service_server_defined(self._service_name) - if not srv_declared: - print("Error: SCXML service response: " - f"srv server {self._service_name} not declared.") - return False - valid_fields = ros_declarations.check_valid_srv_res_fields(self._service_name, self._fields) - if not valid_fields: - print("Error: SCXML service response: invalid fields in response.") - return False - return True + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.check_valid_srv_res_fields(self._interface_name, self._fields) - def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): - """Update the values of potential entries making use of BT ports.""" - for field in self._fields: - field.update_bt_ports_values(bt_ports_handler) - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response: invalid ROS instantiations." - interface_name, _ = ros_declarations.get_service_server_info(self._service_name) - event_name = generate_srv_server_response_event(interface_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_srv_server_response_event( + ros_declarations.get_service_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Send Response: invalid parameters." xml_srv_response = ET.Element(RosServiceSendResponse.get_tag_name(), {"name": self._service_name}) - if self._fields is not None: - for field in self._fields: - xml_srv_response.append(field.as_xml()) + for field in self._fields: + xml_srv_response.append(field.as_xml()) return xml_srv_response -class RosServiceHandleResponse(ScxmlTransition): +class RosServiceHandleResponse(RosCallback): """SCXML object representing the handler of a service response for a service client.""" @staticmethod def get_tag_name() -> str: return "ros_service_handle_response" + @staticmethod + def get_declaration_type() -> Type[RosServiceClient]: + return RosServiceClient + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleResponse": """Create a RosServiceHandleResponse object from an XML tree.""" @@ -382,62 +257,18 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleResponse": exec_body = execution_body_from_xml(xml_tree) return RosServiceHandleResponse(srv_name, target_name, exec_body) - def __init__(self, service_decl: Union[str, RosServiceClient], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: - """ - Initialize a new RosServiceClient object. - - :param service_name: Topic used by the service. - :param type: ROS type of the service. - """ - if isinstance(service_decl, RosServiceClient): - self._service_name = service_decl.get_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(service_decl, str), \ - "Error: SCXML Service Handle Response: invalid service name." - self._service_name = service_decl - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_service_client_defined(self._interface_name) - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosServiceHandleResponse, "name", self._service_name) - valid_target = is_non_empty_string(RosServiceHandleResponse, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML Service Handle Response: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML Service Handle Response: invalid ROS declarations container." - srv_declared = ros_declarations.is_service_client_defined(self._service_name) - if not srv_declared: - print("Error: SCXML Service Handle Response: " - f"srv server {self._service_name} not declared.") - return False - valid_body = super().check_valid_ros_instantiations(ros_declarations) - if not valid_body: - print("Error: SCXML Service Handle Response: body has invalid ROS instantiations.") - return valid_body - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_service_client_info(self._service_name) - event_name = generate_srv_response_event(interface_name, automaton_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_srv_response_event( + ros_declarations.get_service_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." xml_srv_response = ET.Element(RosServiceHandleResponse.get_tag_name(), {"name": self._service_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_srv_response.append(body_elem.as_xml()) + for body_elem in self._body: + xml_srv_response.append(body_elem.as_xml()) return xml_srv_response From 86cf975c7ff246b8926f5c1788fdf735f1573b4d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 13:20:41 +0200 Subject: [PATCH 29/49] More base functionalities in RosBase classes and make use of it in action clients Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_client.py | 229 +++--------------- .../scxml_entries/scxml_ros_base.py | 53 +++- 2 files changed, 78 insertions(+), 204 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 3d136636..35957c12 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -19,14 +19,12 @@ Based loosely on https://design.ros2.org/articles/actions.html """ -from typing import List, Optional, Union +from typing import List, Union, Type from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlExecutionBody, ScxmlSend, ScxmlTransition, BtGetValueInputPort, - as_plain_execution_body, execution_body_from_xml, valid_execution_body, - ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration + ScxmlTransition, BtGetValueInputPort, ScxmlRosDeclarationsContainer) +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_req_event, @@ -70,7 +68,7 @@ def as_xml(self) -> ET.Element: return xml_action_server -class RosActionSendGoal(ScxmlSend): +class RosActionSendGoal(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" @staticmethod @@ -78,72 +76,19 @@ def get_tag_name() -> str: return "ros_action_send_goal" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionSendGoal, xml_tree) - action_name = get_xml_argument(RosActionSendGoal, xml_tree, "name") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendGoal(action_name, fields) + def get_declaration_type() -> Type[RosActionClient]: + return RosActionClient - def __init__(self, action_client: Union[str, RosActionClient], - fields: List[RosField] = None) -> None: - """ - Initialize a new RosActionSendGoal object. - - :param action_client: The ActionClient object used by the sender, or its name. - :param fields: List of fields to be sent in the goal request. - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionSendGoal, "name", action_client) - self._client_name = action_client - if fields is None: - fields = [] - self._fields = fields - assert self.check_validity(), "Error: SCXML Action Goal Request: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionSendGoal, "name", self._client_name) - valid_fields = all([isinstance(field, RosField) and field.check_validity() - for field in self._fields]) - if not valid_fields: - print("Error: SCXML service request: fields are not valid.") - return valid_name and valid_fields - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML action goal request: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML action goal request: " - f"action client {self._client_name} not declared.") - return False - if not ros_declarations.check_valid_action_goal_fields(self._client_name, self._fields): - print("Error: SCXML action goal request: invalid fields in request.") - return False - return True + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_client_defined(self._client_name) - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML action goal request: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - action_interface, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_goal_req_event(action_interface, automaton_name) - event_params = [field.as_plain_scxml(ros_declarations) for field in self._fields] - return ScxmlSend(event_name, event_params) + def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.check_valid_action_goal_fields(self._interface_name, self._fields) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - xml_goal_request = ET.Element(RosActionSendGoal.get_tag_name(), { - "name": self._client_name}) - if self._fields is not None: - for field in self._fields: - xml_goal_request.append(field.as_xml()) - return xml_goal_request + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_goal_req_event( + ros_declarations.get_action_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) class RosActionHandleGoalResponse(ScxmlTransition): @@ -225,7 +170,7 @@ def as_xml(self) -> ET.Element: "accept": self._accept_target, "reject": self._reject_target}) -class RosActionHandleFeedback(ScxmlTransition): +class RosActionHandleFeedback(RosCallback): """SCXML object representing the handler of an action feedback.""" @staticmethod @@ -233,74 +178,19 @@ def get_tag_name() -> str: return "ros_action_handle_feedback" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleFeedback": - """Create a RosActionHandleFeedback object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleFeedback, xml_tree) - client_name = get_xml_argument(RosActionHandleFeedback, xml_tree, "name") - target_name = get_xml_argument(RosActionHandleFeedback, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleFeedback(client_name, target_name, exec_body) - - def __init__(self, action_client: Union[str, RosActionClient], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: - """ - Initialize a new RosActionHandleFeedback object. - - :param action_client: Action client used by this handler, or its name. - :param target: Target state to transition to after the feedback is received. - :param body: Execution body to be executed upon feedback reception (before transition). - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionHandleFeedback, "name", action_client) - self._client_name = action_client - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML RosActionHandleFeedback: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionHandleFeedback, "name", self._client_name) - valid_target = is_non_empty_string(RosActionHandleFeedback, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML RosActionHandleFeedback: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML RosActionHandleFeedback: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML RosActionHandleFeedback: " - f"action client {self._client_name} not declared.") - return False - if not super().check_valid_ros_instantiations(ros_declarations): - print("Error: SCXML RosActionHandleFeedback: invalid ROS instantiations in exec body.") - return False - return True + def get_declaration_type() -> Type[RosActionClient]: + return RosActionClient - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_feedback_handle_event(interface_name, automaton_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_client_defined(self._interface_name) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosActionHandleFeedback: invalid parameters." - xml_handle_feedback = ET.Element(RosActionHandleFeedback.get_tag_name(), - {"name": self._client_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_handle_feedback.append(body_elem.as_xml()) - return xml_handle_feedback + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_feedback_handle_event( + ros_declarations.get_action_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) -class RosActionHandleResult(ScxmlTransition): +class RosActionHandleResult(RosCallback): """SCXML object representing the handler of am action result for a service client.""" @staticmethod @@ -308,68 +198,13 @@ def get_tag_name() -> str: return "ros_action_handle_result" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleResult": - """Create a RosActionHandleResult object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleResult, xml_tree) - client_name = get_xml_argument(RosActionHandleResult, xml_tree, "name") - target_name = get_xml_argument(RosActionHandleResult, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleResult(client_name, target_name, exec_body) - - def __init__(self, action_client: Union[str, RosActionClient], target: str, - body: Optional[ScxmlExecutionBody] = None) -> None: - """ - Initialize a new RosActionHandleResult object. - - :param action_client: Action client used by this handler, or its name. - :param target: Target state to transition to after the feedback is received. - :param body: Execution body to be executed upon feedback reception (before transition). - """ - if isinstance(action_client, RosActionClient): - self._client_name = action_client.get_name() - else: - assert is_non_empty_string(RosActionHandleResult, "name", action_client) - self._client_name = action_client - self._target = target - self._body = body - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." - - def check_validity(self) -> bool: - valid_name = is_non_empty_string(RosActionHandleResult, "name", self._client_name) - valid_target = is_non_empty_string(RosActionHandleResult, "target", self._target) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_body: - print("Error: SCXML RosActionHandleResult: body is not valid.") - return valid_name and valid_target and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML RosActionHandleResult: invalid ROS declarations container." - if not ros_declarations.is_action_client_defined(self._client_name): - print("Error: SCXML RosActionHandleResult: " - f"action client {self._client_name} not declared.") - return False - if not super().check_valid_ros_instantiations(ros_declarations): - print("Error: SCXML RosActionHandleResult: invalid ROS instantiations in exec body.") - return False - return True + def get_declaration_type() -> Type[RosActionClient]: + return RosActionClient - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self.check_valid_ros_instantiations(ros_declarations), \ - "Error: SCXML service response handler: invalid ROS instantiations." - automaton_name = ros_declarations.get_automaton_name() - interface_name, _ = ros_declarations.get_action_client_info(self._client_name) - event_name = generate_action_result_handle_event(interface_name, automaton_name) - target = self._target - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_client_defined(self._interface_name) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosActionHandleResult: invalid parameters." - xml_handle_feedback = ET.Element(RosActionHandleResult.get_tag_name(), - {"name": self._client_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_handle_feedback.append(body_elem.as_xml()) - return xml_handle_feedback + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_result_handle_event( + ros_declarations.get_action_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index 75c64259..1aadd1e5 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -19,12 +19,16 @@ from scxml_converter.scxml_entries import ( ScxmlBase, ScxmlTransition, ScxmlSend, ScxmlExecutionBody, RosField, BtGetValueInputPort, - ScxmlRosDeclarationsContainer, as_plain_execution_body, valid_execution_body) + ScxmlRosDeclarationsContainer, execution_body_from_xml, as_plain_execution_body, + valid_execution_body) from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument from scxml_converter.scxml_entries.utils import is_non_empty_string +from xml.etree import ElementTree as ET + class RosDeclaration(ScxmlBase): """Base class for ROS declarations in SCXML.""" @@ -109,6 +113,15 @@ def get_declaration_type(cls) -> Type[RosDeclaration]: """ raise NotImplementedError(f"{cls.__name__} doesn't implement get_declaration_type.") + @classmethod + def from_xml_tree(cls: Type['RosCallback'], xml_tree: ET.Element) -> 'RosCallback': + """Create an instance of the class from an XML tree.""" + assert_xml_tag_ok(cls, xml_tree) + interface_name = get_xml_argument(cls, xml_tree, "name") + target_state = get_xml_argument(cls, xml_tree, "target") + exec_body = execution_body_from_xml(xml_tree) + return cls(interface_name, target_state, exec_body) + def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str, exec_body: Optional[ScxmlExecutionBody] = None) -> None: """ @@ -118,13 +131,16 @@ def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str :param target_state: Name of the state to transition to after the callback. :param exec_body: Executable body of the callback. """ + if exec_body is None: + exec_body: ScxmlExecutionBody = [] + self._interface_name: str = "" if isinstance(interface_decl, self.get_declaration_type()): self._interface_name = interface_decl.get_name() else: assert is_non_empty_string(self.__class__, "name", interface_decl) self._interface_name = interface_decl - self._target = target_state - self._body = exec_body + self._target: str = target_state + self._body: ScxmlExecutionBody = exec_body assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." def check_validity(self) -> bool: @@ -167,6 +183,15 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], None, body) + def as_xml(self) -> ET.Element: + """Convert the ROS callback to an XML element.""" + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_callback = ET.Element(self.get_tag_name(), + {"name": self._interface_name, "target": self._target}) + for body_elem in self._body: + xml_callback.append(body_elem.as_xml()) + return xml_callback + class RosTrigger(ScxmlSend): """Base class for ROS triggers in SCXML.""" @@ -185,6 +210,14 @@ def get_declaration_type(cls) -> Type[RosDeclaration]: """ raise NotImplementedError(f"{cls.__name__} doesn't implement get_declaration_type.") + @classmethod + def from_xml_tree(cls: Type['RosTrigger'], xml_tree: ET.Element) -> 'RosTrigger': + """Create an instance of the class from an XML tree.""" + assert_xml_tag_ok(cls, xml_tree) + interface_name = get_xml_argument(cls, xml_tree, "name") + fields = [RosField.from_xml_tree(field) for field in xml_tree] + return cls(interface_name, fields) + def __init__(self, interface_decl: Union[str, RosDeclaration], fields: Optional[List[RosField]] = None) -> None: """ @@ -193,20 +226,19 @@ def __init__(self, interface_decl: Union[str, RosDeclaration], :param interface_decl: ROS interface declaration to be used in the trigger, or its name. :param fields: Name of fields that are sent together with the trigger. """ + if fields is None: + fields = [] + self._interface_name: str = "" if isinstance(interface_decl, self.get_declaration_type()): self._interface_name = interface_decl.get_name() else: assert is_non_empty_string(self.__class__, "name", interface_decl) self._interface_name = interface_decl - if fields is None: - fields = [] self._fields: List[RosField] = fields assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." def append_field(self, field: RosField) -> None: assert isinstance(field, RosField), "Error: SCXML topic publish: invalid field." - if self._fields is None: - self._fields = [] self._fields.append(field) def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): @@ -256,3 +288,10 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx event_name = self.get_plain_scxml_event(ros_declarations) params = [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, params) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_trigger = ET.Element(self.get_tag_name(), {"name": self._interface_name}) + for field in self._fields: + xml_trigger.append(field.as_xml()) + return xml_trigger From 613ca8436530e932e356c30aa3771bb049f6ecee Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 13:27:27 +0200 Subject: [PATCH 30/49] Code reduction in action server Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 103 ++---------------- 1 file changed, 7 insertions(+), 96 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 2e702c09..8a3ebd04 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -23,7 +23,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, BtGetValueInputPort, ScxmlRosDeclarationsContainer, execution_body_from_xml) + RosField, BtGetValueInputPort, ScxmlRosDeclarationsContainer) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger @@ -85,15 +85,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleGoalRequest": - """Create a RosActionHandleGoalRequest object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleGoalRequest, xml_tree) - server_name = get_xml_argument(RosActionHandleGoalRequest, xml_tree, "name") - target_name = get_xml_argument(RosActionHandleGoalRequest, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleGoalRequest(server_name, target_name, exec_body) - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) @@ -101,15 +92,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_action_goal_handle_event( ros_declarations.get_action_server_info(self._interface_name)[0]) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." - xml_goal_handler = ET.Element(RosActionHandleGoalRequest.get_tag_name(), - {"name": self._interface_name, "target": self._target}) - if self._body is not None: - for entry in self._body: - xml_goal_handler.append(entry.as_xml()) - return xml_goal_handler - class RosActionAcceptGoal(RosTrigger): """ @@ -124,16 +106,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionAcceptGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionAcceptGoal, xml_tree) - action_name = get_xml_argument(RosActionAcceptGoal, xml_tree, "name") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionAcceptGoal(action_name, fields) - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) @@ -146,12 +118,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - assert self.check_fields_validity(None), "Error: SCXML action goal Request: invalid fields." - xml_goal_accepted = ET.Element(RosActionAcceptGoal.get_tag_name(), - {"name": self._interface_name}) - xml_goal_accepted.append(self._fields[0].as_xml()) - return xml_goal_accepted + assert self.check_fields_validity(None), "Error: SCXML RosActionAcceptGoal: invalid fields." + return super().as_xml() class RosActionRejectGoal(RosTrigger): @@ -168,16 +136,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionRejectGoal": - """Create a RosActionSendGoal object from an XML tree.""" - assert_xml_tag_ok(RosActionRejectGoal, xml_tree) - action_name = get_xml_argument(RosActionRejectGoal, xml_tree, "name") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionRejectGoal(action_name, fields) - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) @@ -190,12 +148,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_action_server_info(self._interface_name)[0]) def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML action goal Request: invalid parameters." - assert self.check_fields_validity(None), "Error: SCXML action goal Request: invalid fields." - xml_goal_accepted = ET.Element(RosActionRejectGoal.get_tag_name(), - {"name": self._interface_name}) - xml_goal_accepted.append(self._fields[0].as_xml()) - return xml_goal_accepted + assert self.check_fields_validity(None), "Error: SCXML RosActionRejectGoal: invalid fields." + return super().as_xml() class RosActionStartThread(RosTrigger): @@ -252,13 +206,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." - xml_thread_start_req = ET.Element(RosActionStartThread.get_tag_name(), - {"name": self._interface_name, - "thread_id": self._thread_id}) - if self._fields is not None: - for field in self._fields: - xml_thread_start_req.append(field.as_xml()) + xml_thread_start_req = super().as_xml() + xml_thread_start_req.set("thread_id", self._thread_id) return xml_thread_start_req @@ -273,16 +222,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendFeedback": - """Create a RosActionSendFeedback object from an XML tree.""" - assert_xml_tag_ok(RosActionSendFeedback, xml_tree) - action_name = get_xml_argument(RosActionSendFeedback, xml_tree, "name") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendFeedback(action_name, fields) - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) @@ -297,15 +236,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_action_feedback_event( ros_declarations.get_action_server_info(self._interface_name)[0]) - def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." - xml_action_feedback = ET.Element(RosActionSendFeedback.get_tag_name(), - {"name": self._interface_name}) - if self._fields is not None: - for field in self._fields: - xml_action_feedback.append(field.as_xml()) - return xml_action_feedback - class RosActionSendResult(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" @@ -318,16 +248,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionSendResult": - """Create a RosActionSendResult object from an XML tree.""" - assert_xml_tag_ok(RosActionSendResult, xml_tree) - action_name = get_xml_argument(RosActionSendResult, xml_tree, "name") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionSendResult(action_name, fields) - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_action_server_defined(self._interface_name) @@ -341,12 +261,3 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_result_event( ros_declarations.get_action_server_info(self._interface_name)[0]) - - def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." - xml_action_result = ET.Element(RosActionSendResult.get_tag_name(), - {"name": self._interface_name}) - if self._fields is not None: - for field in self._fields: - xml_action_result.append(field.as_xml()) - return xml_action_result From d73d6f14394a746b713060cd7578eaa60a6075ff Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 13:45:32 +0200 Subject: [PATCH 31/49] Additional code reduction Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_base.py | 35 ++++++++- .../scxml_entries/scxml_ros_service.py | 75 ++----------------- .../scxml_entries/scxml_ros_topic.py | 63 ++-------------- 3 files changed, 46 insertions(+), 127 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index 1aadd1e5..61e23aba 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -23,7 +23,8 @@ valid_execution_body) from scxml_converter.scxml_entries.bt_utils import BtPortsHandler -from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -35,8 +36,32 @@ class RosDeclaration(ScxmlBase): @classmethod def get_tag_name(cls) -> str: + """The xml tag related to the ROS declaration.""" raise NotImplementedError(f"{cls.__name__} doesn't implement get_tag_name.") + @classmethod + def get_communication_interface(cls) -> str: + """ + Which communication interface is used by the ROS declaration. + + Expected values: "topic", "service", "action" + """ + raise NotImplementedError(f"{cls.__name__} doesn't implement get_communication_interface.") + + @classmethod + def get_xml_arg_interface_name(cls) -> str: + return f"{cls.get_communication_interface()}_name" + + @classmethod + def from_xml_tree(cls: Type['RosDeclaration'], xml_tree: ET.Element) -> 'RosDeclaration': + """Create an instance of the class from an XML tree.""" + assert_xml_tag_ok(cls, xml_tree) + interface_name = read_value_from_xml_arg_or_child( + cls, xml_tree, cls.get_xml_arg_interface_name(), (BtGetValueInputPort, str)) + interface_type = get_xml_argument(cls, xml_tree, "type") + interface_alias = get_xml_argument(cls, xml_tree, "name", none_allowed=True) + return cls(interface_name, interface_type, interface_alias) + def __init__(self, interface_name: Union[str, BtGetValueInputPort], interface_type: str, interface_alias: Optional[str] = None): """ @@ -95,6 +120,14 @@ def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot raise RuntimeError(f"Error: SCXML {self.__class__} cannot be converted to plain SCXML.") + def as_xml(self) -> ET.Element: + assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + xml_declaration = ET.Element(self.get_tag_name(), + {"name": self._interface_alias, + self.get_xml_arg_interface_name(): self._interface_name, + "type": self._interface_type}) + return xml_declaration + class RosCallback(ScxmlTransition): """Base class for ROS callbacks in SCXML.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 49893722..2c1d32e4 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -24,15 +24,14 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlRosDeclarationsContainer, BtGetValueInputPort, execution_body_from_xml) + RosField, ScxmlRosDeclarationsContainer, execution_body_from_xml) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger from scxml_converter.scxml_entries.ros_utils import ( generate_srv_request_event, generate_srv_response_event, generate_srv_server_request_event, generate_srv_server_response_event, is_srv_type_known) -from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.xml_utils import (assert_xml_tag_ok, get_xml_argument) class RosServiceServer(RosDeclaration): @@ -43,15 +42,8 @@ def get_tag_name() -> str: return "ros_service_server" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosServiceServer": - """Create a RosServiceServer object from an XML tree.""" - assert_xml_tag_ok(RosServiceServer, xml_tree) - service_name = read_value_from_xml_arg_or_child(RosServiceServer, xml_tree, "service_name", - (BtGetValueInputPort, str)) - service_type = get_xml_argument(RosServiceServer, xml_tree, "type") - service_alias = get_xml_argument( - RosServiceServer, xml_tree, "name", none_allowed=True) - return RosServiceServer(service_name, service_type, service_alias) + def get_communication_interface() -> str: + return "service" def check_valid_interface_type(self) -> bool: if not is_srv_type_known(self._interface_type): @@ -59,14 +51,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosServiceServer: invalid parameters." - xml_srv_server = ET.Element( - RosServiceServer.get_tag_name(), - {"name": self._interface_alias, - "service_name": self._interface_name, "type": self._interface_type}) - return xml_srv_server - class RosServiceClient(RosDeclaration): """Object used in SCXML root to declare a new service client.""" @@ -76,15 +60,8 @@ def get_tag_name() -> str: return "ros_service_client" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": - """Create a RosServiceClient object from an XML tree.""" - assert_xml_tag_ok(RosServiceClient, xml_tree) - service_name = read_value_from_xml_arg_or_child(RosServiceClient, xml_tree, "service_name", - (BtGetValueInputPort, str)) - service_type = get_xml_argument(RosServiceClient, xml_tree, "type") - service_alias = get_xml_argument( - RosServiceClient, xml_tree, "name", none_allowed=True) - return RosServiceClient(service_name, service_type, service_alias) + def get_communication_interface() -> str: + return "service" def check_valid_interface_type(self) -> bool: if not is_srv_type_known(self._interface_type): @@ -92,14 +69,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosServiceClient: invalid parameters." - xml_srv_server = ET.Element( - RosServiceClient.get_tag_name(), - {"name": self._interface_alias, - "service_name": self._interface_name, "type": self._interface_type}) - return xml_srv_server - class RosServiceSendRequest(RosTrigger): """Object representing a ROS service request (from the client side) in SCXML.""" @@ -137,14 +106,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_service_client_info(self._interface_name)[0], ros_declarations.get_automaton_name()) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Send Request: invalid parameters." - xml_srv_request = ET.Element(RosServiceSendRequest.get_tag_name(), - {"name": self._interface_name}) - for field in self._fields: - xml_srv_request.append(field.as_xml()) - return xml_srv_request - class RosServiceHandleRequest(RosCallback): """SCXML object representing a ROS service callback on the server, acting upon a request.""" @@ -177,14 +138,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_srv_server_request_event( ros_declarations.get_service_server_info(self._interface_name)[0]) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Handle Request: invalid parameters." - xml_srv_request = ET.Element(RosServiceHandleRequest.get_tag_name(), - {"name": self._interface_name, "target": self._target}) - for body_elem in self._body: - xml_srv_request.append(body_elem.as_xml()) - return xml_srv_request - class RosServiceSendResponse(RosTrigger): """SCXML object representing the response from a service server.""" @@ -224,14 +177,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_srv_server_response_event( ros_declarations.get_service_server_info(self._interface_name)[0]) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Send Response: invalid parameters." - xml_srv_response = ET.Element(RosServiceSendResponse.get_tag_name(), - {"name": self._service_name}) - for field in self._fields: - xml_srv_response.append(field.as_xml()) - return xml_srv_response - class RosServiceHandleResponse(RosCallback): """SCXML object representing the handler of a service response for a service client.""" @@ -264,11 +209,3 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_srv_response_event( ros_declarations.get_service_client_info(self._interface_name)[0], ros_declarations.get_automaton_name()) - - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Service Handle Response: invalid parameters." - xml_srv_response = ET.Element(RosServiceHandleResponse.get_tag_name(), - {"name": self._service_name, "target": self._target}) - for body_elem in self._body: - xml_srv_response.append(body_elem.as_xml()) - return xml_srv_response diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index 5c0602c5..090f0794 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -24,13 +24,12 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - RosField, ScxmlRosDeclarationsContainer, ScxmlSend, BtGetValueInputPort, - execution_body_from_xml) + RosField, ScxmlRosDeclarationsContainer, ScxmlSend, execution_body_from_xml) from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosTrigger, RosDeclaration from scxml_converter.scxml_entries.ros_utils import (is_msg_type_known, generate_topic_event) from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, get_children_as_scxml, read_value_from_xml_child) + assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) class RosTopicPublisher(RosDeclaration): @@ -41,16 +40,8 @@ def get_tag_name() -> str: return "ros_topic_publisher" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosTopicPublisher": - """Create a RosTopicPublisher object from an XML tree.""" - assert_xml_tag_ok(RosTopicPublisher, xml_tree) - topic_name = get_xml_argument(RosTopicPublisher, xml_tree, "topic", none_allowed=True) - topic_type = get_xml_argument(RosTopicPublisher, xml_tree, "type") - pub_name = get_xml_argument(RosTopicPublisher, xml_tree, "name", none_allowed=True) - if topic_name is None: - topic_name = read_value_from_xml_child(xml_tree, "topic", (BtGetValueInputPort, str)) - assert topic_name is not None, "Error: SCXML topic publisher: topic name not found." - return RosTopicPublisher(topic_name, topic_type, pub_name) + def get_xml_arg_interface_name() -> str: + return "topic" def check_valid_interface_type(self) -> bool: if not is_msg_type_known(self._interface_type): @@ -58,14 +49,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: RosTopicPublisher: invalid parameters." - xml_topic_publisher = ET.Element( - RosTopicPublisher.get_tag_name(), - {"name": self._interface_alias, - "topic": self._interface_name, "type": self._interface_type}) - return xml_topic_publisher - class RosTopicSubscriber(RosDeclaration): """Object used in SCXML root to declare a new topic subscriber.""" @@ -75,16 +58,8 @@ def get_tag_name() -> str: return "ros_topic_subscriber" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosTopicSubscriber": - """Create a RosTopicSubscriber object from an XML tree.""" - assert_xml_tag_ok(RosTopicSubscriber, xml_tree) - topic_name = get_xml_argument(RosTopicSubscriber, xml_tree, "topic", none_allowed=True) - topic_type = get_xml_argument(RosTopicSubscriber, xml_tree, "type") - sub_name = get_xml_argument(RosTopicSubscriber, xml_tree, "name", none_allowed=True) - if topic_name is None: - topic_name = read_value_from_xml_child(xml_tree, "topic", (BtGetValueInputPort, str)) - assert topic_name is not None, "Error: SCXML topic subscriber: topic name not found." - return RosTopicSubscriber(topic_name, topic_type, sub_name) + def get_xml_arg_interface_name() -> str: + return "topic" def check_valid_interface_type(self) -> bool: if not is_msg_type_known(self._interface_type): @@ -92,14 +67,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosTopicSubscriber: invalid parameters." - xml_topic_subscriber = ET.Element( - RosTopicSubscriber.get_tag_name(), - {"name": self._interface_alias, - "topic": self._interface_name, "type": self._interface_type}) - return xml_topic_subscriber - class RosTopicCallback(RosCallback): """Object representing a transition to perform when a new ROS msg is received.""" @@ -131,15 +98,6 @@ def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContaine def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_topic_event(ros_declarations.get_subscriber_info(self._interface_name)[0]) - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." - xml_topic_callback = ET.Element(RosTopicCallback.get_tag_name(), - {"name": self._interface_name, "target": self._target}) - if self._body is not None: - for entry in self._body: - xml_topic_callback.append(entry.as_xml()) - return xml_topic_callback - class RosTopicPublish(RosTrigger): """Object representing the shipping of a ROS msg through a topic.""" @@ -173,12 +131,3 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_topic_event(ros_declarations.get_publisher_info(self._interface_name)[0]) - - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." - xml_topic_publish = ET.Element(RosTopicPublish.get_tag_name(), - {"name": self._interface_name}) - if self._fields is not None: - for field in self._fields: - xml_topic_publish.append(field.as_xml()) - return xml_topic_publish From 9b26539d0552c151384cc9a92b10e45467e6126d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 14:01:21 +0200 Subject: [PATCH 32/49] Reduce duplication in action scxml classes, too Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_client.py | 25 +++------------- .../scxml_entries/scxml_ros_action_server.py | 25 +++------------- .../scxml_ros_action_server_thread.py | 30 +------------------ 3 files changed, 9 insertions(+), 71 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 35957c12..56e54744 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -22,16 +22,14 @@ from typing import List, Union, Type from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ( - ScxmlTransition, BtGetValueInputPort, ScxmlRosDeclarationsContainer) +from scxml_converter.scxml_entries import ScxmlTransition, ScxmlRosDeclarationsContainer from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_req_event, generate_action_goal_accepted_event, generate_action_goal_rejected_event, generate_action_feedback_handle_event, generate_action_result_handle_event) -from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -43,14 +41,8 @@ def get_tag_name() -> str: return "ros_action_client" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionClient": - """Create a RosActionClient object from an XML tree.""" - assert_xml_tag_ok(RosActionClient, xml_tree) - action_alias = get_xml_argument(RosActionClient, xml_tree, "name", none_allowed=True) - action_name = read_value_from_xml_arg_or_child(RosActionClient, xml_tree, "action_name", - (BtGetValueInputPort, str)) - action_type = get_xml_argument(RosActionClient, xml_tree, "type") - return RosActionClient(action_name, action_type, action_alias) + def get_communication_interface() -> str: + return "action" def check_valid_interface_type(self) -> bool: if not is_action_type_known(self._interface_type): @@ -58,15 +50,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML Action Client: invalid parameters." - xml_action_server = ET.Element( - RosActionClient.get_tag_name(), - {"name": self._interface_alias, - "action_name": self._interface_name, - "type": self._interface_type}) - return xml_action_server - class RosActionSendGoal(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 8a3ebd04..65f72ede 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -22,8 +22,7 @@ from typing import List, Union, Type from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ( - RosField, BtGetValueInputPort, ScxmlRosDeclarationsContainer) +from scxml_converter.scxml_entries import RosField, ScxmlRosDeclarationsContainer from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger @@ -32,8 +31,7 @@ generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, generate_action_thread_execution_start_event, generate_action_feedback_event, generate_action_result_event) -from scxml_converter.scxml_entries.xml_utils import ( - assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) +from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument class RosActionServer(RosDeclaration): @@ -44,14 +42,8 @@ def get_tag_name() -> str: return "ros_action_client" @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionServer": - """Create a RosActionServer object from an XML tree.""" - assert_xml_tag_ok(RosActionServer, xml_tree) - action_alias = get_xml_argument(RosActionServer, xml_tree, "name", none_allowed=True) - action_name = read_value_from_xml_arg_or_child(RosActionServer, xml_tree, "action_name", - (BtGetValueInputPort, str)) - action_type = get_xml_argument(RosActionServer, xml_tree, "type") - return RosActionServer(action_name, action_type, action_alias) + def get_communication_interface() -> str: + return "action" def check_valid_interface_type(self) -> bool: if not is_action_type_known(self._interface_type): @@ -59,15 +51,6 @@ def check_valid_interface_type(self) -> bool: return False return True - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML RosActionServer: invalid parameters." - xml_action_server = ET.Element( - RosActionServer.get_tag_name(), - {"name": self._interface_alias, - "action_name": self._interface_name, - "type": self._interface_type}) - return xml_action_server - class RosActionHandleGoalRequest(RosCallback): """ diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 7a246f0f..881062c9 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -23,8 +23,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer, - execution_body_from_xml) + ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer from scxml_converter.scxml_entries.scxml_ros_base import RosCallback @@ -186,15 +185,6 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleThreadStart": - """Create a RosActionHandleThreadStart object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleThreadStart, xml_tree) - server_alias = get_xml_argument(RosActionHandleThreadStart, xml_tree, "name") - target_state = get_xml_argument(RosActionHandleThreadStart, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleThreadStart(server_alias, target_state, exec_body) - def __init__(self, server_alias: Union[str, RosActionServer], target_state: str, exec_body: Optional[ScxmlExecutionBody] = None) -> None: """ @@ -224,15 +214,6 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_action_thread_execution_start_event( ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) - def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." - xml_thread_start = ET.Element(self.get_tag_name(), - {"name": self._interface_name, "target": self._target}) - if self._body is not None: - for body_elem in self._body: - xml_thread_start.append(body_elem.as_xml()) - return xml_thread_start - class RosActionHandleThreadCancel(RosActionHandleThreadStart): """ @@ -246,15 +227,6 @@ class RosActionHandleThreadCancel(RosActionHandleThreadStart): def get_tag_name() -> str: return "ros_action_thread_cancel" - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionHandleThreadCancel": - """Create a RosActionHandleThreadCancel object from an XML tree.""" - assert_xml_tag_ok(RosActionHandleThreadCancel, xml_tree) - server_alias = get_xml_argument(RosActionHandleThreadCancel, xml_tree, "name") - target_state = get_xml_argument(RosActionHandleThreadCancel, xml_tree, "target") - exec_body = execution_body_from_xml(xml_tree) - return RosActionHandleThreadCancel(server_alias, target_state, exec_body) - def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." return generate_action_thread_execution_cancel_event( From 661268b7ff1e27089c3d6f7d093c5fc164420dab Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 14:06:06 +0200 Subject: [PATCH 33/49] Set max line size in flake8 Signed-off-by: Marco Lampacrescia --- .github/workflows/lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 551052b1..991744fa 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -71,7 +71,7 @@ jobs: use-pydocstyle: false extra-pylint-options: "" extra-pycodestyle-options: "" - extra-flake8-options: "" + extra-flake8-options: "--max-line-length=100" extra-black-options: "" extra-mypy-options: "--ignore-missing-imports" extra-isort-options: "" \ No newline at end of file From 7b07aefd1175029df9cd1d05661fd48dd258f79f Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 15:27:10 +0200 Subject: [PATCH 34/49] Prepare tests to support multiple returned Scxml objects Signed-off-by: Marco Lampacrescia --- .../scxml_helpers/top_level_interpreter.py | 8 +- .../scxml_entries/scxml_root.py | 29 ++-- .../{client_1.scxml => addition_client.scxml} | 0 .../{server.scxml => addition_server.scxml} | 0 .../{client_1.scxml => addition_client.scxml} | 0 .../{server.scxml => addition_server.scxml} | 0 .../_test_data/battery_drainer_w_bt/bt.xml | 4 +- .../bt_topic_action.scxml | 2 +- .../bt_topic_condition.scxml | 2 +- ...ion.scxml => 10000_BtTopicCondition.scxml} | 2 +- ...cAction.scxml => 1001_BtTopicAction.scxml} | 2 +- .../battery_drainer_w_bt/gt_bt_scxml/bt.scxml | 8 +- .../gt_parsed_scxml/bt_topic_condition.scxml | 2 +- .../gt_plain_scxml/bt_topic_action.scxml | 2 +- .../gt_plain_scxml/bt_topic_condition.scxml | 2 +- .../test/_test_data/bt_ports_only/bt.xml | 4 +- .../bt_ports_only/bt_topic_action.scxml | 2 +- ...cAction.scxml => 1000_BtTopicAction.scxml} | 2 +- ...cAction.scxml => 1001_BtTopicAction.scxml} | 2 +- .../bt_ports_only/gt_bt_scxml/bt.scxml | 8 +- .../gt_parsed_scxml/bt_topic_action.scxml | 2 +- .../gt_plain_scxml/bt_topic_action.scxml | 2 +- .../ros_fibonacci_action_example/.gitignore | 3 + .../client_1.scxml | 46 ++++++ .../client_2.scxml | 45 ++++++ .../ros_fibonacci_action_example/server.scxml | 149 ++++++++++++++++++ .../test/test_systemtest_scxml_entries.py | 2 +- scxml_converter/test/test_systemtest_xml.py | 52 ++++-- scxml_converter/test/test_utils.py | 6 + 29 files changed, 334 insertions(+), 54 deletions(-) rename scxml_converter/test/_test_data/add_int_srv_example/{client_1.scxml => addition_client.scxml} (100%) rename scxml_converter/test/_test_data/add_int_srv_example/{server.scxml => addition_server.scxml} (100%) rename scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/{client_1.scxml => addition_client.scxml} (100%) rename scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/{server.scxml => addition_server.scxml} (100%) rename scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/{10000_TopicCondition.scxml => 10000_BtTopicCondition.scxml} (96%) rename scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/{1001_TopicAction.scxml => 1001_BtTopicAction.scxml} (95%) rename scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/{1000_TopicAction.scxml => 1000_BtTopicAction.scxml} (81%) rename scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/{1001_TopicAction.scxml => 1001_BtTopicAction.scxml} (81%) create mode 100644 scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore create mode 100644 scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml create mode 100644 scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml create mode 100644 scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml diff --git a/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py b/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py index 6ea24aff..4616ec33 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py +++ b/jani_generator/src/jani_generator/scxml_helpers/top_level_interpreter.py @@ -141,7 +141,7 @@ def generate_plain_scxml_models_and_timers( all_timers: List[RosTimer] = [] all_services: RosServices = {} for scxml_entry in ros_scxmls: - plain_scxml, ros_declarations = \ + plain_scxmls, ros_declarations = \ scxml_entry.to_plain_scxml_and_declarations() # Handle ROS timers for timer_name, timer_rate in ros_declarations._timers.items(): @@ -153,13 +153,13 @@ def generate_plain_scxml_models_and_timers( if service_name not in all_services: all_services[service_name] = RosService() all_services[service_name].append_service_client( - service_name, service_type, plain_scxml.get_name()) + service_name, service_type, scxml_entry.get_name()) for service_name, service_type in ros_declarations._service_servers.values(): if service_name not in all_services: all_services[service_name] = RosService() all_services[service_name].set_service_server( - service_name, service_type, plain_scxml.get_name()) - plain_scxml_models.append(plain_scxml) + service_name, service_type, scxml_entry.get_name()) + plain_scxml_models.extend(plain_scxmls) # Generate service sync SCXML models for service_info in all_services.values(): plain_scxml_models.append(service_info.to_scxml()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index b1edbf31..53eec8ff 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -258,25 +258,34 @@ def is_plain_scxml(self) -> bool: # If this is a valid scxml object, checking the absence of declarations is enough return self._ros_declarations is None or len(self._ros_declarations) == 0 - def to_plain_scxml_and_declarations(self) -> Tuple["ScxmlRoot", ScxmlRosDeclarationsContainer]: + def to_plain_scxml_and_declarations(self) -> Tuple[List["ScxmlRoot"], + ScxmlRosDeclarationsContainer]: """ Convert all internal ROS specific entries to plain SCXML. :return: A tuple with: - - a new ScxmlRoot object with all ROS specific entries converted to plain SCXML - - A list of timers with related rate in Hz + - a list of ScxmlRoot objects with all ROS specific entries converted to plain SCXML + - The Ros declarations contained in the original SCXML object """ if self.is_plain_scxml(): - return self, ScxmlRosDeclarationsContainer(self._name) + return [self], ScxmlRosDeclarationsContainer(self._name) + converted_scxmls: List[ScxmlRoot] = [] # Convert the ROS specific entries to plain SCXML - plain_root = ScxmlRoot(self._name) - plain_root._data_model = deepcopy(self._data_model) - plain_root._initial_state = self._initial_state + main_scxml = ScxmlRoot(self._name) + main_scxml._data_model = deepcopy(self._data_model) + main_scxml._initial_state = self._initial_state ros_declarations = self._generate_ros_declarations_helper() assert ros_declarations is not None, "Error: SCXML root: invalid ROS declarations." - plain_root._states = [state.as_plain_scxml(ros_declarations) for state in self._states] - assert plain_root.is_plain_scxml(), "SCXML root: conversion to plain SCXML failed." - return (plain_root, ros_declarations) + main_scxml._states = [state.as_plain_scxml(ros_declarations) for state in self._states] + converted_scxmls.append(main_scxml) + for plain_scxml in converted_scxmls: + assert plain_scxml.check_validity(), \ + f"The SCXML root object {plain_scxml.get_name()} is not valid: " \ + "conversion to plain SCXML failed." + assert plain_scxml.is_plain_scxml(), \ + f"The SCXML root object {plain_scxml.get_name()} is not plain SCXML: " \ + "conversion to plain SCXML failed." + return (converted_scxmls, ros_declarations) def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid root object." diff --git a/scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml b/scxml_converter/test/_test_data/add_int_srv_example/addition_client.scxml similarity index 100% rename from scxml_converter/test/_test_data/add_int_srv_example/client_1.scxml rename to scxml_converter/test/_test_data/add_int_srv_example/addition_client.scxml diff --git a/scxml_converter/test/_test_data/add_int_srv_example/server.scxml b/scxml_converter/test/_test_data/add_int_srv_example/addition_server.scxml similarity index 100% rename from scxml_converter/test/_test_data/add_int_srv_example/server.scxml rename to scxml_converter/test/_test_data/add_int_srv_example/addition_server.scxml diff --git a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/addition_client.scxml similarity index 100% rename from scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/client_1.scxml rename to scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/addition_client.scxml diff --git a/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml b/scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/addition_server.scxml similarity index 100% rename from scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/server.scxml rename to scxml_converter/test/_test_data/add_int_srv_example/gt_plain_scxml/addition_server.scxml diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml index 2a4cb98b..b0c4c5b2 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt.xml @@ -2,9 +2,9 @@ - + - + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml index 53736c4f..e641fd10 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_action.scxml @@ -2,7 +2,7 @@ diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml index cd91b1a0..1947b660 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/bt_topic_condition.scxml @@ -2,7 +2,7 @@ diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_BtTopicCondition.scxml similarity index 96% rename from scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_BtTopicCondition.scxml index 7e07abd3..ebdd927b 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_TopicCondition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/10000_BtTopicCondition.scxml @@ -2,7 +2,7 @@ diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_BtTopicAction.scxml similarity index 95% rename from scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml rename to scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_BtTopicAction.scxml index f76b81f0..e41a3659 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_TopicAction.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/1001_BtTopicAction.scxml @@ -2,7 +2,7 @@ diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml index 0dd4e515..c8eb9860 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_bt_scxml/bt.scxml @@ -2,7 +2,7 @@ - + @@ -13,15 +13,15 @@ - + - + - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml index cd91b1a0..1947b660 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_parsed_scxml/bt_topic_condition.scxml @@ -2,7 +2,7 @@ diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml index 13bbf09f..569ab465 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_action.scxml @@ -1,4 +1,4 @@ - + diff --git a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml index aec642b6..f4fdadc1 100644 --- a/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml +++ b/scxml_converter/test/_test_data/battery_drainer_w_bt/gt_plain_scxml/bt_topic_condition.scxml @@ -1,4 +1,4 @@ - + diff --git a/scxml_converter/test/_test_data/bt_ports_only/bt.xml b/scxml_converter/test/_test_data/bt_ports_only/bt.xml index f690a4d4..b526d0cd 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/bt.xml +++ b/scxml_converter/test/_test_data/bt_ports_only/bt.xml @@ -1,8 +1,8 @@ - - + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml index 4dbce6c5..f8b0a64a 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/bt_topic_action.scxml @@ -3,7 +3,7 @@ xmlns="http://www.w3.org/2005/07/scxml" initial="initial" version="1.0" - name="TopicAction" + name="BtTopicAction" model_src=""> diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_BtTopicAction.scxml similarity index 81% rename from scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml rename to scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_BtTopicAction.scxml index 8012bf94..a63883e6 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_TopicAction.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1000_BtTopicAction.scxml @@ -1,5 +1,5 @@ - + diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_BtTopicAction.scxml similarity index 81% rename from scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml rename to scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_BtTopicAction.scxml index ffbcd88a..2ca7c28e 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_TopicAction.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/1001_BtTopicAction.scxml @@ -1,5 +1,5 @@ - + diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml index 05cc4fb6..143a3edb 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_bt_scxml/bt.scxml @@ -2,7 +2,7 @@ - + @@ -13,15 +13,15 @@ - + - + - + diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml index c29c0222..7d47ca6f 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_parsed_scxml/bt_topic_action.scxml @@ -3,7 +3,7 @@ xmlns="http://www.w3.org/2005/07/scxml" initial="initial" version="1.0" - name="TopicAction" + name="BtTopicAction" model_src=""> diff --git a/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml index 753d90f4..b578cb46 100644 --- a/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml +++ b/scxml_converter/test/_test_data/bt_ports_only/gt_plain_scxml/bt_topic_action.scxml @@ -1,5 +1,5 @@ - + diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore b/scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore new file mode 100644 index 00000000..b941f4e4 --- /dev/null +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore @@ -0,0 +1,3 @@ +generated_bt_scxml +generated_plain_scxml +main.jani \ No newline at end of file diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml new file mode 100644 index 00000000..0fc88cd2 --- /dev/null +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml new file mode 100644 index 00000000..c4691bbd --- /dev/null +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml new file mode 100644 index 00000000..8d4fdb1b --- /dev/null +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scxml_converter/test/test_systemtest_scxml_entries.py b/scxml_converter/test/test_systemtest_scxml_entries.py index a136fa17..c700b8db 100644 --- a/scxml_converter/test/test_systemtest_scxml_entries.py +++ b/scxml_converter/test/test_systemtest_scxml_entries.py @@ -131,7 +131,7 @@ def test_bt_action_with_ports_from_code(): RosTopicPublish(topic_publisher, [RosField("data", "number")]) ]) ]) - scxml_root = ScxmlRoot("TopicAction") + scxml_root = ScxmlRoot("BtTopicAction") scxml_root.set_data_model(data_model) scxml_root.add_bt_port_declaration(BtInputPortDeclaration("name", "string")) scxml_root.add_bt_port_declaration(BtInputPortDeclaration("data", "int16")) diff --git a/scxml_converter/test/test_systemtest_xml.py b/scxml_converter/test/test_systemtest_xml.py index 8dcaa234..6544547b 100644 --- a/scxml_converter/test/test_systemtest_xml.py +++ b/scxml_converter/test/test_systemtest_xml.py @@ -17,7 +17,7 @@ from typing import Dict, List, Tuple -from test_utils import canonicalize_xml, remove_empty_lines +from test_utils import canonicalize_xml, remove_empty_lines, to_snake_case from scxml_converter.bt_converter import bt_converter from scxml_converter.scxml_entries import ScxmlRoot @@ -72,30 +72,52 @@ def bt_to_scxml_test( def ros_to_plain_scxml_test(test_folder: str, scxml_bt_ports: Dict[str, List[Tuple[str, str]]], + expected_scxmls: Dict[str, List[str]], store_generated: bool = False): - """Test the conversion of SCXML with ROS-specific macros to plain SCXML.""" + """ + Test the conversion of SCXML with ROS-specific macros to plain SCXML. + + :param test_folder: The path of the folder with the files to evaluate. + :param scxml_bt_ports: The BT ports to set to the specified SCXML file. + :param expected_scxmls: The SCXML object names expected from the specified input files. + :param store_generated: If True, the generated SCXML files are stored in the output folder. + """ test_data_path = os.path.join(os.path.dirname(__file__), '_test_data', test_folder) scxml_files = [file for file in os.listdir(test_data_path) if file.endswith('.scxml')] if store_generated: clear_output_folder(test_folder) for fname in scxml_files: input_file = os.path.join(test_data_path, fname) - gt_file = os.path.join(test_data_path, 'gt_plain_scxml', fname) + # gt_file = os.path.join(test_data_path, 'gt_plain_scxml', fname) try: scxml_obj = ScxmlRoot.from_scxml_file(input_file) if fname in scxml_bt_ports: scxml_obj.set_bt_ports_values(scxml_bt_ports[fname]) scxml_obj.update_bt_ports_values() - plain_scxml, _ = scxml_obj.to_plain_scxml_and_declarations() + plain_scxmls, _ = scxml_obj.to_plain_scxml_and_declarations() if store_generated: - output_file = os.path.join(get_output_folder(test_folder), fname) - with open(output_file, 'w') as f_o: - f_o.write(plain_scxml.as_xml_string()) - scxml_str = plain_scxml.as_xml_string() - with open(gt_file, 'r', encoding='utf-8') as f_o: - gt_output = f_o.read() - assert remove_empty_lines(canonicalize_xml(scxml_str)) == \ - remove_empty_lines(canonicalize_xml(gt_output)) + for generated_scxml in plain_scxmls: + output_file = os.path.join(get_output_folder(test_folder), + f'{generated_scxml.get_name()}.scxml') + with open(output_file, 'w') as f_o: + f_o.write(generated_scxml.as_xml_string()) + if fname not in expected_scxmls: + gt_files: List[str] = [fname.removesuffix('.scxml')] + else: + gt_files: List[str] = expected_scxmls[fname] + assert len(plain_scxmls) == len(gt_files), \ + f"Expecting {len(gt_files)} scxml objects, found {len(plain_scxmls)}." + for generated_scxml in plain_scxmls: + # Make sure the comparison uses snake case + scxml_object_name = to_snake_case(generated_scxml.get_name()) + assert scxml_object_name in gt_files, \ + f"Generated SCXML {scxml_object_name} not in gt SCXMLs {gt_files}." + gt_file_path = os.path.join(test_data_path, 'gt_plain_scxml', + f'{scxml_object_name}.scxml') + with open(gt_file_path, 'r', encoding='utf-8') as f_o: + gt_output = f_o.read() + assert remove_empty_lines(canonicalize_xml(generated_scxml.as_xml_string())) == \ + remove_empty_lines(canonicalize_xml(gt_output)) except Exception as e: print(f"Error in file {fname}:") raise e @@ -107,7 +129,7 @@ def test_bt_to_scxml_battery_drainer(): def test_ros_to_plain_scxml_battery_drainer(): - ros_to_plain_scxml_test('battery_drainer_w_bt', {}, True) + ros_to_plain_scxml_test('battery_drainer_w_bt', {}, {}, True) def test_bt_to_scxml_bt_ports(): @@ -116,9 +138,9 @@ def test_bt_to_scxml_bt_ports(): def test_ros_to_plain_scxml_bt_ports(): ros_to_plain_scxml_test('bt_ports_only', - {'bt_topic_action.scxml': [('name', 'out'), ('data', '123')]}, + {'bt_topic_action.scxml': [('name', 'out'), ('data', '123')]}, {}, True) def test_ros_to_plain_scxml_add_int_srv(): - ros_to_plain_scxml_test('add_int_srv_example', {}, True) + ros_to_plain_scxml_test('add_int_srv_example', {}, {}, True) diff --git a/scxml_converter/test/test_utils.py b/scxml_converter/test/test_utils.py index 063871e1..01ac6da9 100644 --- a/scxml_converter/test/test_utils.py +++ b/scxml_converter/test/test_utils.py @@ -14,6 +14,12 @@ # limitations under the License. from xml.etree import ElementTree as ET +import re + + +def to_snake_case(text: str) -> str: + """Convert a string to snake case.""" + return re.sub(r'(? str: From 47b11fd67487a75a21bef75cd9a6748a271b0e9d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 16:49:15 +0200 Subject: [PATCH 35/49] Get rid of scxml_ros_entries Signed-off-by: Marco Lampacrescia --- .../src/scxml_converter/scxml_converter.py | 5 +- .../scxml_converter/scxml_entries/__init__.py | 2 - .../scxml_entries/ros_utils.py | 6 + .../scxml_entries/scxml_executable_entries.py | 6 +- .../scxml_entries/scxml_root.py | 14 +- .../scxml_entries/scxml_ros_base.py | 2 +- .../scxml_entries/scxml_ros_entries.py | 38 ------ .../scxml_entries/scxml_ros_timer.py | 126 +++++++----------- .../scxml_entries/xml_utils.py | 8 +- 9 files changed, 71 insertions(+), 136 deletions(-) delete mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py diff --git a/scxml_converter/src/scxml_converter/scxml_converter.py b/scxml_converter/src/scxml_converter/scxml_converter.py index 4791f663..fb42bf4f 100644 --- a/scxml_converter/src/scxml_converter/scxml_converter.py +++ b/scxml_converter/src/scxml_converter/scxml_converter.py @@ -20,13 +20,10 @@ into generic SCXML code. """ -import xml.etree.ElementTree as ET -from typing import Dict, Tuple, Union +from typing import Dict, Union from as2fm_common.common import ros_type_name_to_python_type from as2fm_common.ecmascript_interpretation import interpret_ecma_script_expr -from scxml_converter.scxml_entries import (ScxmlRoot, - ScxmlRosDeclarationsContainer) BASIC_FIELD_TYPES = ['boolean', 'int32', 'int16', 'float', 'double'] diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index 124266b5..5771e753 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -22,7 +22,5 @@ RosServiceServer, RosServiceClient, RosServiceHandleRequest, # noqa: F401 RosServiceHandleResponse, RosServiceSendRequest, RosServiceSendResponse) # noqa: F401 from .scxml_ros_timer import (RosTimeRate, RosRateCallback) # noqa: F401 -from .scxml_ros_entries import ( # noqa: F401 - ScxmlRosDeclarations, ScxmlRosSends, ScxmlRosTransitions) # noqa: F401 from .scxml_state import ScxmlState # noqa: F401 from .scxml_root import ScxmlRoot # noqa: F401 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index df09e95b..218b1221 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -163,6 +163,12 @@ def sanitize_ros_interface_name(interface_name: str) -> str: return interface_name.replace("/", "__") +def generate_rate_timer_event(timer_name: str) -> str: + """Generate the name of the event triggered by a rate timer.""" + # TODO: Remove dot notation + return f"ros_time_rate.{timer_name}" + + def generate_topic_event(topic_name: str) -> str: """Generate the name of the event that triggers a message reception from a topic.""" return f"topic_{sanitize_ros_interface_name(topic_name)}_msg" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index 952f324b..fbb34e5b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -374,11 +374,11 @@ def execution_entry_from_xml(xml_tree: ET.Element) -> ScxmlExecutableEntry: :param xml_tree: The XML tree to create the execution entry from :return: The execution entry """ - # TODO: This is pretty bad, need to re-check how to break the circle - from .scxml_ros_entries import ScxmlRosSends + from scxml_converter.scxml_entries.scxml_ros_base import RosTrigger # TODO: This should be generated only once, since it stays as it is - tag_to_cls = {cls.get_tag_name(): cls for cls in _ResolvedScxmlExecutableEntry + ScxmlRosSends} + tag_to_cls = {cls.get_tag_name(): cls for cls in _ResolvedScxmlExecutableEntry} + tag_to_cls.update({cls.get_tag_name(): cls for cls in RosTrigger.__subclasses__()}) exec_tag = xml_tree.tag assert exec_tag in tag_to_cls, \ f"Error: SCXML conversion: tag {exec_tag} isn't an executable entry." diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 53eec8ff..e2adae1f 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -25,7 +25,9 @@ from scxml_converter.scxml_entries import ( BtInputPortDeclaration, BtOutputPortDeclaration, RosServiceClient, RosServiceServer, RosTimeRate, RosTopicPublisher, RosTopicSubscriber, ScxmlBase, ScxmlDataModel, - ScxmlRosDeclarations, ScxmlRosDeclarationsContainer, ScxmlState) + ScxmlRosDeclarationsContainer, ScxmlState) + +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration from scxml_converter.scxml_entries.scxml_bt import BtPortDeclarations from scxml_converter.scxml_entries.bt_utils import BtPortsHandler @@ -56,8 +58,8 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": assert len(datamodel_elements) <= 1, \ f"Error: SCXML root: {len(datamodel_elements)} datamodels found, max 1 allowed." # ROS Declarations - ros_declarations: List[ScxmlRosDeclarations] = get_children_as_scxml( - xml_tree, get_args(ScxmlRosDeclarations)) + ros_declarations: List[RosDeclaration] = get_children_as_scxml( + xml_tree, RosDeclaration.__subclasses__()) # BT Declarations bt_port_declarations: List[BtPortDeclarations] = get_children_as_scxml( xml_tree, get_args(BtPortDeclarations)) @@ -102,7 +104,7 @@ def __init__(self, name: str): self._initial_state: Optional[str] = None self._states: List[ScxmlState] = [] self._data_model: Optional[ScxmlDataModel] = None - self._ros_declarations: List[ScxmlRosDeclarations] = [] + self._ros_declarations: List[RosDeclaration] = [] self._bt_ports_handler = BtPortsHandler() def get_name(self) -> str: @@ -147,8 +149,8 @@ def set_data_model(self, data_model: ScxmlDataModel): assert self._data_model is None, "Data model already set" self._data_model = data_model - def add_ros_declaration(self, ros_declaration: ScxmlRosDeclarations): - assert isinstance(ros_declaration, get_args(ScxmlRosDeclarations)), \ + def add_ros_declaration(self, ros_declaration: RosDeclaration): + assert isinstance(ros_declaration, RosDeclaration), \ "Error: SCXML root: invalid ROS declaration type." assert ros_declaration.check_validity(), "Error: SCXML root: invalid ROS declaration." if self._ros_declarations is None: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index 61e23aba..dff8ac92 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -44,7 +44,7 @@ def get_communication_interface(cls) -> str: """ Which communication interface is used by the ROS declaration. - Expected values: "topic", "service", "action" + Expected values: "service", "action" """ raise NotImplementedError(f"{cls.__name__} doesn't implement get_communication_interface.") diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py deleted file mode 100644 index b9a4dfb3..00000000 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py +++ /dev/null @@ -1,38 +0,0 @@ -# Copyright (c) 2024 - for information on the respective copyright owner -# see the NOTICE file - -# 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. - -"""Declaration of ROS-Specific SCXML tags extensions.""" - -from typing import Union - -from scxml_converter.scxml_entries import (RosRateCallback, RosServiceClient, - RosServiceHandleRequest, - RosServiceHandleResponse, - RosServiceSendRequest, - RosServiceSendResponse, - RosServiceServer, RosTimeRate, - RosTopicCallback, RosTopicPublish, - RosTopicPublisher, - RosTopicSubscriber) - -ScxmlRosDeclarations = Union[RosTimeRate, RosTopicPublisher, RosTopicSubscriber, - RosServiceServer, RosServiceClient] - -# List of Ros entries inheriting from ScxmlTransition -ScxmlRosTransitions = (RosServiceHandleRequest, RosServiceHandleResponse, - RosTopicCallback, RosRateCallback) - -# List of Ros entries inheriting from ScxmlSend -ScxmlRosSends = (RosServiceSendRequest, RosServiceSendResponse, RosTopicPublish) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py index 93ab0e6c..8df284b8 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py @@ -15,16 +15,21 @@ """Declaration of SCXML tags related to ROS Timers.""" -from typing import Optional, Union +from typing import Optional, Type, Union from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlTransition, - as_plain_execution_body, execution_body_from_xml, valid_execution_body) + ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlTransition, + as_plain_execution_body, execution_body_from_xml) +from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback + from scxml_converter.scxml_entries.bt_utils import BtPortsHandler +from scxml_converter.scxml_entries.ros_utils import generate_rate_timer_event +from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument +from scxml_converter.scxml_entries.utils import is_non_empty_string -class RosTimeRate(ScxmlBase): +class RosTimeRate(RosDeclaration): """Object used in the SCXML root to declare a new timer with its related tick rate.""" @staticmethod @@ -34,12 +39,9 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosTimeRate": """Create a RosTimeRate object from an XML tree.""" - assert xml_tree.tag == RosTimeRate.get_tag_name(), \ - f"Error: SCXML rate timer: XML tag name is not {RosTimeRate.get_tag_name()}" - timer_name = xml_tree.attrib.get("name") - timer_rate_str = xml_tree.attrib.get("rate_hz") - assert timer_name is not None and timer_rate_str is not None, \ - "Error: SCXML rate timer: 'name' or 'rate_hz' attribute not found in input xml." + assert_xml_tag_ok(RosTimeRate, xml_tree) + timer_name = get_xml_argument(RosTimeRate, xml_tree, "name") + timer_rate_str = get_xml_argument(RosTimeRate, xml_tree, "rate_hz") try: timer_rate = float(timer_rate_str) except ValueError as e: @@ -50,33 +52,33 @@ def __init__(self, name: str, rate_hz: float): self._name = name self._rate_hz = float(rate_hz) + def get_interface_name(self) -> str: + raise RuntimeError("Error: SCXML rate timer: deleted method 'get_interface_name'.") + + def get_interface_type(self) -> str: + raise RuntimeError("Error: SCXML rate timer: deleted method 'get_interface_type'.") + + def get_name(self) -> str: + return self._name + + def get_rate(self) -> float: + return self._rate_hz + def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: """Update the values of potential entries making use of BT ports.""" pass def check_validity(self) -> bool: - valid_name = isinstance(self._name, str) and len(self._name) > 0 + valid_name = is_non_empty_string(RosTimeRate, "name", self._name) valid_rate = isinstance(self._rate_hz, float) and self._rate_hz > 0 - if not valid_name: - print("Error: SCXML rate timer: name is not valid.") if not valid_rate: print("Error: SCXML rate timer: rate is not valid.") return valid_name and valid_rate def check_valid_instantiation(self) -> bool: - """Check if the topic publisher has undefined entries (i.e. from BT ports).""" + """Check if the timer has undefined entries (i.e. from BT ports).""" return True - def get_name(self) -> str: - return self._name - - def get_rate(self) -> float: - return self._rate_hz - - def as_plain_scxml(self, _) -> ScxmlBase: - # This is discarded in the as_plain_scxml method from ScxmlRoot - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") - def as_xml(self) -> ET.Element: assert self.check_validity(), "Error: SCXML rate timer: invalid parameters." xml_time_rate = ET.Element( @@ -84,26 +86,25 @@ def as_xml(self) -> ET.Element: return xml_time_rate -class RosRateCallback(ScxmlTransition): +class RosRateCallback(RosCallback): """Callback that triggers each time the associated timer ticks.""" @staticmethod def get_tag_name() -> str: return "ros_rate_callback" + @staticmethod + def get_declaration_type() -> Type[RosTimeRate]: + return RosTimeRate + @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "RosRateCallback": """Create a RosRateCallback object from an XML tree.""" - assert xml_tree.tag == RosRateCallback.get_tag_name(), \ - f"Error: SCXML rate callback: XML tag name is not {RosRateCallback.get_tag_name()}" - timer_name = xml_tree.attrib.get("name") - target = xml_tree.attrib.get("target") - assert timer_name is not None and target is not None, \ - "Error: SCXML rate callback: 'name' or 'target' attribute not found in input xml." - condition = xml_tree.get("cond") - condition = condition if condition is not None and len(condition) > 0 else None + assert_xml_tag_ok(RosRateCallback, xml_tree) + timer_name = get_xml_argument(RosRateCallback, xml_tree, "name") + target = get_xml_argument(RosRateCallback, xml_tree, "target") + condition = get_xml_argument(RosRateCallback, xml_tree, "cond", none_allowed=True) exec_body = execution_body_from_xml(xml_tree) - exec_body = exec_body if exec_body is not None else None return RosRateCallback(timer_name, target, condition, exec_body) def __init__(self, timer: Union[RosTimeRate, str], target: str, condition: Optional[str] = None, @@ -116,61 +117,30 @@ def __init__(self, timer: Union[RosTimeRate, str], target: str, condition: Optio :param timer: The RosTimeRate instance triggering the callback, or its name :param body: The body of the callback """ - if isinstance(timer, RosTimeRate): - self._timer_name = timer.get_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(timer, str), "Error: SCXML rate callback: invalid timer type." - self._timer_name = timer - self._target = target self._condition = condition - self._body = body - assert self.check_validity(), "Error: SCXML rate callback: invalid parameters." + super().__init__(timer, target, body) def check_validity(self) -> bool: - valid_timer = isinstance(self._timer_name, str) and len(self._timer_name) > 0 - valid_target = isinstance(self._target, str) and len(self._target) > 0 - valid_cond = self._condition is None or ( - isinstance(self._condition, str) and len(self._condition) > 0) - valid_body = self._body is None or valid_execution_body(self._body) - if not valid_timer: - print("Error: SCXML rate callback: timer name is not valid.") - if not valid_target: - print("Error: SCXML rate callback: target is not valid.") - if not valid_cond: - print("Error: SCXML rate callback: condition is not valid.") - if not valid_body: - print("Error: SCXML rate callback: body is not valid.") - return valid_timer and valid_target and valid_cond and valid_body - - def check_valid_ros_instantiations(self, - ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - "Error: SCXML rate callback: invalid ROS declarations container." - timer_cb_declared = ros_declarations.is_timer_defined(self._timer_name) - if not timer_cb_declared: - print(f"Error: SCXML rate callback: timer {self._timer_name} not declared.") - return False - valid_body = super().check_valid_ros_instantiations(ros_declarations) - if not valid_body: - print("Error: SCXML rate callback: body has invalid ROS instantiations.") - return valid_body + valid_parent = super().check_validity() + valid_condition = self._condition is None or \ + is_non_empty_string(RosRateCallback, "cond", self._condition) + return valid_parent and valid_condition + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_timer_defined(self._interface_name) + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_rate_timer_event(self._interface_name) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - event_name = "ros_time_rate." + self._timer_name + event_name = self.get_plain_scxml_event(ros_declarations) target = self._target cond = self._condition body = as_plain_execution_body(self._body, ros_declarations) return ScxmlTransition(target, [event_name], cond, body) def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML rate callback: invalid parameters." - xml_rate_callback = ET.Element( - "ros_rate_callback", {"name": self._timer_name, "target": self._target}) + xml_rate_callback = super().as_xml() if self._condition is not None: xml_rate_callback.set("cond", self._condition) - if self._body is not None: - for entry in self._body: - xml_rate_callback.append(entry.as_xml()) return xml_rate_callback diff --git a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py index 9700cbcb..27d8fc7e 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py @@ -13,7 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import List, Optional, Tuple, Type, Union +from typing import List, Iterable, Optional, Union, Type from scxml_converter.scxml_entries import ScxmlBase from xml.etree.ElementTree import Element @@ -39,7 +39,7 @@ def get_xml_argument(scxml_type: Type[ScxmlBase], xml_tree: Element, arg_name: s def get_children_as_scxml( - xml_tree: Element, scxml_types: Tuple[Type[ScxmlBase]]) -> List[ScxmlBase]: + xml_tree: Element, scxml_types: Iterable[Type[ScxmlBase]]) -> List[ScxmlBase]: """ Load the children of the xml tree as scxml entries. @@ -56,7 +56,7 @@ def get_children_as_scxml( def read_value_from_xml_child( - xml_tree: Element, child_tag: str, valid_types: Tuple[Type[Union[ScxmlBase, str]]] + xml_tree: Element, child_tag: str, valid_types: Iterable[Type[Union[ScxmlBase, str]]] ) -> Optional[Union[str, ScxmlBase]]: """ Try to read the value of a child tag from the xml tree. If the child is not found, return None. @@ -90,7 +90,7 @@ def read_value_from_xml_child( def read_value_from_xml_arg_or_child( scxml_type: Type[ScxmlBase], xml_tree: Element, tag_name: str, - valid_types: Tuple[Type[Union[ScxmlBase, str]]], + valid_types: Iterable[Type[Union[ScxmlBase, str]]], none_allowed=False) -> Optional[Union[str, ScxmlBase]]: """ Read a value from an xml attribute or, if not found, the child tag with the same name. From 12e605edd2a17f9ae218519f3bdd216c5e4f4a38 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 17:09:49 +0200 Subject: [PATCH 36/49] Move ros declaration handling out of scxml root Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 65 ++++++++++++++++--- .../scxml_entries/scxml_root.py | 25 +------ 2 files changed, 59 insertions(+), 31 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 218b1221..1da13d6a 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -17,7 +17,7 @@ from typing import Any, Dict, List, Tuple, Type -from scxml_converter.scxml_entries.scxml_ros_field import RosField +from scxml_converter.scxml_entries import ScxmlBase, RosField from scxml_converter.scxml_entries.utils import all_non_empty_strings @@ -282,7 +282,54 @@ def get_automaton_name(self) -> str: """Get name of the automaton that these declarations are defined in.""" return self._automaton_name - def append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> None: + def append_ros_declaration(self, scxml_ros_declaration: ScxmlBase) -> None: + """ + Add a ROS declaration to the container instance. + + :param scxml_ros_declaration: The ROS declaration to add (inheriting from RosDeclaration). + """ + from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration + from scxml_converter.scxml_entries.scxml_ros_timer import RosTimeRate + from scxml_converter.scxml_entries.scxml_ros_topic import ( + RosTopicPublisher, RosTopicSubscriber) + from scxml_converter.scxml_entries.scxml_ros_service import ( + RosServiceServer, RosServiceClient) + from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer + from scxml_converter.scxml_entries.scxml_ros_action_client import RosActionClient + assert isinstance(scxml_ros_declaration, RosDeclaration), \ + f"Error: SCXML ROS declarations: {type(scxml_ros_declaration)} isn't a ROS declaration." + if isinstance(scxml_ros_declaration, RosTimeRate): + self._append_timer(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_rate()) + elif isinstance(scxml_ros_declaration, RosTopicPublisher): + self._append_publisher(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + elif isinstance(scxml_ros_declaration, RosTopicSubscriber): + self._append_subscriber(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + elif isinstance(scxml_ros_declaration, RosServiceServer): + self._append_service_server(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + elif isinstance(scxml_ros_declaration, RosServiceClient): + self._append_service_client(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + elif isinstance(scxml_ros_declaration, RosActionServer): + self._append_action_server(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + elif isinstance(scxml_ros_declaration, RosActionClient): + self._append_action_client(scxml_ros_declaration.get_name(), + scxml_ros_declaration.get_interface_name(), + scxml_ros_declaration.get_interface_type()) + else: + raise NotImplementedError(f"Error: SCXML ROS declaration type " + f"{type(scxml_ros_declaration)}.") + + def _append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> None: """ Add a publisher to the container. @@ -296,7 +343,7 @@ def append_publisher(self, pub_name: str, topic_name: str, topic_type: str) -> N f"Error: ROS declarations: topic publisher {pub_name} already declared." self._publishers[pub_name] = (topic_name, topic_type) - def append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> None: + def _append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> None: """ Add a subscriber to the container. @@ -310,7 +357,8 @@ def append_subscriber(self, sub_name: str, topic_name: str, topic_type: str) -> f"Error: ROS declarations: topic subscriber {sub_name} already declared." self._subscribers[sub_name] = (topic_name, topic_type) - def append_service_client(self, client_name: str, service_name: str, service_type: str) -> None: + def _append_service_client( + self, client_name: str, service_name: str, service_type: str) -> None: """ Add a service client to the container. @@ -324,7 +372,8 @@ def append_service_client(self, client_name: str, service_name: str, service_typ f"Error: ROS declarations: service client {client_name} already declared." self._service_clients[client_name] = (service_name, service_type) - def append_service_server(self, server_name: str, service_name: str, service_type: str) -> None: + def _append_service_server( + self, server_name: str, service_name: str, service_type: str) -> None: """ Add a service server to the container. @@ -338,21 +387,21 @@ def append_service_server(self, server_name: str, service_name: str, service_typ f"Error: ROS declarations: service server {server_name} already declared." self._service_servers[server_name] = (service_name, service_type) - def append_action_client(self, client_name: str, action_name: str, action_type: str) -> None: + def _append_action_client(self, client_name: str, action_name: str, action_type: str) -> None: assert all_non_empty_strings(client_name, action_name, action_type), \ "Error: ROS declarations: client name, action name and type must be strings." assert client_name not in self._action_clients, \ f"Error: ROS declarations: action client {client_name} already declared." self._action_clients[client_name] = (action_name, action_type) - def append_action_server(self, server_name: str, action_name: str, action_type: str) -> None: + def _append_action_server(self, server_name: str, action_name: str, action_type: str) -> None: assert all_non_empty_strings(server_name, action_name, action_type), \ "Error: ROS declarations: server name, action name and type must be strings." assert server_name not in self._action_servers, \ f"Error: ROS declarations: action server {server_name} already declared." self._action_servers[server_name] = (action_name, action_type) - def append_timer(self, timer_name: str, timer_rate: float) -> None: + def _append_timer(self, timer_name: str, timer_rate: float) -> None: assert isinstance(timer_name, str), "Error: ROS declarations: timer name must be a string." assert isinstance(timer_rate, float) and timer_rate > 0, \ "Error: ROS declarations: timer rate must be a positive number." diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index e2adae1f..0e8ff05e 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -23,8 +23,7 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - BtInputPortDeclaration, BtOutputPortDeclaration, RosServiceClient, RosServiceServer, - RosTimeRate, RosTopicPublisher, RosTopicSubscriber, ScxmlBase, ScxmlDataModel, + BtInputPortDeclaration, BtOutputPortDeclaration, ScxmlBase, ScxmlDataModel, ScxmlRosDeclarationsContainer, ScxmlState) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration @@ -195,27 +194,7 @@ def _generate_ros_declarations_helper(self) -> Optional[ScxmlRosDeclarationsCont if not (ros_declaration.check_validity() and ros_declaration.check_valid_instantiation()): return None - if isinstance(ros_declaration, RosTimeRate): - ros_decl_container.append_timer(ros_declaration.get_name(), - ros_declaration.get_rate()) - elif isinstance(ros_declaration, RosTopicSubscriber): - ros_decl_container.append_subscriber(ros_declaration.get_name(), - ros_declaration.get_interface_name(), - ros_declaration.get_interface_type()) - elif isinstance(ros_declaration, RosTopicPublisher): - ros_decl_container.append_publisher(ros_declaration.get_name(), - ros_declaration.get_interface_name(), - ros_declaration.get_interface_type()) - elif isinstance(ros_declaration, RosServiceServer): - ros_decl_container.append_service_server(ros_declaration.get_name(), - ros_declaration.get_interface_name(), - ros_declaration.get_interface_type()) - elif isinstance(ros_declaration, RosServiceClient): - ros_decl_container.append_service_client(ros_declaration.get_name(), - ros_declaration.get_interface_name(), - ros_declaration.get_interface_type()) - else: - raise ValueError("Error: SCXML root: invalid ROS declaration type.") + ros_decl_container.append_ros_declaration(ros_declaration) return ros_decl_container def check_validity(self) -> bool: From 2c590fa72aff3067ba8d01d12ae318095567b791 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Thu, 22 Aug 2024 17:41:56 +0200 Subject: [PATCH 37/49] Start integration of threads in scxml root Signed-off-by: Marco Lampacrescia --- .../scxml_converter/scxml_entries/__init__.py | 13 +++- .../scxml_entries/scxml_root.py | 74 +++++++++++-------- 2 files changed, 54 insertions(+), 33 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index 5771e753..e8e20ba3 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -16,11 +16,20 @@ execution_entry_from_xml, valid_execution_body, # noqa: F401 valid_execution_body_entry_types, instantiate_exec_body_bt_events) # noqa: F401 from .scxml_transition import ScxmlTransition # noqa: F401 +from .scxml_state import ScxmlState # noqa: F401 +from .scxml_ros_timer import (RosTimeRate, RosRateCallback) # noqa: F401 from .scxml_ros_topic import ( # noqa: F401 RosTopicPublisher, RosTopicSubscriber, RosTopicCallback, RosTopicPublish) # noqa: F401 from .scxml_ros_service import ( # noqa: F401 RosServiceServer, RosServiceClient, RosServiceHandleRequest, # noqa: F401 RosServiceHandleResponse, RosServiceSendRequest, RosServiceSendResponse) # noqa: F401 -from .scxml_ros_timer import (RosTimeRate, RosRateCallback) # noqa: F401 -from .scxml_state import ScxmlState # noqa: F401 +from .scxml_ros_action_client import ( # noqa: F401 + RosActionClient, RosActionSendGoal, RosActionHandleGoalResponse, # noqa: F401 + RosActionHandleFeedback, RosActionHandleResult) # noqa: F401 +from .scxml_ros_action_server import ( # noqa: F401 + RosActionServer, RosActionHandleGoalRequest, RosActionAcceptGoal, # noqa: F401 + RosActionRejectGoal, RosActionStartThread, RosActionSendFeedback, # noqa: F401 + RosActionSendResult) # noqa: F401 +from .scxml_ros_action_server_thread import ( # noqa: F401 + RosActionThread, RosActionHandleThreadStart, RosActionHandleThreadCancel) # noqa: F401 from .scxml_root import ScxmlRoot # noqa: F401 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 0e8ff05e..3e2a95d5 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -24,7 +24,7 @@ from scxml_converter.scxml_entries import ( BtInputPortDeclaration, BtOutputPortDeclaration, ScxmlBase, ScxmlDataModel, - ScxmlRosDeclarationsContainer, ScxmlState) + ScxmlRosDeclarationsContainer, ScxmlState, RosActionThread) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration @@ -62,6 +62,8 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": # BT Declarations bt_port_declarations: List[BtPortDeclarations] = get_children_as_scxml( xml_tree, get_args(BtPortDeclarations)) + # Additional threads + additional_threads = get_children_as_scxml(xml_tree, (RosActionThread,)) # States scxml_states: List[ScxmlState] = get_children_as_scxml(xml_tree, (ScxmlState,)) assert len(scxml_states) > 0, "Error: SCXML root: no state found in input xml." @@ -75,6 +77,9 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": # BT Declarations for bt_port_declaration in bt_port_declarations: scxml_root.add_bt_port_declaration(bt_port_declaration) + # Additional threads + for scxml_thread in additional_threads: + scxml_root.add_thread(scxml_thread) # States for scxml_state in scxml_states: is_initial = scxml_state.get_id() == scxml_init_state @@ -105,6 +110,7 @@ def __init__(self, name: str): self._data_model: Optional[ScxmlDataModel] = None self._ros_declarations: List[RosDeclaration] = [] self._bt_ports_handler = BtPortsHandler() + self._additional_threads: List[RosActionThread] = [] def get_name(self) -> str: """Get the name of the automaton represented by this SCXML model.""" @@ -152,8 +158,6 @@ def add_ros_declaration(self, ros_declaration: RosDeclaration): assert isinstance(ros_declaration, RosDeclaration), \ "Error: SCXML root: invalid ROS declaration type." assert ros_declaration.check_validity(), "Error: SCXML root: invalid ROS declaration." - if self._ros_declarations is None: - self._ros_declarations = [] self._ros_declarations.append(ros_declaration) def add_bt_port_declaration(self, bt_port_decl: BtPortDeclarations): @@ -168,6 +172,11 @@ def add_bt_port_declaration(self, bt_port_decl: BtPortDeclarations): raise ValueError( f"Error: SCXML root: invalid BT port declaration type {type(bt_port_decl)}.") + def add_action_thread(self, action_thread: RosActionThread): + assert isinstance(action_thread, RosActionThread), \ + f"Error: SCXML root: invalid action thread type {type(action_thread)}." + self._additional_threads.append(action_thread) + def set_bt_port_value(self, port_name: str, port_value: str): """Set the value of an input port.""" self._bt_ports_handler.set_port_value(port_name, port_value) @@ -189,32 +198,28 @@ def update_bt_ports_values(self): def _generate_ros_declarations_helper(self) -> Optional[ScxmlRosDeclarationsContainer]: """Generate a HelperRosDeclarations object from the existing ROS declarations.""" ros_decl_container = ScxmlRosDeclarationsContainer(self._name) - if self._ros_declarations is not None: - for ros_declaration in self._ros_declarations: - if not (ros_declaration.check_validity() and - ros_declaration.check_valid_instantiation()): - return None - ros_decl_container.append_ros_declaration(ros_declaration) + for ros_declaration in self._ros_declarations: + if not (ros_declaration.check_validity() and + ros_declaration.check_valid_instantiation()): + return None + ros_decl_container.append_ros_declaration(ros_declaration) return ros_decl_container def check_validity(self) -> bool: - valid_name = isinstance(self._name, str) and len(self._name) > 0 - valid_initial_state = self._initial_state is not None - valid_states = isinstance(self._states, list) and len(self._states) > 0 - if valid_states: - for state in self._states: - valid_states = isinstance(state, ScxmlState) and state.check_validity() - if not valid_states: - break + valid_name = is_non_empty_string(ScxmlRoot, "name", self._name) + valid_initial_state = is_non_empty_string(ScxmlRoot, "initial state", self._initial_state) valid_data_model = self._data_model is None or self._data_model.check_validity() - if not valid_name: - print("Error: SCXML root: name is not valid.") - if not valid_initial_state: - print("Error: SCXML root: no initial state set.") - if not valid_states: - print("Error: SCXML root: states are not valid.") + valid_states = all(isinstance(state, ScxmlState) and state.check_validity() + for state in self._states) + valid_threads = all(isinstance(scxml_thread, RosActionThread) and + scxml_thread.check_validity() for scxml_thread in + self._additional_threads) if not valid_data_model: print("Error: SCXML root: datamodel is not valid.") + if not valid_states: + print("Error: SCXML root: states are not valid.") + if not valid_threads: + print("Error: SCXML root: additional threads are not valid.") valid_ros = self._check_valid_ros_declarations() if not valid_ros: print("Error: SCXML root: ROS declarations are not valid.") @@ -228,16 +233,19 @@ def _check_valid_ros_declarations(self) -> bool: if ros_decl_container is None: return False # Check the ROS instantiations - for state in self._states: - if not state.check_valid_ros_instantiations(ros_decl_container): - return False + if not all(state.check_valid_ros_instantiations(ros_decl_container) + for state in self._states): + return False + if not all(scxml_thread.check_valid_ros_instantiations(ros_decl_container) + for scxml_thread in self._additional_threads): + return False return True def is_plain_scxml(self) -> bool: """Check whether there are ROS specific features or all entries are plain SCXML.""" assert self.check_validity(), "SCXML: found invalid root object." - # If this is a valid scxml object, checking the absence of declarations is enough - return self._ros_declarations is None or len(self._ros_declarations) == 0 + # If this is a valid scxml object, just check the absence of ROS and thread declarations + return len(self._ros_declarations) == 0 and len(self._additional_threads) == 0 def to_plain_scxml_and_declarations(self) -> Tuple[List["ScxmlRoot"], ScxmlRosDeclarationsContainer]: @@ -259,6 +267,9 @@ def to_plain_scxml_and_declarations(self) -> Tuple[List["ScxmlRoot"], assert ros_declarations is not None, "Error: SCXML root: invalid ROS declarations." main_scxml._states = [state.as_plain_scxml(ros_declarations) for state in self._states] converted_scxmls.append(main_scxml) + for scxml_thread in self._additional_threads: + # TODO: Append additional threads here + pass for plain_scxml in converted_scxmls: assert plain_scxml.check_validity(), \ f"The SCXML root object {plain_scxml.get_name()} is not valid: " \ @@ -282,9 +293,10 @@ def as_xml(self) -> ET.Element: data_model_xml = self._data_model.as_xml() assert data_model_xml is not None, "Error: SCXML root: invalid data model." xml_root.append(data_model_xml) - if self._ros_declarations is not None: - for ros_declaration in self._ros_declarations: - xml_root.append(ros_declaration.as_xml()) + for ros_declaration in self._ros_declarations: + xml_root.append(ros_declaration.as_xml()) + for scxml_thread in self._additional_threads: + xml_root.append(scxml_thread.as_xml()) for state in self._states: xml_root.append(state.as_xml()) ET.indent(xml_root, " ") From c66743ff97f335d86d2a219e160b90ba4103f069 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 10:18:59 +0200 Subject: [PATCH 38/49] Add thread cancel option Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_root.py | 8 +++- .../scxml_entries/scxml_ros_action_server.py | 45 ++++++++++++++----- .../scxml_ros_action_server_thread.py | 4 +- .../ros_fibonacci_action_example/server.scxml | 8 ++-- 4 files changed, 46 insertions(+), 19 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 3e2a95d5..2b748ed8 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -192,6 +192,8 @@ def update_bt_ports_values(self): self._data_model.update_bt_ports_values(self._bt_ports_handler) for ros_decl_scxml in self._ros_declarations: ros_decl_scxml.update_bt_ports_values(self._bt_ports_handler) + for scxml_thread in self._additional_threads: + scxml_thread.update_bt_ports_values(self._bt_ports_handler) for state in self._states: state.update_bt_ports_values(self._bt_ports_handler) @@ -268,9 +270,11 @@ def to_plain_scxml_and_declarations(self) -> Tuple[List["ScxmlRoot"], main_scxml._states = [state.as_plain_scxml(ros_declarations) for state in self._states] converted_scxmls.append(main_scxml) for scxml_thread in self._additional_threads: - # TODO: Append additional threads here - pass + converted_scxmls.extend(scxml_thread.as_plain_scxml(ros_declarations)) for plain_scxml in converted_scxmls: + assert isinstance(plain_scxml, ScxmlRoot), \ + "Error: SCXML root: conversion to plain SCXML resulted in invalid object " \ + f"(expected ScxmlRoot, obtained {type(plain_scxml)}." assert plain_scxml.check_validity(), \ f"The SCXML root object {plain_scxml.get_name()} is not valid: " \ "conversion to plain SCXML failed." diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 65f72ede..d9f23aee 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -29,9 +29,10 @@ from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_handle_event, generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, - generate_action_thread_execution_start_event, generate_action_feedback_event, - generate_action_result_event) -from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument + generate_action_thread_execution_start_event, generate_action_thread_execution_cancel_event, + generate_action_feedback_event, generate_action_result_event) +from scxml_converter.scxml_entries.xml_utils import ( + assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) class RosActionServer(RosDeclaration): @@ -148,16 +149,14 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosActionServer]: return RosActionServer - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosActionStartThread": + @classmethod + def from_xml_tree(cls: Type[RosTrigger], xml_tree: ET.Element) -> "RosActionStartThread": """Create a RosActionStartThread object from an XML tree.""" - assert_xml_tag_ok(RosActionStartThread, xml_tree) - action_name = get_xml_argument(RosActionStartThread, xml_tree, "name") - thread_id = get_xml_argument(RosActionStartThread, xml_tree, "thread_id") - fields: List[RosField] = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - return RosActionStartThread(action_name, thread_id, fields) + assert_xml_tag_ok(cls, xml_tree) + action_name = get_xml_argument(cls, xml_tree, "name") + thread_id = get_xml_argument(cls, xml_tree, "thread_id") + fields: List[RosField] = get_children_as_scxml(xml_tree, (RosField,)) + return cls(action_name, thread_id, fields) def __init__(self, action_name: Union[str, RosActionServer], thread_id: str, fields: List[RosField] = None) -> None: @@ -194,6 +193,28 @@ def as_xml(self) -> ET.Element: return xml_thread_start_req +class RosActionCancelThread(RosActionStartThread): + """ + Object representing the request, from an action server, to cancel a running thread instance. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_start_thread" + + def check_fields_validity(self, _) -> bool: + """Check if the goal_id and the request fields have been defined.""" + n_fields = len(self._fields) + if n_fields > 0: + print(f"Error: SCXML {self.__class__}: no fields expected, found {n_fields}.") + return False + return True + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_execution_cancel_event( + ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) + + class RosActionSendFeedback(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 881062c9..6aadf686 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -179,7 +179,7 @@ class RosActionHandleThreadStart(RosCallback): @staticmethod def get_tag_name() -> str: - return "ros_action_thread_start" + return "ros_action_handle_thread_start" @staticmethod def get_declaration_type() -> Type[RosActionServer]: @@ -225,7 +225,7 @@ class RosActionHandleThreadCancel(RosActionHandleThreadStart): @staticmethod def get_tag_name() -> str: - return "ros_action_thread_cancel" + return "ros_action_handle_thread_cancel" def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml index 8d4fdb1b..a9b8d167 100644 --- a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -26,9 +26,9 @@ - + - + @@ -42,7 +42,7 @@ - + @@ -125,8 +125,10 @@ + + From ffacbb2499e4f5fe83d3890fe0c0d517270aa409 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 11:28:23 +0200 Subject: [PATCH 39/49] Allow conditions in RosCallback Signed-off-by: Marco Lampacrescia --- .../scxml_converter/scxml_entries/__init__.py | 2 +- .../scxml_entries/ros_utils.py | 9 +--- .../scxml_entries/scxml_ros_action_server.py | 26 +--------- .../scxml_ros_action_server_thread.py | 19 +++---- .../scxml_entries/scxml_ros_base.py | 17 +++++-- .../scxml_entries/scxml_ros_service.py | 4 +- .../scxml_entries/scxml_ros_timer.py | 50 ++----------------- .../scxml_entries/scxml_ros_topic.py | 2 +- .../ros_fibonacci_action_example/server.scxml | 21 +++++--- .../test/test_systemtest_scxml_entries.py | 3 +- 10 files changed, 47 insertions(+), 106 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index e8e20ba3..38c4b723 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -31,5 +31,5 @@ RosActionRejectGoal, RosActionStartThread, RosActionSendFeedback, # noqa: F401 RosActionSendResult) # noqa: F401 from .scxml_ros_action_server_thread import ( # noqa: F401 - RosActionThread, RosActionHandleThreadStart, RosActionHandleThreadCancel) # noqa: F401 + RosActionThread, RosActionHandleThreadStart) # noqa: F401 from .scxml_root import ScxmlRoot # noqa: F401 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 1da13d6a..0c390ad6 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -224,14 +224,9 @@ def generate_action_goal_handle_rejected_event(action_name: str) -> str: return f"action_{sanitize_ros_interface_name(action_name)}_goal_rejected" -def generate_action_thread_execution_start_event(action_name: str, thread_id: str) -> str: +def generate_action_thread_execution_start_event(action_name: str) -> str: """Generate the name of the event that triggers the start of an action thread execution.""" - return f"action_{sanitize_ros_interface_name(action_name)}_thread_{thread_id}_start" - - -def generate_action_thread_execution_cancel_event(action_name: str, thread_id: str) -> str: - """Generate the name of the event that triggers the start of an action thread execution.""" - return f"action_{sanitize_ros_interface_name(action_name)}_thread_{thread_id}_cancel" + return f"action_{sanitize_ros_interface_name(action_name)}_thread_start" def generate_action_feedback_event(action_name: str) -> str: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index d9f23aee..143c0e48 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -29,8 +29,8 @@ from scxml_converter.scxml_entries.ros_utils import ( is_action_type_known, generate_action_goal_handle_event, generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, - generate_action_thread_execution_start_event, generate_action_thread_execution_cancel_event, - generate_action_feedback_event, generate_action_result_event) + generate_action_thread_execution_start_event, generate_action_feedback_event, + generate_action_result_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) @@ -193,28 +193,6 @@ def as_xml(self) -> ET.Element: return xml_thread_start_req -class RosActionCancelThread(RosActionStartThread): - """ - Object representing the request, from an action server, to cancel a running thread instance. - """ - - @staticmethod - def get_tag_name() -> str: - return "ros_action_start_thread" - - def check_fields_validity(self, _) -> bool: - """Check if the goal_id and the request fields have been defined.""" - n_fields = len(self._fields) - if n_fields > 0: - print(f"Error: SCXML {self.__class__}: no fields expected, found {n_fields}.") - return False - return True - - def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: - return generate_action_thread_execution_cancel_event( - ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) - - class RosActionSendFeedback(RosTrigger): """Object representing a ROS Action Goal (request, from the client side) in SCXML.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 6aadf686..cc49b6dc 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -25,11 +25,11 @@ from scxml_converter.scxml_entries import ( ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer -from scxml_converter.scxml_entries.scxml_ros_base import RosCallback +from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosTrigger from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - generate_action_thread_execution_start_event, generate_action_thread_execution_cancel_event) + generate_action_thread_execution_start_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -186,15 +186,17 @@ def get_declaration_type() -> Type[RosActionServer]: return RosActionServer def __init__(self, server_alias: Union[str, RosActionServer], target_state: str, - exec_body: Optional[ScxmlExecutionBody] = None) -> None: + condition: Optional[str] = None, exec_body: Optional[ScxmlExecutionBody] = None + ) -> None: """ Initialize a new RosActionHandleResult object. :param server_alias: Action Server used by this handler, or its name. :param target_state: Target state to transition to after the start trigger is received. + :param condition: Condition to be met for the callback to be executed. :param exec_body: Execution body to be executed upon thread start (before transition). """ - super().__init__(server_alias, target_state, exec_body) + super().__init__(server_alias, target_state, condition, exec_body) # The thread ID depends on the plain scxml instance, so it is set later self._thread_id: Optional[int] = None @@ -215,7 +217,7 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) -class RosActionHandleThreadCancel(RosActionHandleThreadStart): +class RosActionThreadFree(RosTrigger): """ SCXML object receiving a trigger from the action server to stop a thread. @@ -225,9 +227,4 @@ class RosActionHandleThreadCancel(RosActionHandleThreadStart): @staticmethod def get_tag_name() -> str: - return "ros_action_handle_thread_cancel" - - def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: - assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." - return generate_action_thread_execution_cancel_event( - ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) + return "ros_action_thread_free" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index dff8ac92..f1d8e199 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -152,16 +152,19 @@ def from_xml_tree(cls: Type['RosCallback'], xml_tree: ET.Element) -> 'RosCallbac assert_xml_tag_ok(cls, xml_tree) interface_name = get_xml_argument(cls, xml_tree, "name") target_state = get_xml_argument(cls, xml_tree, "target") + condition = get_xml_argument(cls, xml_tree, "cond", none_allowed=True) exec_body = execution_body_from_xml(xml_tree) - return cls(interface_name, target_state, exec_body) + return cls(interface_name, target_state, condition, exec_body) def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str, - exec_body: Optional[ScxmlExecutionBody] = None) -> None: + condition: Optional[str] = None, exec_body: Optional[ScxmlExecutionBody] = None + ) -> None: """ Constructor of ROS callback. :param interface_decl: ROS interface declaration to be used in the callback, or its name. :param target_state: Name of the state to transition to after the callback. + :param condition: Condition to be met for the callback to be executed. :param exec_body: Executable body of the callback. """ if exec_body is None: @@ -173,16 +176,19 @@ def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str assert is_non_empty_string(self.__class__, "name", interface_decl) self._interface_name = interface_decl self._target: str = target_state + self._condition: Optional[str] = condition self._body: ScxmlExecutionBody = exec_body assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." def check_validity(self) -> bool: valid_name = is_non_empty_string(self.__class__, "name", self._interface_name) valid_target = is_non_empty_string(self.__class__, "target", self._target) + valid_condition = self._condition is None or \ + is_non_empty_string(self.__class__, "cond", self._condition) valid_body = self._body is None or valid_execution_body(self._body) if not valid_body: print(f"Error: SCXML {self.__class__}: invalid entries in executable body.") - return valid_name and valid_target and valid_body + return valid_name and valid_target and valid_condition and valid_body def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ROS interface used in the callback exists.""" @@ -213,14 +219,17 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx f"Error: SCXML {self.__class__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) target = self._target + condition = self._condition body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], None, body) + return ScxmlTransition(target, [event_name], condition, body) def as_xml(self) -> ET.Element: """Convert the ROS callback to an XML element.""" assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." xml_callback = ET.Element(self.get_tag_name(), {"name": self._interface_name, "target": self._target}) + if self._condition is not None: + xml_callback.set("cond", self._condition) for body_elem in self._body: xml_callback.append(body_elem.as_xml()) return xml_callback diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py index 2c1d32e4..d5d291d9 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -129,7 +129,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleRequest": "Use 'name' instead.") target_name = get_xml_argument(RosServiceHandleRequest, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) - return RosServiceHandleRequest(srv_name, target_name, exec_body) + return RosServiceHandleRequest(srv_name, target_name, None, exec_body) def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_service_server_defined(self._interface_name) @@ -200,7 +200,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosServiceHandleResponse": "Use 'name' instead.") target_name = get_xml_argument(RosServiceHandleResponse, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) - return RosServiceHandleResponse(srv_name, target_name, exec_body) + return RosServiceHandleResponse(srv_name, target_name, None, exec_body) def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_service_client_defined(self._interface_name) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py index 8df284b8..1bce4e46 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py @@ -15,12 +15,10 @@ """Declaration of SCXML tags related to ROS Timers.""" -from typing import Optional, Type, Union +from typing import Type from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import ( - ScxmlExecutionBody, ScxmlRosDeclarationsContainer, ScxmlTransition, - as_plain_execution_body, execution_body_from_xml) +from scxml_converter.scxml_entries import ScxmlRosDeclarationsContainer from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback from scxml_converter.scxml_entries.bt_utils import BtPortsHandler @@ -97,50 +95,8 @@ def get_tag_name() -> str: def get_declaration_type() -> Type[RosTimeRate]: return RosTimeRate - @staticmethod - def from_xml_tree(xml_tree: ET.Element) -> "RosRateCallback": - """Create a RosRateCallback object from an XML tree.""" - assert_xml_tag_ok(RosRateCallback, xml_tree) - timer_name = get_xml_argument(RosRateCallback, xml_tree, "name") - target = get_xml_argument(RosRateCallback, xml_tree, "target") - condition = get_xml_argument(RosRateCallback, xml_tree, "cond", none_allowed=True) - exec_body = execution_body_from_xml(xml_tree) - return RosRateCallback(timer_name, target, condition, exec_body) - - def __init__(self, timer: Union[RosTimeRate, str], target: str, condition: Optional[str] = None, - body: Optional[ScxmlExecutionBody] = None): - """ - Generate a new rate timer and callback. - - Multiple rate callbacks can share the same timer name, but the rate must match. - - :param timer: The RosTimeRate instance triggering the callback, or its name - :param body: The body of the callback - """ - self._condition = condition - super().__init__(timer, target, body) - - def check_validity(self) -> bool: - valid_parent = super().check_validity() - valid_condition = self._condition is None or \ - is_non_empty_string(RosRateCallback, "cond", self._condition) - return valid_parent and valid_condition - def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_timer_defined(self._interface_name) - def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + def get_plain_scxml_event(self, _) -> str: return generate_rate_timer_event(self._interface_name) - - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - event_name = self.get_plain_scxml_event(ros_declarations) - target = self._target - cond = self._condition - body = as_plain_execution_body(self._body, ros_declarations) - return ScxmlTransition(target, [event_name], cond, body) - - def as_xml(self) -> ET.Element: - xml_rate_callback = super().as_xml() - if self._condition is not None: - xml_rate_callback.set("cond", self._condition) - return xml_rate_callback diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py index 090f0794..f5a6f10c 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -90,7 +90,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosTopicCallback": "Use 'name' instead.") target = get_xml_argument(RosTopicCallback, xml_tree, "target") exec_body = execution_body_from_xml(xml_tree) - return RosTopicCallback(sub_name, target, exec_body) + return RosTopicCallback(sub_name, target, None, exec_body) def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.is_subscriber_defined(self._interface_name) diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml index a9b8d167..0195e32f 100644 --- a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -15,6 +15,7 @@ + @@ -24,6 +25,9 @@ + + + @@ -42,7 +46,8 @@ - + + @@ -80,13 +85,13 @@ - - - - - - - + + + + + + + diff --git a/scxml_converter/test/test_systemtest_scxml_entries.py b/scxml_converter/test/test_systemtest_scxml_entries.py index c700b8db..88f03a3b 100644 --- a/scxml_converter/test/test_systemtest_scxml_entries.py +++ b/scxml_converter/test/test_systemtest_scxml_entries.py @@ -112,7 +112,8 @@ def test_battery_drainer_ros_from_code(): RosRateCallback(ros_timer, "use_battery", None, [ScxmlAssign("battery_percent", "battery_percent - 1")])) use_battery_state.add_transition( - RosTopicCallback(ros_topic_sub, "use_battery", [ScxmlAssign("battery_percent", "100")])) + RosTopicCallback(ros_topic_sub, "use_battery", None, + [ScxmlAssign("battery_percent", "100")])) battery_drainer_scxml.add_state(use_battery_state, initial=True) _test_scxml_from_code(battery_drainer_scxml, os.path.join( os.path.dirname(__file__), '_test_data', 'battery_drainer_w_bt', From bf936e99638e9edb0ec124b454ce668351265134 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 12:05:33 +0200 Subject: [PATCH 40/49] Adapt thread start interface to new sxcml action server example Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_server.py | 11 ++- .../scxml_ros_action_server_thread.py | 21 ++++-- .../scxml_entries/scxml_ros_base.py | 4 +- .../ros_fibonacci_action_example/server.scxml | 68 +++++-------------- 4 files changed, 46 insertions(+), 58 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 143c0e48..b6a1ad9b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -22,7 +22,8 @@ from typing import List, Union, Type from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import RosField, ScxmlRosDeclarationsContainer +from scxml_converter.scxml_entries import ( + RosField, ScxmlSend, ScxmlParam, ScxmlRosDeclarationsContainer) from scxml_converter.scxml_entries.scxml_ros_base import RosDeclaration, RosCallback, RosTrigger @@ -185,7 +186,13 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_thread_execution_start_event( - ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) + ros_declarations.get_action_server_info(self._interface_name)[0]) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: + plain_send = super().as_plain_scxml(ros_declarations) + # Append to the transition fields the thread ID + plain_send.append_param(ScxmlParam("thread_id", expr=self._thread_id)) + return plain_send def as_xml(self) -> ET.Element: xml_thread_start_req = super().as_xml() diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index cc49b6dc..0178bd0d 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -23,7 +23,8 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer) + ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer, + ScxmlTransition) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosTrigger @@ -193,13 +194,20 @@ def __init__(self, server_alias: Union[str, RosActionServer], target_state: str, :param server_alias: Action Server used by this handler, or its name. :param target_state: Target state to transition to after the start trigger is received. - :param condition: Condition to be met for the callback to be executed. + :param condition: Condition to be met for the callback to be executed. Expected None. :param exec_body: Execution body to be executed upon thread start (before transition). """ super().__init__(server_alias, target_state, condition, exec_body) # The thread ID depends on the plain scxml instance, so it is set later self._thread_id: Optional[int] = None + def check_validity(self) -> bool: + # A condition for the thread start will be autogenerated. Avoid having more than one + if self._condition is not None: + print("Error: SCXML RosActionHandleThreadStart: no condition expected.") + return False + return super().check_validity() + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the action server has been declared.""" return ros_declarations.is_action_server_defined(self._interface_name) @@ -212,9 +220,14 @@ def set_thread_id(self, thread_id: int) -> None: self._thread_id = thread_id def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: - assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." return generate_action_thread_execution_start_event( - ros_declarations.get_action_server_info(self._interface_name)[0], self._thread_id) + ros_declarations.get_action_server_info(self._interface_name)[0]) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." + # Append a condition checking the thread ID matches the request + self._condition = "_req.thread_id == " + str(self._thread_id) + return super().as_plain_scxml(ros_declarations) class RosActionThreadFree(RosTrigger): diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index f1d8e199..d53ac0f7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -214,7 +214,7 @@ def check_valid_ros_instantiations(self, f"body of {self._interface_name} has invalid ROS instantiations.") return valid_body - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlBase: + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: assert self.check_valid_ros_instantiations(ros_declarations), \ f"Error: SCXML {self.__class__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) @@ -324,7 +324,7 @@ def check_valid_ros_instantiations(self, return False return True - def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlBase: + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ f"Error: SCXML {self.__class__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml index 0195e32f..e0616cc1 100644 --- a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml +++ b/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml @@ -66,23 +66,18 @@ + - - - - - - - + + - - + - + @@ -96,61 +91,34 @@ - + + + + - - - - + - - - - + + + - - + + - + - + - - - - - - - - - - - - - - - - - - - - - - - - - - From 84046d01df7d8b7a6023bdcbee9bf4731b7031c0 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 14:31:06 +0200 Subject: [PATCH 41/49] Bugfix in scxml_if Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 5 ++ .../scxml_entries/scxml_executable_entries.py | 70 +++++++++---------- .../scxml_entries/scxml_ros_action_server.py | 25 ++++++- .../scxml_ros_action_server_thread.py | 62 ++++++++++++---- .../.gitignore | 0 .../client_1.scxml | 2 - .../client_2.scxml | 11 ++- .../server.scxml | 0 scxml_converter/test/test_systemtest_xml.py | 7 ++ 9 files changed, 123 insertions(+), 59 deletions(-) rename scxml_converter/test/_test_data/{ros_fibonacci_action_example => fibonacci_action_example}/.gitignore (100%) rename scxml_converter/test/_test_data/{ros_fibonacci_action_example => fibonacci_action_example}/client_1.scxml (89%) rename scxml_converter/test/_test_data/{ros_fibonacci_action_example => fibonacci_action_example}/client_2.scxml (72%) rename scxml_converter/test/_test_data/{ros_fibonacci_action_example => fibonacci_action_example}/server.scxml (100%) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 0c390ad6..4e185104 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -229,6 +229,11 @@ def generate_action_thread_execution_start_event(action_name: str) -> str: return f"action_{sanitize_ros_interface_name(action_name)}_thread_start" +def generate_action_thread_free_event(action_name: str) -> str: + """Generate the name of the event sent when an action thread becomes free.""" + return f"action_{sanitize_ros_interface_name(action_name)}_thread_free" + + def generate_action_feedback_event(action_name: str) -> str: """Generate the name of the event that sends a feedback from the action server.""" return f"action_{sanitize_ros_interface_name(action_name)}_feedback" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index fbb34e5b..4f09dd67 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -60,18 +60,6 @@ def update_exec_body_bt_ports_values( class ScxmlIf(ScxmlBase): """This class represents SCXML conditionals.""" - def __init__(self, - conditional_executions: List[ConditionalExecutionBody], - else_execution: Optional[ScxmlExecutionBody] = None): - """ - Class representing a conditional execution in SCXML. - - :param conditional_executions: List of (condition - exec. body) pairs. Min n. pairs is one. - :param else_execution: Execution to be done if no condition is met. - """ - self._conditional_executions = conditional_executions - self._else_execution = else_execution - @staticmethod def get_tag_name() -> str: return "if" @@ -79,28 +67,47 @@ def get_tag_name() -> str: @staticmethod def from_xml_tree(xml_tree: ET.Element) -> "ScxmlIf": """Create a ScxmlIf object from an XML tree.""" - assert xml_tree.tag == ScxmlIf.get_tag_name(), \ - f"Error: SCXML if: XML tag name is not {ScxmlIf.get_tag_name()}." + assert_xml_tag_ok(ScxmlIf, xml_tree) conditions: List[str] = [] exec_bodies: List[ScxmlExecutionBody] = [] conditions.append(xml_tree.attrib["cond"]) - current_body: Optional[ScxmlExecutionBody] = [] - assert current_body is not None, "Error: SCXML if: current body is not valid." + current_body: ScxmlExecutionBody = [] + else_tag_found = False for child in xml_tree: if child.tag == "elseif": + assert not else_tag_found, "Error: SCXML if: 'elseif' tag found after 'else' tag." conditions.append(child.attrib["cond"]) exec_bodies.append(current_body) current_body = [] elif child.tag == "else": + assert not else_tag_found, "Error: SCXML if: multiple 'else' tags found." + else_tag_found = True exec_bodies.append(current_body) current_body = [] else: current_body.append(execution_entry_from_xml(child)) + else_body: Optional[ScxmlExecutionBody] = None + if else_tag_found: + else_body = current_body + else: + exec_bodies.append(current_body) assert len(conditions) == len(exec_bodies), \ - "Error: SCXML if: number of conditions and bodies do not match." - if len(current_body) == 0: - current_body = None - return ScxmlIf(list(zip(conditions, exec_bodies)), current_body) + "Error: SCXML if: number of conditions and bodies do not match " \ + f"({len(conditions)} != {len(exec_bodies)}). Conditions: {conditions}." + return ScxmlIf(list(zip(conditions, exec_bodies)), else_body) + + def __init__(self, + conditional_executions: List[ConditionalExecutionBody], + else_execution: Optional[ScxmlExecutionBody] = None): + """ + Class representing a conditional execution in SCXML. + + :param conditional_executions: List of (condition - exec. body) pairs. Min n. pairs is one. + :param else_execution: Execution to be done if no condition is met. + """ + self._conditional_executions = conditional_executions + self._else_execution = else_execution + assert self.check_validity(), "Error: SCXML if: invalid if object." def get_conditional_executions(self) -> List[ConditionalExecutionBody]: """Get the conditional executions.""" @@ -122,25 +129,14 @@ def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): update_exec_body_bt_ports_values(self._else_execution, bt_ports_handler) def check_validity(self) -> bool: - valid_conditional_executions = len(self._conditional_executions) > 0 + valid_conditional_executions = len(self._conditional_executions) > 0 and \ + all(isinstance(condition, str) and len(body) > 0 and valid_execution_body(body) + for condition, body in self._conditional_executions) if not valid_conditional_executions: - print("Error: SCXML if: no conditional executions found.") - for condition_execution in self._conditional_executions: - valid_tuple = isinstance(condition_execution, tuple) and len(condition_execution) == 2 - if not valid_tuple: - print("Error: SCXML if: invalid conditional execution found.") - condition, execution = condition_execution - valid_condition = isinstance(condition, str) and len(condition) > 0 - valid_execution = valid_execution_body(execution) - if not valid_condition: - print("Error: SCXML if: invalid condition found.") - if not valid_execution: - print("Error: SCXML if: invalid execution body found.") - valid_conditional_executions = valid_tuple and valid_condition and valid_execution - if not valid_conditional_executions: - break + print("Error: SCXML if: Found invalid entries in conditional executions.") valid_else_execution = \ - self._else_execution is None or valid_execution_body(self._else_execution) + self._else_execution is None or \ + (len(self._else_execution) > 0 and valid_execution_body(self._else_execution)) if not valid_else_execution: print("Error: SCXML if: invalid else execution body found.") return valid_conditional_executions and valid_else_execution diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index b6a1ad9b..11acf3af 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -31,7 +31,7 @@ is_action_type_known, generate_action_goal_handle_event, generate_action_goal_handle_accepted_event, generate_action_goal_handle_rejected_event, generate_action_thread_execution_start_event, generate_action_feedback_event, - generate_action_result_event) + generate_action_result_event, generate_action_thread_free_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) @@ -190,7 +190,7 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: plain_send = super().as_plain_scxml(ros_declarations) - # Append to the transition fields the thread ID + # Append the thread ID to the param list plain_send.append_param(ScxmlParam("thread_id", expr=self._thread_id)) return plain_send @@ -250,3 +250,24 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_result_event( ros_declarations.get_action_server_info(self._interface_name)[0]) + + +class RosActionHandleThreadFree(RosCallback): + """ + Object representing the callback executed when an action thread report it is free. + """ + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_thread_free" + + @staticmethod + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_free_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 0178bd0d..64d15a39 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -23,14 +23,14 @@ from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import ( - ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlRosDeclarationsContainer, - ScxmlTransition) + ScxmlBase, ScxmlDataModel, ScxmlExecutionBody, ScxmlState, ScxmlTransition, ScxmlParam, + ScxmlRosDeclarationsContainer, RosField) from scxml_converter.scxml_entries.scxml_ros_action_server import RosActionServer from scxml_converter.scxml_entries.scxml_ros_base import RosCallback, RosTrigger from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - generate_action_thread_execution_start_event) + generate_action_thread_execution_start_event, generate_action_thread_free_event) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -53,12 +53,12 @@ def from_xml_tree(xml_tree: ET.Element) -> "RosActionThread": n_threads = get_xml_argument(RosActionThread, xml_tree, "n_threads") n_threads = int(n_threads) assert n_threads > 0, f"Error: SCXML Action Thread: invalid n. of threads ({n_threads})." - initial_state = get_xml_argument(RosActionThread, xml_tree, "initial_state") + initial_state = get_xml_argument(RosActionThread, xml_tree, "initial") datamodel = get_children_as_scxml(xml_tree, (ScxmlDataModel,)) # ros declarations and bt ports are expected to be defined in the parent tag (scxml_root) scxml_states: List[ScxmlState] = get_children_as_scxml(xml_tree, (ScxmlState,)) assert len(datamodel) <= 1, "Error: SCXML Action Thread: multiple datamodels." - assert scxml_states > 0, "Error: SCXML Action Thread: no states defined." + assert len(scxml_states) > 0, "Error: SCXML Action Thread: no states defined." # The non-plain SCXML Action thread has the same name as the action scxml_action_thread = RosActionThread(action_alias, n_threads) # Fill the thread with the states and datamodel @@ -88,7 +88,7 @@ def __init__(self, action_server: Union[str, RosActionServer], n_threads: int) - self._name = action_server self._n_threads: int = n_threads self._initial_state: Optional[str] = None - self._datamodel: Optional[ScxmlDataModel] = None + self._data_model: Optional[ScxmlDataModel] = None self._states: List[ScxmlState] = [] def add_state(self, state: ScxmlState, *, initial: bool = False): @@ -103,8 +103,8 @@ def set_data_model(self, data_model: ScxmlDataModel): self._data_model = data_model def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: - if self._datamodel is not None: - self._datamodel.update_bt_ports_values(bt_ports_handler) + if self._data_model is not None: + self._data_model.update_bt_ports_values(bt_ports_handler) for state in self._states: state.update_bt_ports_values(bt_ports_handler) @@ -112,7 +112,7 @@ def check_validity(self) -> bool: valid_name = is_non_empty_string(RosActionThread, "name", self._name) valid_n_threads = isinstance(self._n_threads, int) and self._n_threads > 0 valid_initial_state = self._initial_state is not None - valid_datamodel = self._datamodel is None or self._datamodel.check_validity() + valid_data_model = self._data_model is None or self._data_model.check_validity() valid_states = all(isinstance(state, ScxmlState) and state.check_validity() for state in self._states) if not valid_name: @@ -122,11 +122,11 @@ def check_validity(self) -> bool: f"{self._name} has invalid n_threads ({self._n_threads}).") if not valid_initial_state: print(f"Error: SCXML RosActionThread: {self._name} has no initial state.") - if not valid_datamodel: + if not valid_data_model: print(f"Error: SCXML RosActionThread: {self._name} nas an invalid datamodel.") if not valid_states: print(f"Error: SCXML RosActionThread: {self._name} has invalid states.") - return valid_n_threads and valid_initial_state and valid_datamodel and valid_states + return valid_n_threads and valid_initial_state and valid_data_model and valid_states def check_valid_ros_instantiations(self, ros_decls: ScxmlRosDeclarationsContainer) -> bool: assert isinstance(ros_decls, ScxmlRosDeclarationsContainer), \ @@ -152,7 +152,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Lis for thread_idx in range(self._n_threads): thread_name = f"{action_name}_thread_{thread_idx}" plain_thread_instance = ScxmlRoot(thread_name) - plain_thread_instance.set_data_model(self._datamodel) + plain_thread_instance.set_data_model(self._data_model) for state in self._states: initial_state = state.get_id() == self._initial_state state.set_thread_id(thread_idx) @@ -241,3 +241,41 @@ class RosActionThreadFree(RosTrigger): @staticmethod def get_tag_name() -> str: return "ros_action_thread_free" + + @staticmethod + def get_declaration_type() -> Type[RosActionServer]: + return RosActionServer + + def __init__(self, action_name: Union[str, RosActionServer], + fields: Optional[List[RosField]] = None) -> None: + super().__init__(action_name, fields) + self._thread_id: Optional[int] = None + + def check_validity(self) -> bool: + if len(self._fields) > 0: + print("Error: SCXML RosActionThreadFree: no fields expected.") + return False + return super().check_validity() + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_server_defined(self._interface_name) + + def check_fields_validity(self, _) -> bool: + return len(self._fields) == 0 + + def set_thread_id(self, thread_id: int) -> None: + """Set the thread ID for this handler.""" + # The thread ID is expected to be overwritten every time a new thread is generated. + assert isinstance(thread_id, int) and thread_id >= 0, \ + f"Error: SCXML {self.__class__}: invalid thread ID ({thread_id})." + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_thread_free_event( + ros_declarations.get_action_server_info(self._interface_name)[0]) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." + plain_trigger = super().as_plain_scxml(ros_declarations) + # Add the thread id to the (empty) param list + plain_trigger.append_param(ScxmlParam("thread_id", str(self._thread_id))) + return plain_trigger diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore b/scxml_converter/test/_test_data/fibonacci_action_example/.gitignore similarity index 100% rename from scxml_converter/test/_test_data/ros_fibonacci_action_example/.gitignore rename to scxml_converter/test/_test_data/fibonacci_action_example/.gitignore diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml similarity index 89% rename from scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml rename to scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml index 0fc88cd2..560058b7 100644 --- a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_1.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml @@ -35,8 +35,6 @@ - - diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml similarity index 72% rename from scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml rename to scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml index c4691bbd..8bde849a 100644 --- a/scxml_converter/test/_test_data/ros_fibonacci_action_example/client_2.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml @@ -14,28 +14,27 @@ - + - + - + - + - + - diff --git a/scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml similarity index 100% rename from scxml_converter/test/_test_data/ros_fibonacci_action_example/server.scxml rename to scxml_converter/test/_test_data/fibonacci_action_example/server.scxml diff --git a/scxml_converter/test/test_systemtest_xml.py b/scxml_converter/test/test_systemtest_xml.py index 6544547b..b59d9971 100644 --- a/scxml_converter/test/test_systemtest_xml.py +++ b/scxml_converter/test/test_systemtest_xml.py @@ -144,3 +144,10 @@ def test_ros_to_plain_scxml_bt_ports(): def test_ros_to_plain_scxml_add_int_srv(): ros_to_plain_scxml_test('add_int_srv_example', {}, {}, True) + + +def test_ros_to_plain_scxml_fibonacci_action(): + ros_to_plain_scxml_test( + 'fibonacci_action_example', {}, + {"server.scxml": ["server", "fibonacci_thread_0", "fibonacci_thread_1"]}, + True) From 11d40a36f8d587aeaa31cb929387b08fa91d161a Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 15:43:54 +0200 Subject: [PATCH 42/49] Handle array in scxml data Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_data.py | 72 +++++++++---------- .../scxml_entries/scxml_data_model.py | 23 +++--- .../scxml_entries/scxml_root.py | 2 +- .../scxml_ros_action_server_thread.py | 2 +- .../scxml_converter/scxml_entries/utils.py | 34 +++++++-- 5 files changed, 78 insertions(+), 55 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py index 9f96f02a..b10b797e 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py @@ -25,29 +25,13 @@ from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, read_value_from_xml_arg_or_child) -from scxml_converter.scxml_entries.utils import SCXML_DATA_STR_TO_TYPE, is_non_empty_string +from scxml_converter.scxml_entries.utils import ( + SCXML_DATA_STR_TO_TYPE, convert_string_to_type, is_non_empty_string) ValidExpr = Union[BtGetValueInputPort, str, int, float] -def get_valid_entry_data_type( - value: Optional[Union[str, int, float]], data_type: str) -> Optional[Any]: - """ - Convert a value to the provided data type. Raise if impossible. - """ - if value is None: - return None - assert data_type in SCXML_DATA_STR_TO_TYPE, \ - f"Error: SCXML conversion of data entry: Unknown data type {data_type}." - if isinstance(value, str): - assert len(value) > 0, "Error: SCXML conversion of data bounds: Empty string." - return SCXML_DATA_STR_TO_TYPE[data_type](value) - assert isinstance(value, SCXML_DATA_STR_TO_TYPE[data_type]), \ - f"Error: SCXML conversion of data entry: Expected {data_type}, but got {type(value)}." - return value - - def valid_bound(bound_value: Any) -> bool: """Check if a bound is invalid.""" if bound_value is None: @@ -81,11 +65,11 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlData": def __init__( self, id_: str, expr: ValidExpr, data_type: str, lower_bound: Optional[ValidExpr] = None, upper_bound: Optional[ValidExpr] = None): - self._id = id_ - self._expr = expr - self._data_type = data_type - self._lower_bound = lower_bound - self._upper_bound = upper_bound + self._id: str = id_ + self._expr: str = expr + self._data_type: str = data_type + self._lower_bound: str = lower_bound + self._upper_bound: str = upper_bound def get_name(self) -> str: return self._id @@ -96,23 +80,37 @@ def get_type(self) -> type: def get_expr(self) -> str: return self._expr + def check_valid_bounds(self) -> bool: + if all(bound is None for bound in [self._lower_bound, self._upper_bound]): + # Nothing to check + return True + python_type = SCXML_DATA_STR_TO_TYPE[self._data_type] + if python_type not in (float, int): + print(f"Error: SCXML data: '{self._id}' has bounds but has type {self._data_type}, " + "not a number.") + return False + lower_bound = None + upper_bound = None + if self._lower_bound is not None: + lower_bound = convert_string_to_type(self._lower_bound, self._data_type) + if self._upper_bound is not None: + upper_bound = convert_string_to_type(self._upper_bound, self._data_type) + if all(bound is not None for bound in [lower_bound, upper_bound]): + if lower_bound > upper_bound: + print(f"Error: SCXML data: 'lower_bound_incl' {lower_bound} is not smaller " + f"than 'upper_bound_incl' {upper_bound}.") + return False + return True + def check_validity(self) -> bool: valid_id = is_non_empty_string(ScxmlData, "id", self._id) + valid_type = is_non_empty_string(ScxmlData, "type", self._data_type) + if valid_type: + if self._data_type not in SCXML_DATA_STR_TO_TYPE: + print(f"Error: SCXML data: '{self._id}' has unknown type '{self._data_type}'.") + return False valid_expr = is_non_empty_string(ScxmlData, "expr", self._expr) - valid_type = is_non_empty_string(ScxmlData, "type", self._data_type) and \ - self._data_type in SCXML_DATA_STR_TO_TYPE - if not (valid_bound(self._lower_bound) and valid_bound(self._upper_bound)): - print("Error: SCXML data: invalid lower_bound_incl or upper_bound_incl. " - f"lower_bound_incl: {self._lower_bound}, upper_bound_incl: {self._upper_bound}") - return False - lower_bound = get_valid_entry_data_type(self._lower_bound, self._data_type) - upper_bound = get_valid_entry_data_type(self._upper_bound, self._data_type) - valid_bounds = True - if lower_bound is not None and upper_bound is not None: - valid_bounds = lower_bound <= upper_bound - if not valid_bounds: - print(f"Error: SCXML data: 'lower_bound_incl' {lower_bound} is not smaller " - f"than 'upper_bound_incl' {upper_bound}.") + valid_bounds = self.check_valid_bounds() return valid_id and valid_expr and valid_type and valid_bounds def as_xml(self) -> ET.Element: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py index 78c3ec1e..eb1e2913 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data_model.py @@ -55,19 +55,18 @@ def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler): data_entry.update_bt_ports_values(bt_ports_handler) def check_validity(self) -> bool: - valid_data_entries = True if self._data_entries is not None: - valid_data_entries = isinstance(self._data_entries, list) - if valid_data_entries: - for data_entry in self._data_entries: - valid_data_entry = isinstance(data_entry, ScxmlData) and \ - data_entry.check_validity() - if not valid_data_entry: - valid_data_entries = False - break - if not valid_data_entries: - print("Error: SCXML datamodel: data entries are not valid.") - return valid_data_entries + if not isinstance(self._data_entries, list): + print("Error: SCXML datamodel: data entries are not a list.") + return False + for data_entry in self._data_entries: + if not isinstance(data_entry, ScxmlData): + print(f"Error: SCXML datamodel: invalid data entry type {type(data_entry)}.") + return False + if not data_entry.check_validity(): + print(f"Error: SCXML datamodel: invalid data entry '{data_entry.get_name()}'.") + return False + return True def as_xml(self) -> Optional[ET.Element]: assert self.check_validity(), "SCXML: found invalid datamodel object." diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py index 2b748ed8..ae5066ff 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -79,7 +79,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": scxml_root.add_bt_port_declaration(bt_port_declaration) # Additional threads for scxml_thread in additional_threads: - scxml_root.add_thread(scxml_thread) + scxml_root.add_action_thread(scxml_thread) # States for scxml_state in scxml_states: is_initial = scxml_state.get_id() == scxml_init_state diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index 64d15a39..d38095bc 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -123,7 +123,7 @@ def check_validity(self) -> bool: if not valid_initial_state: print(f"Error: SCXML RosActionThread: {self._name} has no initial state.") if not valid_data_model: - print(f"Error: SCXML RosActionThread: {self._name} nas an invalid datamodel.") + print(f"Error: SCXML RosActionThread: {self._name} has an invalid datamodel.") if not valid_states: print(f"Error: SCXML RosActionThread: {self._name} has invalid states.") return valid_n_threads and valid_initial_state and valid_data_model and valid_states diff --git a/scxml_converter/src/scxml_converter/scxml_entries/utils.py b/scxml_converter/src/scxml_converter/scxml_entries/utils.py index 7e62e3bc..b1c554b8 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/utils.py @@ -15,7 +15,8 @@ """Collection of various utilities for scxml entries.""" -from typing import Dict, Type +from typing import Any, Dict, Type, MutableSequence +from array import array from scxml_converter.scxml_entries import ScxmlBase @@ -28,7 +29,8 @@ "int8": int, "int16": int, "int32": int, - "int64": int + "int64": int, + "int32[]": MutableSequence[int], # array.array('i): https://stackoverflow.com/a/67775675 } @@ -56,11 +58,35 @@ def is_non_empty_string(scxml_type: Type[ScxmlBase], arg_name: str, arg_value: s """ valid_str = isinstance(arg_value, str) and len(arg_value) > 0 if not valid_str: - print(f"Error: SCXML conversion of {scxml_type.get_tag_name()}: " - f"Expected non-empty argument {arg_name}.") + print(f"Error: SCXML entry from {scxml_type}: Expected non-empty argument {arg_name}.") return valid_str def get_default_expression_for_type(field_type: str) -> str: """Generate a default expression for a field type.""" + if field_type not in SCXML_DATA_STR_TO_TYPE: + raise ValueError(f"Error: SCXML conversion of data entry: Unknown data type {field_type}.") + if '[' in field_type: + # array type, special handling + if field_type.startswith('int'): + return array('i') + elif field_type.startswith('float'): + return array('f') + else: + raise ValueError( + f"Error: SCXML conversion of data entry: unhandled array type {field_type}.") return str(SCXML_DATA_STR_TO_TYPE[field_type]()) + + +def convert_string_to_type(value: str, data_type: str) -> Any: + """ + Convert a value to the provided data type. Raise if impossible. + """ + assert data_type in SCXML_DATA_STR_TO_TYPE, \ + f"Error: SCXML conversion of data entry: Unknown data type {data_type}." + assert isinstance(value, str), \ + f"Error: SCXML conversion of data entry: expected a string, got {type(value)}." + assert len(value) > 0, "Error: SCXML conversion of data entry: Empty string." + assert '[' not in data_type, \ + f"Error: SCXML conversion of data entry: Cannot convert array type {data_type}." + return SCXML_DATA_STR_TO_TYPE[data_type](value) From e2ef0fbdd5fd72799558b622e781f0f5ce56b401 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 15:50:52 +0200 Subject: [PATCH 43/49] Improve printed messages Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_executable_entries.py | 2 +- .../scxml_entries/scxml_ros_action_server.py | 6 +-- .../scxml_ros_action_server_thread.py | 10 +++-- .../scxml_entries/scxml_ros_base.py | 39 +++++++++++-------- .../scxml_converter/scxml_entries/utils.py | 3 +- 5 files changed, 34 insertions(+), 26 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index 4f09dd67..c3fb6d40 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -246,7 +246,7 @@ def check_valid_ros_instantiations(self, _) -> bool: def append_param(self, param: ScxmlParam) -> None: assert self.__class__ is ScxmlSend, \ - f"Error: SCXML send: cannot append param to derived class {self.__class__}." + f"Error: SCXML send: cannot append param to derived class {self.__class__.__name__}." assert isinstance(param, ScxmlParam), "Error: SCXML send: invalid param." self._params.append(param) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 11acf3af..785394b7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -179,7 +179,7 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) """Check if the goal_id and the request fields have been defined.""" if not ros_declarations.check_valid_action_goal_fields(self._interface_name, self._fields, has_goal_id=True): - print(f"Error: SCXML {self.__class__}: " + print(f"Error: SCXML {self.__class__.__name__}: " f"invalid fields in goal request {self._interface_name}.") return False return True @@ -218,7 +218,7 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) """Check if the goal_id and the request fields have been defined.""" if not ros_declarations.check_valid_action_feedback_fields(self._interface_name, self._fields, has_goal_id=True): - print(f"Error: SCXML {self.__class__}: " + print(f"Error: SCXML {self.__class__.__name__}: " f"invalid fields in feedback request {self._interface_name}.") def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: @@ -244,7 +244,7 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) """Check if the goal_id and the request fields have been defined.""" if not ros_declarations.check_valid_action_result_fields(self._interface_name, self._fields, has_goal_id=True): - print(f"Error: SCXML {self.__class__}: " + print(f"Error: SCXML {self.__class__.__name__}: " f"invalid fields in result request {self._interface_name}.") def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index d38095bc..e8ba0734 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -216,7 +216,7 @@ def set_thread_id(self, thread_id: int) -> None: """Set the thread ID for this handler.""" # The thread ID is expected to be overwritten every time a new thread is generated. assert isinstance(thread_id, int) and thread_id >= 0, \ - f"Error: SCXML {self.__class__}: invalid thread ID ({thread_id})." + f"Error: SCXML {self.__class__.__name__}: invalid thread ID ({thread_id})." self._thread_id = thread_id def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: @@ -224,7 +224,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_action_server_info(self._interface_name)[0]) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." + assert self._thread_id is not None, \ + f"Error: SCXML {self.__class__.__name__}: thread ID not set." # Append a condition checking the thread ID matches the request self._condition = "_req.thread_id == " + str(self._thread_id) return super().as_plain_scxml(ros_declarations) @@ -267,14 +268,15 @@ def set_thread_id(self, thread_id: int) -> None: """Set the thread ID for this handler.""" # The thread ID is expected to be overwritten every time a new thread is generated. assert isinstance(thread_id, int) and thread_id >= 0, \ - f"Error: SCXML {self.__class__}: invalid thread ID ({thread_id})." + f"Error: SCXML {self.__class__.__name__}: invalid thread ID ({thread_id})." def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_thread_free_event( ros_declarations.get_action_server_info(self._interface_name)[0]) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: - assert self._thread_id is not None, f"Error: SCXML {self.__class__}: thread ID not set." + assert self._thread_id is not None, \ + f"Error: SCXML {self.__class__.__name__}: thread ID not set." plain_trigger = super().as_plain_scxml(ros_declarations) # Add the thread id to the (empty) param list plain_trigger.append_param(ScxmlParam("thread_id", str(self._thread_id))) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py index d53ac0f7..aa0b51e8 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_base.py @@ -79,7 +79,7 @@ def __init__(self, interface_name: Union[str, BtGetValueInputPort], interface_ty f"invalid type of interface_name {type(interface_name)}." if self._interface_alias is None: assert is_non_empty_string(self.__class__, "interface_name", self._interface_name), \ - f"Error: SCXML {self.get_tag_name()}: " \ + f"Error: SCXML {self.__class__.__name__}: " \ "an alias name is required for dynamic ROS interfaces." self._interface_alias = interface_name @@ -118,10 +118,11 @@ def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: def as_plain_scxml(self, _) -> ScxmlBase: # This is discarded in the to_plain_scxml_and_declarations method from ScxmlRoot - raise RuntimeError(f"Error: SCXML {self.__class__} cannot be converted to plain SCXML.") + raise RuntimeError( + f"Error: SCXML {self.__class__.__name__} cannot be converted to plain SCXML.") def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + assert self.check_validity(), f"Error: SCXML {self.__class__.__name__}: invalid parameters." xml_declaration = ET.Element(self.get_tag_name(), {"name": self._interface_alias, self.get_xml_arg_interface_name(): self._interface_name, @@ -178,7 +179,8 @@ def __init__(self, interface_decl: Union[str, RosDeclaration], target_state: str self._target: str = target_state self._condition: Optional[str] = condition self._body: ScxmlExecutionBody = exec_body - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + assert self.check_validity(), \ + f"Error: SCXML {self.__class__.__name__}: invalid parameters." def check_validity(self) -> bool: valid_name = is_non_empty_string(self.__class__, "name", self._interface_name) @@ -187,7 +189,7 @@ def check_validity(self) -> bool: is_non_empty_string(self.__class__, "cond", self._condition) valid_body = self._body is None or valid_execution_body(self._body) if not valid_body: - print(f"Error: SCXML {self.__class__}: invalid entries in executable body.") + print(f"Error: SCXML {self.__class__.__name__}: invalid entries in executable body.") return valid_name and valid_target and valid_condition and valid_body def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: @@ -204,19 +206,20 @@ def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ROS entries in the callback are correctly defined.""" assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - f"Error: SCXML {self.__class__}: invalid type of ROS declarations container." + f"Error: SCXML {self.__class__.__name__}: invalid type of ROS declarations container." if not self.check_interface_defined(ros_declarations): - print(f"Error: SCXML {self.__class__}: undefined ROS interface {self._interface_name}.") + print(f"Error: SCXML {self.__class__.__name__}: " + f"undefined ROS interface {self._interface_name}.") return False valid_body = super().check_valid_ros_instantiations(ros_declarations) if not valid_body: - print(f"Error: SCXML {self.__class__}: " + print(f"Error: SCXML {self.__class__.__name__}: " f"body of {self._interface_name} has invalid ROS instantiations.") return valid_body def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: assert self.check_valid_ros_instantiations(ros_declarations), \ - f"Error: SCXML {self.__class__}: invalid ROS instantiations." + f"Error: SCXML {self.__class__.__name__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) target = self._target condition = self._condition @@ -225,7 +228,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx def as_xml(self) -> ET.Element: """Convert the ROS callback to an XML element.""" - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + assert self.check_validity(), f"Error: SCXML {self.__class__.__name__}: invalid parameters." xml_callback = ET.Element(self.get_tag_name(), {"name": self._interface_name, "target": self._target}) if self._condition is not None: @@ -277,7 +280,7 @@ def __init__(self, interface_decl: Union[str, RosDeclaration], assert is_non_empty_string(self.__class__, "name", interface_decl) self._interface_name = interface_decl self._fields: List[RosField] = fields - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + assert self.check_validity(), f"Error: SCXML {self.__class__.__name__}: invalid parameters." def append_field(self, field: RosField) -> None: assert isinstance(field, RosField), "Error: SCXML topic publish: invalid field." @@ -292,7 +295,7 @@ def check_validity(self) -> bool: valid_name = is_non_empty_string(self.__class__, "name", self._interface_name) valid_fields = all(isinstance(field, RosField) for field in self._fields) if not valid_fields: - print(f"Error: SCXML {self.__class__}: " + print(f"Error: SCXML {self.__class__.__name__}: " f"invalid entries in fields of {self._interface_name}.") return valid_name and valid_fields @@ -315,24 +318,26 @@ def check_valid_ros_instantiations(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ROS entries in the trigger are correctly defined.""" assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ - f"Error: SCXML {self.__class__}: invalid type of ROS declarations container." + f"Error: SCXML {self.__class__.__name__}: invalid type of ROS declarations container." if not self.check_interface_defined(ros_declarations): - print(f"Error: SCXML {self.__class__}: undefined ROS interface {self._interface_name}.") + print(f"Error: SCXML {self.__class__.__name__}: " + f"undefined ROS interface {self._interface_name}.") return False if not self.check_fields_validity(ros_declarations): - print(f"Error: SCXML {self.__class__}: invalid fields for {self._interface_name}.") + print(f"Error: SCXML {self.__class__.__name__}: " + f"invalid fields for {self._interface_name}.") return False return True def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: assert self.check_valid_ros_instantiations(ros_declarations), \ - f"Error: SCXML {self.__class__}: invalid ROS instantiations." + f"Error: SCXML {self.__class__.__name__}: invalid ROS instantiations." event_name = self.get_plain_scxml_event(ros_declarations) params = [field.as_plain_scxml(ros_declarations) for field in self._fields] return ScxmlSend(event_name, params) def as_xml(self) -> ET.Element: - assert self.check_validity(), f"Error: SCXML {self.__class__}: invalid parameters." + assert self.check_validity(), f"Error: SCXML {self.__class__.__name__}: invalid parameters." xml_trigger = ET.Element(self.get_tag_name(), {"name": self._interface_name}) for field in self._fields: xml_trigger.append(field.as_xml()) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/utils.py b/scxml_converter/src/scxml_converter/scxml_entries/utils.py index b1c554b8..3810a73a 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/utils.py @@ -58,7 +58,8 @@ def is_non_empty_string(scxml_type: Type[ScxmlBase], arg_name: str, arg_value: s """ valid_str = isinstance(arg_value, str) and len(arg_value) > 0 if not valid_str: - print(f"Error: SCXML entry from {scxml_type}: Expected non-empty argument {arg_name}.") + print(f"Error: SCXML entry from {scxml_type.__name__}: " + f"Expected non-empty argument {arg_name}.") return valid_str From c3a664b9beb95fe38cd5ad631a266e7914b9ed4a Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 16:19:27 +0200 Subject: [PATCH 44/49] Bugfixing Signed-off-by: Marco Lampacrescia --- .../scxml_converter/scxml_entries/ros_utils.py | 18 ++++++++++++------ .../scxml_entries/scxml_ros_action_server.py | 2 +- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 4e185104..b3ae7f79 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -53,7 +53,8 @@ def is_ros_type_known(type_definition: str, ros_interface: str) -> bool: interface_ns, interface_type = type_definition.split("/") if len(interface_ns) == 0 or len(interface_type) == 0: return False - assert ros_interface in ["msg", "srv"], "Error: SCXML ROS declarations: unknown ROS interface." + assert ros_interface in ["msg", "srv", "action"], \ + "Error: SCXML ROS declarations: unknown ROS interface." try: interface_importer = __import__(interface_ns + f'.{ros_interface}', fromlist=['']) _ = getattr(interface_importer, interface_type) @@ -493,23 +494,28 @@ def check_valid_srv_res_fields(self, server_name: str, ros_fields: List[RosField return True def check_valid_action_goal_fields( - self, client_name: str, ros_fields: List[RosField], has_goal_id: bool = False) -> bool: + self, alias_name: str, ros_fields: List[RosField], has_goal_id: bool = False) -> bool: """ Check if the provided fields match with the action type's goal entries. - :param client_name: Name of the action client. + :param alias_name: Name of the action client. :param ros_fields: List of fields to check. :param has_goal_id: Whether the goal_id shall be included among the fields. """ - _, action_type = self.get_action_client_info(client_name) - goal_fields, _, _ = get_action_type_params(action_type) + if self.is_action_client_defined(alias_name): + action_type = self.get_action_client_info(alias_name)[1] + else: + assert self.is_action_server_defined(alias_name), \ + f"Error: SCXML ROS declarations: unknown action {alias_name}." + action_type = self.get_action_server_info(alias_name)[1] + goal_fields = get_action_type_params(action_type)[0] # We use the goal ID as a reserved field for the action. Make sure it is available. assert "goal_id" not in goal_fields, \ f"Error: SCXML ROS declarations: action {action_type} goal has the 'goal_id' field." if has_goal_id: goal_fields["goal_id"] = "int32" if not check_all_fields_known(ros_fields, goal_fields): - print(f"Error: SCXML ROS declarations: Action goal {client_name} has invalid fields.") + print(f"Error: SCXML ROS declarations: Action goal {alias_name} has invalid fields.") return False return True diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 785394b7..0aec7a65 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -41,7 +41,7 @@ class RosActionServer(RosDeclaration): @staticmethod def get_tag_name() -> str: - return "ros_action_client" + return "ros_action_server" @staticmethod def get_communication_interface() -> str: From 4a1524c18675c2fc3e19d25b7b713269e3ba8cb3 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 16:36:10 +0200 Subject: [PATCH 45/49] Make scxml parser less verbose Signed-off-by: Marco Lampacrescia --- .../scxml_entries/ros_utils.py | 3 ++- .../scxml_entries/scxml_executable_entries.py | 1 - .../scxml_entries/xml_utils.py | 20 ++++++++++--------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index b3ae7f79..5d818bf2 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -28,7 +28,8 @@ BASIC_FIELD_TYPES = ['boolean', 'int8', 'int16', 'int32', 'int64', - 'float', 'double'] + 'float', 'double', + 'sequence'] """List of prefixes for ROS-related event data in SCXML.""" ROS_EVENT_PREFIXES = [ diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index c3fb6d40..11c76650 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -282,7 +282,6 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlAssign": def __init__(self, location: str, expr: Union[str, BtGetValueInputPort]): self._location = location self._expr = expr - print(f"ScxmlAssign: {location} = {expr}") def get_location(self) -> str: """Get the location to assign.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py index 27d8fc7e..2ed167fa 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/xml_utils.py @@ -56,34 +56,35 @@ def get_children_as_scxml( def read_value_from_xml_child( - xml_tree: Element, child_tag: str, valid_types: Iterable[Type[Union[ScxmlBase, str]]] - ) -> Optional[Union[str, ScxmlBase]]: + xml_tree: Element, child_tag: str, valid_types: Iterable[Type[Union[ScxmlBase, str]]], *, + none_allowed: bool = False) -> Optional[Union[str, ScxmlBase]]: """ Try to read the value of a child tag from the xml tree. If the child is not found, return None. """ xml_child = xml_tree.findall(child_tag) if xml_child is None or len(xml_child) == 0: - print(f"Warn: reading from {xml_tree.tag}: Cannot find child '{child_tag}'.") + if not none_allowed: + print(f"Error: reading from {xml_tree.tag}: Cannot find child '{child_tag}'.") return None if len(xml_child) > 1: - print(f"Warn: reading from {xml_tree.tag}: multiple children '{child_tag}', expected one.") + print(f"Error: reading from {xml_tree.tag}: multiple children '{child_tag}', expected one.") return None n_tag_children = len(xml_child[0]) if n_tag_children == 0 and str in valid_types: # Try to read the text value text_value = xml_child[0].text if text_value is None or len(text_value) == 0: - print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has no text value.") + print(f"Error: reading from {xml_tree.tag}: Child '{child_tag}' has no text value.") return None return text_value if n_tag_children > 1: - print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has multiple children.") + print(f"Error: reading from {xml_tree.tag}: Child '{child_tag}' has multiple children.") return None # Remove string from valid types, if present valid_types = tuple(t for t in valid_types if t != str) scxml_entry = get_children_as_scxml(xml_child[0], valid_types) if len(scxml_entry) == 0: - print(f"Warn: reading from {xml_tree.tag}: Child '{child_tag}' has no valid children.") + print(f"Error: reading from {xml_tree.tag}: Child '{child_tag}' has no valid children.") return None return scxml_entry[0] @@ -91,7 +92,7 @@ def read_value_from_xml_child( def read_value_from_xml_arg_or_child( scxml_type: Type[ScxmlBase], xml_tree: Element, tag_name: str, valid_types: Iterable[Type[Union[ScxmlBase, str]]], - none_allowed=False) -> Optional[Union[str, ScxmlBase]]: + none_allowed: bool = False) -> Optional[Union[str, ScxmlBase]]: """ Read a value from an xml attribute or, if not found, the child tag with the same name. @@ -102,7 +103,8 @@ def read_value_from_xml_arg_or_child( "If strings are not expected, use 'read_value_from_xml_child'." read_value = get_xml_argument(scxml_type, xml_tree, tag_name, none_allowed=True) if read_value is None: - read_value = read_value_from_xml_child(xml_tree, tag_name, valid_types) + read_value = read_value_from_xml_child(xml_tree, tag_name, valid_types, + none_allowed=none_allowed) if not none_allowed: assert read_value is not None, \ f"Error: SCXML conversion of {scxml_type.get_tag_name()}: Missing argument {tag_name}." From 61e7b259c7c2224c1e93e7ce26dbf36899776f34 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 16:50:31 +0200 Subject: [PATCH 46/49] More fixing Signed-off-by: Marco Lampacrescia --- .../src/scxml_converter/scxml_entries/ros_utils.py | 6 +++++- .../scxml_entries/scxml_ros_action_server.py | 4 ++++ .../test/_test_data/fibonacci_action_example/server.scxml | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 5d818bf2..f11462d3 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -85,12 +85,16 @@ def extract_params_from_ros_type(ros_interface_type: Type[Any]) -> Dict[str, str Extract the data fields of a ROS message type as pairs of name and type objects. """ fields = ros_interface_type.get_fields_and_field_types() + additional_fields = {} for key in fields.keys(): assert fields[key] in BASIC_FIELD_TYPES, \ f"Error: SCXML ROS declarations: {ros_interface_type} {key} field is " \ f"of type {fields[key]}, that is not supported." fields[key] = MSG_TYPE_SUBSTITUTIONS.get(fields[key], fields[key]) - return fields + # For array fields (or sequences), we append also a "__len" entry + if fields[key].startswith("sequence<"): + additional_fields[key + "__len"] = "int32" + return fields | additional_fields def check_all_fields_known(ros_fields: List[RosField], field_types: Dict[str, str]) -> bool: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index 0aec7a65..0b9f4b9f 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -220,6 +220,8 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) self._fields, has_goal_id=True): print(f"Error: SCXML {self.__class__.__name__}: " f"invalid fields in feedback request {self._interface_name}.") + return False + return True def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_feedback_event( @@ -246,6 +248,8 @@ def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) self._fields, has_goal_id=True): print(f"Error: SCXML {self.__class__.__name__}: " f"invalid fields in result request {self._interface_name}.") + return False + return True def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_result_event( diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml index e0616cc1..d148577a 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml @@ -51,12 +51,14 @@ + + From a3ea205230c5fa5479a9a5959fb1c38442f6b39d Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 17:09:01 +0200 Subject: [PATCH 47/49] First running action server + threads conversion Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_executable_entries.py | 11 +++++++++++ .../scxml_entries/scxml_ros_action_server_thread.py | 12 ++++++++---- .../src/scxml_converter/scxml_entries/scxml_state.py | 8 ++++---- .../scxml_entries/scxml_transition.py | 7 +++++++ 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py index 11c76650..33abac62 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_executable_entries.py @@ -157,6 +157,17 @@ def check_valid_ros_instantiations(self, return False return True + def set_thread_id(self, thread_id: int) -> None: + """Set the thread ID for the executable entries contained in the if object.""" + for _, exec_body in self._conditional_executions: + for entry in exec_body: + if hasattr(entry, "set_thread_id"): + entry.set_thread_id(thread_id) + if self._else_execution is not None: + for entry in self._else_execution: + if hasattr(entry, "set_thread_id"): + entry.set_thread_id(thread_id) + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "ScxmlIf": condional_executions = [] for condition, execution in self._conditional_executions: diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py index e8ba0734..cdbb01c2 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server_thread.py @@ -30,7 +30,8 @@ from scxml_converter.scxml_entries.bt_utils import BtPortsHandler from scxml_converter.scxml_entries.ros_utils import ( - generate_action_thread_execution_start_event, generate_action_thread_free_event) + generate_action_thread_execution_start_event, generate_action_thread_free_event, + sanitize_ros_interface_name) from scxml_converter.scxml_entries.xml_utils import ( assert_xml_tag_ok, get_xml_argument, get_children_as_scxml) from scxml_converter.scxml_entries.utils import is_non_empty_string @@ -148,7 +149,8 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Lis """ from scxml_converter.scxml_entries import ScxmlRoot thread_instances: List[ScxmlRoot] = [] - action_name = ros_declarations.get_action_server_info(self._name)[0] + action_name = sanitize_ros_interface_name( + ros_declarations.get_action_server_info(self._name)[0]) for thread_idx in range(self._n_threads): thread_name = f"{action_name}_thread_{thread_idx}" plain_thread_instance = ScxmlRoot(thread_name) @@ -162,7 +164,7 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Lis "Error: SCXML RosActionThread: " \ f"failed to generate a plain-SCXML instance from thread '{self._name}'" thread_instances.append(plain_thread_instance) - return [thread_instances] + return thread_instances def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid state object." @@ -218,6 +220,7 @@ def set_thread_id(self, thread_id: int) -> None: assert isinstance(thread_id, int) and thread_id >= 0, \ f"Error: SCXML {self.__class__.__name__}: invalid thread ID ({thread_id})." self._thread_id = thread_id + super().set_thread_id(thread_id) def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_thread_execution_start_event( @@ -269,6 +272,7 @@ def set_thread_id(self, thread_id: int) -> None: # The thread ID is expected to be overwritten every time a new thread is generated. assert isinstance(thread_id, int) and thread_id >= 0, \ f"Error: SCXML {self.__class__.__name__}: invalid thread ID ({thread_id})." + self._thread_id = thread_id def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: return generate_action_thread_free_event( @@ -279,5 +283,5 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> Scx f"Error: SCXML {self.__class__.__name__}: thread ID not set." plain_trigger = super().as_plain_scxml(ros_declarations) # Add the thread id to the (empty) param list - plain_trigger.append_param(ScxmlParam("thread_id", str(self._thread_id))) + plain_trigger.append_param(ScxmlParam("thread_id", expr=str(self._thread_id))) return plain_trigger diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index 61555aec..395f7ca9 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -100,10 +100,10 @@ def get_body(self) -> List[ScxmlTransition]: def set_thread_id(self, thread_idx: int): """Assign the thread ID to the thread-specific transitions in the body.""" - for transition in self._body: - # Assign the thread only to thread specific transitions - if hasattr(transition, 'set_thread_id'): - transition.set_thread_id(thread_idx) + for entry in self._on_entry + self._on_exit + self._body: + # Assign the thread only to the entries supporting it + if hasattr(entry, 'set_thread_id'): + entry.set_thread_id(thread_idx) def instantiate_bt_events(self, instance_id: str) -> None: """Instantiate the BT events in all entries belonging to a state.""" diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py index 1bd4bf02..9d62fed7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py @@ -141,6 +141,13 @@ def check_valid_ros_instantiations(self, return self._body is None or \ all(entry.check_valid_ros_instantiations(ros_declarations) for entry in self._body) + def set_thread_id(self, thread_id: int) -> None: + """Set the thread ID for the executable entries of this transition.""" + if self._body is not None: + for entry in self._body: + if hasattr(entry, "set_thread_id"): + entry.set_thread_id(thread_id) + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "ScxmlTransition": assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ "Error: SCXML transition: invalid ROS declarations container." From 597ff25141ae8efceaa2598ce0f4f12b4064d02f Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Fri, 23 Aug 2024 18:00:57 +0200 Subject: [PATCH 48/49] First complete translation of action server and client Signed-off-by: Marco Lampacrescia --- .../scxml_entries/scxml_ros_action_client.py | 2 +- .../scxml_entries/scxml_state.py | 25 ++++++--- .../fibonacci_action_example/client_2.scxml | 8 +-- .../gt_plain_scxml/client_1.scxml | 31 +++++++++++ .../gt_plain_scxml/client_2.scxml | 31 +++++++++++ .../gt_plain_scxml/fibonacci_thread_0.scxml | 47 +++++++++++++++++ .../gt_plain_scxml/fibonacci_thread_1.scxml | 47 +++++++++++++++++ .../gt_plain_scxml/server.scxml | 51 +++++++++++++++++++ 8 files changed, 230 insertions(+), 12 deletions(-) create mode 100644 scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml create mode 100644 scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml create mode 100644 scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml create mode 100644 scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml create mode 100644 scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 56e54744..449f22c4 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -63,7 +63,7 @@ def get_declaration_type() -> Type[RosActionClient]: return RosActionClient def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: - return ros_declarations.is_action_client_defined(self._client_name) + return ros_declarations.is_action_client_defined(self._interface_name) def check_fields_validity(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: return ros_declarations.check_valid_action_goal_fields(self._interface_name, self._fields) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index 395f7ca9..aa31eb19 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -162,11 +162,11 @@ def check_validity(self) -> bool: if not valid_id: print("Error: SCXML state: id is not valid.") if not valid_on_entry: - print("Error: SCXML state: on_entry is not valid.") + print(f"Error: SCXML state {self._id}: on_entry is not valid.") if not valid_on_exit: - print("Error: SCXML state: on_exit is not valid.") + print(f"Error: SCXML state {self._id}: on_exit is not valid.") if not valid_body: - print("Error: SCXML state: executable body is not valid.") + print(f"Error: SCXML state {self._id}: executable body is not valid.") return valid_on_entry and valid_on_exit and valid_body def check_valid_ros_instantiations(self, @@ -176,11 +176,11 @@ def check_valid_ros_instantiations(self, valid_exit = ScxmlState._check_valid_ros_instantiations(self._on_exit, ros_declarations) valid_body = ScxmlState._check_valid_ros_instantiations(self._body, ros_declarations) if not valid_entry: - print("Error: SCXML state: onentry has invalid ROS instantiations.") + print(f"Error: SCXML state {self._id}: onentry has invalid ROS instantiations.") if not valid_exit: - print("Error: SCXML state: onexit has invalid ROS instantiations.") + print(f"Error: SCXML state {self._id}: onexit has invalid ROS instantiations.") if not valid_body: - print("Error: SCXML state: found invalid transition in state body.") + print(f"Error: SCXML state {self._id}: found invalid transition in state body.") return valid_entry and valid_exit and valid_body @staticmethod @@ -195,7 +195,18 @@ def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "Sc """Convert the ROS-specific entries to be plain SCXML""" plain_entry = as_plain_execution_body(self._on_entry, ros_declarations) plain_exit = as_plain_execution_body(self._on_exit, ros_declarations) - plain_body = [entry.as_plain_scxml(ros_declarations) for entry in self._body] + plain_body: List[ScxmlTransition] = [] + for entry in self._body: + plain_entries = entry.as_plain_scxml(ros_declarations) + if isinstance(plain_entries, ScxmlTransition): + plain_body.append(plain_entries) + elif isinstance(plain_entries, list) and \ + all(isinstance(e, ScxmlTransition) for e in plain_entries): + # Some special entries return multiple transitions + plain_body.extend(plain_entries) + else: + raise ValueError(f"Error: SCXML state {self._id}: found invalid transition in " + "state body after conversion to plain SCXML.") return ScxmlState(self._id, on_entry=plain_entry, on_exit=plain_exit, body=plain_body) def as_xml(self) -> ET.Element: diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml index 8bde849a..caa2cfbf 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml @@ -12,8 +12,8 @@ - - + + @@ -28,13 +28,13 @@ - + - + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml new file mode 100644 index 00000000..7de2b775 --- /dev/null +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml new file mode 100644 index 00000000..db07c273 --- /dev/null +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml new file mode 100644 index 00000000..4771f6c4 --- /dev/null +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml new file mode 100644 index 00000000..a8cca946 --- /dev/null +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml new file mode 100644 index 00000000..a73f70b1 --- /dev/null +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 54538a123d9e1ea1e3e87621414c46c2840df2f5 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 26 Aug 2024 10:36:34 +0200 Subject: [PATCH 49/49] Assert on unknown state tags and improve action example Signed-off-by: Marco Lampacrescia --- .../graphics/ros_action_to_scxml.drawio.svg | 559 ++++++++++++++++++ .../scxml_entries/scxml_state.py | 26 +- .../gt_plain_scxml/server.scxml | 27 +- .../fibonacci_action_example/server.scxml | 35 +- 4 files changed, 602 insertions(+), 45 deletions(-) create mode 100644 docs/source/graphics/ros_action_to_scxml.drawio.svg diff --git a/docs/source/graphics/ros_action_to_scxml.drawio.svg b/docs/source/graphics/ros_action_to_scxml.drawio.svg new file mode 100644 index 00000000..93811e65 --- /dev/null +++ b/docs/source/graphics/ros_action_to_scxml.drawio.svg @@ -0,0 +1,559 @@ + + + + + + + +
+
+
+ Action Server-Client - SCXML +
+
+
+ + + Action Server-Client - SCXML + + + + + + + +
+
+
+ Action Communication Handler +
+
+
+ + + Action Communication Handler + + + + + + + + +
+
+
+ + transition srv_x_req_cllient_2 + +
+ + - send srv_x_req + +
+
+ - param req_field_<1, ..., n> +
+
+
+
+ + + transition srv_x_req_cllient_2... + + + + + + + + +
+
+
+ transition srv_x_req_cllient_1 +
+ - send srv_x_req +
+
+ - param req_field_<1, ..., n> +
+
+
+
+ + + transition srv_x_req_cllient_1... + + + + + + + +
+
+
+ + waiting + +
+
+
+ + + waiting + + + + + + + + +
+
+
+
+ transition srv_x_res +
+
+ - send srv_x_res_client_1 +
+
+ - param res_field_<1, ..., m> +
+
+
+
+ + + transition srv_x_res... + + + + + + + +
+
+
+ + processing_client_1 + +
+
+
+ + + processing_client_1 + + + + + + + + +
+
+
+
+ transition srv_x_res +
+
+ - send srv_x_res_client_2 +
+
+ - param res_field_<1, ..., m> +
+
+
+
+ + + transition srv_x_res... + + + + + + + +
+
+
+ + processing_client_2 + +
+
+
+ + + processing_client_2 + + + + + + + +
+
+
+ Server +
+
+
+ + + Server + + + + + + + + +
+
+
+
+ + transition action_handle_goal_request: + +
+
+ + - assign goal_id + +
+
+ + - assign goal_fields + +
+
+
+
+ + + transition action_handle_goal_request:... + + + + + + + +
+
+
+ + idle + +
+
+
+ + + idle + + + + + + + +
+
+
+ Client X +
+
+
+ + + Client X + + + + + + + +
+
+
+ + entry + +
+
+
+ + + entry + + + + + + + + +
+
+
+
+ + transition action_goal_accept_client_X + +
+
+
+
+ + + transition action_goal_accept_client_X + + + + + + + + +
+
+
+ + transition action_goal_reject_client_X + +
+
+
+ + + transition action_goal_reject_client_X + + + + + + + +
+
+
+ + send_request + +
+
+
+ + + send_request + + + + + + + + +
+
+
+ + transition: +
+ - send action_goal_req_client_X +
+ - param goal_field_<1, ..., n> + +
+
+
+ + + transition:... + + + + + + + + +
+
+
+ + transition action_result_client_X: + +
+ + - param result_field_<1, .., m> + +
+
+
+ + + transition action_result_client_X:... + + + + + + + +
+
+
+ + wait_result + +
+
+
+ + + wait_result + + + + + + + + +
+
+
+ + transition action_feedback_client_X: +
+ - param feedback_field_<1, .., k> + +
+
+
+ + + transition action_feedback_client_X:... + + + + + + + +
+
+
+ + done +
+ +
+
+
+ + + done + + + + + + + + +
+
+
+ transition action_thread_free: +
+ - assign thread_x_free +
+
+
+ + + transition action_thread_free:... + + + + + + + + +
+
+
+ transition: +
+ - if thread_available: +
+ - send action_goal_accepted +
+ - param goal_id +
+ - send action_thread_start +
+ - param thread_id +
+ - param goal_id +
+ - param goal_fields +
+ - else +
+ - send action_goal_rejected +
+ - param goal_id +
+
+
+ + + transition:... + + + + + + + +
+
+
+ + handle_goal + +
+
+
+ + + handle_goal + + + + + + + + + Text is not SVG - cannot display + + + + \ No newline at end of file diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py index aa31eb19..a6c07116 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -43,19 +43,11 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlState": assert id_ is not None and len(id_) > 0, "Error: SCXML state: id is not valid." scxml_state = ScxmlState(id_) # Get the onentry and onexit execution bodies - on_entry_xml = xml_tree.findall("onentry") - if on_entry_xml is None: - on_entry = [] - else: - on_entry = on_entry_xml - assert len(on_entry) == 0 or len(on_entry) == 1, \ + on_entry = xml_tree.findall("onentry") + assert len(on_entry) <= 1, \ f"Error: SCXML state: {len(on_entry)} onentry tags found, expected 0 or 1." - on_exit_xml = xml_tree.findall("onexit") - if on_exit_xml is None: - on_exit = [] - else: - on_exit = on_exit_xml - assert len(on_exit) == 0 or len(on_exit) == 1, \ + on_exit = xml_tree.findall("onexit") + assert len(on_exit) <= 1, \ f"Error: SCXML state: {len(on_exit)} onexit tags found, expected 0 or 1." if len(on_entry) > 0: for exec_entry in execution_body_from_xml(on_entry[0]): @@ -64,7 +56,7 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlState": for exec_entry in execution_body_from_xml(on_exit[0]): scxml_state.append_on_exit(exec_entry) # Get the transitions in the state body - for body_entry in ScxmlState._transitions_from_xml(xml_tree): + for body_entry in ScxmlState._transitions_from_xml(id_, xml_tree): scxml_state.add_transition(body_entry) return scxml_state @@ -122,18 +114,22 @@ def update_bt_ports_values(self, bt_ports_handler: BtPortsHandler) -> None: entry.update_bt_ports_values(bt_ports_handler) @staticmethod - def _transitions_from_xml(xml_tree: ET.Element) -> List[ScxmlTransition]: + def _transitions_from_xml(state_id: str, xml_tree: ET.Element) -> List[ScxmlTransition]: from scxml_converter.scxml_entries.scxml_ros_base import RosCallback transitions: List[ScxmlTransition] = [] tag_to_cls = {cls.get_tag_name(): cls for cls in ScxmlTransition.__subclasses__() if cls != RosCallback} - # TODO: Temporary workaroud, to fix once all ROS callbacks are implemented tag_to_cls.update({cls.get_tag_name(): cls for cls in RosCallback.__subclasses__()}) tag_to_cls.update({ScxmlTransition.get_tag_name(): ScxmlTransition}) for child in xml_tree: if child.tag in tag_to_cls: transitions.append(tag_to_cls[child.tag].from_xml_tree(child)) + expected_transitions = \ + len(xml_tree) - len(xml_tree.findall("onentry")) - len(xml_tree.findall("onexit")) + assert len(transitions) == expected_transitions, \ + f"Error: SCXML state {state_id}: Expected {expected_transitions} transitions, " \ + f"found {len(transitions)}." return transitions def add_transition(self, transition: ScxmlTransition): diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml index a73f70b1..2019c327 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/server.scxml @@ -32,20 +32,21 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml index d148577a..5e6d6420 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/server.scxml @@ -46,8 +46,9 @@ + - + @@ -103,23 +104,23 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + +