diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index c4bc2e9a..84d0d104 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -307,6 +307,35 @@ jobs: with: name: test-example-multirobot-result path: test_example_multirobot/test.xml + test-example-external-method: + needs: [build] + runs-on: intellabs-01 + container: + image: ghcr.io/intellabs/scenario-execution:humble + 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 Example External Method + shell: bash + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + 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} + - name: Upload result + uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb + if: always() + with: + name: test-example-external-method-result + path: test_example_external_method/test.xml test-scenario-execution-gazebo: needs: [build] runs-on: intellabs-01 @@ -332,7 +361,7 @@ jobs: export ROS_DOMAIN_ID=2 export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} # shellcheck disable=SC1083 - scenario_batch_execution -i simulation/gazebo/test_scenario_execution_gazebo/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} spawn:=False nav2:=False + scenario_batch_execution -i simulation/gazebo/test_scenario_execution_gazebo/scenarios/ -o test_scenario_execution_gazebo -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} - name: Upload result uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb if: always() @@ -340,7 +369,18 @@ jobs: name: test-example-simulation-result path: test_example_simulation/test.xml tests: - needs: [test-scenario-execution, test-scenario-execution-ros, scenario-files-validation, test-example-scenario, test-example-library, test-example-variation, test-example-nav2, test-example-simulation, test-example-multirobot, test-scenario-execution-gazebo] + needs: + - test-scenario-execution + - test-scenario-execution-ros + - scenario-files-validation + - test-example-scenario + - test-example-library + - test-example-variation + - test-example-nav2 + - test-example-simulation + - test-example-multirobot + - test-example-external-method + - test-scenario-execution-gazebo runs-on: intellabs-01 if: ${{ always() }} steps: @@ -364,4 +404,5 @@ jobs: downloaded-artifacts/test-example-variation-result/test.xml downloaded-artifacts/test-example-nav2-result/test.xml downloaded-artifacts/test-example-simulation-result/test.xml - downloaded-artifacts/test-example-multirobot-result/test.xml \ No newline at end of file + downloaded-artifacts/test-example-multirobot-result/test.xml + downloaded-artifacts/test-example-external-method-result/test.xml diff --git a/docs/libraries.rst b/docs/libraries.rst index 7a34af9a..c70c1a7d 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -150,8 +150,8 @@ Use nav2 to navigate to goal pose. Wait until a defined distance was traveled, based on odometry. -- ``namespace: string``: Namespace of the odometry topic - ``distance: length``: Traveled distance at which the action succeeds. +- ``namespace_override: string``: if set, it's used as namespace (instead of the associated actor's namespace) ``differential_drive_robot.tf_close_to()`` """""""""""""""""""""""""""""""""""""""""" @@ -251,6 +251,18 @@ Delete an object from the simulation. - ``entity_name``: Entity name within simulation - ``world_name: string``: Gazebo world name (default: ``default``) +``osc_object.relative_spawn()`` +""""""""""""""""""""""""""""""" + +Spawn an actor relative to a given ``frame_id`` within simulation (at a specified ``distance`` in front of ``frame_id``). + +- ``frame_id``: The frame Id to spawn the actor relative to. (default: ``base_link``) +- ``parent_frame_id``: The parent frame ID against which movement is evaluated. (default: ``map``) +- ``distance``: distance value relative to the frame_id at which to spawn the new actor (default: 1.0) +- ``world_name: string``: Gazebo world name (default: ``default``) +- ``model: string``: Model definition +- ``xacro_arguments: string``: (optional) Comma-separated list of argument key:=value pairs + ``osc_object.spawn()`` """""""""""""""""""""" @@ -272,7 +284,6 @@ Spawn an actor within simulation. If the file ending is ``.xacro`` the model is forwarded to `xacro `__ before getting spawned. - ``wait_for_sim()`` """""""""""""""""" Wait for simulation to become active (checks for simulation clock). diff --git a/examples/example_simulation/models/box.sdf b/examples/example_simulation/models/box.sdf index 03d45b9a..d0e72fa4 100644 --- a/examples/example_simulation/models/box.sdf +++ b/examples/example_simulation/models/box.sdf @@ -32,4 +32,4 @@ - \ No newline at end of file + diff --git a/scenario_execution/scenario_execution/external_methods/random.py b/scenario_execution/scenario_execution/external_methods/random.py index 77af79c5..fcb2bc0b 100644 --- a/scenario_execution/scenario_execution/external_methods/random.py +++ b/scenario_execution/scenario_execution/external_methods/random.py @@ -13,8 +13,10 @@ # and limitations under the License. # # SPDX-License-Identifier: Apache-2.0 -import random +import random as rd +def seed(seed_value: int = 0): + rd.seed(seed_value) # nosec B311 def get_float(min_val: dict, max_val: float): - return random.uniform(min_val, max_val) # nosec B311 + return rd.uniform(min_val, max_val) # nosec B311 diff --git a/scenario_execution/scenario_execution/lib_osc/helpers.osc b/scenario_execution/scenario_execution/lib_osc/helpers.osc index b7964e05..0a6ebaad 100644 --- a/scenario_execution/scenario_execution/lib_osc/helpers.osc +++ b/scenario_execution/scenario_execution/lib_osc/helpers.osc @@ -8,4 +8,5 @@ action run_process: command: string # Command to execute struct random: + def seed(seed_value: int = 0) is external scenario_execution.external_methods.random.seed() def get_float(min_val: float, max_val: float) -> float is external scenario_execution.external_methods.random.get_float() diff --git a/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py b/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py new file mode 100644 index 00000000..9cc5dcd8 --- /dev/null +++ b/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py @@ -0,0 +1,98 @@ +# 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 + +import rclpy +from math import sin, cos, atan2 +from tf2_ros import TransformException # pylint: disable= no-name-in-module +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_geometry_msgs import PoseStamped +from .gazebo_spawn_actor import GazeboSpawnActor + + +class GazeboRelativeSpawnActor(GazeboSpawnActor): + """ + Class to spawn an entity into simulation + + """ + + def __init__(self, name, associated_actor, + frame_id: str, parent_frame_id: str, + distance: float, world_name: str, xacro_arguments: list, + model: str, **kwargs): + """ + init + """ + super().__init__(name, associated_actor, [], world_name, xacro_arguments, model) + + self.frame_id = frame_id + self.parent_frame_id = parent_frame_id + self.distance = distance + self._pose = '{}' + self.tf_buffer = Buffer() + self.tf_listener = None + + def setup(self, **kwargs): + super().setup(**kwargs) + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + def get_spawn_pose(self): + self.calculate_new_pose() + return self._pose + + def calculate_new_pose(self): + """ + Get position of the frame with frame_id relative to the parent_frame_id + and create a pose with specified distance in front of it + + """ + try: + now = rclpy.time.Time() + trans = self.tf_buffer.lookup_transform( + self.parent_frame_id, + self.frame_id, + now, + timeout=rclpy.duration.Duration(seconds=0.0), + ) + + # Extract current position and orientation + current_position = trans.transform.translation + current_orientation = trans.transform.rotation + + rotation_angle = 2 * atan2(current_orientation.z, current_orientation.w) + + # Calculate new position with distance in front + new_x = current_position.x + self.distance * cos(rotation_angle) + new_y = current_position.y + self.distance * sin(rotation_angle) + + # Create new pose + new_pose = PoseStamped() + new_pose.header.stamp = now.to_msg() + new_pose.header.frame_id = self.parent_frame_id + new_pose.pose.position.x = new_x + new_pose.pose.position.y = new_y + new_pose.pose.position.z = current_position.z # Assuming same height + + # The orientation remains the same + new_pose.pose.orientation = current_orientation + + self._pose = '{ position: {' \ + f' x: {new_pose.pose.position.x} y: {new_pose.pose.position.y} z: {new_pose.pose.position.z}' \ + ' } orientation: {' \ + f' w: {new_pose.pose.orientation.w} x: {new_pose.pose.orientation.x} y: {new_pose.pose.orientation.y} z: {new_pose.pose.orientation.z}' \ + ' } }' + except TransformException as e: + raise ValueError(f"No transform available ({self.parent_frame_id}->{self.frame_id})") from e diff --git a/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py b/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py index 4abb0bf1..94b8006c 100644 --- a/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py +++ b/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py @@ -51,7 +51,7 @@ def __init__(self, name, associated_actor, spawn_pose: list, world_name: str, xa """ super().__init__(name, "") self.entity_name = associated_actor["name"] - self.model_file = model + self.model = model self.spawn_pose = spawn_pose self.world_name = world_name self.xacro_arguments = xacro_arguments @@ -77,25 +77,24 @@ def setup(self, **kwargs): self.logger = get_logger(self.name) self.utils = SpawnUtils(logger=self.logger) - if self.model_file.startswith('topic://'): + if self.model.startswith('topic://'): transient_local_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, depth=1) - topic = self.model_file.replace('topic://', '', 1) + topic = self.model.replace('topic://', '', 1) self.current_state = SpawnActionState.WAITING_FOR_TOPIC self.feedback_message = f"Waiting for model on topic {topic}" # pylint: disable= attribute-defined-outside-init self.model_sub = self.node.create_subscription( String, topic, self.topic_callback, transient_local_qos) else: - sdf = self.utils.parse_model_file( - self.model_file, self.entity_name, self.xacro_arguments) + self.sdf = self.utils.parse_model_file( + self.model, self.entity_name, self.xacro_arguments) - if sdf: - self.set_command(sdf) - else: - raise ValueError(f'Invalid model specified ({self.model_file})') + if not self.sdf: + raise ValueError(f'Invalid model specified ({self.model})') + self.current_state = SpawnActionState.MODEL_AVAILABLE def update(self) -> py_trees.common.Status: """ @@ -103,6 +102,15 @@ def update(self) -> py_trees.common.Status: """ if self.current_state == SpawnActionState.WAITING_FOR_TOPIC: return py_trees.common.Status.RUNNING + elif self.current_state == SpawnActionState.MODEL_AVAILABLE: + try: + self.set_command(self.sdf) + self.current_state = SpawnActionState.WAITING_FOR_RESPONSE + return super().update() + except ValueError as e: + self.feedback_message = str(e) # pylint: disable= attribute-defined-outside-init + self.current_state = SpawnActionState.FAILURE + return py_trees.common.Status.FAILURE else: return super().update() @@ -110,7 +118,6 @@ def on_executed(self): """ Hook when process gets executed """ - self.current_state = SpawnActionState.WAITING_FOR_RESPONSE self.feedback_message = f"Executed spawning, waiting for response..." # pylint: disable= attribute-defined-outside-init def shutdown(self): @@ -154,10 +161,7 @@ def on_process_finished(self, ret): else: return py_trees.common.Status.INVALID - def set_command(self, command): - """ - Set execution command - """ + def get_spawn_pose(self): # euler2quat() requires "zyx" convention, # while in YAML, we define as pitch-roll-yaw (xyz), since it's more intuitive. try: @@ -171,14 +175,19 @@ def set_command(self, command): ' } }' except KeyError as e: raise ValueError("Could not get values") from e + return pose + + def set_command(self, command): + """ + Set execution command + """ + pose = self.get_spawn_pose() + super().set_command(["ign", "service", "-s", "/world/" + self.world_name + "/create", "--reqtype", "ignition.msgs.EntityFactory", "--reptype", "ignition.msgs.Boolean", "--timeout", "30000", "--req", "pose: " + pose + " name: \"" + self.entity_name + "\" allow_renaming: false sdf: \"" + command + "\""]) - self.logger.info(f'Command: {" ".join(self.get_command())}') - self.current_state = SpawnActionState.MODEL_AVAILABLE - def topic_callback(self, msg): ''' Callback to receive model description from topic @@ -188,3 +197,4 @@ def topic_callback(self, msg): self.logger.info("Received robot_description.") self.node.destroy_subscription(self.model_sub) self.set_command(msg.data.replace("\"", "\\\"").replace("\n", "")) + self.current_state = SpawnActionState.MODEL_AVAILABLE diff --git a/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc b/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc index fa033cc9..9e4e1a83 100644 --- a/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc +++ b/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc @@ -11,6 +11,15 @@ action osc_actor.delete: entity_name: string # name of the actor within simulation world_name: string = 'default' # simulation world name +action osc_actor.relative_spawn: + # Spawn a simulation entity at a position relative to the frame_id (at a specified distance in front of frame_id) + frame_id: string = 'base_link' # frame_id to spawn the actor relative to + parent_frame_id: string = "map" # parent frame_id of the relative frame_id + distance: float = 1.0 # distance value relative to the frame_id at which to spawn the new actor + world_name: string = 'default' # simulation world name + model: string # model definition + xacro_arguments: string = '' # comma-separated list of argument key:=value pairs + action osc_actor.spawn: # Spawn a simulation entity. spawn_pose: pose_3d # position at which the object gets spawned @@ -21,4 +30,4 @@ action osc_actor.spawn: action wait_for_sim: # Wait for simulation to become active world_name: string = 'default' # simulation world name - timeout: time = 60s # time to wait for the simulation. return failure afterwards. \ No newline at end of file + timeout: time = 60s # time to wait for the simulation. return failure afterwards. diff --git a/scenario_execution_gazebo/setup.py b/scenario_execution_gazebo/setup.py index 32486071..b717b10a 100644 --- a/scenario_execution_gazebo/setup.py +++ b/scenario_execution_gazebo/setup.py @@ -38,6 +38,7 @@ include_package_data=True, entry_points={ 'scenario_execution.actions': [ + 'osc_actor.relative_spawn = scenario_execution_gazebo.actions.gazebo_relative_spawn_actor:GazeboRelativeSpawnActor', 'osc_actor.spawn = scenario_execution_gazebo.actions.gazebo_spawn_actor:GazeboSpawnActor', 'actor_exists = scenario_execution_gazebo.actions.gazebo_actor_exists:GazeboActorExists', 'osc_actor.delete = scenario_execution_gazebo.actions.gazebo_delete_actor:GazeboDeleteActor', diff --git a/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py b/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py index 65c3bac2..03299ac0 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py @@ -27,11 +27,11 @@ class OdometryDistanceTraveled(py_trees.behaviour.Behaviour): Class to wait for a certain covered distance, based on odometry """ - def __init__(self, name, associated_actor, distance: float): + def __init__(self, name, associated_actor, distance: float, namespace_override: str): super().__init__(name) self.namespace = associated_actor["namespace"] self.distance_expected = distance - self.distance_traveled = 0. + self.distance_traveled = 0.0 self.previous_x = 0 self.previous_y = 0 self.first_run = True @@ -39,6 +39,8 @@ def __init__(self, name, associated_actor, distance: float): self.node = None self.subscriber = None self.callback_group = None + if namespace_override: + self.namespace = namespace_override def setup(self, **kwargs): """ @@ -86,7 +88,7 @@ def initialise(self): ''' Initialize before ticking. ''' - self.distance_traveled = 0. + self.distance_traveled = 0.0 self.previous_x = 0 self.previous_y = 0 self.first_run = True @@ -101,8 +103,8 @@ def update(self) -> py_trees.common.Status: self.logger.debug(f"ticking: {self.distance_traveled}") if self.distance_traveled >= self.distance_expected: - self.feedback_message = f"expected traveled distance reached: {self.distance_expected:.3}" # pylint: disable= attribute-defined-outside-init + self.feedback_message = f"expected traveled distance reached: {float(self.distance_expected):.3}" # pylint: disable= attribute-defined-outside-init return Status.SUCCESS else: - self.feedback_message = f"distance traveled: {self.distance_traveled:.3} < {self.distance_expected:.3}" # pylint: disable= attribute-defined-outside-init + self.feedback_message = f"distance traveled: {float(self.distance_traveled):.3} < {float(self.distance_expected):.3}" # pylint: disable= attribute-defined-outside-init return Status.RUNNING 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 f45da696..4add0c73 100644 --- a/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc +++ b/scenario_execution_ros/scenario_execution_ros/lib_osc/ros.osc @@ -100,7 +100,7 @@ action differential_drive_robot.nav_to_pose: 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 differential_drive_robot.odometry_distance_traveled: - namespace: string = '' + namespace_override: string = '' distance: length action differential_drive_robot.tf_close_to: diff --git a/scenario_execution_ros/setup.py b/scenario_execution_ros/setup.py index 13b2dfb6..c72ea982 100644 --- a/scenario_execution_ros/setup.py +++ b/scenario_execution_ros/setup.py @@ -54,8 +54,7 @@ 'check_data = scenario_execution_ros.actions.ros_topic_check_data:RosTopicCheckData', 'service_call = scenario_execution_ros.actions.ros_service_call:RosServiceCall', 'topic_publish = scenario_execution_ros.actions.ros_topic_publish:RosTopicPublish', - 'odometry_distance_traveled = ' - 'scenario_execution_ros.actions.odometry_distance_traveled:OdometryDistanceTraveled', + 'differential_drive_robot.odometry_distance_traveled = scenario_execution_ros.actions.odometry_distance_traveled:OdometryDistanceTraveled', 'set_node_parameter = scenario_execution_ros.actions.ros_set_node_parameter:RosSetNodeParameter', 'record_bag = scenario_execution_ros.actions.ros_bag_record:RosBagRecord', 'wait_for_topics = scenario_execution_ros.actions.ros_topic_wait_for_topics:RosTopicWaitForTopics', diff --git a/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_relative_spawn.osc b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_relative_spawn.osc new file mode 100644 index 00000000..8faeaaa8 --- /dev/null +++ b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_relative_spawn.osc @@ -0,0 +1,25 @@ +import osc.ros +import osc.gazebo +import osc.helpers + +scenario test_relative_spawn: + robot: differential_drive_robot + test_obstacle: osc_actor + do parallel: + serial: + robot.init_nav2(pose_3d(position_3d(x: 0.0m, y: 0.0m))) + parallel: + robot.nav_to_pose(goal_pose: pose_3d(position: position_3d(x: -3.0m), orientation: orientation_3d(yaw: 3.14rad))) + serial: + robot.tf_close_to( + reference_point: position_3d(x: -0.5m, y: 0.0m), + threshold: 0.35m, + robot_frame_id: 'turtlebot4_base_link_gt') + test_obstacle.relative_spawn( + distance: 1.5m, + model: 'test_scenario_execution_gazebo://models/box.sdf', + world_name: 'maze') + emit end + time_out: serial: + wait elapsed(600s) + emit fail diff --git a/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_exists_delete.osc b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_exists_delete.osc index eb5b95b3..2af48e4b 100644 --- a/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_exists_delete.osc +++ b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_exists_delete.osc @@ -6,7 +6,7 @@ scenario test_spawn_and_exists: do parallel: test_obstacle.spawn( - spawn_pose: pose_3d(position: position_3d(x: 1m, y: 0m, z: 1m)), + spawn_pose: pose_3d(position: position_3d(x: -3m, y: 0m, z: 1m)), world_name: 'maze', model: 'test_scenario_execution_gazebo://models/box.sdf') test_case: serial: diff --git a/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_xacro_with_parameters.osc b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_xacro_with_parameters.osc index e6ff4ae1..7b76dcd7 100644 --- a/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_xacro_with_parameters.osc +++ b/simulation/gazebo/test_scenario_execution_gazebo/scenarios/test_spawn_xacro_with_parameters.osc @@ -6,7 +6,7 @@ scenario test_spawn_xacro_with_parameters: do parallel: test_obstacle.spawn( - spawn_pose: pose_3d(position: position_3d(z: 2m)), + spawn_pose: pose_3d(position: position_3d(x: -3m, z: 2m)), world_name: 'maze', xacro_arguments: 'box_width_length:=1 1,box_height:=4', model: 'test_scenario_execution_gazebo://models/box.sdf.xacro')