diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_actor.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_actor.py index ba02b3e..a0605af 100644 --- a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_actor.py +++ b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_actor.py @@ -15,68 +15,17 @@ # SPDX-License-Identifier: Apache-2.0 -import py_trees -from enum import Enum +from .gazebo_delete_entity_pose import GazeboDeleteEntity -from scenario_execution.actions.run_process import RunProcess +class GazeboDeleteActor(GazeboDeleteEntity): + """Class to delete an actor in gazebo.""" -class DeleteActionState(Enum): - """ - States for executing a delete-entity in gazebo - """ - IDLE = 1 - WAITING_FOR_RESPONSE = 2 - DONE = 3 - FAILURE = 4 - - -class GazeboDeleteActor(RunProcess): - """ - Class to delete an entity in gazebo - - """ - - def __init__(self, associated_actor): + def __init__(self, associated_actor: dict) -> None: super().__init__() - self.entity_name = None - self.current_state = DeleteActionState.IDLE - - def execute(self, associated_actor, entity_name: str, world_name: str): # pylint: disable=arguments-differ - self.set_command(["gz", "service", "-s", "/world/" + world_name + "/remove", - "--reqtype", "gz.msgs.Entity", - "--reptype", "gz.msgs.Boolean", - "--timeout", "1000", "--req", "name: \"" + entity_name + "\" type: MODEL"]) - - def on_executed(self): - """ - Hook when process gets executed - """ - self.current_state = DeleteActionState.WAITING_FOR_RESPONSE - - def on_process_finished(self, ret): - """ - check result of process + self.associated_actor = associated_actor - return: - py_trees.common.Status - """ - if self.current_state == DeleteActionState.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.feedback_message = f"Found error output while executing '{self.get_command()}'" # pylint: disable= attribute-defined-outside-init - self.current_state = DeleteActionState.FAILURE - return py_trees.common.Status.FAILURE - except IndexError: - break - self.current_state = DeleteActionState.DONE - return py_trees.common.Status.SUCCESS - else: - self.current_state = DeleteActionState.FAILURE - return py_trees.common.Status.FAILURE - else: - return py_trees.common.Status.INVALID + # pylint: disable-next=arguments-differ + def execute(self, associated_actor: dict, world_name: str) -> None: + self.associated_actor = associated_actor + super().execute(entity_name=self.associated_actor["name"], world_name=world_name) diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_entity.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_entity.py new file mode 100644 index 0000000..8e67c54 --- /dev/null +++ b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_delete_entity.py @@ -0,0 +1,82 @@ +# Copyright (C) 2026 Enway GmbH +# +# 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 + + +from enum import Enum +from typing import Optional + +import py_trees +from scenario_execution.actions.run_process import RunProcess + + +class DeleteActionState(Enum): + """States for executing a delete-entity in gazebo.""" + + IDLE = 1 + WAITING_FOR_RESPONSE = 2 + DONE = 3 + FAILURE = 4 + + +class GazeboDeleteEntity(RunProcess): + """Class to delete an entity in gazebo.""" + + def __init__(self) -> None: + super().__init__() + self.entity_name: Optional[str] = None + self.current_state = DeleteActionState.IDLE + + def execute(self, entity_name: str, world_name: str): # pylint: disable=arguments-differ + super().execute(wait_for_shutdown=True) + self.entity_name = entity_name + self.world_name = world_name + + self.set_command(["gz", "service", "-s", f"/world/{self.world_name}/remove", + "--reqtype", "gz.msgs.Entity", + "--reptype", "gz.msgs.Boolean", + "--timeout", "1000", "--req", f'name: "{self.entity_name}" type: MODEL']) + + def on_executed(self) -> None: + """Hook when process gets executed.""" + self.feedback_message = ( + f"Waiting for entity '{self.entity_name}' to be deleted" # pylint: disable= attribute-defined-outside-init + ) + self.current_state = DeleteActionState.WAITING_FOR_RESPONSE + + def on_process_finished(self, ret: int) -> py_trees.common.Status: + """Check result of process.""" + if self.current_state == DeleteActionState.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: + # pylint: disable-next= attribute-defined-outside-init + self.feedback_message = f"Found error output while executing '{self.get_command()}'" + self.current_state = DeleteActionState.FAILURE + return py_trees.common.Status.FAILURE + except IndexError: + break + self.feedback_message = f"Successfully deleted entity '{self.entity_name}'" + self.current_state = DeleteActionState.DONE + return py_trees.common.Status.SUCCESS + else: + self.feedback_message = f"Deleting '{self.entity_name}' failed with {ret}" + self.current_state = DeleteActionState.FAILURE + return py_trees.common.Status.FAILURE + else: + return py_trees.common.Status.INVALID diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py index 939301d..63cd173 100644 --- a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py +++ b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_actor.py @@ -14,187 +14,20 @@ # # SPDX-License-Identifier: Apache-2.0 -import subprocess # nosec B404 -from enum import Enum +from .gazebo_spawn_entity import GazeboSpawnEntity -from transforms3d.taitbryan import euler2quat -from std_msgs.msg import String -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.node import Node -import py_trees -from scenario_execution.actions.base_action import ActionError -from scenario_execution.actions.run_process import RunProcess -from .utils import SpawnUtils +class GazeboSpawnActor(GazeboSpawnEntity): + """Class to spawn an actor into simulation.""" - -class SpawnActionState(Enum): - """ - States for executing a spawn-entity in gazebo - """ - WAITING_FOR_TOPIC = 1 - MODEL_AVAILABLE = 2 - WAITING_FOR_RESPONSE = 3 - DONE = 4 - FAILURE = 5 - - -class GazeboSpawnActor(RunProcess): - """ - Class to spawn an entity into simulation - - """ - - def __init__(self, associated_actor, xacro_arguments: list, model: str): - """ - init - """ - super().__init__() - self.current_state = SpawnActionState.WAITING_FOR_TOPIC - self.entity_name = associated_actor["name"] - self.entity_model = model - self.xacro_arguments = xacro_arguments - self.node = None - self.model_sub = None - self.sdf = None - self.utils = None + def __init__(self, associated_actor: dict, xacro_arguments: list, model: str): + super().__init__(xacro_arguments, model) + self.associated_actor = associated_actor 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 ActionError(error_message, action=self) from e - - self.utils = SpawnUtils(logger=self.logger) - - if self.entity_model.startswith('topic://'): - transient_local_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - topic = self.entity_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: - self.sdf = self.utils.parse_model_file( - self.entity_model, self.entity_name, self.xacro_arguments) - - if not self.sdf: - raise ActionError(f'Invalid model specified ({self.entity_model})', action=self) - self.current_state = SpawnActionState.MODEL_AVAILABLE - - def execute(self, associated_actor, spawn_pose: list, world_name: str): # pylint: disable=arguments-differ - self.spawn_pose = spawn_pose - self.world_name = world_name - - 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.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() - - def on_executed(self): - """ - Hook when process gets executed - """ - 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(["gz", "service", "-s", "/world/" + self.world_name + "/remove", # pylint: disable=subprocess-run-check - "--reqtype", "gz.msgs.Entity", - "--reptype", "gz.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 get_spawn_pose(self): - # euler2quat() requires "zyx" convention, - # while in YAML, we define as pitch-roll-yaw (xyz), since it's more intuitive. - try: - quaternion = euler2quat(self.spawn_pose["orientation"]["yaw"], - self.spawn_pose["orientation"]["roll"], - self.spawn_pose["orientation"]["pitch"]) - pose = '{ position: {' \ - f' x: {self.spawn_pose["position"]["x"]} y: {self.spawn_pose["position"]["y"]} z: {self.spawn_pose["position"]["z"]}' \ - ' } orientation: {' \ - f' w: {quaternion[0]} x: {quaternion[1]} y: {quaternion[2]} z: {quaternion[3]}' \ - ' } }' - except KeyError as e: - raise ActionError("Could not get values", action=self) from e - return pose - - def set_command(self, command): - """ - Set execution command - """ - pose = self.get_spawn_pose() - - super().set_command(["gz", "service", "-s", "/world/" + self.world_name + "/create", - "--reqtype", "gz.msgs.EntityFactory", - "--reptype", "gz.msgs.Boolean", - "--timeout", "30000", "--req", "pose: " + pose + " name: \"" + self.entity_name + "\" allow_renaming: false sdf: \"" + command + "\""]) - - def topic_callback(self, msg): - ''' - Callback to receive model description from topic - ''' + super().setup(**kwargs) - 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.MODEL_AVAILABLE + # pylint: disable-next=arguments-differ + def execute(self, associated_actor: dict, spawn_pose: list, world_name: str) -> None: + self.associated_actor = associated_actor + super().execute(entity_name=self.associated_actor["name"], spawn_pose=spawn_pose, world_name=world_name) diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_entity.py b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_entity.py new file mode 100644 index 0000000..75187d1 --- /dev/null +++ b/libs/scenario_execution_gazebo/scenario_execution_gazebo/actions/gazebo_spawn_entity.py @@ -0,0 +1,203 @@ +# Copyright (C) 2026 Enway GmbH +# +# 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 typing import Optional + +from transforms3d.taitbryan import euler2quat +from std_msgs.msg import String + +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.node import Node +import py_trees +from scenario_execution.actions.base_action import ActionError +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 + MODEL_AVAILABLE = 2 + WAITING_FOR_RESPONSE = 3 + DONE = 4 + FAILURE = 5 + + +class GazeboSpawnEntity(RunProcess): + """ + Class to spawn an entity into simulation + + """ + + def __init__(self, xacro_arguments: list, model: str): + """ + init + """ + super().__init__() + self.current_state = SpawnActionState.WAITING_FOR_TOPIC + self.entity_name: Optional[str] = None + self.entity_model = model + self.xacro_arguments = xacro_arguments + self.node = None + self.model_sub = None + self.sdf = None + self.utils = None + + 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 ActionError(error_message, action=self) from e + + self.utils = SpawnUtils(logger=self.logger) + + if self.entity_model.startswith('topic://'): + transient_local_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) + topic = self.entity_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: + self.sdf = self.utils.parse_model_file( + self.entity_model, self.entity_name, self.xacro_arguments) + + if not self.sdf: + raise ActionError(f'Invalid model specified ({self.entity_model})', action=self) + self.current_state = SpawnActionState.MODEL_AVAILABLE + + def execute(self, entity_name: str, spawn_pose: list, world_name: str): # pylint: disable=arguments-differ + self.entity_name = entity_name + self.spawn_pose = spawn_pose + self.world_name = world_name + + 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.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() + + def on_executed(self): + """ + Hook when process gets executed + """ + self.feedback_message = "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(["gz", "service", "-s", f"/world/{self.world_name}/remove", # pylint: disable=subprocess-run-check + "--reqtype", "gz.msgs.Entity", + "--reptype", "gz.msgs.Boolean", + "--timeout", "1000", "--req", f'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 get_spawn_pose(self): + # euler2quat() requires "zyx" convention, + # while in YAML, we define as pitch-roll-yaw (xyz), since it's more intuitive. + try: + quaternion = euler2quat(self.spawn_pose["orientation"]["yaw"], + self.spawn_pose["orientation"]["roll"], + self.spawn_pose["orientation"]["pitch"]) + pose = '{ position: {' \ + f' x: {self.spawn_pose["position"]["x"]} y: {self.spawn_pose["position"]["y"]} z: {self.spawn_pose["position"]["z"]}' \ + ' } orientation: {' \ + f' w: {quaternion[0]} x: {quaternion[1]} y: {quaternion[2]} z: {quaternion[3]}' \ + ' } }' + except KeyError as e: + raise ActionError("Could not get values", action=self) from e + return pose + + def set_command(self, command): + """ + Set execution command + """ + pose = self.get_spawn_pose() + + req = f'pose: {pose} name: "{self.entity_name}" allow_renaming: false sdf: "{command}"' + super().set_command(["gz", "service", "-s", f"/world/{self.world_name}/create", + "--reqtype", "gz.msgs.EntityFactory", + "--reptype", "gz.msgs.Boolean", + "--timeout", "30000", "--req", req]) + + 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.MODEL_AVAILABLE diff --git a/libs/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc b/libs/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc index 3aaf061..91c8639 100644 --- a/libs/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc +++ b/libs/scenario_execution_gazebo/scenario_execution_gazebo/lib_osc/gazebo.osc @@ -6,9 +6,13 @@ action actor_exists: entity_name: string # name of the actor within simulation world_name: string = 'default' # simulation world name +action gz_delete_entity: + # Delete an entity from simulation + entity_name: string # name of the entity + world_name: string = 'default' # simulation world name + action osc_actor.delete: # Delete an actor from simulation - entity_name: string # name of the actor within simulation world_name: string = 'default' # simulation world name action osc_actor.relative_spawn: @@ -20,8 +24,16 @@ action osc_actor.relative_spawn: world_name: string = 'default' # simulation world name xacro_arguments: string = '' # comma-separated list of argument key:=value pairs -action osc_actor.spawn: +action gz_spawn_entity: # Spawn a simulation entity. + entity_name: string # name of the entity + spawn_pose: pose_3d # position at which the object gets spawned + model: string # model definition + world_name: string = 'default' # simulation world name + xacro_arguments: string = '' # comma-separated list of argument key:=value pairs + +action osc_actor.spawn: + # Spawn an actor as a simulation entity. spawn_pose: pose_3d # position at which the object gets spawned model: string # model definition world_name: string = 'default' # simulation world name @@ -38,7 +50,7 @@ action osc_actor.set_actor_pose: pose: pose_3d # desired pose world_name: string = 'default' # simulation world name -struct spawn_entity: +struct spawn_entity_struct: entity_name: string # name of entity in simulation spawn_pose: pose_3d # position at which the object gets spawned model: string # model definition @@ -46,7 +58,7 @@ struct spawn_entity: action spawn_multiple: # Spawn multiple simulation entities. - entities: list of spawn_entity # entities to spawn + entities: list of spawn_entity_struct # entities to spawn world_name: string = 'default' # simulation world name action wait_for_sim: diff --git a/libs/scenario_execution_gazebo/setup.py b/libs/scenario_execution_gazebo/setup.py index 4046554..1bffe6a 100644 --- a/libs/scenario_execution_gazebo/setup.py +++ b/libs/scenario_execution_gazebo/setup.py @@ -43,10 +43,12 @@ entry_points={ 'scenario_execution.actions': [ 'osc_actor.relative_spawn = scenario_execution_gazebo.actions.gazebo_relative_spawn_actor:GazeboRelativeSpawnActor', + 'gz_spawn_entity = scenario_execution_gazebo.actions.gazebo_spawn_entity:GazeboSpawnEntity', 'osc_actor.spawn = scenario_execution_gazebo.actions.gazebo_spawn_actor:GazeboSpawnActor', 'actor_exists = scenario_execution_gazebo.actions.gazebo_actor_exists:GazeboActorExists', 'set_entity_pose = scenario_execution_gazebo.actions.gazebo_set_entity_pose:GazeboSetEntityPose', 'osc_actor.set_actor_pose = scenario_execution_gazebo.actions.gazebo_set_actor_pose:GazeboSetActorPose', + 'gz_delete_entity = scenario_execution_gazebo.actions.gazebo_delete_entity:GazeboDeleteEntity', 'osc_actor.delete = scenario_execution_gazebo.actions.gazebo_delete_actor:GazeboDeleteActor', 'spawn_multiple = scenario_execution_gazebo.actions.gazebo_spawn_multiple:GazeboSpawnMultiple', 'wait_for_sim = scenario_execution_gazebo.actions.gazebo_wait_for_sim:GazeboWaitForSim',