Introduce canceled and aborted actions

Signed-off-by: Marco Lampacrescia <[email protected]>
convince-project · Sep 9, 2024 · 8c7cc10 · 8c7cc10
1 parent ae39582
commit 8c7cc10
Show file tree

Hide file tree

Showing 14 changed files with 109 additions and 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
@@ -75,21 +75,27 @@ 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.
 
         :param client_to_goal_id: List of tuples (client_id, goal_id) relating clients to goal ids.
         :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
@@ -31,12 +31,12 @@
                 <field name="data" expr="counter" />
             </ros_topic_publish>
         </ros_action_handle_feedback>
-        <ros_action_handle_result name="example_action" target="done">
+        <ros_action_handle_success_result name="example_action" target="done">
             <assign location="fibonacci_result" expr="_wrapped_result.result.sequence" />
             <ros_topic_publish name="result_pub">
                 <field name="data" expr="fibonacci_result[5]" />
             </ros_topic_publish>
-        </ros_action_handle_result>
+        </ros_action_handle_success_result>
     </state>
 
     <state id="done" />

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
@@ -31,12 +31,12 @@
                 <field name="data" expr="counter" />
             </ros_topic_publish>
         </ros_action_handle_feedback>
-        <ros_action_handle_result name="example_action" target="done">
+        <ros_action_handle_success_result name="example_action" target="done">
             <assign location="fibonacci_result" expr="_wrapped_result.result.sequence" />
             <ros_topic_publish name="res_topic">
                 <field name="data" expr="fibonacci_result[7]" />
             </ros_topic_publish>
-        </ros_action_handle_result>
+        </ros_action_handle_success_result>
     </state>
 
     <state id="done" />

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
@@ -31,12 +31,12 @@
                 <field name="data" expr="counter" />
             </ros_topic_publish>
         </ros_action_handle_feedback>
-        <ros_action_handle_result name="example_action" target="done">
+        <ros_action_handle_success_result name="example_action" target="done">
             <assign location="fibonacci_result" expr="_wrapped_result.result.sequence" />
             <ros_topic_publish name="result_pub">
                 <field name="data" expr="fibonacci_result[5]" />
             </ros_topic_publish>
-        </ros_action_handle_result>
+        </ros_action_handle_success_result>
     </state>
 
     <state id="done" />

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -31,12 +31,12 @@
                 <field name="data" expr="counter" />
             </ros_topic_publish>
         </ros_action_handle_feedback>
-        <ros_action_handle_result name="example_action" target="done">
+        <ros_action_handle_success_result name="example_action" target="done">
             <assign location="fibonacci_result" expr="_wrapped_result.result.sequence" />
             <ros_topic_publish name="result_pub">
                 <field name="data" expr="fibonacci_result[4]" />
             </ros_topic_publish>
-        </ros_action_handle_result>
+        </ros_action_handle_success_result>
     </state>
 
     <state id="done" />

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
@@ -31,12 +31,12 @@
                 <field name="data" expr="counter" />
             </ros_topic_publish>
         </ros_action_handle_feedback>
-        <ros_action_handle_result name="example_action" target="done">
+        <ros_action_handle_success_result name="example_action" target="done">
             <assign location="fibonacci_result" expr="_wrapped_result.result.sequence" />
             <ros_topic_publish name="res_topic">
                 <field name="data" expr="fibonacci_result[6]" />
             </ros_topic_publish>
-        </ros_action_handle_result>
+        </ros_action_handle_success_result>
     </state>
 
     <state id="done" />

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
@@ -20,7 +20,7 @@
                 <param name="ros_fields__data" expr="counter" />
             </send>
         </transition>
-        <transition target="done" event="action_fibonacci_result_handle_client_client_1">
+        <transition target="done" cond="_event.code == 4" event="action_fibonacci_result_handle_client_client_1">
             <assign location="fibonacci_result" expr="_event.ros_fields__sequence" />
             <send event="topic_client_1_last_fibonacci_msg">
                 <param name="ros_fields__data" expr="fibonacci_result[4]" />

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
@@ -20,7 +20,7 @@
                 <param name="ros_fields__data" expr="counter" />
             </send>
         </transition>
-        <transition target="done" event="action_fibonacci_result_handle_client_client_2">
+        <transition target="done" cond="_event.code == 4" event="action_fibonacci_result_handle_client_client_2">
             <assign location="fibonacci_result" expr="_event.ros_fields__sequence" />
             <send event="topic_client_2_last_fibonacci_msg">
                 <param name="ros_fields__data" expr="fibonacci_result[6]" />

diff --git a/...onverter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml b/...onverter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_0.scxml
@@ -32,6 +32,7 @@
             <send event="action_fibonacci_result">
                 <param name="ros_fields__sequence" expr="sequence" />
                 <param name="goal_id" expr="goal_id" />
+                <param expr="4" name="code" />
             </send>
         </transition>
         <transition target="execute">

diff --git a/...onverter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml b/...onverter/test/_test_data/fibonacci_action_example/gt_plain_scxml/fibonacci_thread_1.scxml
@@ -32,6 +32,7 @@
             <send event="action_fibonacci_result">
                 <param name="ros_fields__sequence" expr="sequence" />
                 <param name="goal_id" expr="goal_id" />
+                <param expr="4" name="code" />
             </send>
         </transition>
         <transition target="execute">