From 26c44520a345f1c1f343db282e2eaa3c4f7c9377 Mon Sep 17 00:00:00 2001 From: "Pasch, Frederik" Date: Mon, 5 Aug 2024 08:22:04 +0200 Subject: [PATCH] add test package --- .github/workflows/test_build.yml | 52 ++++++- docs/libraries.rst | 12 +- .../actions/nav_through_poses.py | 132 ++++-------------- .../actions/nav_to_pose.py | 17 ++- .../scenario_execution_nav2/lib_osc/nav2.osc | 5 +- .../actions/ros_action_call.py | 28 ++-- test/scenario_execution_nav2_test/README.md | 3 + test/scenario_execution_nav2_test/package.xml | 24 ++++ .../resource/scenario_execution_nav2_test | 0 .../test_scenario_execution_nav2.osc | 15 ++ test/scenario_execution_nav2_test/setup.cfg | 4 + test/scenario_execution_nav2_test/setup.py | 42 ++++++ 12 files changed, 206 insertions(+), 128 deletions(-) create mode 100644 test/scenario_execution_nav2_test/README.md create mode 100644 test/scenario_execution_nav2_test/package.xml create mode 100644 test/scenario_execution_nav2_test/resource/scenario_execution_nav2_test create mode 100644 test/scenario_execution_nav2_test/scenarios/test_scenario_execution_nav2.osc create mode 100644 test/scenario_execution_nav2_test/setup.cfg create mode 100644 test/scenario_execution_nav2_test/setup.py diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index d508f195..bc3f6fb0 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -351,7 +351,7 @@ jobs: export -n CYCLONEDDS_URI export ROS_DOMAIN_ID=2 # shellcheck disable=SC1083 - scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} + scenario_batch_execution -i examples/example_external_method/scenarios -o test_example_external_method -- ros2 launch scenario_execution_ros scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True - name: Upload result uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 if: always() @@ -384,13 +384,59 @@ jobs: export ROS_DOMAIN_ID=2 export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} # shellcheck disable=SC1083 - scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} + scenario_batch_execution -i test/scenario_execution_gazebo_test/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True - name: Upload result uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 if: always() with: name: test-scenario-execution-gazebo path: test_scenario_execution_gazebo/test.xml + test-scenario-execution-nav2: + needs: [build] + runs-on: intellabs-01 + timeout-minutes: 30 + container: + image: ghcr.io/intellabs/scenario-execution:${{ github.event.pull_request.base.ref }} + credentials: + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + steps: + - name: Restore cache + uses: actions/cache@0c45773b623bea8c8e75f6c82b208c3cf94ea4f9 # v4.0.2 + with: + key: ${{ runner.os }}-build-${{ github.run_number }} + path: . + - name: Test Scenario Execution Nav2 + shell: bash + run: | + source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash + source install/setup.bash + Xvfb :1 -screen 0 800x600x16 & + export DISPLAY=:1.0 + export -n CYCLONEDDS_URI + export ROS_DOMAIN_ID=2 + export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} + # shellcheck disable=SC1083 + scenario_batch_execution -i test/scenario_execution_nav2_test/scenarios/ -o test_scenario_execution_nav2 -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True + - name: Upload result + uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 + if: always() + with: + name: test-scenario-execution-nav2 + path: test_scenario_execution_nav2/test.xml + - name: Convert video + if: always() + shell: bash + run: | + source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash + source /rosbag2_to_video/install/setup.bash + ros2 bag to_video -t /static_camera/image_raw -o test_scenario_execution_nav2/test_scenario_execution_nav2/example_nav2.mp4 test_scenario_execution_nav2/test_scenario_execution_nav2/rosbag2_* --fps 5 --codec mp4v + - name: Upload video + uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 + if: always() + with: + name: test-scenario-execution-nav2-video + path: test_scenario_execution_nav2/test_scenario_execution_nav2/example_nav2.mp4 test-scenario-execution-pybullet: needs: [build] runs-on: intellabs-01 @@ -434,6 +480,7 @@ jobs: - test-example-multirobot - test-example-external-method - test-scenario-execution-gazebo + - test-scenario-execution-nav2 - test-scenario-execution-pybullet runs-on: intellabs-01 if: ${{ always() }} @@ -471,4 +518,5 @@ jobs: downloaded-artifacts/test-example-multirobot-result/test.xml downloaded-artifacts/test-example-external-method-result/test.xml downloaded-artifacts/test-scenario-execution-gazebo/test.xml + downloaded-artifacts/test-scenario-execution-nav2/test.xml downloaded-artifacts/test-scenario-execution-pybullet/test.xml diff --git a/docs/libraries.rst b/docs/libraries.rst index 0b1989e2..9df4723d 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -586,10 +586,10 @@ Use nav2 to navigate through poses. - ``string`` - ``''`` - If set, it's used as namespace (instead of the associated actor's namespace) - * - ``monitor_progress`` - - ``bool`` - - ``true`` - - If yes, the action returns after the goal is reached or on failure. If no, the action returns after request. + * - ``action_topic`` + - ``string`` + - ``navigate_through_poses`` + - Action name ``differential_drive_robot.nav_to_pose()`` """""""""""""""""""""""""""""""""""""""""" @@ -616,10 +616,6 @@ Use nav2 to navigate to goal pose. - ``string`` - ``navigate_to_pose`` - Action name - * - ``monitor_progress`` - - ``bool`` - - ``true`` - - If yes, the action returns after the goal is reached or on failure. If no, the action returns after request. OS 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 2ee5f0f9..e2a14190 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 @@ -14,117 +14,47 @@ # # SPDX-License-Identifier: Apache-2.0 -from enum import Enum - -from rclpy.node import Node from rclpy.duration import Duration - -import py_trees - -from nav2_simple_commander.robot_navigator import TaskResult # pylint: disable=import-error - -from .nav2_common import NamespaceAwareBasicNavigator +from nav2_msgs.action import NavigateThroughPoses from scenario_execution_ros.actions.common import get_pose_stamped -from scenario_execution.actions.base_action import BaseAction - - -class NavThroughPosesState(Enum): - """ - States for executing a nav-through-poses with nav2 - """ - IDLE = 1 - NAV_TO_GOAL = 2 - DONE = 3 +from scenario_execution_ros.actions.ros_action_call import RosActionCall, ActionCallActionState -class NavThroughPoses(BaseAction): +class NavThroughPoses(RosActionCall): """ Class to navigate through poses """ - def __init__(self, associated_actor, goal_poses: list, monitor_progress: bool, namespace_override: str): - super().__init__() + def __init__(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str): self.namespace = associated_actor["namespace"] - self.monitor_progress = monitor_progress - self.node = None - self.future = None - self.current_state = NavThroughPosesState.IDLE - self.nav = None if namespace_override: self.namespace = namespace_override + self.goal_poses = None + super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", "") - if not isinstance(goal_poses, list): - raise TypeError(f'goal_poses needs to be list of position_3d, got {type(goal_poses)}.') - else: - self.goal_poses = goal_poses - - def setup(self, **kwargs): - """ - Setup ROS2 node and service client - - """ - try: - self.node: Node = kwargs['node'] - except KeyError as e: - error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( - self.name, self.__class__.__name__) - raise KeyError(error_message) from e - - self.nav = NamespaceAwareBasicNavigator( - node_name="basic_nav_nav_through_poses", namespace=self.namespace) - - def update(self) -> py_trees.common.Status: - """ - Execute states - """ - self.logger.debug(f"Current State {self.current_state}") - result = py_trees.common.Status.FAILURE - if self.current_state == NavThroughPosesState.IDLE: - self.current_state = NavThroughPosesState.NAV_TO_GOAL - goal_poses = [] - for pose in self.goal_poses: - goal_poses.append(get_pose_stamped(self.nav.get_clock().now().to_msg(), pose)) - self.feedback_message = "Execute navigation." # pylint: disable= attribute-defined-outside-init - go_to_pose_result = self.nav.goThroughPoses(goal_poses) - if go_to_pose_result: - if self.monitor_progress: - result = py_trees.common.Status.RUNNING - self.current_state = NavThroughPosesState.NAV_TO_GOAL - else: - result = py_trees.common.Status.SUCCESS - self.current_state = NavThroughPosesState.DONE - else: - self.current_state = NavThroughPosesState.DONE - result = py_trees.common.Status.FAILURE - elif self.current_state == NavThroughPosesState.NAV_TO_GOAL: - if not self.nav.isTaskComplete(): - feedback = self.nav.getFeedback() - if feedback: - self.feedback_message = 'Estimated time of arrival: ' + '{0:.0f}'.format( # pylint: disable= attribute-defined-outside-init - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.' - - if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600): - self.nav.cancelTask() - result = py_trees.common.Status.RUNNING + def execute(self, associated_actor, goal_poses: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ + self.namespace = associated_actor["namespace"] + if namespace_override: + self.namespace = namespace_override + self.goal_poses = goal_poses + super().execute(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateThroughPoses", "") + + def get_goal_msg(self): + goal_msg = NavigateThroughPoses.Goal() + for pose in self.goal_poses: + goal_msg.poses.append(get_pose_stamped(self.node.get_clock().now().to_msg(), pose)) + return goal_msg + + def get_feedback_message(self, current_state): + feedback_message = super().get_feedback_message(current_state) + + if self.current_state == ActionCallActionState.IDLE: + feedback_message = "Waiting for navigation" + elif self.current_state == ActionCallActionState.ACTION_CALLED: + if self.received_feedback: + feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}, poses left {self.received_feedback.number_of_poses_remaining}.' else: - self.current_state = NavThroughPosesState.DONE - result = self.nav.getResult() - if result == TaskResult.SUCCEEDED: - self.feedback_message = 'Goal succeeded!' # pylint: disable= attribute-defined-outside-init - result = py_trees.common.Status.SUCCESS - elif result == TaskResult.CANCELED: - self.feedback_message = 'Goal was canceled!' # pylint: disable= attribute-defined-outside-init - result = py_trees.common.Status.FAILURE - elif result == TaskResult.FAILED: - self.feedback_message = 'Goal failed!' # pylint: disable= attribute-defined-outside-init - result = py_trees.common.Status.FAILURE - elif self.current_state == NavThroughPosesState.DONE: - self.logger.debug("Nothing to do!") - else: - self.logger.error(f"Invalid state {self.current_state}") - - return result - - def shutdown(self): - self.nav.cancelTask() - self.nav.destroy_node() + feedback_message = f"Executing navigation to ({self.goal_poses})." + elif current_state == ActionCallActionState.DONE: + feedback_message = f"Goal reached." + return feedback_message 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 cb465e89..91cad925 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,13 +26,20 @@ class NavToPose(RosActionCall): Class to navigate to a pose """ - def __init__(self, associated_actor, goal_pose: list, monitor_progress: bool, action_topic: str, namespace_override: str) -> None: + def __init__(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None: self.namespace = associated_actor["namespace"] if namespace_override: self.namespace = namespace_override - self.goal_pose = goal_pose + self.goal_pose = None super().__init__(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "") + def execute(self, associated_actor, goal_pose: list, action_topic: str, namespace_override: str) -> None: # pylint: disable=arguments-differ + self.namespace = associated_actor["namespace"] + if namespace_override: + self.namespace = namespace_override + self.goal_pose = goal_pose + super().execute(self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "") + def get_goal_msg(self): goal_msg = NavigateToPose.Goal() goal_msg.pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose) @@ -45,11 +52,9 @@ def get_feedback_message(self, current_state): feedback_message = "Waiting for navigation" elif self.current_state == ActionCallActionState.ACTION_CALLED: if self.received_feedback: - feedback_message = 'Estimated time of arrival: ' + \ - '{0:.0f}'.format(Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.' + feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}.' else: - goal_msg = self.get_goal_msg() - feedback_message = f"Executing navigation to ({goal_msg.pose.pose.position.x}, {goal_msg.pose.pose.position.y})." + feedback_message = f"Executing navigation to ({self.goal_pose})." elif current_state == ActionCallActionState.DONE: feedback_message = f"Goal reached." return feedback_message 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 61a4fc40..e7bcb4f2 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 @@ -13,11 +13,10 @@ action differential_drive_robot.nav_through_poses: # Use nav2 to navigate 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) - monitor_progress: bool = true # if true, the action returns after the goal is reached or on failure. If no, the action returns after request. + action_topic: string = 'navigate_through_poses' # Name of action 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 - monitor_progress: bool = true # if yes, the action returns after the goal is reached or on failure. If no, the action returns after request. + action_topic: string = 'navigate_to_pose' # Name of action \ 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 d8d59beb..7e82e5ce 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 @@ -48,15 +48,12 @@ def __init__(self, action_name: str, action_type: str, data: str): self.client = None self.send_goal_future = None self.goal_handle = None - self.action_type = action_type + self.action_type_string = action_type + self.action_type = None self.action_name = action_name self.received_feedback = None - if data: - try: - trimmed_data = data.encode('utf-8').decode('unicode_escape') - self.data = literal_eval(trimmed_data) - except Exception as e: # pylint: disable=broad-except - raise ValueError(f"Error while parsing sevice call data:") from e + self.data = None + self.parse_data(data) self.current_state = ActionCallActionState.IDLE self.cb_group = ReentrantCallbackGroup() @@ -72,7 +69,7 @@ def setup(self, **kwargs): self.name, self.__class__.__name__) raise KeyError(error_message) from e - datatype_in_list = self.action_type.split(".") + datatype_in_list = self.action_type_string.split(".") try: self.action_type = getattr( importlib.import_module(".".join(datatype_in_list[0:-1])), @@ -82,6 +79,21 @@ def setup(self, **kwargs): self.client = ActionClient(self.node, self.action_type, self.action_name, callback_group=self.cb_group) + def execute(self, action_name: str, action_type: str, data: str): + if self.action_name != action_name or self.action_type_string != action_type: + raise ValueError(f"Updating action_name or action_type_string not supported.") + + self.parse_data(data) + self.current_state = ActionCallActionState.IDLE + + def parse_data(self, data): + if data: + try: + trimmed_data = data.encode('utf-8').decode('unicode_escape') + self.data = literal_eval(trimmed_data) + except Exception as e: # pylint: disable=broad-except + raise ValueError(f"Error while parsing sevice call data:") from e + def update(self) -> py_trees.common.Status: """ Execute states diff --git a/test/scenario_execution_nav2_test/README.md b/test/scenario_execution_nav2_test/README.md new file mode 100644 index 00000000..fadcaaa9 --- /dev/null +++ b/test/scenario_execution_nav2_test/README.md @@ -0,0 +1,3 @@ +# Tests of Scenario Execution Library for Nav2 + +The `scenario_execution_nav2_test` package provides test scenarios to test `scenario_execution_nav2` within the `tb4_sim_scenario` setup. diff --git a/test/scenario_execution_nav2_test/package.xml b/test/scenario_execution_nav2_test/package.xml new file mode 100644 index 00000000..f300e504 --- /dev/null +++ b/test/scenario_execution_nav2_test/package.xml @@ -0,0 +1,24 @@ + + + + scenario_execution_nav2_test + 1.2.0 + Tests for Scenario Execution library for Nav2 + Intel Labs + Intel Labs + Apache-2.0 + + rclpy + scenario_execution_nav2 + tb4_sim_scenario + gazebo_static_camera + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/test/scenario_execution_nav2_test/resource/scenario_execution_nav2_test b/test/scenario_execution_nav2_test/resource/scenario_execution_nav2_test new file mode 100644 index 00000000..e69de29b diff --git a/test/scenario_execution_nav2_test/scenarios/test_scenario_execution_nav2.osc b/test/scenario_execution_nav2_test/scenarios/test_scenario_execution_nav2.osc new file mode 100644 index 00000000..6e6f4667 --- /dev/null +++ b/test/scenario_execution_nav2_test/scenarios/test_scenario_execution_nav2.osc @@ -0,0 +1,15 @@ +import osc.helpers +import osc.ros +import osc.nav2 + +scenario example_nav2: + timeout(600s) + robot: differential_drive_robot + do serial: + ros_launch("gazebo_static_camera", "spawn_static_camera_launch.py", [ key_value('z', '10'), key_value('pitch', '1.57')], wait_for_shutdown: false) + record_bag(['/tf', '/tf_static', '/scenario_execution/snapshots', '/map', '/static_camera/image_raw', '/local_costmap/costmap'], use_sim_time: true) + robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m))) + serial: + repeat(2) + robot.nav_through_poses([pose_3d(position_3d(x: 0.75m, y: -0.75m)), pose_3d(position_3d(x: 1.5m, y: -1.5m))]) + robot.nav_to_pose(pose_3d(position_3d(x: 0.0m, y: 0.0m))) diff --git a/test/scenario_execution_nav2_test/setup.cfg b/test/scenario_execution_nav2_test/setup.cfg new file mode 100644 index 00000000..5042cc88 --- /dev/null +++ b/test/scenario_execution_nav2_test/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/scenario_execution_nav2_test +[install] +install_scripts=$base/lib/scenario_execution_nav2_test diff --git a/test/scenario_execution_nav2_test/setup.py b/test/scenario_execution_nav2_test/setup.py new file mode 100644 index 00000000..0bed80a3 --- /dev/null +++ b/test/scenario_execution_nav2_test/setup.py @@ -0,0 +1,42 @@ +# Copyright (C) 2024 Intel Corporation +# +# 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. +# +# SPDX-License-Identifier: Apache-2.0 + +""" Setup python package """ +from glob import glob +import os +from setuptools import find_namespace_packages, setup + +PACKAGE_NAME = 'scenario_execution_nav2_test' + +setup( + name=PACKAGE_NAME, + version='1.2.0', + packages=find_namespace_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + (os.path.join('share', PACKAGE_NAME, 'scenarios'), glob('scenarios/*.osc')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Intel Labs', + maintainer_email='scenario-execution@intel.com', + description='Tests for Scenario Execution library for Nav2', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={}, +)