action_call: add parameter to succeed on goal acceptance (#178)

docs/libraries.rst

@@ -653,6 +653,10 @@ Use nav2 to navigate through poses.
      - ``string``
      - ``navigate_through_poses``
      - Action name
+   * - ``success_on_acceptance``
+     - ``bool``
+     - ``false``
+     -  succeed on goal acceptance
 
 ``differential_drive_robot.nav_to_pose()``
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -680,7 +684,10 @@ Use nav2 to navigate to goal pose.
      - ``string``
      - ``navigate_to_pose``
      - Action name
-
+   * - ``success_on_acceptance``
+     - ``bool``
+     - ``false``
+     -  succeed on goal acceptance
 
 OS
 --
@@ -759,6 +766,10 @@ Call a ROS action and wait for the result.
      - ``string``
      - 
      - Call content (e.g. ``{\"order\": 3}``)
+   * - ``success_on_acceptance``
+     - ``bool``
+     - ``false``
+     -  succeed on goal acceptance
 
 ``assert_lifecycle_state()``
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^

libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py

@@ -25,12 +25,12 @@ class NavThroughPoses(RosActionCall):
     Class to navigate through poses
     """
 
-    def __init__(self, associated_actor, action_topic: str, namespace_override: str):
+    def __init__(self, associated_actor, action_topic: str, namespace_override: str, success_on_acceptance: bool):
         self.namespace = associated_actor["namespace"]
         if namespace_override:
             self.namespace = namespace_override
         self.goal_poses = None
-        super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses")
+        super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", success_on_acceptance=success_on_acceptance)
 
     def execute(self, associated_actor, goal_poses: list) -> None:  # pylint: disable=arguments-differ,arguments-renamed
         self.goal_poses = goal_poses

libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py

@@ -26,12 +26,12 @@ class NavToPose(RosActionCall):
     Class to navigate to a pose
     """
 
-    def __init__(self, associated_actor, action_topic: str, namespace_override: str) -> None:
+    def __init__(self, associated_actor, action_topic: str, namespace_override: str, success_on_acceptance: bool) -> None:
         self.namespace = associated_actor["namespace"]
         if namespace_override:
             self.namespace = namespace_override
         self.goal_pose = None
-        super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose")
+        super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", success_on_acceptance=success_on_acceptance)
 
     def execute(self, associated_actor, goal_pose: list) -> None:  # pylint: disable=arguments-differ,arguments-renamed
         self.goal_pose = goal_pose

libs/scenario_execution_nav2/scenario_execution_nav2/lib_osc/nav2.osc

@@ -14,9 +14,11 @@ action differential_drive_robot.nav_through_poses:
     goal_poses: list of pose_3d          # goal poses to navigate through
     namespace_override: string = ''      # if set, it's used as namespace (instead of the associated actor's name)
     action_topic: string = 'navigate_through_poses' # Name of action
+    success_on_acceptance: bool = false  # succeed on goal acceptance
 
 action differential_drive_robot.nav_to_pose:
     # Nav2 to navigate to goal pose.
     goal_pose: pose_3d                   # goal pose to navigate to
     namespace_override: string = ''      # if set, it's used as namespace (instead of the associated actor's name)
-    action_topic: string = 'navigate_to_pose' # Name of action
+    action_topic: string = 'navigate_to_pose' # Name of action
+    success_on_acceptance: bool = false  # succeed on goal acceptance

scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py

@@ -34,16 +34,17 @@ class ActionCallActionState(Enum):
     IDLE = 1
     ACTION_SERVER_AVAILABLE = 2
     ACTION_CALLED = 3
-    DONE = 4
-    ERROR = 5
+    ACTION_ACCEPTED = 4
+    DONE = 5
+    ERROR = 6
 
 
 class RosActionCall(BaseAction):
     """
     ros service call behavior
     """
 
-    def __init__(self, action_name: str, action_type: str, transient_local: bool = False):
+    def __init__(self, action_name: str, action_type: str, success_on_acceptance: bool = False, transient_local: bool = False):
         super().__init__()
         self.node = None
         self.client = None
@@ -56,6 +57,7 @@ def __init__(self, action_name: str, action_type: str, transient_local: bool = F
         self.data = None
         self.current_state = ActionCallActionState.IDLE
         self.cb_group = ReentrantCallbackGroup()
+        self.success_on_acceptance = success_on_acceptance
         self.transient_local = transient_local
 
     def setup(self, **kwargs):
@@ -120,6 +122,10 @@ def update(self) -> py_trees.common.Status:
             result = py_trees.common.Status.RUNNING
         elif self.current_state == ActionCallActionState.ACTION_CALLED:
             result = py_trees.common.Status.RUNNING
+        elif self.current_state == ActionCallActionState.ACTION_ACCEPTED:
+            if self.success_on_acceptance:
+                return py_trees.common.Status.SUCCESS
+            result = py_trees.common.Status.RUNNING
         elif self.current_state == ActionCallActionState.DONE:
             result = py_trees.common.Status.SUCCESS
         else:
@@ -144,6 +150,7 @@ def goal_response_callback(self, future):
             self.feedback_message = f"Goal rejected."  # pylint: disable= attribute-defined-outside-init
             self.current_state = ActionCallActionState.ERROR
             return
+        self.current_state = ActionCallActionState.ACTION_ACCEPTED
         self.feedback_message = f"Goal accepted."  # pylint: disable= attribute-defined-outside-init
         get_result_future = self.goal_handle.get_result_async()
         get_result_future.add_done_callback(self.get_result_callback)
@@ -154,7 +161,7 @@ def get_result_callback(self, future):
         """
         status = future.result().status
         self.logger.debug(f"Received state {status}")
-        if self.current_state == ActionCallActionState.ACTION_CALLED:
+        if self.current_state == ActionCallActionState.ACTION_ACCEPTED:
             if status == GoalStatus.STATUS_SUCCEEDED:
                 self.current_state = ActionCallActionState.DONE
                 self.goal_handle = None
@@ -167,7 +174,8 @@ def get_result_callback(self, future):
                 self.feedback_message = f"Goal aborted."   # pylint: disable= attribute-defined-outside-init
                 self.goal_handle = None
         else:
-            self.current_state = ActionCallActionState.ERROR
+            if not self.success_on_acceptance:
+                self.current_state = ActionCallActionState.ERROR
 
     def shutdown(self):
         if self.goal_handle:
@@ -177,7 +185,7 @@ def get_feedback_message(self, current_state):
         feedback_message = None
         if current_state == ActionCallActionState.IDLE:
             feedback_message = f"Waiting for action server {self.action_name}"
-        elif current_state == ActionCallActionState.ACTION_CALLED:
+        elif current_state == ActionCallActionState.ACTION_ACCEPTED:
             if self.received_feedback is not None:
                 feedback_message = f"Current: {self.received_feedback}"
             else:

scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc

@@ -36,6 +36,7 @@ action action_call:
     action_name: string                  # name of the action to connect to
     action_type: string                  # class of the message type
     data: string                         # call content
+    success_on_acceptance: bool = false  # succeed on goal acceptance
     transient_local: bool = false
 
 action assert_lifecycle_state:

scenario_execution_ros/test/test_ros_action_call.py

@@ -17,6 +17,7 @@
 import os
 import unittest
 import rclpy
+import tempfile
 import py_trees
 from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.action.server import GoalEvent
@@ -120,6 +121,26 @@ def test_success(self):
         self.execute(scenario_content)
         self.assertTrue(self.scenario_execution_ros.process_results())
 
+    def test_success_success_on_acceptance(self):
+        self.tmp_file1 = tempfile.NamedTemporaryFile()
+        self.tmp_file2 = tempfile.NamedTemporaryFile()
+        scenario_content = """
+import osc.helpers
+import osc.ros
+
+scenario test:
+    timeout(10s)
+    do serial:
+        wait elapsed(1s)
+        run_process('touch """ + self.tmp_file1.name + """')
+        action_call(action_name: "/test_action", action_type: "example_interfaces.action.Fibonacci", data: '{\\"order\\": 3}', success_on_acceptance: true)
+        run_process('touch """ + self.tmp_file2.name + """')
+"""
+        self.execute(scenario_content)
+        diff = os.path.getmtime(self.tmp_file2.name) - os.path.getmtime(self.tmp_file1.name)
+        self.assertLessEqual(diff, 2)
+        self.assertTrue(self.scenario_execution_ros.process_results())
+
     def test_goal_not_accepted(self):
         scenario_content = """
 import osc.helpers