diff --git a/docs/libraries.rst b/docs/libraries.rst index 82d4655c..0d51f8e1 100644 --- a/docs/libraries.rst +++ b/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()`` ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py index 64989c30..34aa4a9b 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_through_poses.py +++ b/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 diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py b/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py index c79f9cf8..1e28a311 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/actions/nav_to_pose.py +++ b/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 diff --git a/libs/scenario_execution_nav2/scenario_execution_nav2/lib_osc/nav2.osc b/libs/scenario_execution_nav2/scenario_execution_nav2/lib_osc/nav2.osc index e7bcb4f2..439c9e27 100644 --- a/libs/scenario_execution_nav2/scenario_execution_nav2/lib_osc/nav2.osc +++ b/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 \ No newline at end of file + action_topic: string = 'navigate_to_pose' # Name of action + success_on_acceptance: bool = false # succeed on goal acceptance \ No newline at end of file diff --git a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py index 7e3965a6..43161f6d 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py @@ -34,8 +34,9 @@ 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): @@ -43,7 +44,7 @@ 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: diff --git a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc index 18ae923d..b1b4b12b 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/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: diff --git a/scenario_execution_ros/test/test_ros_action_call.py b/scenario_execution_ros/test/test_ros_action_call.py index d20b18d6..0183fa62 100644 --- a/scenario_execution_ros/test/test_ros_action_call.py +++ b/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