From 8c7cc10199b73e71ee9ea0075942d95de349b688 Mon Sep 17 00:00:00 2001 From: Marco Lampacrescia Date: Mon, 9 Sep 2024 16:51:29 +0200 Subject: [PATCH] Introduce canceled and aborted actions Signed-off-by: Marco Lampacrescia --- .../ros_helpers/ros_action_handler.py | 27 ++++--- .../fibonacci_action_example/client_1.scxml | 4 +- .../fibonacci_action_example/client_2.scxml | 4 +- .../client_1.scxml | 4 +- .../scxml_converter/scxml_entries/__init__.py | 3 +- .../scxml_entries/ros_utils.py | 15 ---- .../scxml_entries/scxml_ros_action_client.py | 72 ++++++++++++++++++- .../scxml_entries/scxml_ros_action_server.py | 12 ++-- .../fibonacci_action_example/client_1.scxml | 4 +- .../fibonacci_action_example/client_2.scxml | 4 +- .../gt_plain_scxml/client_1.scxml | 2 +- .../gt_plain_scxml/client_2.scxml | 2 +- .../gt_plain_scxml/fibonacci_thread_0.scxml | 1 + .../gt_plain_scxml/fibonacci_thread_1.scxml | 1 + 14 files changed, 109 insertions(+), 46 deletions(-) diff --git a/jani_generator/src/jani_generator/ros_helpers/ros_action_handler.py b/jani_generator/src/jani_generator/ros_helpers/ros_action_handler.py index 5536d890..94090483 100644 --- a/jani_generator/src/jani_generator/ros_helpers/ros_action_handler.py +++ b/jani_generator/src/jani_generator/ros_helpers/ros_action_handler.py @@ -75,7 +75,8 @@ def _generate_goal_request_transition( def _generate_srv_event_transition( self, goal_state: ScxmlState, client_to_goal_id: List[Tuple[str, int]], event_fields: Dict[str, str], srv_event_function: Callable[[str], str], - client_event_function: Callable[[str, str], str]) -> ScxmlTransition: + client_event_function: Callable[[str, str], str], + additional_data: List[str]) -> ScxmlTransition: """ Generate a scxml transition that triggers the client related to the input event's goal_id. @@ -83,13 +84,18 @@ def _generate_srv_event_transition( :param event_fields: Dictionary of the parameters of the event. :param srv_event_function: Function to generate the server (input) event name. :param client_event_function: Function to generate the client (output) event name. + :param additional_fields: List of additional fields to be added to the event. """ goal_id_name = get_action_goal_id_definition()[0] + extra_entries = additional_data + [goal_id_name] srv_event_name = srv_event_function(self._interface_name) scxml_transition = ScxmlTransition(goal_state.get_id(), [srv_event_name]) - scxml_transition.append_body_executable_entry( - ScxmlAssign(goal_id_name, PLAIN_SCXML_EVENT_PREFIX + goal_id_name)) + for entry_name in extra_entries: + scxml_transition.append_body_executable_entry( + ScxmlAssign(entry_name, PLAIN_SCXML_EVENT_PREFIX + entry_name)) out_params: List[ScxmlParam] = [] + for entry_name in additional_data: + out_params.append(ScxmlParam(entry_name, expr=entry_name)) for field_name in event_fields: field_w_pref = ROS_FIELD_PREFIX + field_name scxml_transition.append_body_executable_entry( @@ -113,7 +119,7 @@ def _generate_goal_accept_transition( """ return self._generate_srv_event_transition( goal_state, client_to_goal_id, {}, generate_action_goal_accepted_event, - generate_action_goal_handle_accepted_event) + generate_action_goal_handle_accepted_event, []) def _generate_goal_reject_transition( self, goal_state: ScxmlState, client_to_goal_id: List[Tuple[str, int]] @@ -125,7 +131,7 @@ def _generate_goal_reject_transition( """ return self._generate_srv_event_transition( goal_state, client_to_goal_id, {}, generate_action_goal_rejected_event, - generate_action_goal_handle_rejected_event) + generate_action_goal_handle_rejected_event, []) def _generate_feedback_response_transition( self, goal_state: ScxmlState, client_to_goal_id: List[Tuple[str, int]], @@ -138,7 +144,7 @@ def _generate_feedback_response_transition( """ return self._generate_srv_event_transition( goal_state, client_to_goal_id, feedback_params, generate_action_feedback_event, - generate_action_feedback_handle_event) + generate_action_feedback_handle_event, []) def _generate_result_response_transition( self, goal_state: ScxmlState, client_to_goal_id: List[Tuple[str, int]], @@ -151,7 +157,7 @@ def _generate_result_response_transition( """ return self._generate_srv_event_transition( goal_state, client_to_goal_id, result_params, generate_action_result_event, - generate_action_result_handle_event) + generate_action_result_handle_event, ["code"]) def to_scxml(self) -> ScxmlRoot: """ @@ -173,9 +179,10 @@ def to_scxml(self) -> ScxmlRoot: # Hack: Using support variables in the data model to avoid having _event in send params goal_id_def = get_action_goal_id_definition() - req_fields_as_data = self._generate_datamodel_from_ros_fields( + action_fields_as_data = self._generate_datamodel_from_ros_fields( goal_params | feedback_params | result_params) - req_fields_as_data.append(ScxmlData(goal_id_def[0], "0", goal_id_def[1])) + action_fields_as_data.append(ScxmlData(goal_id_def[0], "0", goal_id_def[1])) + action_fields_as_data.append(ScxmlData("code", "0", "int32")) # Make sure the service name has no slashes and spaces scxml_root_name = \ self.get_interface_prefix() + sanitize_ros_interface_name(self._interface_name) @@ -194,7 +201,7 @@ def to_scxml(self) -> ScxmlRoot: wait_state.add_transition(self._generate_result_response_transition( wait_state, client_to_goal_id, result_params)) scxml_root = ScxmlRoot(scxml_root_name) - scxml_root.set_data_model(ScxmlDataModel(req_fields_as_data)) + scxml_root.set_data_model(ScxmlDataModel(action_fields_as_data)) scxml_root.add_state(wait_state, initial=True) scxml_root.add_state(goal_requested_state) assert scxml_root.is_plain_scxml(), "Generated SCXML for srv sync is not plain SCXML." diff --git a/jani_generator/test/_test_data/fibonacci_action_example/client_1.scxml b/jani_generator/test/_test_data/fibonacci_action_example/client_1.scxml index 54654a01..3e57c807 100644 --- a/jani_generator/test/_test_data/fibonacci_action_example/client_1.scxml +++ b/jani_generator/test/_test_data/fibonacci_action_example/client_1.scxml @@ -31,12 +31,12 @@ - + - + diff --git a/jani_generator/test/_test_data/fibonacci_action_example/client_2.scxml b/jani_generator/test/_test_data/fibonacci_action_example/client_2.scxml index 0356883f..31fa5280 100644 --- a/jani_generator/test/_test_data/fibonacci_action_example/client_2.scxml +++ b/jani_generator/test/_test_data/fibonacci_action_example/client_2.scxml @@ -31,12 +31,12 @@ - + - + diff --git a/jani_generator/test/_test_data/fibonacci_action_single_thread/client_1.scxml b/jani_generator/test/_test_data/fibonacci_action_single_thread/client_1.scxml index 54654a01..3e57c807 100644 --- a/jani_generator/test/_test_data/fibonacci_action_single_thread/client_1.scxml +++ b/jani_generator/test/_test_data/fibonacci_action_single_thread/client_1.scxml @@ -31,12 +31,12 @@ - + - + diff --git a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py index d2efa66f..4981bbf2 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/__init__.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/__init__.py @@ -26,7 +26,8 @@ RosServiceHandleResponse, RosServiceSendRequest, RosServiceSendResponse) # noqa: F401 from .scxml_ros_action_client import ( # noqa: F401 RosActionClient, RosActionSendGoal, RosActionHandleGoalResponse, # noqa: F401 - RosActionHandleFeedback, RosActionHandleResult) # noqa: F401 + RosActionHandleFeedback, RosActionHandleSuccessResult, # noqa: F401 + RosActionHandleCanceledResult, RosActionHandleAbortedResult) # noqa: F401 from .scxml_ros_action_server import ( # noqa: F401 RosActionServer, RosActionHandleGoalRequest, RosActionAcceptGoal, # noqa: F401 RosActionRejectGoal, RosActionStartThread, RosActionSendFeedback, # noqa: F401 diff --git a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py index 9e36bb79..e58e96e3 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/ros_utils.py @@ -499,11 +499,6 @@ def check_valid_action_goal_fields( f"Error: SCXML ROS declarations: unknown action {alias_name}." action_type = self.get_action_server_info(alias_name)[1] goal_fields = get_action_type_params(action_type)[0] - # We use the goal ID as a reserved field for the action. Make sure it is available. - goal_id_name, goal_id_type = get_action_goal_id_definition() - assert goal_id_name not in goal_fields, \ - "Error: SCXML ROS declarations: "\ - f"found reserved '{goal_id_name}' field in action {action_type} goal." if not check_all_fields_known(ros_fields, goal_fields): print(f"Error: SCXML ROS declarations: Action goal {alias_name} has invalid fields.") return False @@ -519,11 +514,6 @@ def check_valid_action_feedback_fields( """ _, action_type = self.get_action_server_info(server_name) _, feedback_fields, _ = get_action_type_params(action_type) - # We use the goal ID as a reserved field for the action. Make sure it is available. - goal_id_name, goal_id_type = get_action_goal_id_definition() - assert goal_id_name not in feedback_fields, \ - "Error: SCXML ROS declarations: "\ - f"found reserved '{goal_id_name}' field in action {action_type} feedback." if not check_all_fields_known(ros_fields, feedback_fields): print(f"Error: SCXML ROS declarations: Action feedback {server_name} " "has invalid fields.") @@ -540,11 +530,6 @@ def check_valid_action_result_fields( """ _, action_type = self.get_action_server_info(server_name) _, _, result_fields = get_action_type_params(action_type) - # We use the goal ID as a reserved field for the action. Make sure it is available. - goal_id_name, goal_id_type = get_action_goal_id_definition() - assert goal_id_name not in result_fields, \ - "Error: SCXML ROS declarations: "\ - f"found reserved '{goal_id_name}' field in action {action_type} result." if not check_all_fields_known(ros_fields, result_fields): print(f"Error: SCXML ROS declarations: Action result {server_name} has invalid fields.") return False diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py index 9eac452d..1fd034b2 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_client.py @@ -32,6 +32,8 @@ from scxml_converter.scxml_entries.xml_utils import assert_xml_tag_ok, get_xml_argument from scxml_converter.scxml_entries.utils import CallbackType, is_non_empty_string +from action_msgs.msg import GoalStatus + class RosActionClient(RosDeclaration): """Object used in SCXML root to declare a new action client.""" @@ -177,12 +179,72 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) ros_declarations.get_automaton_name()) -class RosActionHandleResult(RosCallback): +class RosActionHandleSuccessResult(RosCallback): + """SCXML object representing the handler of am action result for a service client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_success_result" + + @staticmethod + def get_declaration_type() -> Type[RosActionClient]: + return RosActionClient + + @staticmethod + def get_callback_type() -> CallbackType: + return CallbackType.ROS_ACTION_RESULT + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_client_defined(self._interface_name) + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_result_handle_event( + ros_declarations.get_action_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self._condition is None, \ + "Error: SCXML RosActionHandleSuccessResult: condition not supported." + self._condition = f"_wrapped_result.code == {GoalStatus.STATUS_SUCCEEDED}" + return super().as_plain_scxml(ros_declarations) + + +class RosActionHandleCanceledResult(RosCallback): + """SCXML object representing the handler of am action result for a service client.""" + + @staticmethod + def get_tag_name() -> str: + return "ros_action_handle_canceled_result" + + @staticmethod + def get_declaration_type() -> Type[RosActionClient]: + return RosActionClient + + @staticmethod + def get_callback_type() -> CallbackType: + return CallbackType.ROS_ACTION_RESULT + + def check_interface_defined(self, ros_declarations: ScxmlRosDeclarationsContainer) -> bool: + return ros_declarations.is_action_client_defined(self._interface_name) + + def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) -> str: + return generate_action_result_handle_event( + ros_declarations.get_action_client_info(self._interface_name)[0], + ros_declarations.get_automaton_name()) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self._condition is None, \ + "Error: SCXML RosActionHandleSuccessResult: condition not supported." + self._condition = f"_wrapped_result.code == {GoalStatus.STATUS_CANCELED}" + return super().as_plain_scxml(ros_declarations) + + +class RosActionHandleAbortedResult(RosCallback): """SCXML object representing the handler of am action result for a service client.""" @staticmethod def get_tag_name() -> str: - return "ros_action_handle_result" + return "ros_action_handle_aborted_result" @staticmethod def get_declaration_type() -> Type[RosActionClient]: @@ -199,3 +261,9 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) return generate_action_result_handle_event( ros_declarations.get_action_client_info(self._interface_name)[0], ros_declarations.get_automaton_name()) + + def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlTransition: + assert self._condition is None, \ + "Error: SCXML RosActionHandleSuccessResult: condition not supported." + self._condition = f"_wrapped_result.code == {GoalStatus.STATUS_ABORTED}" + return super().as_plain_scxml(ros_declarations) diff --git a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py index dc1ef682..c6316708 100644 --- a/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py +++ b/scxml_converter/src/scxml_converter/scxml_entries/scxml_ros_action_server.py @@ -236,8 +236,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: plain_send = super().as_plain_scxml(ros_declarations) - plain_send.append_param(ScxmlParam("status", expr=f"{GoalStatus.STATUS_SUCCEEDED}")) - return super().as_plain_scxml(ros_declarations) + plain_send.append_param(ScxmlParam("code", expr=f"{GoalStatus.STATUS_SUCCEEDED}")) + return plain_send class RosActionSendCanceledResult(RosTrigger): @@ -268,8 +268,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: plain_send = super().as_plain_scxml(ros_declarations) - plain_send.append_param(ScxmlParam("status", expr=f"{GoalStatus.STATUS_CANCELED}")) - return super().as_plain_scxml(ros_declarations) + plain_send.append_param(ScxmlParam("code", expr=f"{GoalStatus.STATUS_CANCELED}")) + return plain_send class RosActionSendAbortedResult(RosTrigger): @@ -300,8 +300,8 @@ def get_plain_scxml_event(self, ros_declarations: ScxmlRosDeclarationsContainer) def as_plain_scxml(self, ros_declarations: ScxmlRosDeclarationsContainer) -> ScxmlSend: plain_send = super().as_plain_scxml(ros_declarations) - plain_send.append_param(ScxmlParam("status", expr=f"{GoalStatus.STATUS_ABORTED}")) - return super().as_plain_scxml(ros_declarations) + plain_send.append_param(ScxmlParam("code", expr=f"{GoalStatus.STATUS_ABORTED}")) + return plain_send class RosActionHandleThreadFree(RosCallback): diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml index cca4d325..6880a6f5 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/client_1.scxml @@ -31,12 +31,12 @@ - + - + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml index 4d2e1e80..021151a7 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/client_2.scxml @@ -31,12 +31,12 @@ - + - + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml index 95c8b400..9f926b37 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_1.scxml @@ -20,7 +20,7 @@ - + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml index feb518e9..d8fdff06 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/client_2.scxml @@ -20,7 +20,7 @@ - + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml index 7b2e39f8..5eb8c9ab 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml @@ -32,6 +32,7 @@ + diff --git a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml index 67362059..f4351be3 100644 --- a/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml +++ b/scxml_converter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml @@ -32,6 +32,7 @@ +