From 8c6666a9e6e2e1c90b619076909b1ad498bb7fca Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia <65171491+MarcoLm993@users.noreply.github.com> Date: Tue, 6 Aug 2024 15:41:07 +0200 Subject: [PATCH] Introduce action support (#17) Signed-off-by: Marco Lampacrescia Signed-off-by: Christian Henkel --- .github/workflows/test.yml | 2 +- .../graphics/ros_service_to_scxml.drawio.svg | 469 +++++++++++++++++ docs/source/scxml-jani-conversion.rst | 26 +- .../jani_entries/jani_automaton.py | 5 +- .../jani_convince_expression_expansion.py | 98 ++-- .../jani_generator/jani_entries/jani_edge.py | 5 + .../jani_entries/jani_expression.py | 13 +- .../jani_generator/jani_entries/jani_model.py | 37 +- .../jani_generator/jani_entries/jani_value.py | 3 +- .../ros_helpers/ros_services.py | 160 ++++++ .../scxml_helpers/scxml_data.py | 2 +- .../scxml_helpers/scxml_expression.py | 11 +- .../scxml_helpers/scxml_tags.py | 4 + .../scxml_helpers/scxml_to_jani.py | 28 +- .../scxml_helpers/top_level_interpreter.py | 140 +++-- .../ros_add_int_srv_example/.gitignore | 3 + .../ros_add_int_srv_example/client_1.scxml | 32 ++ .../ros_add_int_srv_example/client_2.scxml | 32 ++ .../happy_clients.jani | 33 ++ .../ros_add_int_srv_example/main.xml | 15 + .../ros_add_int_srv_example/server.scxml | 25 + .../_test_data/ros_example_w_bt/.gitignore | 7 +- .../test/test_systemtest_scxml_to_jani.py | 50 +- .../src/scxml_converter/bt_converter.py | 2 +- .../src/scxml_converter/scxml_converter.py | 16 +- .../scxml_converter/scxml_entries/__init__.py | 36 +- .../scxml_entries/scxml_data.py | 21 +- .../scxml_entries/scxml_executable_entries.py | 35 +- .../scxml_entries/scxml_param.py | 1 + .../scxml_entries/scxml_root.py | 26 +- .../scxml_entries/scxml_ros_entries.py | 460 +---------------- .../scxml_entries/scxml_ros_field.py | 59 +++ .../scxml_entries/scxml_ros_service.py | 486 ++++++++++++++++++ .../scxml_entries/scxml_ros_timer.py | 165 ++++++ .../scxml_entries/scxml_ros_topic.py | 291 +++++++++++ .../scxml_entries/scxml_state.py | 47 +- .../scxml_entries/scxml_transition.py | 31 +- .../scxml_converter/scxml_entries/utils.py | 213 +++++++- .../test/test_systemtest_scxml_entries.py | 10 +- scxml_converter/test/test_systemtest_xml.py | 9 +- 40 files changed, 2370 insertions(+), 738 deletions(-) create mode 100644 docs/source/graphics/ros_service_to_scxml.drawio.svg create mode 100644 jani_generator/src/jani_generator/ros_helpers/ros_services.py create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/.gitignore create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/client_1.scxml create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/client_2.scxml create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/happy_clients.jani create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/main.xml create mode 100644 jani_generator/test/_test_data/ros_add_int_srv_example/server.scxml create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py create mode 100644 scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index c3ad70d6..57973028 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -27,7 +27,7 @@ jobs: uses: actions/checkout@v2 with: repository: boschresearch/bt_tools - ref: feature/fsm_conversion + ref: main path: colcon_ws/src/bt_tools # Compile bt_tools TODO: remove after the release of bt_tools - name: Compile bt_tools diff --git a/docs/source/graphics/ros_service_to_scxml.drawio.svg b/docs/source/graphics/ros_service_to_scxml.drawio.svg new file mode 100644 index 00000000..f9da8852 --- /dev/null +++ b/docs/source/graphics/ros_service_to_scxml.drawio.svg @@ -0,0 +1,469 @@ + + + + + + + +
+
+
+ Service Server-Client - SCXML +
+
+
+ + + Service Server-Client - SCXML + + + + + + + +
+
+
+ Extra Service Handler +
+
+
+ + + Extra Service 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 + + + + + + + +
+
+
+ + idle + +
+
+
+ + + idle + + + + + + + + +
+
+
+ transition srv_x_req: +
+
+ - send srv_x_res +
+
+ - param res_field_<1, ..., m> +
+
+
+
+ + + transition s... + + + + + + + +
+
+
+ Client 2 +
+
+
+ + + Client 2 + + + + + + + +
+
+
+ + entry + +
+
+
+ + + entry + + + + + + + + +
+
+
+
+ transition srv_x_res_client2 +
+
+ - <use _event.res_field_<1, ... ,m>> +
+
+
+
+ + + transition srv_x_res_client2... + + + + + + + +
+
+
+ + wait_res + +
+
+
+ + + wait_res + + + + + + + + +
+
+
+ transition: +
+ - send srv_x_req_client_2 +
+ - param req_field_<1, ..., n> +
+
+
+ + + transition:... + + + + + + + +
+
+
+ Client 1 +
+
+
+ + + Client 1 + + + + + + + +
+
+
+ + entry + +
+
+
+ + + entry + + + + + + + + +
+
+
+
+ transition srv_x_res_client1 +
+
+ - <use _event.res_field_<1, ... ,m>> +
+
+
+
+ + + transition srv_x_res_client1... + + + + + + + +
+
+
+ + wait_res + +
+
+
+ + + wait_res + + + + + + + + +
+
+
+ transition: +
+ - send srv_x_req_client_1 +
+ - param req_field_<1, ..., n> +
+
+
+ + + transition:... + + + + + + + + + Text is not SVG - cannot display + + + + \ No newline at end of file diff --git a/docs/source/scxml-jani-conversion.rst b/docs/source/scxml-jani-conversion.rst index ebbda272..3d8346be 100644 --- a/docs/source/scxml-jani-conversion.rst +++ b/docs/source/scxml-jani-conversion.rst @@ -81,11 +81,6 @@ TODO :alt: How execution blocks and conditions are translated :align: center -Handling of (ROS) Timers -__________________________ - -TODO - Handling events ________________ .. _handling_events: @@ -116,3 +111,24 @@ The Jani model resulting from applying the conversion strategies we just describ :align: center It can be noticed how new self loop edges are added in the `A_B_receiver` automaton (the dashed ones) and how the `ev_a_on_send` is now duplicated in the Composition table, one advancing the `A sender` automaton and the other advancing the `A_B sender` automaton. + + +Handling of (ROS) Timers +__________________________ + +TODO + +Handling of (ROS) Services +_____________________________ + +TODO + +ROS services, as well as ROS topics, can be handled directly in the ROS to Plain SCXML conversion, without the need of adding Jani-specific features, as for the ROS Timers. + +The main structure of the generated Jani models can be seen in the diagram below: + +.. image:: graphics/ros_service_to_scxml.drawio.svg + :alt: Handling of ROS Services + :align: center + +The automata of clients and service are converted directly from the existing ROS-SCXML files, while the "Extra Service Handler" one, is autogenerated starting from the provided clients and services. 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 b6a68222..b722ebee 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_automaton.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_automaton.py @@ -80,6 +80,10 @@ def remove_edges_with_action_name(self, action_name: str): assert isinstance(action_name, str), "Action name must be a string" self._edges = [edge for edge in self._edges if edge.get_action() != action_name] + 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[str], initial_locations: List[str]): for location in location_list: self._locations.add(location["name"]) @@ -99,7 +103,6 @@ def _generate_variables(self, variable_list: List[dict]): variable["name"], var_type, init_expr, is_transient)}) def _generate_edges(self, edge_list: List[dict]): - # TODO: Proposal -> Edges might require support variables? In case we want to provide standard ones... for edge in edge_list: jani_edge = JaniEdge(edge) self.add_edge(jani_edge) 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 6c17eac5..edaa26c3 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 @@ -42,6 +42,7 @@ "≤": "≤", "<=": "≤", "=": "=", + "==": "=", "≠": "≠", "!=": "≠", "!": "¬", @@ -62,15 +63,25 @@ # Custom operators (CONVINCE, specific to mobile 2D robot use case) def intersection_operator(left, right) -> JaniExpression: - return JaniExpression({"op": "intersect", "robot": JaniExpression(left), "barrier": JaniExpression(right)}) + return JaniExpression({ + "op": "intersect", + "robot": JaniExpression(left), + "barrier": JaniExpression(right)}) def distance_operator(left, right) -> JaniExpression: - return JaniExpression({"op": "distance", "robot": JaniExpression(left), "barrier": JaniExpression(right)}) + return JaniExpression({ + "op": "distance", + "robot": JaniExpression(left), + "barrier": JaniExpression(right)}) def distance_to_point_operator(robot, target_x, target_y) -> JaniExpression: - return JaniExpression({"op": "distance_to_point", "robot": JaniExpression(robot), "x": JaniExpression(target_x), "y": JaniExpression(target_y)}) + return JaniExpression({ + "op": "distance_to_point", + "robot": JaniExpression(robot), + "x": JaniExpression(target_x), + "y": JaniExpression(target_y)}) def norm2d_operator(x=None, y=None, *, exp=None) -> JaniExpression: @@ -102,7 +113,8 @@ def cross2d_operator(x1=None, y1=None, x2=None, y2=None, *, exp=None) -> JaniExp exp_y1 = y1 exp_x2 = x2 exp_y2 = y2 - assert exp_x1 is not None and exp_y1 is not None and exp_x2 is not None and exp_y2 is not None, "The 2D vectors components must be provided" + assert all(exp is not None for exp in [exp_x1, exp_y1, exp_x2, exp_y2]), \ + "The 2D vectors components must be provided" return minus_operator(multiply_operator(exp_x1, exp_y2), multiply_operator(exp_y1, exp_x2)) @@ -119,7 +131,8 @@ def dot2d_operator(x1=None, y1=None, x2=None, y2=None, *, exp=None) -> JaniExpre exp_y1 = y1 exp_x2 = x2 exp_y2 = y2 - assert exp_x1 is not None and exp_y1 is not None and exp_x2 is not None and exp_y2 is not None, "The 2D vectors components must be provided" + assert all(exp is not None for exp in [exp_x1, exp_y1, exp_x2, exp_y2]), \ + "The 2D vectors components must be provided" return plus_operator(multiply_operator(exp_x1, exp_x2), multiply_operator(exp_y1, exp_y2)) @@ -176,7 +189,9 @@ def to_rad_operator(value=None, *, exp=None) -> JaniExpression: # Functionalities for interpolation -def __expression_interpolation_single_boundary(jani_constants: Dict[str, JaniConstant], robot_name: str, boundary_id: int) -> JaniExpression: +def __expression_interpolation_single_boundary( + jani_constants: Dict[str, JaniConstant], + robot_name: str, boundary_id: int) -> JaniExpression: n_vertices = jani_constants["boundaries.count"].value() # Variables names robot_radius = f"robots.{robot_name}.shape.radius" @@ -203,7 +218,8 @@ def __expression_interpolation_single_boundary(jani_constants: Dict[str, JaniCon # Boundary length boundary_norm_exp = norm2d_operator(ab_x, ab_y) # Distance from the robot to the boundary perpendicular to the boundary segment - v_dist_exp = divide_operator(abs_operator(cross2d_operator(ab_x, ab_y, ea_x, ea_y)), boundary_norm_exp) + v_dist_exp = divide_operator( + abs_operator(cross2d_operator(ab_x, ab_y, ea_x, ea_y)), boundary_norm_exp) # Distance between the boundary extreme points and the robot parallel to the boundary segment ha_dist_exp = divide_operator(dot2d_operator(ab_x, ab_y, ea_x, ea_y), boundary_norm_exp) hb_dist_exp = divide_operator(dot2d_operator(ba_x, ba_y, eb_x, eb_y), boundary_norm_exp) @@ -212,29 +228,38 @@ def __expression_interpolation_single_boundary(jani_constants: Dict[str, JaniCon is_parallel_exp = equal_operator(cross2d_operator(ab_x, ab_y, es_x, es_y), 0.0) # Interpolation factors ha_interp_exp = if_operator( - and_operator(greater_equal_operator(ha_dist_exp, 0.0), lower_operator(ha_dist_exp, robot_radius)), - divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), dot2d_operator(ab_x, ab_y, ea_x, ea_y)), + and_operator(greater_equal_operator(ha_dist_exp, 0.0), + lower_operator(ha_dist_exp, robot_radius)), + divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), + dot2d_operator(ab_x, ab_y, ea_x, ea_y)), dot2d_operator(ba_x, ba_y, es_x, es_y)), 1.0) hb_interp_exp = if_operator( - and_operator(greater_equal_operator(hb_dist_exp, 0.0), lower_operator(hb_dist_exp, robot_radius)), - divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), dot2d_operator(ba_x, ba_y, eb_x, eb_y)), + and_operator(greater_equal_operator(hb_dist_exp, 0.0), + lower_operator(hb_dist_exp, robot_radius)), + divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), + dot2d_operator(ba_x, ba_y, eb_x, eb_y)), dot2d_operator(ab_x, ab_y, es_x, es_y)), 1.0) - h_interp_exp = if_operator(is_perpendicular_exp, 1.0, min_operator(ha_interp_exp, hb_interp_exp)) + h_interp_exp = if_operator(is_perpendicular_exp, + 1.0, min_operator(ha_interp_exp, hb_interp_exp)) v_interp_exp = if_operator( or_operator(is_parallel_exp, greater_equal_operator(v_dist_exp, robot_radius)), 1.0, - divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), abs_operator(cross2d_operator(ab_x, ab_y, ea_x, ea_y))), + divide_operator(minus_operator(multiply_operator(boundary_norm_exp, robot_radius), + abs_operator(cross2d_operator(ab_x, ab_y, ea_x, ea_y))), abs_operator(cross2d_operator(ab_x, ab_y, es_x, es_y)))) return if_operator( - greater_equal_operator(max_operator(v_dist_exp, max_operator(ha_dist_exp, hb_dist_exp)), robot_radius), + greater_equal_operator(max_operator(v_dist_exp, max_operator(ha_dist_exp, hb_dist_exp)), + robot_radius), 0.0, min_operator(h_interp_exp, v_interp_exp)) -def __expression_interpolation_next_boundaries(jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: +def __expression_interpolation_next_boundaries( + jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: n_vertices = jani_constants["boundaries.count"].value() - assert isinstance(n_vertices, int) and n_vertices > 1, f"The number of boundaries ({n_vertices}) must greater than 1" + assert isinstance(n_vertices, int) and n_vertices > 1, \ + f"The number of boundaries ({n_vertices}) must greater than 1" if boundary_id >= n_vertices: return JaniExpression(0.0) return max_operator( @@ -242,12 +267,14 @@ def __expression_interpolation_next_boundaries(jani_constants: Dict[str, JaniCon __expression_interpolation_next_boundaries(jani_constants, robot_name, boundary_id + 1)) -def __expression_interpolation_next_obstacles(jani_constants, robot_name, obstacle_id) -> JaniExpression: +def __expression_interpolation_next_obstacles( + jani_constants, robot_name, obstacle_id) -> JaniExpression: # TODO return JaniExpression(0.0) -def __expression_interpolation(jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: +def __expression_interpolation( + jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: assert isinstance(jani_expression, JaniExpression), "The input must be a JaniExpression" assert jani_expression.op == "intersect" robot_name = jani_expression.operands["robot"].identifier @@ -264,7 +291,8 @@ def __expression_interpolation(jani_expression: JaniExpression, jani_constants: # Functionalities for validity check -def __expression_distance_single_boundary(jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: +def __expression_distance_single_boundary( + jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: n_vertices = jani_constants["boundaries.count"].value() # Variables names robot_radius = f"robots.{robot_name}.shape.radius" @@ -287,7 +315,8 @@ def __expression_distance_single_boundary(jani_constants: Dict[str, JaniConstant # Boundary length boundary_norm_exp = norm2d_operator(ab_x, ab_y) # Distance from the robot to the boundary perpendicular to the boundary segment - v_dist_exp = divide_operator(abs_operator(cross2d_operator(ab_x, ab_y, ra_x, ra_y)), boundary_norm_exp) + v_dist_exp = divide_operator(abs_operator(cross2d_operator(ab_x, ab_y, ra_x, ra_y)), + boundary_norm_exp) # Distance between the boundary extreme points and the robot parallel to the boundary segment ha_dist_exp = divide_operator(dot2d_operator(ab_x, ab_y, ra_x, ra_y), boundary_norm_exp) hb_dist_exp = divide_operator(dot2d_operator(ba_x, ba_y, rb_x, rb_y), boundary_norm_exp) @@ -295,9 +324,11 @@ def __expression_distance_single_boundary(jani_constants: Dict[str, JaniConstant return minus_operator(norm2d_operator(h_dist_exp, v_dist_exp), robot_radius) -def __expression_distance_next_boundaries(jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: +def __expression_distance_next_boundaries( + jani_constants: Dict[str, JaniConstant], robot_name, boundary_id) -> JaniExpression: n_vertices = jani_constants["boundaries.count"].value() - assert isinstance(n_vertices, int) and n_vertices > 1, f"The number of boundaries ({n_vertices}) must greater than 1" + assert isinstance(n_vertices, int) and n_vertices > 1, \ + f"The number of boundaries ({n_vertices}) must greater than 1" if boundary_id >= n_vertices: return JaniExpression(True) return min_operator( @@ -310,7 +341,8 @@ def __expression_distance_next_obstacles(jani_constants, robot_name, obstacle_id return JaniExpression(True) -def __expression_distance(jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: +def __expression_distance( + jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: assert isinstance(jani_expression, JaniExpression), "The input must be a JaniExpression" assert jani_expression.op == "distance" robot_name = jani_expression.operands["robot"].identifier @@ -326,7 +358,8 @@ def __expression_distance(jani_expression: JaniExpression, jani_constants: Dict[ raise NotImplementedError("The barrier type is not implemented") -def __expression_distance_to_point(jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: +def __expression_distance_to_point( + jani_expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: assert isinstance(jani_expression, JaniExpression), "The input must be a JaniExpression" assert jani_expression.op == "distance_to_point" robot_name = jani_expression.operands["robot"].identifier @@ -334,20 +367,25 @@ def __expression_distance_to_point(jani_expression: JaniExpression, jani_constan target_y_cm = to_cm_operator(expand_expression(jani_expression.operands["y"], jani_constants)) robot_x_cm = f"robots.{robot_name}.pose.x_cm" robot_y_cm = f"robots.{robot_name}.pose.y_cm" - return to_m_operator(norm2d_operator(minus_operator(robot_x_cm, target_x_cm), minus_operator(robot_y_cm, target_y_cm))) + return to_m_operator(norm2d_operator(minus_operator(robot_x_cm, target_x_cm), + minus_operator(robot_y_cm, target_y_cm))) def __substitute_expression_op(expression: JaniExpression) -> JaniExpression: assert isinstance(expression, JaniExpression), "The input must be a JaniExpression" - assert expression.op in BASIC_EXPRESSIONS_MAPPING, f"The operator {expression.op} is not supported" + assert expression.op in BASIC_EXPRESSIONS_MAPPING, \ + f"The operator {expression.op} is not supported" expression.op = BASIC_EXPRESSIONS_MAPPING[expression.op] return expression -def expand_expression(expression: JaniExpression, jani_constants: Dict[str, JaniConstant]) -> JaniExpression: +def expand_expression( + expression: JaniExpression, 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." - assert expression.is_valid(), "The expression is not valid: it defines no value, nor variable, nor operation to be done." + assert isinstance(expression, JaniExpression), \ + f"The expression should be a JaniExpression instance, found {type(expression)} instead." + assert expression.is_valid(), \ + "The expression is not valid: it defines no value, nor variable, nor operation to be done." if expression.op is None: # It is either a variable/constant identifier or a value return expression @@ -357,7 +395,7 @@ def expand_expression(expression: JaniExpression, jani_constants: Dict[str, Jani return __expression_distance(expression, jani_constants) if expression.op == "distance_to_point": return __expression_distance_to_point(expression, jani_constants) - # If the expressions is neither of the above, we expand the operands and then we return the expanded expression + # If the expressions is neither of the above, we expand the operands and return them for key, value in expression.operands.items(): expression.operands[key] = expand_expression(value, jani_constants) if expression.op == "norm2d": 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 e9195996..ce024857 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_edge.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_edge.py @@ -52,6 +52,11 @@ def get_action(self) -> Optional[str]: """Get the action name, if set.""" return self.action + def is_empty_self_loop(self) -> bool: + """Check if the edge is an empty self loop (i.e. has no assignments).""" + return len(self.destinations) == 1 and self.location == self.destinations[0]["location"] \ + and len(self.destinations[0]["assignments"]) == 0 + def set_action(self, action_name: str): """Set the action name.""" self.action = action_name 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 9d674888..90d61ceb 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_expression.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_expression.py @@ -24,6 +24,17 @@ class JaniExpression: + """ + Jani Expression class. + + Content of an instance of this class can be: + - identifier: a string representing a reference to a constant or variable (literal) + or + - value: a JaniValue object + or + - op: a string representing an operator + - operands: a dictionary of operands, related to the specified operator + """ def __init__(self, expression: Union[SupportedExp, 'JaniExpression', JaniValue]): self.identifier: str = None self.value: JaniValue = None @@ -74,7 +85,7 @@ def _get_operands(self, expression_dict: dict): "&&", "||", "and", "or", "∨", "∧", "⇒", "=>", "=", "≠", "!=", "+", "-", "*", "%", "pow", "log", "/", "min", "max", - "<", "≤", ">", "≥", "<=", ">=")): + "<", "≤", ">", "≥", "<=", ">=", "==")): return { "left": JaniExpression(expression_dict["left"]), "right": JaniExpression(expression_dict["right"])} 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 aefeea10..ce00f99b 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_model.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_model.py @@ -19,15 +19,19 @@ from typing import List, Dict, Optional, Union, Type -from jani_generator.jani_entries import JaniValue, JaniVariable, JaniConstant, JaniAutomaton, JaniComposition, \ - JaniProperty, JaniExpression +from jani_generator.jani_entries import ( + JaniValue, JaniVariable, JaniConstant, JaniAutomaton, JaniComposition, + JaniProperty, JaniExpression) ValidValue = Union[int, float, bool, dict, JaniExpression] class JaniModel: - """Class representing a complete Jani Model, containing all necessary information to generate a plain Jani file.""" + """ + Class representing a complete Jani Model, containing all necessary information to generate + a plain Jani file. + """ def __init__(self): self._name = "" self._type = "mdp" @@ -48,13 +52,17 @@ def add_jani_variable(self, variable: JaniVariable): self._variables.update({variable.name(): variable}) def add_variable(self, variable_name: str, variable_type: Type, - variable_init_expression: Optional[ValidValue] = None, transient: bool = False): + variable_init_expression: Optional[ValidValue] = None, + transient: bool = False): if variable_init_expression is None or isinstance(variable_init_expression, JaniExpression): - self.add_jani_variable(JaniVariable(variable_name, variable_type, variable_init_expression, transient)) + self.add_jani_variable( + JaniVariable(variable_name, variable_type, variable_init_expression, transient)) else: - assert JaniValue(variable_init_expression).is_valid(), f"Invalid value for variable {variable_name}" + assert JaniValue(variable_init_expression).is_valid(), \ + f"Invalid value for variable {variable_name}" self.add_jani_variable( - JaniVariable(variable_name, variable_type, JaniExpression(variable_init_expression), transient)) + JaniVariable(variable_name, variable_type, + JaniExpression(variable_init_expression), transient)) def add_jani_constant(self, constant: JaniConstant): self._constants.update({constant.name(): constant}) @@ -63,8 +71,10 @@ def add_constant(self, constant_name: str, constant_type: Type, constant_value: if isinstance(constant_value, JaniExpression): self.add_jani_constant(JaniConstant(constant_name, constant_type, constant_value)) else: - assert JaniValue(constant_value).is_valid(), f"Invalid value for constant {constant_name}" - self.add_jani_constant(JaniConstant(constant_name, constant_type, JaniExpression(constant_value))) + assert JaniValue(constant_value).is_valid(), \ + f"Invalid value for constant {constant_name}" + self.add_jani_constant( + JaniConstant(constant_name, constant_type, JaniExpression(constant_value))) def add_jani_automaton(self, automaton: JaniAutomaton): self._automata.append(automaton) @@ -108,7 +118,8 @@ def add_jani_property(self, property: JaniProperty): def as_dict(self): assert self._system is not None, "The system composition is not set" model_dict = {} - # The available actions need to be stored explicitly in jani: extract from existing automaton + # The available actions need to be stored explicitly in jani: + # we extract them from all the automaton in the model available_actions = set() for automaton in self._automata: available_actions.update(automaton.get_actions()) @@ -122,8 +133,10 @@ def as_dict(self): "variables": [jani_variable.as_dict() for jani_variable in self._variables.values()], "constants": [jani_constant.as_dict() for jani_constant in self._constants.values()], "actions": [{"name": action} for action in sorted(list(available_actions))], - "automata": [jani_automaton.as_dict(self._constants) for jani_automaton in self._automata], + "automata": [jani_automaton.as_dict(self._constants) for + jani_automaton in self._automata], "system": self._system.as_dict(), - "properties": [jani_property.as_dict(self._constants) for jani_property in self._properties] + "properties": [jani_property.as_dict(self._constants) for + jani_property in self._properties] }) return model_dict diff --git a/jani_generator/src/jani_generator/jani_entries/jani_value.py b/jani_generator/src/jani_generator/jani_entries/jani_value.py index c4390310..434f38df 100644 --- a/jani_generator/src/jani_generator/jani_entries/jani_value.py +++ b/jani_generator/src/jani_generator/jani_entries/jani_value.py @@ -29,7 +29,8 @@ def __init__(self, value): def is_valid(self) -> bool: if isinstance(self._value, dict): if "constant" in self._value: - assert self._value["constant"] in ("e", "π"), f"Unknown constant value {self._value['constant']}. Only 'e' and 'π' are supported" + assert self._value["constant"] in ("e", "π"), \ + f"Unknown constant value {self._value['constant']}. Only 'e' and 'π' supported." return True return False return isinstance(self._value, (int, float, bool)) diff --git a/jani_generator/src/jani_generator/ros_helpers/ros_services.py b/jani_generator/src/jani_generator/ros_helpers/ros_services.py new file mode 100644 index 00000000..9f1869e4 --- /dev/null +++ b/jani_generator/src/jani_generator/ros_helpers/ros_services.py @@ -0,0 +1,160 @@ +# 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. + +""" +Representation of ROS Services. +""" + +from typing import Dict, List, Optional +from scxml_converter.scxml_entries import ( + ScxmlRoot, ScxmlData, ScxmlDataModel, ScxmlState, + ScxmlParam, ScxmlAssign, ScxmlTransition, ScxmlSend) +from scxml_converter.scxml_entries.utils import ( + get_srv_type_params, sanitize_ros_interface_name, generate_srv_request_event, + generate_srv_response_event, generate_srv_server_request_event, + generate_srv_server_response_event, get_default_expression_for_type) + +from jani_generator.jani_entries import JaniModel + + +SRV_PREFIX = "srv_handler_" + + +class RosService: + """Object that contains a description of a ROS service with its server and clients.""" + + def __init__(self): + self._service_name: Optional[str] = None + self._service_type: Optional[str] = None + self._service_server_automaton: Optional[str] = None + self._service_client_automata: List[str] = [] + + def _set_name_and_type(self, service_name: str, service_type: str) -> None: + if self._service_name is None: + self._service_name = service_name + self._service_type = service_type + else: + assert self._service_name == service_name, \ + f"Service name {service_name} does not match {self._service_name}." + assert self._service_type == service_type, \ + f"Service type {service_type} does not match {self._service_type}." + + def _assert_validity(self): + """ + Make sure service_name and service_type are set and a server and at least one client exist. + """ + assert self._service_name is not None, "Service name not set." + assert self._service_type is not None, "Service type not set." + assert self._service_server_automaton is not None, \ + f"Service server not set for {self._service_name}." + assert len(self._service_client_automata) > 0, \ + f"No service clients set for {self._service_name}." + + def set_service_server(self, service_name: str, service_type: str, automaton_name: str) -> None: + """ + Set the server of the service. + There must be exactly one. + + :service_name: The name of the ROS service. + :service_type: The type of the ROS service (e.g. std_srvs/SetBool). + :automaton_name: The name of the JANI automaton that implements this server. + """ + self._set_name_and_type(service_name, service_type) + assert self._service_server_automaton is None, \ + f"Found more than one server for service {service_name}." + self._service_server_automaton = automaton_name + + def append_service_client(self, + service_name: str, service_type: str, automaton_name: str) -> None: + """ + Set the client of the service. + There must be one or more. + + :service_name: The name of the ROS service. + :service_type: The type of the ROS service (e.g. std_srvs/SetBool). + :automaton_name: The name of the JANI automaton that implements this client. + """ + self._set_name_and_type(service_name, service_type) + assert automaton_name not in self._service_client_automata, \ + f"Service client for {automaton_name} already declared for service {service_name}." + self._service_client_automata.append(automaton_name) + + def to_scxml(self) -> ScxmlRoot: + """ + Generate the srv_handler automaton that implements the link between the server of this + service and its clients. + This ensures that only one request can be processed at the time and that the client receives + only the response related to it's request. + + :return: Scxml object representing the necessary file content. + """ + self._assert_validity() + req_params, res_params = get_srv_type_params(self._service_type) + # Hack: Using support variables in the data model to avoid having _event in send params + req_fields_as_data = [] + for field_name, field_type in req_params.items() | res_params.items(): + default_expr = get_default_expression_for_type(field_type) + req_fields_as_data.append(ScxmlData(field_name, default_expr, field_type)) + # Make sure the service name has no slashes and spaces + scxml_root_name = SRV_PREFIX + sanitize_ros_interface_name(self._service_name) + wait_state = ScxmlState("waiting", + body=[ + ScxmlTransition( + f"processing_client_{client_id}", + [generate_srv_request_event(self._service_name, client_id)], + body=[ScxmlAssign(field_name, f"_event.{field_name}") for + field_name in req_params] + [ + ScxmlSend(generate_srv_server_request_event( + self._service_name), + [ScxmlParam(field_name, expr=field_name) for + field_name in req_params])]) + for client_id in self._service_client_automata]) + processing_states = [ + ScxmlState(f"processing_client_{client_id}", + body=[ + ScxmlTransition( + "waiting", [generate_srv_server_response_event(self._service_name)], + body=[ + ScxmlAssign(field_name, f"_event.{field_name}") for + field_name in res_params] + [ + ScxmlSend(generate_srv_response_event( + self._service_name, client_id), + [ScxmlParam(field_name, expr=field_name) for + field_name in res_params])])]) + for client_id in self._service_client_automata] + # Prepare the ScxmlRoot object and return it + scxml_root = ScxmlRoot(scxml_root_name) + scxml_root.set_data_model(ScxmlDataModel(req_fields_as_data)) + scxml_root.add_state(wait_state, initial=True) + for processing_state in processing_states: + scxml_root.add_state(processing_state) + assert scxml_root.is_plain_scxml(), "Generated SCXML for srv sync is not plain SCXML." + return scxml_root + + +# Mapping from RosService name and RosService information +RosServices = Dict[str, RosService] + + +def remove_empty_self_loops_from_srv_handlers_in_jani(jani_model: JaniModel) -> None: + """ + Remove self-loops from srv_handler automata in the Jani model. + + :param jani_model: The Jani model to modify. + """ + for automaton in jani_model.get_automata(): + # Modify the automaton in place + if automaton.get_name().startswith(SRV_PREFIX): + automaton.remove_empty_self_loop_edges() diff --git a/jani_generator/src/jani_generator/scxml_helpers/scxml_data.py b/jani_generator/src/jani_generator/scxml_helpers/scxml_data.py index e37c8572..18ae9bc3 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/scxml_data.py +++ b/jani_generator/src/jani_generator/scxml_helpers/scxml_data.py @@ -145,7 +145,7 @@ def _evalute_possible_types( if len(types) > 1: raise ValueError( f"Multiple types found for data {self.id}: {types}") - + def get_type(self) -> type: """Get the type of the data. diff --git a/jani_generator/src/jani_generator/scxml_helpers/scxml_expression.py b/jani_generator/src/jani_generator/scxml_helpers/scxml_expression.py index e552ab83..7813e3dc 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/scxml_expression.py +++ b/jani_generator/src/jani_generator/scxml_helpers/scxml_expression.py @@ -17,8 +17,6 @@ Module producing jani expressions from ecmascript. """ -from typing import Union - import esprima from jani_generator.jani_entries.jani_expression import JaniExpression @@ -26,7 +24,7 @@ from jani_generator.jani_entries.jani_convince_expression_expansion import BASIC_EXPRESSIONS_MAPPING -def parse_ecmascript_to_jani_expression(ecmascript: str) -> Union[JaniValue, JaniExpression]: +def parse_ecmascript_to_jani_expression(ecmascript: str) -> JaniExpression: """ Parse ecmascript to jani expression. @@ -39,8 +37,7 @@ def parse_ecmascript_to_jani_expression(ecmascript: str) -> Union[JaniValue, Jan return _parse_ecmascript_to_jani_expression(ast) -def _parse_ecmascript_to_jani_expression(ast: esprima.nodes.Script - ) -> Union[JaniValue, JaniExpression]: +def _parse_ecmascript_to_jani_expression(ast: esprima.nodes.Script) -> JaniExpression: """ Parse ecmascript to jani expression. @@ -48,7 +45,7 @@ def _parse_ecmascript_to_jani_expression(ast: esprima.nodes.Script :return: The jani expression. """ if ast.type == "Literal": - return JaniValue(ast.value) + return JaniExpression(JaniValue(ast.value)) elif ast.type == "Identifier": # If it is an identifier, we do not need to expand further return JaniExpression(ast.name) @@ -71,4 +68,4 @@ def _parse_ecmascript_to_jani_expression(ast: esprima.nodes.Script "right": _parse_ecmascript_to_jani_expression(ast.right) }) else: - raise NotImplementedError(f"Unsupported ecmascript type: {ast.type}") \ No newline at end of file + raise NotImplementedError(f"Unsupported ecmascript type: {ast.type}") 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 1d7e48b7..167a6ef3 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py +++ b/jani_generator/src/jani_generator/scxml_helpers/scxml_tags.py @@ -155,6 +155,9 @@ def _append_scxml_body_to_jani_automaton(jani_automaton: JaniAutomaton, events_h variables = {} for n, v in jani_automaton.get_variables().items(): variables[n] = v.get_type()() + # TODO: We should get the type explicitly: sometimes the expression is underdefined + print(f"Interpreting {expr} with {variables}") + # This might contain reference to event variables, that have no type specified data_structure_for_event[param.get_name()] = \ type(interpret_ecma_script_expr(expr, variables)) new_edge.destinations[0]['assignments'].append(JaniAssignment({ @@ -293,6 +296,7 @@ def write_model(self): expr_type = type(interpret_ecma_script_expr(scxml_data.get_expr())) assert expr_type == scxml_data.get_type(), \ f"Expected type {scxml_data.get_type()}, got {expr_type}." + # TODO: Add support for lower and upper bounds self.automaton.add_variable( JaniVariable(scxml_data.get_name(), scxml_data.get_type(), init_value)) diff --git a/jani_generator/src/jani_generator/scxml_helpers/scxml_to_jani.py b/jani_generator/src/jani_generator/scxml_helpers/scxml_to_jani.py index 19d925fa..624d0193 100644 --- a/jani_generator/src/jani_generator/scxml_helpers/scxml_to_jani.py +++ b/jani_generator/src/jani_generator/scxml_helpers/scxml_to_jani.py @@ -17,14 +17,16 @@ Module handling the conversion from SCXML to Jani. """ -from typing import List +from typing import List, Union from scxml_converter.scxml_entries import ScxmlRoot from jani_generator.jani_entries.jani_automaton import JaniAutomaton from jani_generator.jani_entries.jani_model import JaniModel -from jani_generator.ros_helpers.ros_timer import (RosTimer, - make_global_timer_automaton) +from jani_generator.ros_helpers.ros_timer import ( + RosTimer, make_global_timer_automaton) +from jani_generator.ros_helpers.ros_services import ( + remove_empty_self_loops_from_srv_handlers_in_jani) from jani_generator.scxml_helpers.scxml_event import EventsHolder from jani_generator.scxml_helpers.scxml_event_processor import \ implement_scxml_events_as_jani_syncs @@ -46,7 +48,7 @@ def convert_scxml_root_to_jani_automaton( def convert_multiple_scxmls_to_jani( - scxmls: List[str], + scxmls: List[Union[str, ScxmlRoot]], timers: List[RosTimer], max_time_ns: int ) -> JaniModel: @@ -58,14 +60,14 @@ def convert_multiple_scxmls_to_jani( """ base_model = JaniModel() events_holder = EventsHolder() - for scxml_str in scxmls: - try: - scxml_root = ScxmlRoot.from_scxml_file(scxml_str) - assert scxml_root.is_plain_scxml(), f"{scxml_str} does not contain a plain SCXML model." - except Exception as e: - print(">>>") - print(scxml_str) - raise e + for input_scxml in scxmls: + if isinstance(input_scxml, str): + scxml_root = ScxmlRoot.from_scxml_file(input_scxml) + else: + assert isinstance(input_scxml, ScxmlRoot) + scxml_root = input_scxml + assert scxml_root.is_plain_scxml(), \ + f"Input model {scxml_root.get_name()} does not contain a plain SCXML model." automaton = JaniAutomaton() convert_scxml_root_to_jani_automaton(scxml_root, automaton, events_holder) base_model.add_jani_automaton(automaton) @@ -73,5 +75,5 @@ def convert_multiple_scxmls_to_jani( if timer_automaton is not None: base_model.add_jani_automaton(timer_automaton) implement_scxml_events_as_jani_syncs(events_holder, timers, base_model) - + remove_empty_self_loops_from_srv_handlers_in_jani(base_model) return base_model 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 e2ad65ee..c12a5014 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,15 +20,16 @@ import os import json -from typing import List, Optional +from typing import Any, Dict, List, Tuple from xml.etree import ElementTree as ET from as2fm_common.common import remove_namespace from scxml_converter.bt_converter import bt_converter -from scxml_converter.scxml_converter import ros_to_scxml_converter +from scxml_converter.scxml_entries import ScxmlRoot from jani_generator.jani_entries import JaniModel from jani_generator.ros_helpers.ros_timer import RosTimer +from jani_generator.ros_helpers.ros_services import RosServices, RosService from jani_generator.scxml_helpers.scxml_to_jani import convert_multiple_scxmls_to_jani @@ -50,97 +51,148 @@ def _parse_time_element(time_element: ET.Element) -> int: return int(time_element.attrib["value"]) * TIME_MULTIPLIERS[time_unit] -def interpret_top_level_xml(xml_path: str) -> JaniModel: +def parse_main_xml(xml_path: str) -> Dict[str, Any]: """ - Interpret the top-level XML file as a Jani model. - - :param xml_path: The path to the XML file to interpret. - :return: The interpreted Jani model. + Interpret the top-level XML file as a dictionary. + + The returned dictionary contains the following keys: + - max_time: The maximum time in nanoseconds. + - bt: The path to the Behavior Tree definition. + - plugins: A list of paths to the Behavior Tree plugins. + - skills: A list of paths to SCXML files encoding an FSM. + - components: Similar to skills, but representing abstract models of existing skills + - properties: A list of paths to Jani properties. """ + # Used to generate absolute paths of scxml models folder_of_xml = os.path.dirname(xml_path) with open(xml_path, 'r', encoding='utf-8') as f: xml = ET.parse(f) assert remove_namespace(xml.getroot().tag) == "convince_mc_tc", \ "The top-level XML element must be convince_mc_tc." - - scxml_files_to_convert = [] - bt: Optional[str] = None # The path to the Behavior Tree definition - + main_dict = { + "max_time": None, + "bt": None, + "plugins": [], + "skills": [], + "components": [], + "properties": [] + } for first_level in xml.getroot(): if remove_namespace(first_level.tag) == "mc_parameters": for mc_parameter in first_level: # if remove_namespace(mc_parameter.tag) == "time_resolution": # time_resolution = _parse_time_element(mc_parameter) if remove_namespace(mc_parameter.tag) == "max_time": - max_time_ns = _parse_time_element(mc_parameter) + main_dict["max_time"] = _parse_time_element(mc_parameter) else: raise ValueError( f"Invalid mc_parameter tag: {mc_parameter.tag}") elif remove_namespace(first_level.tag) == "behavior_tree": - plugins = [] for child in first_level: if remove_namespace(child.tag) == "input": if child.attrib["type"] == "bt.cpp-xml": - assert bt is None, "Only one BT is supported." - bt = child.attrib["src"] + assert main_dict["bt"] is None, "Only one BT is supported." + main_dict["bt"] = os.path.join(folder_of_xml, child.attrib["src"]) elif child.attrib["type"] == "bt-plugin-ros-scxml": - plugins.append(child.attrib["src"]) + main_dict["plugins"].append( + os.path.join(folder_of_xml, child.attrib["src"])) else: - raise ValueError( - f"Invalid input tag type: {child.attrib['type']}") + raise ValueError(f"Invalid input type: {child.attrib['type']}") else: raise ValueError( - f"Invalid behavior_tree tag: {child.tag}") - assert bt is not None, "There must be a Behavior Tree defined." + f"Invalid behavior_tree tag: {child.tag} != input") + assert main_dict["bt"] is not None, "There must be a Behavior Tree defined." elif remove_namespace(first_level.tag) == "node_models": for node_model in first_level: assert remove_namespace(node_model.tag) == "input", \ "Only input tags are supported." - assert node_model.attrib['type'] == "ros-scxml", \ + assert node_model.attrib["type"] == "ros-scxml", \ "Only ROS-SCXML node models are supported." - scxml_files_to_convert.append( - os.path.join(folder_of_xml, node_model.attrib["src"])) + main_dict["skills"].append(os.path.join(folder_of_xml, node_model.attrib["src"])) elif remove_namespace(first_level.tag) == "properties": - properties = [] for property in first_level: assert remove_namespace(property.tag) == "input", \ "Only input tags are supported." - assert property.attrib['type'] == "jani", \ + assert property.attrib["type"] == "jani", \ "Only Jani properties are supported." - properties.append(property.attrib["src"]) + main_dict["properties"].append(os.path.join(folder_of_xml, property.attrib["src"])) else: raise ValueError(f"Invalid main point tag: {first_level.tag}") + return main_dict - # Preprocess behavior tree and plugins - if bt is not None: - bt_path = os.path.join(folder_of_xml, bt) - plugin_paths = [] - for plugin in plugins: - plugin_paths.append(os.path.join(folder_of_xml, plugin)) - output_folder = folder_of_xml # TODO: Think about better folder structure - scxml_files = bt_converter(bt_path, plugin_paths, output_folder) - scxml_files_to_convert.extend(scxml_files) +def generate_plain_scxml_models_and_timers( + full_model_dict: str) -> Tuple[List[ScxmlRoot], List[RosTimer]]: + """ + Generate plain SCXML models and ROS timers from the full model dictionary. + """ + # Convert behavior tree and plugins to ROS-scxml + scxml_files_to_convert: list = full_model_dict["skills"] + full_model_dict["components"] + if full_model_dict["bt"] is not None: + bt_out_dir = os.path.join(os.path.dirname(full_model_dict["bt"]), "generated_bt_scxml") + os.makedirs(bt_out_dir, exist_ok=True) + expanded_bt_plugin_scxmls = bt_converter( + full_model_dict["bt"], full_model_dict["plugins"], bt_out_dir) + scxml_files_to_convert.extend(expanded_bt_plugin_scxmls) + + # Convert ROS-SCXML FSMs to plain SCXML plain_scxml_models = [] all_timers: List[RosTimer] = [] + all_services: RosServices = {} for fname in scxml_files_to_convert: - with open(fname, 'r', encoding='utf-8') as f: - model, timers = ros_to_scxml_converter(f.read()) - for timer_name, timer_rate in timers: + plain_scxml, ros_declarations = \ + ScxmlRoot.from_scxml_file(fname).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)) - plain_scxml_models.append(model) + # Handle ROS Services + for service_name, service_type in ros_declarations._service_clients.items(): + 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(): + 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) + # Generate service sync SCXML models + for service_info in all_services.values(): + plain_scxml_models.append(service_info.to_scxml()) + return plain_scxml_models, all_timers + + +def interpret_top_level_xml(xml_path: str, store_generated_scxmls: bool = False) -> JaniModel: + """ + Interpret the top-level XML file as a Jani model. + + :param xml_path: The path to the XML file to interpret. + :return: The interpreted Jani model. + """ + model_dir = os.path.dirname(xml_path) + full_model_dict = parse_main_xml(xml_path) + assert full_model_dict["max_time"] is not None, f"Max time must be defined in {xml_path}." + plain_scxml_models, all_timers = generate_plain_scxml_models_and_timers(full_model_dict) + + if store_generated_scxmls: + plain_scxml_dir = os.path.join(model_dir, "generated_plain_scxml") + os.makedirs(plain_scxml_dir, exist_ok=True) + for scxml_model in plain_scxml_models: + with open(os.path.join(plain_scxml_dir, f"{scxml_model.get_name()}.scxml"), "w", + encoding='utf-8') as f: + f.write(scxml_model.as_xml_string()) jani_model = convert_multiple_scxmls_to_jani( - plain_scxml_models, all_timers, max_time_ns) + plain_scxml_models, all_timers, full_model_dict["max_time"]) jani_dict = jani_model.as_dict() - assert len(properties) == 1, "Only one property is supported right now." - with open(os.path.join(folder_of_xml, properties[0]), - "r", encoding='utf-8') as f: + assert len(full_model_dict["properties"]) == 1, "Only one property is supported right now." + with open(full_model_dict["properties"][0], "r", encoding='utf-8') as f: jani_dict["properties"] = json.load(f)["properties"] - output_path = os.path.join(folder_of_xml, "main.jani") + output_path = os.path.join(model_dir, "main.jani") with open(output_path, "w", encoding='utf-8') as f: json.dump(jani_dict, f, indent=2, ensure_ascii=False) diff --git a/jani_generator/test/_test_data/ros_add_int_srv_example/.gitignore b/jani_generator/test/_test_data/ros_add_int_srv_example/.gitignore new file mode 100644 index 00000000..b941f4e4 --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_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_add_int_srv_example/client_1.scxml b/jani_generator/test/_test_data/ros_add_int_srv_example/client_1.scxml new file mode 100644 index 00000000..706c0bf3 --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/client_1.scxml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/_test_data/ros_add_int_srv_example/client_2.scxml b/jani_generator/test/_test_data/ros_add_int_srv_example/client_2.scxml new file mode 100644 index 00000000..efa9c0af --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/client_2.scxml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 00000000..418cab82 --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_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_add_int_srv_example/main.xml b/jani_generator/test/_test_data/ros_add_int_srv_example/main.xml new file mode 100644 index 00000000..36074c02 --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/main.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/jani_generator/test/_test_data/ros_add_int_srv_example/server.scxml b/jani_generator/test/_test_data/ros_add_int_srv_example/server.scxml new file mode 100644 index 00000000..08c4ac7f --- /dev/null +++ b/jani_generator/test/_test_data/ros_add_int_srv_example/server.scxml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/jani_generator/test/_test_data/ros_example_w_bt/.gitignore b/jani_generator/test/_test_data/ros_example_w_bt/.gitignore index d09b91c8..b941f4e4 100644 --- a/jani_generator/test/_test_data/ros_example_w_bt/.gitignore +++ b/jani_generator/test/_test_data/ros_example_w_bt/.gitignore @@ -1,6 +1,3 @@ -*_plain.scxml -*_plain_timer_*.scxml -*_TopicAction.scxml -*_TopicCondition.scxml -bt.scxml +generated_bt_scxml +generated_plain_scxml main.jani \ No newline at end of file diff --git a/jani_generator/test/test_systemtest_scxml_to_jani.py b/jani_generator/test/test_systemtest_scxml_to_jani.py index 93a230b6..96529f31 100644 --- a/jani_generator/test/test_systemtest_scxml_to_jani.py +++ b/jani_generator/test/test_systemtest_scxml_to_jani.py @@ -193,15 +193,19 @@ def test_example_with_sync(self): if os.path.exists(TEST_FILE): os.remove(TEST_FILE) - def _test_with_main(self, main_xml: str, folder: str, property_name: str, success: bool): + # Tests using main.xml ... + + def _test_with_main(self, + folder: str, property_name: str, success: bool, + store_generated_scxmls: bool = False): """Testing the conversion of the main.xml file with the entrypoint.""" test_data_dir = os.path.join( os.path.dirname(__file__), '_test_data', folder) - xml_main_path = os.path.join(test_data_dir, main_xml) + xml_main_path = os.path.join(test_data_dir, 'main.xml') ouput_path = os.path.join(test_data_dir, 'main.jani') if os.path.exists(ouput_path): os.remove(ouput_path) - interpret_top_level_xml(xml_main_path) + interpret_top_level_xml(xml_main_path, store_generated_scxmls) self.assertTrue(os.path.exists(ouput_path)) # ground_truth = os.path.join( # test_data_dir, @@ -224,43 +228,41 @@ def _test_with_main(self, main_xml: str, folder: str, property_name: str, succes # os.remove(ouput_path) def test_with_main_success(self): - """Test with main.xml as entrypoint. - Here we expect the property to be satisfied.""" - self._test_with_main('main.xml', 'ros_example', 'battery_depleted', True) + """Test the battery_depleted property is satisfied.""" + self._test_with_main('ros_example', 'battery_depleted', True) def test_with_main_fail(self): - """Test with main.xml as entrypoint. - Here we expect the property to be *not* satisfied.""" - self._test_with_main('main.xml', 'ros_example', 'battery_over_depleted', False) + """Here we expect the property to be *not* satisfied.""" + self._test_with_main('ros_example', 'battery_over_depleted', False) def test_with_w_bt_main_battery_depleted(self): - """Test with main.xml as entrypoint. - Here we expect the property to be *not* satisfied.""" + """Here we expect the property to be *not* satisfied.""" # TODO: Improve properties under evaluation! - self._test_with_main('main.xml', 'ros_example_w_bt', 'battery_depleted', False) + self._test_with_main('ros_example_w_bt', 'battery_depleted', False) def test_with_w_bt_main_battery_under_twenty(self): - """Test with main.xml as entrypoint. - Here we expect the property to be *not* satisfied.""" + """Here we expect the property to be *not* satisfied.""" # TODO: Improve properties under evaluation! - self._test_with_main('main.xml', 'ros_example_w_bt', 'battery_below_20', False) + self._test_with_main('ros_example_w_bt', 'battery_below_20', False) def test_with_w_bt_main_alarm_and_charge(self): - """Test with main.xml as entrypoint. - Here we expect the property to be satisfied.""" - self._test_with_main('main.xml', 'ros_example_w_bt', 'battery_alarm_on', True) + """Here we expect the property to be satisfied in a battery example + with charging feature.""" + self._test_with_main('ros_example_w_bt', 'battery_alarm_on', True) def test_events_sync_handling(self): - """Test with main.xml as entrypoint. - Here we make sure, the synchronization can handle events + """Here we make sure, the synchronization can handle events being sent in different orders without deadlocks.""" - self._test_with_main('main.xml', 'events_sync_examples', 'seq_check', True) + self._test_with_main('events_sync_examples', 'seq_check', True) def test_multiple_senders_same_event(self): - """Test with main.xml as entrypoint. - Here we make sure, the synchronization can handle events + """Test topic synchronization, handling events being sent in different orders without deadlocks.""" - self._test_with_main('main.xml', 'multiple_senders_same_event', 'seq_check', True) + self._test_with_main('multiple_senders_same_event', 'seq_check', True) + + def test_ros_add_int_srv_example(self): + """Test the services are properly handled in Jani.""" + self._test_with_main('ros_add_int_srv_example', 'happy_clients', True, True) if __name__ == '__main__': diff --git a/scxml_converter/src/scxml_converter/bt_converter.py b/scxml_converter/src/scxml_converter/bt_converter.py index 369d25f9..15d4e1b5 100644 --- a/scxml_converter/src/scxml_converter/bt_converter.py +++ b/scxml_converter/src/scxml_converter/bt_converter.py @@ -158,6 +158,6 @@ def bt_converter( 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(ET.tostring(root_tag.as_xml(), encoding='unicode', xml_declaration=True)) + f.write(root_tag.as_xml_string()) return generated_files diff --git a/scxml_converter/src/scxml_converter/scxml_converter.py b/scxml_converter/src/scxml_converter/scxml_converter.py index 621108da..157689f1 100644 --- a/scxml_converter/src/scxml_converter/scxml_converter.py +++ b/scxml_converter/src/scxml_converter/scxml_converter.py @@ -21,9 +21,9 @@ """ import xml.etree.ElementTree as ET -from typing import Dict, List, Tuple, Union +from typing import Dict, Tuple, Union -from scxml_converter.scxml_entries import ScxmlRoot +from scxml_converter.scxml_entries import ScxmlRoot, ScxmlRosDeclarationsContainer from as2fm_common.common import ros_type_name_to_python_type from as2fm_common.ecmascript_interpretation import \ @@ -101,15 +101,3 @@ def _check_topic_type( raise ConversionStaticAnalysisError( f"Field {name} has type {expression_type}, " + f"expected {expected_python_type}") - - -def ros_to_scxml_converter(input_xml: str) -> Tuple[str, List[Tuple[str, float]]]: - """Convert one SCXML file that contains ROS-specific tags. - - :param input_file: The input SCXML file. - :return: The converted SCXML and the timers as a list of tuples. - Each tuple contains the timer name and the rate in Hz. - """ - scxml_root = ScxmlRoot.from_scxml_file(input_xml) - plain_scxml, timers = scxml_root.as_plain_scxml() - return ET.tostring(plain_scxml.as_xml(), encoding='unicode', xml_declaration=True), timers diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index 50c4ad3a..88c3d328 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -1,16 +1,22 @@ -from .scxml_base import ScxmlBase # noqa: F401 -from .utils import HelperRosDeclarations # noqa: F401 -from .scxml_data import ScxmlData # noqa: F401 -from .scxml_data_model import ScxmlDataModel # noqa: F401 -from .scxml_param import ScxmlParam # noqa: F401 -from .scxml_executable_entries import ScxmlAssign, ScxmlIf, ScxmlSend # noqa: F401 +from .scxml_base import ScxmlBase # 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 .scxml_executable_entries import ScxmlAssign, ScxmlIf, ScxmlSend # noqa: F401 from .scxml_executable_entries import ScxmlExecutableEntry, ScxmlExecutionBody # noqa: F401 -from .scxml_executable_entries import (execution_body_from_xml, # noqa: F401 - as_plain_execution_body, # noqa: F401 - execution_entry_from_xml, valid_execution_body) # noqa: F401 -from .scxml_transition import ScxmlTransition # noqa: F401 -from .scxml_ros_entries import (RosTimeRate, RosTopicPublisher, RosTopicSubscriber, # noqa: F401 - RosRateCallback, RosTopicCallback, RosTopicPublish, # noqa: F401 - RosField, ScxmlRosDeclarations) # noqa: F401 -from .scxml_state import ScxmlState # noqa: F401 -from .scxml_root import ScxmlRoot # 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 +from .scxml_transition import ScxmlTransition # 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_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/scxml_data.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py index 2c509fdd..8537d062 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_data.py @@ -20,20 +20,9 @@ from typing import Any from scxml_converter.scxml_entries import ScxmlBase -from xml.etree import ElementTree as ET - +from scxml_converter.scxml_entries.utils import SCXML_DATA_STR_TO_TYPE -# TODO: add lower and upper bounds depending on the n. of bits used. -# TODO: add support to uint -SCXML_DATA_MAPPING = { - "bool": bool, - "float32": float, - "float64": float, - "int8": int, - "int16": int, - "int32": int, - "int64": int -} +from xml.etree import ElementTree as ET class ScxmlData(ScxmlBase): @@ -71,7 +60,7 @@ def get_name(self) -> str: return self._id def get_type(self) -> type: - return SCXML_DATA_MAPPING[self._data_type] + return SCXML_DATA_STR_TO_TYPE[self._data_type] def get_expr(self) -> str: return self._expr @@ -87,10 +76,10 @@ def check_validity(self) -> bool: 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_MAPPING): + 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_MAPPING[self._data_type] + 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): 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 073882a8..1032b18b 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,9 +20,9 @@ from typing import List, Optional, Union, Tuple, get_args from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlParam, HelperRosDeclarations) +from scxml_converter.scxml_entries import (ScxmlBase, ScxmlParam, ScxmlRosDeclarationsContainer) -from scxml_converter.scxml_entries.utils import replace_ros_msg_expression +from scxml_converter.scxml_entries.utils import replace_ros_interface_expression # Use delayed type evaluation: https://peps.python.org/pep-0484/#forward-references ScxmlExecutableEntry = Union['ScxmlAssign', 'ScxmlIf', 'ScxmlSend'] @@ -104,10 +104,11 @@ def check_validity(self) -> bool: print("Error: SCXML if: invalid else execution body found.") return valid_conditional_executions and valid_else_execution - def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations) -> bool: + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared.""" # Check the executable content - assert isinstance(ros_declarations, HelperRosDeclarations), \ + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ "Error: SCXML if: invalid ROS declarations type provided." for _, exec_body in self._conditional_executions: for exec_entry in exec_body: @@ -119,10 +120,10 @@ def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations return False return True - def as_plain_scxml(self, ros_declarations: HelperRosDeclarations) -> "ScxmlIf": + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "ScxmlIf": condional_executions = [] for condition, execution in self._conditional_executions: - condional_executions.append((replace_ros_msg_expression(condition), + condional_executions.append((replace_ros_interface_expression(condition), as_plain_execution_body(execution, ros_declarations))) else_execution = as_plain_execution_body(self._else_execution, ros_declarations) return ScxmlIf(condional_executions, else_execution) @@ -253,7 +254,7 @@ def check_valid_ros_instantiations(self, _) -> bool: def as_plain_scxml(self, _) -> "ScxmlAssign": # TODO: Might make sense to check if the assignment happens in a topic callback - expr = replace_ros_msg_expression(self._expr) + expr = replace_ros_interface_expression(self._expr) return ScxmlAssign(self._location, expr) def as_xml(self) -> ET.Element: @@ -299,19 +300,13 @@ def execution_entry_from_xml(xml_tree: ET.Element) -> ScxmlExecutableEntry: :return: The execution entry """ # TODO: This is pretty bad, need to re-check how to break the circle - from .scxml_ros_entries import RosTopicPublish - # Switch based on the tag name + from .scxml_ros_entries import ScxmlRosSends + # 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} exec_tag = xml_tree.tag - if exec_tag == ScxmlIf.get_tag_name(): - return ScxmlIf.from_xml_tree(xml_tree) - elif exec_tag == ScxmlAssign.get_tag_name(): - return ScxmlAssign.from_xml_tree(xml_tree) - elif exec_tag == ScxmlSend.get_tag_name(): - return ScxmlSend.from_xml_tree(xml_tree) - elif exec_tag == RosTopicPublish.get_tag_name(): - return RosTopicPublish.from_xml_tree(xml_tree) - else: - raise ValueError(f"Error: SCXML conversion: tag {exec_tag} isn't an executable entry.") + assert exec_tag in tag_to_cls, \ + f"Error: SCXML conversion: tag {exec_tag} isn't an executable entry." + return tag_to_cls[exec_tag].from_xml_tree(xml_tree) def execution_body_from_xml(xml_tree: ET.Element) -> ScxmlExecutionBody: @@ -340,7 +335,7 @@ def append_execution_body_to_xml(xml_parent: ET.Element, exec_body: ScxmlExecuti def as_plain_execution_body( exec_body: Optional[ScxmlExecutionBody], - ros_declarations: HelperRosDeclarations) -> Optional[ScxmlExecutionBody]: + ros_declarations: ScxmlRosDeclarationsContainer) -> Optional[ScxmlExecutionBody]: """ Convert the execution body to plain SCXML. 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 0af93d62..67c6b5e7 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_param.py @@ -28,6 +28,7 @@ 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 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 c1a6505b..4f1f202b 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_root.py @@ -20,7 +20,8 @@ from typing import List, Optional, Tuple, get_args from scxml_converter.scxml_entries import (ScxmlBase, ScxmlState, ScxmlDataModel, ScxmlRosDeclarations, RosTimeRate, RosTopicSubscriber, - RosTopicPublisher, HelperRosDeclarations) + RosTopicPublisher, RosServiceServer, RosServiceClient, + ScxmlRosDeclarationsContainer) from copy import deepcopy from os.path import isfile @@ -58,6 +59,10 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlRoot": 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)) # States assert "initial" in xml_tree.attrib, \ "Error: SCXML root: 'initial' attribute not found in input xml." @@ -142,9 +147,9 @@ def add_ros_declaration(self, ros_declaration: ScxmlRosDeclarations): self._ros_declarations = [] self._ros_declarations.append(ros_declaration) - def _generate_ros_declarations_helper(self) -> Optional[HelperRosDeclarations]: + def _generate_ros_declarations_helper(self) -> ScxmlRosDeclarationsContainer: """Generate a HelperRosDeclarations object from the existing ROS declarations.""" - ros_decl_container = HelperRosDeclarations() + 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(): @@ -158,6 +163,12 @@ def _generate_ros_declarations_helper(self) -> Optional[HelperRosDeclarations]: elif isinstance(ros_declaration, RosTopicPublisher): ros_decl_container.append_publisher(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_declaration.get_service_type()) + elif isinstance(ros_declaration, RosServiceClient): + ros_decl_container.append_service_client(ros_declaration.get_service_name(), + ros_declaration.get_service_type()) else: raise ValueError("Error: SCXML root: invalid ROS declaration type.") return ros_decl_container @@ -204,7 +215,7 @@ 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 as_plain_scxml(self) -> Tuple["ScxmlRoot", List[Tuple[str, float]]]: + def to_plain_scxml_and_declarations(self) -> Tuple["ScxmlRoot", ScxmlRosDeclarationsContainer]: """ Convert all internal ROS specific entries to plain SCXML. @@ -213,7 +224,7 @@ def as_plain_scxml(self) -> Tuple["ScxmlRoot", List[Tuple[str, float]]]: - A list of timers with related rate in Hz """ if self.is_plain_scxml(): - return self, [] + return self, ScxmlRosDeclarationsContainer(self._name) # Convert the ROS specific entries to plain SCXML plain_root = ScxmlRoot(self._name) plain_root._data_model = deepcopy(self._data_model) @@ -221,7 +232,7 @@ def as_plain_scxml(self) -> Tuple["ScxmlRoot", List[Tuple[str, float]]]: ros_declarations = self._generate_ros_declarations_helper() 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, list(ros_declarations.get_timers().items())) + return (plain_root, ros_declarations) def as_xml(self) -> ET.Element: assert self.check_validity(), "SCXML: found invalid root object." @@ -241,3 +252,6 @@ def as_xml(self) -> ET.Element: xml_root.append(state.as_xml()) ET.indent(xml_root, " ") return xml_root + + def as_xml_string(self) -> str: + return ET.tostring(self.as_xml(), encoding='unicode', xml_declaration=True) 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 index 9ecb15bb..af342ad4 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_entries.py @@ -15,447 +15,19 @@ """Declaration of ROS-Specific SCXML tags extensions.""" -from typing import List, Optional, Union -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlSend, ScxmlParam, ScxmlTransition, - ScxmlExecutionBody, HelperRosDeclarations, - valid_execution_body, execution_body_from_xml, - as_plain_execution_body) -from scxml_converter.scxml_entries.utils import replace_ros_msg_expression -from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries.utils import is_topic_type_known - - -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) - - def get_tag_name() -> str: - return "ros_time_rate" - - 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 = xml_tree.attrib.get("rate_hz") - assert timer_name is not None and timer_rate is not None, \ - "Error: SCXML rate timer: 'name' or 'rate_hz' attribute not found in input xml." - try: - timer_rate = float(timer_rate) - except ValueError: - raise ValueError("Error: SCXML rate timer: rate is not a number.") - return RosTimeRate(timer_name, timer_rate) - - 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 - 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 get_name(self) -> str: - return self._name - - def get_rate(self) -> float: - return self._rate_hz - - def as_plain_scxml(self, _) -> ScxmlBase: - 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( - RosTimeRate.get_tag_name(), {"rate_hz": str(self._rate_hz), "name": self._name}) - return xml_time_rate - - -class RosTopicPublisher(ScxmlBase): - """Object used in SCXML root to declare a new topic publisher.""" - - def __init__(self, topic_name: str, topic_type: str) -> None: - self._topic_name = topic_name - self._topic_type = topic_type - - def get_tag_name() -> str: - return "ros_topic_publisher" - - 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 check_validity(self) -> bool: - valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 - valid_type = is_topic_type_known(self._topic_type) - if not valid_name: - print("Error: SCXML topic subscriber: topic name is not valid.") - if not valid_type: - print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_name and valid_type - - def get_topic_name(self) -> str: - return self._topic_name - - def get_topic_type(self) -> str: - return self._topic_type - - def as_plain_scxml(self, _) -> ScxmlBase: - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") - - 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}) - return xml_topic_publisher - - -class RosTopicSubscriber(ScxmlBase): - """Object used in SCXML root to declare a new topic subscriber.""" - - def __init__(self, topic_name: str, topic_type: str) -> None: - self._topic_name = topic_name - self._topic_type = topic_type - - def get_tag_name() -> str: - return "ros_topic_subscriber" - - 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 check_validity(self) -> bool: - valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 - valid_type = is_topic_type_known(self._topic_type) - if not valid_name: - print("Error: SCXML topic subscriber: topic name is not valid.") - if not valid_type: - print("Error: SCXML topic subscriber: topic type is not valid.") - return valid_name and valid_type - - def get_topic_name(self) -> str: - return self._topic_name - - def get_topic_type(self) -> str: - return self._topic_type - - def as_plain_scxml(self, _) -> ScxmlBase: - raise RuntimeError("Error: SCXML ROS declarations cannot be converted to plain SCXML.") - - 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}) - return xml_topic_subscriber - - -class RosRateCallback(ScxmlTransition): - """Callback that triggers each time the associated timer ticks.""" - - 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 - """ - 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." - - def get_tag_name() -> str: - return "ros_rate_callback" - - 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 - 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 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: HelperRosDeclarations) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, HelperRosDeclarations), \ - "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 = self._check_valid_ros_instantiations_exec_body(ros_declarations) - if not valid_body: - print("Error: SCXML rate callback: body has invalid ROS instantiations.") - return valid_body - - def as_plain_scxml(self, ros_declarations: HelperRosDeclarations) -> ScxmlTransition: - event_name = "ros_time_rate." + self._timer_name - 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}) - 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 - - -class RosTopicCallback(ScxmlTransition): - """Object representing a transition to perform when a new ROS msg is received.""" - - def __init__( - self, topic: Union[RosTopicSubscriber, str], target: str, - condition: Optional[str] = None, body: Optional[ScxmlExecutionBody] = None): - """ - Create a new ros_topic_callback object instance. - - :param topic: 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() - else: - # Used for generating ROS entries from xml file - assert isinstance(topic, str), "Error: SCXML topic callback: invalid topic type." - self._topic = topic - self._target = target - self._condition = condition - self._body = body - assert self.check_validity(), "Error: SCXML topic callback: invalid parameters." - - def get_tag_name() -> str: - return "ros_topic_callback" - - 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." - condition = xml_tree.get("cond") - condition = condition if condition is not None and len(condition) > 0 else None - exec_body = execution_body_from_xml(xml_tree) - exec_body = exec_body if exec_body is not None else None - return RosTopicCallback(topic_name, target, condition, exec_body) - - 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_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_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_cond: - print("Error: SCXML topic callback: condition is not valid.") - if not valid_body: - print("Error: SCXML topic callback: body is not valid.") - return valid_topic and valid_target and valid_cond and valid_body - - def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, HelperRosDeclarations), \ - "Error: SCXML topic callback: invalid ROS declarations container." - topic_cb_declared = ros_declarations.is_subscriber_defined(self._topic) - if not topic_cb_declared: - print(f"Error: SCXML topic callback: topic subscriber {self._topic} not declared.") - return False - valid_body = self._check_valid_ros_instantiations_exec_body(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: HelperRosDeclarations) -> ScxmlTransition: - event_name = "ros_topic." + self._topic - 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 topic callback: invalid parameters." - xml_topic_callback = ET.Element( - "ros_topic_callback", {"topic": self._topic, "target": self._target}) - if self._condition is not None: - xml_topic_callback.set("cond", self._condition) - if self._body is not None: - for entry in self._body: - xml_topic_callback.append(entry.as_xml()) - return xml_topic_callback - - -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." - - def get_tag_name() -> str: - return "field" - - 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." - return RosField(name, expr) - - 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.") - return valid_name and valid_expr - - def as_plain_scxml(self) -> ScxmlParam: - return ScxmlParam(self._name, expr=replace_ros_msg_expression(self._expr)) - - def as_xml(self) -> ET.Element: - assert self.check_validity(), "Error: SCXML topic publish field: invalid parameters." - xml_field = ET.Element(RosField.get_tag_name(), {"name": self._name, "expr": self._expr}) - return xml_field - - -class RosTopicPublish(ScxmlSend): - """Object representing the shipping of a ROS msg through a topic.""" - - def __init__(self, topic: Union[RosTopicPublisher, str], - fields: Optional[List[RosField]] = None): - if isinstance(topic, RosTopicPublisher): - self._topic = topic.get_topic_name() - else: - # Used for generating ROS entries from xml file - assert isinstance(topic, str), "Error: SCXML topic publish: invalid topic type." - self._topic = topic - self._fields = fields - assert self.check_validity(), "Error: SCXML topic publish: invalid parameters." - - def get_tag_name() -> str: - return "ros_topic_publish" - - 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 = [] - for field_xml in xml_tree: - fields.append(RosField.from_xml_tree(field_xml)) - if len(fields) == 0: - fields = None - return RosTopicPublish(topic_name, fields) - - def check_validity(self) -> bool: - valid_topic = isinstance(self._topic, str) and len(self._topic) > 0 - valid_fields = self._fields is None or \ - all([isinstance(field, RosField) 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 - - def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations) -> bool: - """Check if the ros instantiations have been declared.""" - assert isinstance(ros_declarations, HelperRosDeclarations), \ - "Error: SCXML topic publish: invalid ROS declarations container." - topic_pub_declared = ros_declarations.is_publisher_defined(self._topic) - if not topic_pub_declared: - print(f"Error: SCXML topic publish: topic {self._topic} not declared.") - # 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: - self._fields = [] - self._fields.append(field) - - def as_plain_scxml(self, _) -> ScxmlSend: - event_name = "ros_topic." + self._topic - params = None if self._fields is None else \ - [field.as_plain_scxml() 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}) - if self._fields is not None: - for field in self._fields: - xml_topic_publish.append(field.as_xml()) - return xml_topic_publish - - -ScxmlRosDeclarations = Union[RosTimeRate, RosTopicPublisher, RosTopicSubscriber] +from typing import Union +from scxml_converter.scxml_entries import ( + RosTimeRate, RosTopicPublisher, RosTopicSubscriber, RosServiceServer, RosServiceClient, + RosServiceHandleRequest, RosServiceHandleResponse, + RosServiceSendRequest, RosServiceSendResponse, + RosTopicPublish, RosTopicCallback, RosRateCallback) + +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_field.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py new file mode 100644 index 00000000..1f04d241 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_field.py @@ -0,0 +1,59 @@ +# 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 the ROS Field SCXML tag extension.""" + +from scxml_converter.scxml_entries import ScxmlParam +from xml.etree import ElementTree as ET + + +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." + + def get_tag_name() -> str: + return "field" + + 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." + return RosField(name, expr) + + 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.") + return valid_name and valid_expr + + def as_plain_scxml(self) -> ScxmlParam: + from scxml_converter.scxml_entries.utils import replace_ros_interface_expression + return ScxmlParam(self._name, expr=replace_ros_interface_expression(self._expr)) + + def as_xml(self) -> ET.Element: + assert self.check_validity(), "Error: SCXML topic publish field: invalid parameters." + xml_field = ET.Element(RosField.get_tag_name(), {"name": self._name, "expr": self._expr}) + return xml_field 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 new file mode 100644 index 00000000..449228d7 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_service.py @@ -0,0 +1,486 @@ +# 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 Services. + +Additional information: +https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Services/Understanding-ROS2-Services.html +""" + +from typing import Optional, List, Union +from scxml_converter.scxml_entries import (ScxmlBase, RosField, ScxmlSend, ScxmlTransition, + ScxmlExecutionBody) +from scxml_converter.scxml_entries import (execution_body_from_xml, valid_execution_body, + as_plain_execution_body) +from xml.etree import ElementTree as ET +from scxml_converter.scxml_entries.utils import ScxmlRosDeclarationsContainer +from scxml_converter.scxml_entries.utils import (is_srv_type_known, + generate_srv_request_event, + generate_srv_response_event, + generate_srv_server_request_event, + generate_srv_server_response_event) + + +class RosServiceServer(ScxmlBase): + """Object used in SCXML root to declare a new service server.""" + + @staticmethod + 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_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: + """ + Initialize a new RosServiceServer object. + + :param srv_name: Topic used by the service. + :param srv_type: ROS type of the service. + """ + self._srv_name = srv_name + self._srv_type = srv_type + + 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 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: + print("Error: SCXML Service Server: service type is not valid.") + return valid_name and valid_type + + 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 Service Server: invalid parameters." + xml_srv_server = ET.Element( + RosServiceServer.get_tag_name(), + {"service_name": self._srv_name, "type": self._srv_type}) + return xml_srv_server + + +class RosServiceClient(ScxmlBase): + """Object used in SCXML root to declare a new service client.""" + + @staticmethod + 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_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: + """ + Initialize a new RosServiceClient object. + + :param srv_name: Topic used by the service. + :param srv_type: ROS type of the service. + """ + self._srv_name = srv_name + self._srv_type = srv_type + + 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 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: + print("Error: SCXML Service Client: service type is not valid.") + return valid_name and valid_type + + 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 Service Client: invalid parameters." + xml_srv_server = ET.Element( + RosServiceClient.get_tag_name(), + {"service_name": self._srv_name, "type": self._srv_type}) + return xml_srv_server + + +class RosServiceSendRequest(ScxmlSend): + """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 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." + fields = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + if len(fields) == 0: + fields = None + return RosServiceSendRequest(srv_name, fields) + + def __init__(self, + service_decl: Union[str, RosServiceClient], + fields: Optional[List[RosField]]) -> 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_service_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 + self._fields = fields + 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_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 + + 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 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()) + event_params = [field.as_plain_scxml() 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}) + if self._fields is not None: + for field in self._fields: + xml_srv_request.append(field.as_xml()) + return xml_srv_request + + +class RosServiceHandleRequest(ScxmlTransition): + """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 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." + 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_service_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_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_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 + + 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." + event_name = generate_srv_server_request_event(self._service_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 Service Handle Request: invalid parameters." + xml_srv_request = ET.Element(RosServiceHandleRequest.get_tag_name(), + {"service_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()) + return xml_srv_request + + +class RosServiceSendResponse(ScxmlSend): + """SCXML object representing the response from a service server.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_service_send_response" + + @staticmethod + def from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": + """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." + fields = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + if len(fields) == 0: + 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_service_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 + 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_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 + + 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 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) + event_params = [field.as_plain_scxml() 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}) + if self._fields is not None: + for field in self._fields: + xml_srv_response.append(field.as_xml()) + return xml_srv_response + + +class RosServiceHandleResponse(ScxmlTransition): + """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 from_xml_tree(xml_tree: ET.Element) -> "RosServiceClient": + """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." + 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_service_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_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_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 + + 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." + event_name = generate_srv_response_event( + self._service_name, ros_declarations.get_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 Service Handle Response: invalid parameters." + xml_srv_response = ET.Element(RosServiceHandleResponse.get_tag_name(), + {"service_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()) + return xml_srv_response 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 new file mode 100644 index 00000000..0f619288 --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_timer.py @@ -0,0 +1,165 @@ +# 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 Timers.""" + +from typing import Optional, Union +from scxml_converter.scxml_entries import (ScxmlBase, ScxmlTransition, + ScxmlExecutionBody, ScxmlRosDeclarationsContainer, + valid_execution_body, execution_body_from_xml, + as_plain_execution_body) +from xml.etree import ElementTree as ET + + +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) + + def get_tag_name() -> str: + return "ros_time_rate" + + 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 = xml_tree.attrib.get("rate_hz") + assert timer_name is not None and timer_rate is not None, \ + "Error: SCXML rate timer: 'name' or 'rate_hz' attribute not found in input xml." + try: + timer_rate = float(timer_rate) + except ValueError: + raise ValueError("Error: SCXML rate timer: rate is not a number.") + return RosTimeRate(timer_name, timer_rate) + + 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 + 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 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( + RosTimeRate.get_tag_name(), {"rate_hz": str(self._rate_hz), "name": self._name}) + return xml_time_rate + + +class RosRateCallback(ScxmlTransition): + """Callback that triggers each time the associated timer ticks.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_rate_callback" + + @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 + 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, + 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 + """ + 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." + + 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 + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + event_name = "ros_time_rate." + self._timer_name + 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}) + 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/scxml_ros_topic.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py new file mode 100644 index 00000000..05cc761d --- /dev/null +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_topic.py @@ -0,0 +1,291 @@ +# 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 Topics. + +Additional information: +https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html +""" + +from typing import List, Optional, Union +from scxml_converter.scxml_entries import (RosField, ScxmlBase, ScxmlSend, ScxmlParam, + ScxmlTransition, ScxmlExecutionBody, + ScxmlRosDeclarationsContainer, + valid_execution_body, execution_body_from_xml, + as_plain_execution_body) +from xml.etree import ElementTree as ET +from scxml_converter.scxml_entries.utils import is_msg_type_known + + +class RosTopicPublisher(ScxmlBase): + """Object used in SCXML root to declare a new topic publisher.""" + + @staticmethod + 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_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 + self._topic_type = topic_type + + def check_validity(self) -> bool: + valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 + valid_type = is_msg_type_known(self._topic_type) + if not valid_name: + print("Error: SCXML topic subscriber: topic name is not valid.") + if not valid_type: + print("Error: SCXML topic subscriber: topic type is not valid.") + return valid_name and valid_type + + def get_topic_name(self) -> str: + return self._topic_name + + def get_topic_type(self) -> str: + return self._topic_type + + 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 topic subscriber: invalid parameters." + xml_topic_publisher = ET.Element( + RosTopicPublisher.get_tag_name(), {"topic": self._topic_name, "type": self._topic_type}) + return xml_topic_publisher + + +class RosTopicSubscriber(ScxmlBase): + """Object used in SCXML root to declare a new topic subscriber.""" + + @staticmethod + 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_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 + self._topic_type = topic_type + + def check_validity(self) -> bool: + valid_name = isinstance(self._topic_name, str) and len(self._topic_name) > 0 + valid_type = is_msg_type_known(self._topic_type) + if not valid_name: + print("Error: SCXML topic subscriber: topic name is not valid.") + if not valid_type: + print("Error: SCXML topic subscriber: topic type is not valid.") + return valid_name and valid_type + + def get_topic_name(self) -> str: + return self._topic_name + + def get_topic_type(self) -> str: + return self._topic_type + + 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 topic subscriber: invalid parameters." + xml_topic_subscriber = ET.Element( + RosTopicSubscriber.get_tag_name(), + {"topic": self._topic_name, "type": self._topic_type}) + return xml_topic_subscriber + + +class RosTopicCallback(ScxmlTransition): + """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 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." + exec_body = execution_body_from_xml(xml_tree) + return RosTopicCallback(topic_name, target, exec_body) + + def __init__( + self, topic: 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 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() + else: + # Used for generating ROS entries from xml file + assert isinstance(topic, str), "Error: SCXML topic callback: invalid topic type." + self._topic = topic + 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_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 + + 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) + if not topic_cb_declared: + print(f"Error: SCXML topic callback: topic subscriber {self._topic} 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." + event_name = "ros_topic." + self._topic + 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 topic callback: invalid parameters." + xml_topic_callback = ET.Element( + "ros_topic_callback", {"topic": self._topic, "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(ScxmlSend): + """Object representing the shipping of a ROS msg through a topic.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_topic_publish" + + @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 = [] + for field_xml in xml_tree: + fields.append(RosField.from_xml_tree(field_xml)) + if len(fields) == 0: + fields = None + return RosTopicPublish(topic_name, fields) + + def __init__(self, topic: Union[RosTopicPublisher, str], + fields: Optional[List[RosField]] = None): + if isinstance(topic, RosTopicPublisher): + self._topic = topic.get_topic_name() + else: + # Used for generating ROS entries from xml file + assert isinstance(topic, str), "Error: SCXML topic publish: invalid topic type." + self._topic = topic + 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_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 + + 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) + if not topic_pub_declared: + print(f"Error: SCXML topic publish: topic {self._topic} not declared.") + # 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: + self._fields = [] + self._fields.append(field) + + 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 + params = None if self._fields is None else \ + [field.as_plain_scxml() 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}) + if self._fields is not None: + for field in self._fields: + xml_topic_publish.append(field.as_xml()) + return xml_topic_publish 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 38bba3ac..ce6bb041 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_state.py @@ -20,10 +20,10 @@ from typing import List, Optional, Union from xml.etree import ElementTree as ET -from scxml_converter.scxml_entries import (ScxmlBase, ScxmlExecutableEntry, ScxmlExecutionBody, - ScxmlTransition, HelperRosDeclarations, - as_plain_execution_body, execution_body_from_xml, - valid_execution_body) +from scxml_converter.scxml_entries import ( + ScxmlBase, ScxmlTransition, ScxmlRosDeclarationsContainer, + ScxmlRosTransitions, ScxmlExecutableEntry, ScxmlExecutionBody, + as_plain_execution_body, execution_body_from_xml, valid_execution_body) class ScxmlState(ScxmlBase): @@ -85,16 +85,12 @@ def from_xml_tree(xml_tree: ET.Element) -> "ScxmlState": return scxml_state def _transitions_from_xml(xml_tree: ET.Element) -> List[ScxmlTransition]: - # import ros callbacks inheriting from ScxmlTransition - from .scxml_ros_entries import RosRateCallback, RosTopicCallback transitions: List[ScxmlTransition] = [] + tag_to_cls = {cls.get_tag_name(): cls for cls in ScxmlRosTransitions} + tag_to_cls.update({ScxmlTransition.get_tag_name(): ScxmlTransition}) for child in xml_tree: - if child.tag == ScxmlTransition.get_tag_name(): - transitions.append(ScxmlTransition.from_xml_tree(child)) - elif child.tag == RosRateCallback.get_tag_name(): - transitions.append(RosRateCallback.from_xml_tree(child)) - elif child.tag == RosTopicCallback.get_tag_name(): - transitions.append(RosTopicCallback.from_xml_tree(child)) + if child.tag in tag_to_cls: + transitions.append(tag_to_cls[child.tag].from_xml_tree(child)) return transitions def add_transition(self, transition: ScxmlTransition): @@ -136,15 +132,12 @@ def check_validity(self) -> bool: print("Error: SCXML state: executable body is not valid.") return valid_on_entry and valid_on_exit and valid_body - def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations) -> bool: + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared.""" - # Check onentry and onexit - valid_entry = ScxmlState._check_valid_ros_instantiations(self._on_entry, - ros_declarations) - valid_exit = ScxmlState._check_valid_ros_instantiations(self._on_exit, - ros_declarations) - valid_body = ScxmlState._check_valid_ros_instantiations(self._body, - ros_declarations) + valid_entry = ScxmlState._check_valid_ros_instantiations(self._on_entry, ros_declarations) + 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.") if not valid_exit: @@ -154,16 +147,12 @@ def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations return valid_entry and valid_exit and valid_body def _check_valid_ros_instantiations(body: List[Union[ScxmlExecutableEntry, ScxmlTransition]], - ros_declarations: HelperRosDeclarations) -> bool: + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared in the body.""" - if body is None: - return True - for entry in body: - if not entry.check_valid_ros_instantiations(ros_declarations): - return False - return True - - def as_plain_scxml(self, ros_declarations: HelperRosDeclarations) -> "ScxmlState": + return body is None or \ + all(entry.check_valid_ros_instantiations(ros_declarations) for entry in body) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "ScxmlState": """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) 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 48038315..8910c49f 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_transition.py @@ -19,7 +19,7 @@ from typing import List, Optional from scxml_converter.scxml_entries import (ScxmlBase, ScxmlExecutionBody, ScxmlExecutableEntry, - HelperRosDeclarations, valid_execution_body, + ScxmlRosDeclarationsContainer, valid_execution_body, execution_body_from_xml) from xml.etree import ElementTree as ET @@ -117,29 +117,18 @@ def check_validity(self) -> bool: print("Error: SCXML transition: executable content is not valid.") return valid_target and valid_events and valid_condition and valid_body - def check_valid_ros_instantiations(self, ros_declarations: HelperRosDeclarations) -> bool: + def check_valid_ros_instantiations(self, + ros_declarations: ScxmlRosDeclarationsContainer) -> bool: """Check if the ros instantiations have been declared.""" - # Check the executable content - valid_body = self._check_valid_ros_instantiations_exec_body(ros_declarations) - if not valid_body: - print("Error: SCXML transition: executable content has invalid ROS instantiations.") - return valid_body + # For SCXML transitions, ROS interfaces can be found only in the exec body + return self._body is None or \ + all(entry.check_valid_ros_instantiations(ros_declarations) for entry in self._body) - def _check_valid_ros_instantiations_exec_body(self, - ros_declarations: HelperRosDeclarations) -> bool: - """Check if the ros instantiations have been declared in the executable body.""" - assert isinstance(ros_declarations, HelperRosDeclarations), \ - "Error: SCXML transition: invalid ROS declarations container." - if self._body is None: - return True - for entry in self._body: - if not entry.check_valid_ros_instantiations(ros_declarations): - return False - return True - - def as_plain_scxml(self, ros_declarations: HelperRosDeclarations) -> "ScxmlTransition": - assert isinstance(ros_declarations, HelperRosDeclarations), \ + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> "ScxmlTransition": + assert isinstance(ros_declarations, ScxmlRosDeclarationsContainer), \ "Error: SCXML transition: invalid ROS declarations container." + assert self.check_valid_ros_instantiations(ros_declarations), \ + "Error: SCXML transition: invalid ROS instantiations in transition body." new_body = None if self._body is not None: new_body = [entry.as_plain_scxml(ros_declarations) for entry in self._body] diff --git a/scxml_converter/src/scxml_converter/scxml_entries/utils.py b/scxml_converter/src/scxml_converter/scxml_entries/utils.py index 0fbf52d5..a4e8f8cb 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/utils.py @@ -15,41 +15,156 @@ """Collection of various utilities for scxml entries.""" -from typing import Dict +from typing import Dict, List, Tuple, Optional +from scxml_converter.scxml_entries.scxml_ros_field import RosField -def is_topic_type_known(topic_definition: str) -> bool: - """Check if python can import the provided topic definition.""" - # Check the input type has the expected structure - if not (isinstance(topic_definition, str) and topic_definition.count("/") == 1): +MSG_TYPE_SUBSTITUTIONS = { + "boolean": "bool", +} + +BASIC_FIELD_TYPES = ['boolean', + 'int8', 'int16', 'int32', 'int64', + 'float', 'double'] + +# TODO: add lower and upper bounds depending on the n. of bits used. +# TODO: add support to uint +SCXML_DATA_STR_TO_TYPE = { + "bool": bool, + "float32": float, + "float64": float, + "int8": int, + "int16": int, + "int32": int, + "int64": int +} + + +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 - topic_ns, topic_type = topic_definition.split("/") - if len(topic_ns) == 0 or len(topic_type) == 0: + 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: - msg_importer = __import__(topic_ns + '.msg', fromlist=['']) - _ = getattr(msg_importer, topic_type) + 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 {topic_definition} not found.") + print(f"Error: SCXML ROS declarations: topic type {type_definition} not found.") return False return True -def replace_ros_msg_expression(msg_expr: str) -> str: - """Convert a ROS message expression (referring to ROS msg entries) to plain SCXML.""" - prefix = "_event." if msg_expr.startswith("_msg.") else "" - return prefix + msg_expr.removeprefix("_msg.") +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), \ + "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 + + +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 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]()) -class HelperRosDeclarations: + +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): + 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." @@ -64,6 +179,20 @@ def append_subscriber(self, topic_name: str, topic_type: str) -> None: 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, \ @@ -83,3 +212,55 @@ def is_timer_defined(self, timer_name: str) -> bool: 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, 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 field in fields: + if field.get_name() not in req_fields: + print("Error: SCXML ROS declarations: " + f"unknown field {field.get_name()} in service request.") + return False + req_fields.pop(field.get_name()) + if len(req_fields) > 0: + print("Error: SCXML ROS declarations: missing fields in service request.") + for field in req_fields.keys(): + print(f"\t-{field}.") + return False + return True + + def check_valid_srv_res_fields(self, service_name: str, 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 field in fields: + if field.get_name() not in res_fields: + print("Error: SCXML ROS declarations: " + f"unknown field {field.get_name()} in service response.") + return False + res_fields.pop(field.get_name()) + if len(res_fields) > 0: + print("Error: SCXML ROS declarations: missing fields in service response.") + for field in res_fields.keys(): + print(f"\t-{field}.") + return False + return True diff --git a/scxml_converter/test/test_systemtest_scxml_entries.py b/scxml_converter/test/test_systemtest_scxml_entries.py index 82c25605..99a0a792 100644 --- a/scxml_converter/test/test_systemtest_scxml_entries.py +++ b/scxml_converter/test/test_systemtest_scxml_entries.py @@ -14,7 +14,6 @@ # limitations under the License. import os -from xml.etree import ElementTree as ET from scxml_converter.scxml_entries import (ScxmlAssign, ScxmlData, ScxmlDataModel, ScxmlParam, ScxmlRoot, ScxmlSend, ScxmlState, ScxmlTransition, @@ -64,7 +63,7 @@ def test_battery_drainer_from_code(): 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 = ET.tostring(battery_drainer_scxml.as_xml(), encoding='unicode') + 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 @@ -112,8 +111,7 @@ 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", None, - [ScxmlAssign("battery_percent", "100")])) + RosTopicCallback(ros_topic_sub, "use_battery", [ScxmlAssign("battery_percent", "100")])) battery_drainer_scxml.add_state(use_battery_state, initial=True) # Check output xml @@ -122,7 +120,7 @@ def test_battery_drainer_ros_from_code(): 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 = ET.tostring(battery_drainer_scxml.as_xml(), encoding='unicode') + 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 @@ -134,7 +132,7 @@ 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 = ET.tostring(scxml_root.as_xml(), encoding='unicode') + 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())) diff --git a/scxml_converter/test/test_systemtest_xml.py b/scxml_converter/test/test_systemtest_xml.py index f4748ae2..d8bf3064 100644 --- a/scxml_converter/test/test_systemtest_xml.py +++ b/scxml_converter/test/test_systemtest_xml.py @@ -17,7 +17,7 @@ from test_utils import canonicalize_xml, remove_empty_lines from scxml_converter.bt_converter import bt_converter -from scxml_converter.scxml_converter import ros_to_scxml_converter +from scxml_converter.scxml_entries import ScxmlRoot def get_output_folder(): @@ -45,12 +45,11 @@ def test_ros_scxml_to_plain_scxml(): output_file = os.path.join(os.path.dirname(__file__), '_test_data', 'expected_output_ros_to_scxml', fname) try: - with open(input_file, 'r', encoding='utf-8') as f_i: - input_data = f_i.read() - scxml, _ = ros_to_scxml_converter(input_data) + 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() - assert remove_empty_lines(canonicalize_xml(scxml)) == \ + assert remove_empty_lines(canonicalize_xml(scxml_str)) == \ remove_empty_lines(canonicalize_xml(expected_output)) except Exception as e: clear_output_folder()