Relative spawn

.github/workflows/test_build.yml

@@ -364,4 +364,4 @@ 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
+            downloaded-artifacts/test-example-multirobot-result/test.xml

docs/libraries.rst

@@ -272,6 +272,17 @@ Spawn an actor within simulation.
 
     If the file ending is ``.xacro`` the model is forwarded to `xacro <https://wiki.ros.org/xacro>`__ before getting spawned.
 
+``osc_object.relative_spawn()``
+"""""""""""""""""""""""""""""""
+
+Spawn an actor relative to a given ``frame_id`` within simulation (at a specified ``distance`` in front of ``frame_id``) by using the ``model`` and ``namespace`` of the associated actor. 
+
+- ``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
 
 ``wait_for_sim()``
 """"""""""""""""""

examples/example_simulation/models/box.sdf

@@ -19,14 +19,14 @@
       <collision name="collision">
         <geometry>
           <box>
-            <size>0.25 0.25 0.25</size>
+            <size>0.25 0.25 0.5</size>
           </box>
         </geometry>
       </collision>
       <visual name="visual">
         <geometry>
           <box>
-            <size>0.25 0.25 0.25</size>
+            <size>0.25 0.25 0.5</size>
           </box>
         </geometry>
       </visual>

scenario_execution/scenario_execution/external_methods/random.py

@@ -16,5 +16,6 @@
 import random
 
 
-def get_float(min_val: dict, max_val: float):
+def get_float(min_val: dict, max_val: float, seed: int = 0):
+    random.seed(seed)  # nosec B311
     return random.uniform(min_val, max_val)  # nosec B311

scenario_execution/scenario_execution/lib_osc/helpers.osc

@@ -8,4 +8,4 @@ action run_process:
     command: string # Command to execute
 
 struct random:
-    def get_float(min_val: float, max_val: float) -> float is external scenario_execution.external_methods.random.get_float()
+    def get_float(min_val: float, max_val: float, seed: int) -> float is external scenario_execution.external_methods.random.get_float()

scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_relative_spawn_actor.py

@@ -0,0 +1,248 @@
+# 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 subprocess  # nosec B404
+from enum import Enum
+
+from std_msgs.msg import String
+
+import rclpy
+from math import sin, cos, atan2
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.logging import get_logger
+from rclpy.node import Node
+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
+import py_trees
+from scenario_execution.actions.run_process import RunProcess
+from .utils import SpawnUtils
+
+
+class SpawnActionState(Enum):
+    """
+    States for executing a spawn-entity in gazebo
+    """
+    WAITING_FOR_TOPIC = 1
+    WAITING_FOR_POSE = 2
+    POSE_AVAILABLE = 3
+    MODEL_AVAILABLE = 4
+    WAITING_FOR_RESPONSE = 5
+    DONE = 6
+    FAILURE = 7
+
+
+class GazeboRelativeSpawnActor(RunProcess):
+    """
+    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, "")
+        self.entity_name = associated_actor["name"]
+        self.model_file = model
+        self.frame_id = frame_id
+        self.parent_frame_id = parent_frame_id
+        self.distance = distance
+        self.world_name = world_name
+        self.xacro_arguments = xacro_arguments
+        self.current_state = SpawnActionState.WAITING_FOR_TOPIC
+        self.node = None
+        self.logger = None
+        self.model_sub = None
+        self.sdf = None
+        self.utils = None
+        self._pose = None
+        self.sdf = None
+        self.tf_buffer = Buffer()
+
+    def setup(self, **kwargs):
+        """
+        Setup ROS2 node and model
+
+        """
+        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.logger = get_logger(self.name)
+        self.utils = SpawnUtils(logger=self.logger)
+        _ = TransformListener(self.tf_buffer, self.node)
+
+        if self.model_file.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)
+            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:
+            self.sdf = self.utils.parse_model_file(
+                self.model_file, self.entity_name, self.xacro_arguments)
+            self.current_state = SpawnActionState.WAITING_FOR_POSE
+
+    def update(self) -> py_trees.common.Status:
+        """
+        Send request
+        """
+        if self.current_state == SpawnActionState.WAITING_FOR_TOPIC:
+            return py_trees.common.Status.RUNNING
+        elif self.current_state == SpawnActionState.WAITING_FOR_POSE:
+            self.calculate_new_pose()
+            return py_trees.common.Status.RUNNING
+        elif self.current_state == SpawnActionState.POSE_AVAILABLE:
+            if self.sdf:
+                self.set_command(self.sdf)
+            else:
+                raise ValueError(f'Invalid model specified ({self.model_file})')
+            return py_trees.common.Status.RUNNING
+        else:
+            return super().update()
+
+    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):
+        """
+        Cleanup on shutdown
+        """
+        if self.current_state in [SpawnActionState.WAITING_FOR_TOPIC, SpawnActionState.MODEL_AVAILABLE]:
+            return
+
+        self.logger.info(f"Deleting entity '{self.entity_name}' from simulation.")
+        subprocess.run(["ign", "service", "-s", "/world/" + self.world_name + "/remove",  # pylint: disable=subprocess-run-check
+                        "--reqtype", "ignition.msgs.Entity",
+                        "--reptype", "ignition.msgs.Boolean",
+                        "--timeout", "1000", "--req", "name: \"" + self.entity_name + "\" type: MODEL"])
+
+    def on_process_finished(self, ret):
+        """
+        check result of process
+
+        return:
+            py_trees.common.Status
+        """
+        if self.current_state == SpawnActionState.WAITING_FOR_RESPONSE:
+            if ret == 0:
+                while True:
+                    try:
+                        line = self.output.popleft()
+                        line = line.lower()
+                        if 'error' in line or 'timed out' in line:
+                            self.logger.warn(line)
+                            self.feedback_message = f"Found error output while executing '{self.command}'"  # pylint: disable= attribute-defined-outside-init
+                            self.current_state = SpawnActionState.FAILURE
+                            return py_trees.common.Status.FAILURE
+                    except IndexError:
+                        break
+                self.current_state = SpawnActionState.DONE
+                return py_trees.common.Status.SUCCESS
+            else:
+                self.current_state = SpawnActionState.FAILURE
+                return py_trees.common.Status.FAILURE
+        else:
+            return py_trees.common.Status.INVALID
+
+    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=1.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}' \
+                ' } }'
+
+            self.current_state = SpawnActionState.POSE_AVAILABLE
+
+        except TransformException as ex:
+            self.feedback_message = f"Could not transform {self.parent_frame_id} to {self.frame_id}"  # pylint: disable= attribute-defined-outside-init
+            self.logger().warn(
+                f'Could not transform {self.parent_frame_id} to {self.frame_id} at time {now}: {ex}')
+
+    def set_command(self, command):
+        """
+        Set execution command
+        """
+        super().set_command(["ign", "service", "-s", "/world/" + self.world_name + "/create",
+                             "--reqtype", "ignition.msgs.EntityFactory",
+                             "--reptype", "ignition.msgs.Boolean",
+                             "--timeout", "30000", "--req", "pose: " + str(self._pose) + " name: \"" + self.entity_name + "\" allow_renaming: true 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
+        '''
+
+        self.feedback_message = f"Model received from topic."  # pylint: disable= attribute-defined-outside-init
+        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.WAITING_FOR_POSE

scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc

@@ -18,7 +18,16 @@ action osc_actor.spawn:
     model: string                       # model definition
     xacro_arguments: string = ''        # comma-separated list of argument key:=value pairs
 
+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), uses namespace of entity
+    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 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.
+    timeout: time = 60s                 # time to wait for the simulation. return failure afterwards.

scenario_execution_gazebo/setup.py

@@ -39,6 +39,7 @@
     entry_points={
         'scenario_execution.actions': [
             'osc_actor.spawn = scenario_execution_gazebo.actions.gazebo_spawn_actor:GazeboSpawnActor',
+            'osc_actor.relative_spawn = scenario_execution_gazebo.actions.gazebo_relative_spawn_actor:GazeboRelativeSpawnActor',
             'actor_exists = scenario_execution_gazebo.actions.gazebo_actor_exists:GazeboActorExists',
             'osc_actor.delete = scenario_execution_gazebo.actions.gazebo_delete_actor:GazeboDeleteActor',
             'wait_for_sim = scenario_execution_gazebo.actions.gazebo_wait_for_sim:GazeboWaitForSim',

scenario_execution_ros/scenario_execution_ros/actions/odometry_distance_traveled.py

@@ -27,18 +27,20 @@ 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
         self.logger = None
         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

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:

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',