From cf348729119cd54336182cfdf98e62b69355abfb Mon Sep 17 00:00:00 2001 From: Liam Xu Date: Thu, 11 Jun 2026 21:41:19 -0700 Subject: [PATCH 1/2] add gazebo interface code --- src/scenic/simulators/gazebo/model.scenic | 34 ++ src/scenic/simulators/gazebo/simulator.py | 392 ++++++++++++++++++ .../simulators/gazebo/utils/parse_sdf.py | 207 +++++++++ .../gazebo/utils/quaternion_utils.py | 42 ++ .../gazebo/utils/spawn_delete_model.py | 189 +++++++++ .../simulators/gazebo/utils/start_gazebo.py | 76 ++++ .../simulators/gazebo/utils/state_utils.py | 75 ++++ 7 files changed, 1015 insertions(+) create mode 100644 src/scenic/simulators/gazebo/model.scenic create mode 100644 src/scenic/simulators/gazebo/simulator.py create mode 100644 src/scenic/simulators/gazebo/utils/parse_sdf.py create mode 100644 src/scenic/simulators/gazebo/utils/quaternion_utils.py create mode 100644 src/scenic/simulators/gazebo/utils/spawn_delete_model.py create mode 100644 src/scenic/simulators/gazebo/utils/start_gazebo.py create mode 100644 src/scenic/simulators/gazebo/utils/state_utils.py diff --git a/src/scenic/simulators/gazebo/model.scenic b/src/scenic/simulators/gazebo/model.scenic new file mode 100644 index 000000000..5577503e5 --- /dev/null +++ b/src/scenic/simulators/gazebo/model.scenic @@ -0,0 +1,34 @@ +import math +from scenic.simulators.gazebo.simulator import GazeboSimulator, GazeboSimulation +import os + +simulator GazeboSimulator() +object_prefix = '/mnt/l/scenic/src/scenic/simulators/gazebo/'# placeholder, put the path to the folder storing the Gazebo models you want to spawn +default_file_name = "model.sdf" +get_sdf_dir = lambda s: object_prefix + s + "/" + default_file_name + +class Robot: + name: 'robot' + object_type: 'robot' + +class GazeboObject: + """ + Superclass for non-agent objects + """ + name: 'gazebo_object' + object_type: 'gazebo_object' + description_file: '' + description_file_type: 'sdf' + width: 1 + length: 1 + height: 1 + positionOffset:(0, 0, 0) + +class SDFObject: + """ + Used for SDF parsing to make Scenic aware of predefined objects in the world + """ + name: 'sdf_object' + object_type: 'sdf_object' + description_file: '' + description_file_type: 'sdf' diff --git a/src/scenic/simulators/gazebo/simulator.py b/src/scenic/simulators/gazebo/simulator.py new file mode 100644 index 000000000..681bc26b6 --- /dev/null +++ b/src/scenic/simulators/gazebo/simulator.py @@ -0,0 +1,392 @@ +"""Simulator interface for Gazebo.""" +import logging +import math +import traceback + +from simulation_interfaces.srv import ( + SpawnEntity, + DeleteEntity, + SetSimulationState, + GetEntityState, + SetEntityState, + ResetSimulation, + StepSimulation +) +import numpy as np +import rclpy + +from scenic.core.simulators import Simulation, SimulationCreationError, Simulator +from scenic.core.vectors import Vector +from scenic.simulators.gazebo.utils.spawn_delete_model import DeleteObject, SpawnObject +from scenic.simulators.gazebo.utils.start_gazebo import ( + PauseGazebo, + ResetGazeboWorld, + UnpauseGazebo, + TakeStep +) +from scenic.simulators.gazebo.utils.state_utils import ( + GetObjectState, +) + +from scenic.core.simulators import SimulationCreationError +from scenic.syntax.veneer import verbosePrint + +class GazeboSimCreationError(SimulationCreationError): + """ + If anything went wrong in setting up the scene, this error is thrown. + If Scenic is run using the CLI, The current scene terminates, and a new scene is started + Args: + String msg: the message to be given + """ + + def __init__(self, msg): + self.msg = msg + super().__init__(self.msg) + + +class GazeboSimRuntimeError(SimulationCreationError): + """ + If anything went wrong in running the scene, this error is thrown. + If Scenic is run using the CLI, The current scene terminates, and a new scene is started + Args: + String msg: the message to be given + Exception exc: the exception thrown by other parts of the code that makes us stop scene + """ + + def __init__(self, msg, exc): + self.msg = exc.args[0] + self.file_name, self.lineno = exc.filename, exc.lineno + super().__init__(self.msg) + self.with_traceback(exc.__traceback__) + + +class GazeboSimulator(Simulator): + """Implementation of `Simulator`.""" + + def __init__( + self, + record="", + timestep=0.1, + ): + super().__init__() + verbosePrint(f"Connecting to Gazebo simulator") + self.timestep = timestep + self.record = record + self.scenario_number = 0 + + rclpy.init() + + # create rclpy node and clients to interact with gazebo services + self.scenicNode = rclpy.create_node("scenic") + self.spawnClient = self.scenicNode.create_client(SpawnEntity, "/gzserver/spawn_entity") + self.deleteClient = self.scenicNode.create_client(DeleteEntity, "/gzserver/delete_entity") + self.controlClient = self.scenicNode.create_client(SetSimulationState, "/gzserver/set_simulation_state") + self.resetClient = self.scenicNode.create_client(ResetSimulation, "/gzserver/reset_simulation") + self.observeClient = self.scenicNode.create_client(GetEntityState, "/gzserver/get_entity_state") + self.stepClient = self.scenicNode.create_client(StepSimulation, '/gzserver/step_simulation') + self.poseClient = self.scenicNode.create_client(SetEntityState, "/gzserver/set_entity_state") + + + def createSimulation(self, scene, timestep, **kwargs): + if timestep is not None and timestep != self.timestep: + raise RuntimeError( + "cannot customize timestep for individual Gazebo simulations; " + "set timestep when creating the GazeboSimulator instead" + ) + + self.scenario_number += 1 + return GazeboSimulation( + scene, + self.scenicNode, + self.spawnClient, + self.deleteClient, + self.controlClient, + self.resetClient, + self.observeClient, + self.stepClient, + self.poseClient, + self.record, + timestep=self.timestep, + **kwargs, + ) + + def destroy(self): + self.scenicNode.destroy_node() + rclpy.shutdown() + super().destroy() + + +class GazeboSimulation(Simulation): + """ + Simulation class for Gazebo-Scenic + gazebo__ground_truth: the offset FROM the Gazebo frame TO the robot frame + gazebo_yaw_ground_truth: true offset FROM the Gazebo frame TO the robot frame + """ + + def __init__(self, scene, scenicNode, spawnClient, deleteClient, controlClient, resetClient, observeClient, stepClient, poseClient, record, timestep=0.1, **kwargs): + self.scenicNode = scenicNode + self.spawnClient = spawnClient + self.deleteClient = deleteClient + self.controlClient = controlClient + self.resetClient = resetClient + self.observeClient = observeClient + self.stepClient = stepClient + self.poseClient = poseClient + + self.record = record + self.timestep = timestep + self.step_actions = [] + self.navigation_status = None + + self.transforms_available = True + self.numSteps = 0 + + # TODO the following gazebo_<>_ground_truth are meant to be used in + # coordinate transform functions. Specify them if needed + self.gazebo_x_ground_truth = 0.0 + self.gazebo_y_ground_truth = 0.0 + self.gazebo_z_ground_truth = 0.0 + self.gazebo_to_robot_yaw_rot = 0.0 + self.gazebo_yaw_ground_truth = 0.0 + + super().__init__(scene, timestep=timestep, **kwargs) + + def setup(self): + super().setup() # Calls createObjectInSimulator for each object + + return + + def createObjectInSimulator(self, obj): + """ + Spawns the object in the Gazebo simulator. + Args: + obj: the scenic object, needs to have a name and position field + + Returns: + bool success + """ + + print(f"Object name: {obj.name}\nObject position: {obj.position}") + position = obj.position + position = (position[0], position[1], position[2], obj.roll, obj.pitch, obj.yaw) + + position = self.ScenicToGazeboMap(position, obj=obj) + x, y, z, roll, pitch, yaw = position + success = False + if obj.object_type == "robot": + return True + elif obj.object_type == "sdf_object": + pass + elif obj.object_type == "goal_object": + pass + else: + print(f"spawning: {obj.name}") + print(x, y, z, roll, yaw, pitch) + success = SpawnObject( + obj.name, + object_xml=obj.description_file, + node=self.scenicNode, + client=self.spawnClient, + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + file_type=obj.description_file_type, + ) + return success + + def step(self): + """ + This function takes the actions in the action buffer and executes all of them + in simulation. The simulator will unpause and run for a single timestep and + pause again. + """ + # TODO: if you implemented your actions' applyTo to return anything + # other than a function taking no arguments, change this piece of code + # such that the action is properly executed + + # stepping forward breaks stuff, using pause/unpause is more reliable + # TakeStep(self.scenicNode, self.stepClient) + + UnpauseGazebo(self.scenicNode, self.controlClient) + t0 = self.scenicNode.get_clock().now() + + for a in self.step_actions: + try: + a() + except Exception as e: + print( + f"Failed to execute action, proceed to next action, exception\n{str(e)}" + ) + logging.error(traceback.format_exc()) + self.step_actions = [] + + t1 = self.scenicNode.get_clock().now() + time_elapsed = t1 - t0 + while time_elapsed.to_msg().sec < self.timestep: + t1 = self.scenicNode.get_clock().now() + time_elapsed = t1 - t0 + PauseGazebo(self.scenicNode, self.controlClient) + self.numSteps += 1 + return + + def getProperties(self, obj, properties): + """ + Gets the state of the object at each timestep + Not directly called by the user + """ + try: + obj_gazebo_state = GetObjectState(obj.name, node=self.scenicNode, client=self.observeClient) + pose = ( + obj_gazebo_state["x"], + obj_gazebo_state["y"], + obj_gazebo_state["z"], + obj_gazebo_state["roll"], + obj_gazebo_state["pitch"], + obj_gazebo_state["yaw"], + ) + + pose = self.GazeboToScenicMap(pose, obj=obj) + v = obj_gazebo_state["velocity"] + w = obj_gazebo_state["angularVelocity"] + d = dict( + position=Vector(pose[0], pose[1], pose[2]), + yaw=pose[-1], + pitch=pose[4], + roll=pose[3], + speed=obj_gazebo_state["speed"], + velocity=Vector(v.x, v.y, v.z), + angularSpeed=obj_gazebo_state["angularSpeed"], + angularVelocity=Vector(w.x, w.y, w.z), + ) + + return d + + except Exception as e: + raise RuntimeError( + f"Failed to get {obj.name} states. An exception occured: {e}", e + ) + + def destroy(self): + PauseGazebo(self.scenicNode, self.controlClient) # Finally, pause Gazebo + for obj in self.objects: # Delete all objects spawned by scenic + if ( + obj.object_type != "robot" and obj.object_type != "sdf_object" + ): + success = DeleteObject(obj.name, client=self.deleteClient, node=self.scenicNode, sim=self) + print(f"Deleted Model: {success}") + + ResetGazeboWorld(self.scenicNode, self.resetClient) + super().destroy() + return + + # NOTE: You may need to modify these to your own use case if needed. + def GazeboToRobotMap(self, pose): + """ + Converts from the gazebo map frame to the Robot map frame + Args: + pose = Tuple(x, y, z, roll, pitch, yaw) + """ + x_offset = self.gazebo_x_ground_truth + y_offset = self.gazebo_y_ground_truth + z_offset = self.gazebo_z_ground_truth + yaw_offset = self.gazebo_yaw_ground_truth + yaw_rotation = self.gazebo_to_robot_yaw_rot + + g = np.array( + [ + [np.cos(yaw_offset), -np.sin(yaw_offset), x_offset], + [np.sin(yaw_offset), np.cos(yaw_offset), y_offset], + [0, 0, 1], + ] + ) + g = np.linalg.inv(g) + + x, y, _ = g @ np.array(list(pose[:2]) + [1]) + z = pose[2] - z_offset + yaw = pose[-1] - yaw_offset + + return (x, y, z, pose[3], pose[4], yaw) + + def RobotToGazeboMap(self, pose): + """ + Converts from the Robot map frame to the gazebo map frame + Args: + pose: (x, y, z, roll, pitch, yaw) + """ + x_offset = self.gazebo_x_ground_truth + y_offset = self.gazebo_y_ground_truth + z_offset = self.gazebo_z_ground_truth + yaw_offset = self.gazebo_yaw_ground_truth + g = np.array( + [ + [np.cos(yaw_offset), -np.sin(yaw_offset), x_offset], + [np.sin(yaw_offset), np.cos(yaw_offset), y_offset], + [0, 0, 1], + ] + ) + x, y, _ = g @ np.array(list(pose[:2]) + [1]) + + z = pose[2] + z_offset + yaw = pose[-1] + yaw_offset + + return (x, y, z, pose[3], pose[4], yaw) + + def ScenicToRobotMap(self, pose, obj=None): + """ + Converts from the Scenic map coordinate to the Robot map frame + Args: + pose: (x, y, z, yaw) + """ + x, y, z, roll, pitch, yaw = pose + if obj and hasattr(obj, "positionOffset"): + dx, dy, dz = ( + obj.positionOffset[0], + obj.positionOffset[1], + obj.positionOffset[2], + ) + x = x + dx + y = y + dy + z = z + dz + + return (x, y, z, roll, pitch, yaw + math.pi / 2) + + def RobotToScenicMap(self, pose, obj=None): + """ + Converts from the Robot 'map' frame coordinate to the Scenic map coordinate + Args: + pose: (x, y, z, roll, pitch, yaw) + obj: the Scenic object + """ + x, y, z, roll, pitch, yaw = pose + + if obj and hasattr(obj, "positionOffset"): + dx, dy, dz = ( + obj.positionOffset[0], + obj.positionOffset[1], + obj.positionOffset[2], + ) + x = x - dx + y = y - dy + z = z - dz + return (x, y, z, roll, pitch, yaw - math.pi / 2) + + def ScenicToGazeboMap(self, pose, obj=None): + """ + Converts from the Scenic map coordinate to the Gazebo map frame coordinate + Args: + pose = Tuple(x, y, z, roll, pitch, yaw) + obj: the scenic object + """ + return self.RobotToGazeboMap(self.ScenicToRobotMap(pose, obj=obj)) + + def GazeboToScenicMap(self, pose, obj=None): + """ + Converts from the Gazebo map frame coordinate to the Scenic map coordinate + Args: + pose = Tuple(x, y, z, roll, pitch, yaw) + obj: the scenic object + """ + return self.RobotToScenicMap(self.GazeboToRobotMap(pose), obj=obj) diff --git a/src/scenic/simulators/gazebo/utils/parse_sdf.py b/src/scenic/simulators/gazebo/utils/parse_sdf.py new file mode 100644 index 000000000..b3a85b664 --- /dev/null +++ b/src/scenic/simulators/gazebo/utils/parse_sdf.py @@ -0,0 +1,207 @@ +import sdformat14 as sdf +import sys +import xml.etree.ElementTree as ET + +from trimesh import load_mesh +from trimesh.transformations import compose_matrix +from trimesh.primitives import Box, Capsule, Cylinder, Sphere +from trimesh.creation import cone +from trimesh.boolean import union +from pathlib import Path +from scenic.core.utils import repairMesh + +# to use sdformat python bindings: +# sudo apt install libsdformat14-dev python3-sdformat14 +# add include-system-site-packages = true to your venv config file + +def parse_sdf(input_file): + """ + Returns a single combined mesh of all objects in the SDF file + """ + root = sdf.Root() + unifiedMeshes = [] + try: + root.load(input_file) + except sdf.SDFErrorsException as e: + print(e, file=sys.stderr) + world = root.world_by_index(0) # assume only one world + + # there's no way to get tags under world + # the methods to get them just don't exist in the Python bindings(but they do in the C++ API) + # so it has to be done through parsing the xml + + # get meshes under the level + unifiedMeshes += extractIncludeMeshes(input_file) + + # get meshes from unifying collision geometries of the model's links + for model_index in range(world.model_count()): + sdfModel = world.model_by_index(model_index) + newMesh = unifyModelMeshes(sdfModel) + if newMesh: + unifiedMeshes.append(newMesh) + + finalMesh = union(unifiedMeshes, engine='manifold') + + # Bottom of the mesh needs to be aligned with z=0 in the scenario file: + # spawnHeight = bounds[1][2] / 2 + # setting = new SDFObject at (0, 0, spawnHeight) + return finalMesh, finalMesh.bounds + +def parse_sdf_for_graph(input_file): + """ + Returns individual meshes of all objects in the SDF file + """ + root = sdf.Root() + unifiedMeshes = [] + try: + root.load(input_file) + except sdf.SDFErrorsException as e: + print(e, file=sys.stderr) + world = root.world_by_index(0) # assume only one world + + # there's no way to get tags under world + # the methods to get them just don't exist in the Python bindings(but they do in the C++ API) + # so it has to be done through parsing the xml + + # get meshes under the level + unifiedMeshes += extractIncludeMeshes(input_file) + + # get meshes from unifying collision geometries of the model's links + for model_index in range(world.model_count()): + sdfModel = world.model_by_index(model_index) + newMesh = unifyModelMeshes(sdfModel) + if newMesh: + unifiedMeshes.append(newMesh) + + return unifiedMeshes + +def unifyModelMeshes(sdfModel, meshVersionDir=None): + """ + Unifies the meshes of the links of the provided model into a single mesh + """ + + modelPose = sdfModel.raw_pose() + meshes = [] + + for link_index in range(sdfModel.link_count()): + link = sdfModel.link_by_index(link_index) + linkPose = link.raw_pose() + linkTransform = compose_matrix( + angles=[linkPose.roll(), linkPose.pitch(), linkPose.yaw()], + translate=[linkPose.x(), linkPose.y(), linkPose.z()] + ) + + for collision_index in range(link.collision_count()): + collision = link.collision_by_index(collision_index) + collisionPose = collision.raw_pose() + collisionTransform = compose_matrix( + angles=[collisionPose.roll(), collisionPose.pitch(), collisionPose.yaw()], + translate=[collisionPose.x(), collisionPose.y(), collisionPose.z()] + ) + geometry = collision.geometry() + mesh = None + + if geometry.type() == sdf.GeometryType.BOX: + boxGeometry = geometry.box_shape().size() + mesh = Box([boxGeometry.x(), boxGeometry.y(), boxGeometry.z()], linkTransform) + + elif geometry.type() == sdf.GeometryType.CYLINDER: + cylinderGeometry = geometry.cylinder_shape() + mesh = Cylinder(cylinderGeometry.radius(), cylinderGeometry.length(), linkTransform) + + elif geometry.type() == sdf.GeometryType.SPHERE: + sphereGeometry = geometry.sphere_shape() + mesh = Sphere(sphereGeometry.radius(), transform=linkTransform) + + elif geometry.type() == sdf.GeometryType.CAPSULE: + capsuleGeometry = geometry.capsule_shape() + mesh = Capsule(capsuleGeometry.radius(), capsuleGeometry.length(), linkTransform) + + elif geometry.type() == sdf.GeometryType.CONE: + coneGeometry = geometry.cone_shape() + mesh = cone(coneGeometry.radius(), coneGeometry.length(), transform=linkTransform) + + elif geometry.type() == sdf.GeometryType.MESH: + uri = geometry.mesh_shape().uri() + scale = geometry.mesh_shape().scale() + uriSplit = uri.split('/') + + if uriSplit[0] == 'https:': + meshPath = meshVersionDir / 'meshes' / uriSplit[10] + mesh = load_mesh(meshPath) + mesh = repairMesh(mesh) + mesh.apply_scale(scale[0]) + elif uriSplit[0] == 'meshes': + meshPath = meshVersionDir / 'meshes' / uriSplit[1] + mesh = load_mesh(meshPath) + mesh = repairMesh(mesh) + mesh.apply_scale(scale[0]) + else: + print("Unsupported geometry") # plane, ellipsoid, polyline, heightmap + + if mesh: + # place the collision element relative to others in the link + mesh.apply_transform(collisionTransform) + meshes.append(mesh) + + if len(meshes) > 0: + fullMesh = union(meshes, engine='manifold') + modelTransform = compose_matrix(translate=[modelPose.x(), modelPose.y(), modelPose.z()]) + + # place the model relative to the world and other models + fullMesh.apply_transform(modelTransform) + return fullMesh + else: + return None + +def extractIncludeMeshes(input_file): + """ + Returns a list of meshes from the statements in the provided SDF + """ + + # find path to fuel model directory in filesystem + tree = ET.parse(input_file) + xmlroot = tree.getroot() + includeMeshes = [] + + for include in xmlroot.findall(".//include"): + uri = include.findtext("uri", default="").strip() + name = include.findtext("name", default="").strip() + pose = include.findtext("pose", default="").strip() + uriSplit = uri.split('/') + poseSplit = pose.split(' ') + model_dir_name = uriSplit[6].rstrip().rstrip('\n').lower() + modelroot = ( + Path.home() + / ".gz" + / "fuel" + / uriSplit[2] + / uriSplit[4].lower() + / "models" + / model_dir_name + ) + + version_dir = max( + (p for p in modelroot.iterdir() if p.is_dir() and p.name.isdigit()), + key=lambda p: int(p.name) + ) + + modelSDF = version_dir / 'model.sdf' + root = sdf.Root() + try: + root.load(str(modelSDF)) + except sdf.SDFErrorsException as e: + print(e, file=sys.stderr) + + newMesh = unifyModelMeshes(root.model(), version_dir) + if newMesh: + includeMeshTransform = compose_matrix( + angles=[float(poseSplit[3]), float(poseSplit[4]), float(poseSplit[5])], + translate=[float(poseSplit[0]), float(poseSplit[1]), float(poseSplit[2])] + ) + newMesh.apply_transform(includeMeshTransform) + includeMeshes.append(newMesh) + else: + print("Failed to get mesh!") + + return includeMeshes diff --git a/src/scenic/simulators/gazebo/utils/quaternion_utils.py b/src/scenic/simulators/gazebo/utils/quaternion_utils.py new file mode 100644 index 000000000..f52d0f27f --- /dev/null +++ b/src/scenic/simulators/gazebo/utils/quaternion_utils.py @@ -0,0 +1,42 @@ +"""Quaternion functions removed from ROS2 TF2.""" + +import numpy as np + +# Taken from turtle bot example and stackoverflow + +def quaternion_from_euler(roll, pitch, yaw): + """ + Convert an Euler angle to a quaternion. + + Input + :param roll: The roll (rotation around x-axis) angle in radians. + :param pitch: The pitch (rotation around y-axis) angle in radians. + :param yaw: The yaw (rotation around z-axis) angle in radians. + + Output + :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format + """ + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + + return [qx, qy, qz, qw] + +def euler_from_quaternion(x, y, z, w): + ysqr = y * y + + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + ysqr) + X = np.degrees(np.arctan2(t0, t1)) + + t2 = +2.0 * (w * y - z * x) + + t2 = np.clip(t2, a_min=-1.0, a_max=1.0) + Y = np.degrees(np.arcsin(t2)) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (ysqr + z * z) + Z = np.degrees(np.arctan2(t3, t4)) + + return X, Y, Z \ No newline at end of file diff --git a/src/scenic/simulators/gazebo/utils/spawn_delete_model.py b/src/scenic/simulators/gazebo/utils/spawn_delete_model.py new file mode 100644 index 000000000..abe840261 --- /dev/null +++ b/src/scenic/simulators/gazebo/utils/spawn_delete_model.py @@ -0,0 +1,189 @@ +# Code adapted from the spawn_model script from gazebo_ros + +import os +import xml + +from simulation_interfaces.srv import SpawnEntity, DeleteEntity +from simulation_interfaces.msg import Resource +from geometry_msgs.msg import Pose, Quaternion, Point, PoseStamped + +from scenic.simulators.gazebo.utils.quaternion_utils import quaternion_from_euler + +import rclpy + +def DeleteObject(name, node, client, sim=None): + """ + deletes the object from Gazebo and collision world + Args: + String name: the name of the object + """ + request = DeleteEntity.Request() + request.entity = name + + node.get_logger().info(f"Attempting to delete entity {name}") + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + node.get_logger().error(f"Delete service failed for {name}") + return False + return True + +# def SpawnObject( # for ROS Rolling +# name, +# object_xml, +# node, +# client, +# x=0, +# y=0, +# z=0, +# roll=0, +# pitch=0, +# yaw=0, +# file_type="sdf", +# ref_frame="map", +# ): +# node.get_logger().info(f"Loading model XML from file") +# model_xml = object_xml + +# if not os.path.exists(model_xml): +# node.get_logger().fatal("Error: specified file %s does not exist", model_xml) +# return False +# if not os.path.isfile(model_xml): +# node.get_logger().fatal("Error: specified file %s is not a file", model_xml) +# return False + +# try: +# f = open(model_xml, "r") +# model_xml = f.read() + +# except IOError as e: +# node.get_logger().error(f"Error reading file {model_xml}: {e}") +# return False +# if model_xml == "": +# node.get_logger().error(f"Error: file {model_xml} is empty") +# return False + +# try: +# xml_parsed = xml.etree.ElementTree.fromstring(model_xml) +# except xml.etree.ElementTree.ParseError as e: +# node.get_logger().error(f"Invalid XML: {e}") +# return False + +# model_xml = xml.etree.ElementTree.tostring(xml_parsed) + +# if not isinstance(model_xml, str): +# model_xml = model_xml.decode(encoding="ascii") + +# initial_pose = Pose() +# initial_pose.position = Point() +# initial_pose.position.x = float(x) +# initial_pose.position.y = float(y) +# initial_pose.position.z = float(z) + +# q = quaternion_from_euler(roll, pitch, yaw) +# initial_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + +# request = SpawnEntity.Request() + +# request.name = name +# request.entity_resource = Resource() +# request.entity_resource.uri = object_xml +# request.allow_renaming = False +# stamped_pose = PoseStamped() +# stamped_pose.pose = initial_pose +# request.initial_pose = stamped_pose + +# node.get_logger().info(f"Attempting to spawn entity {name}") + +# future = client.call_async(request) +# rclpy.spin_until_future_complete(node, future) +# if future.result() is not None: +# print('response: %r' % future.result()) +# else: +# node.get_logger().error(f"Spawn service failed for {name}") +# return False + +# return True + + +def SpawnObject( # for ROS Jazzy + name, + object_xml, + node, + client, + x=0, + y=0, + z=0, + roll=0, + pitch=0, + yaw=0, + file_type="sdf", + ref_frame="map", +): + node.get_logger().info(f"Loading model XML from file") + model_xml = object_xml + + if not os.path.exists(model_xml): + node.get_logger().fatal("Error: specified file %s does not exist", model_xml) + return False + if not os.path.isfile(model_xml): + node.get_logger().fatal("Error: specified file %s is not a file", model_xml) + return False + + try: + f = open(model_xml, "r") + model_xml = f.read() + + except IOError as e: + node.get_logger().error(f"Error reading file {model_xml}: {e}") + return False + if model_xml == "": + node.get_logger().error(f"Error: file {model_xml} is empty") + return False + + try: + xml_parsed = xml.etree.ElementTree.fromstring(model_xml) + except xml.etree.ElementTree.ParseError as e: + node.get_logger().error(f"Invalid XML: {e}") + return False + + model_xml = xml.etree.ElementTree.tostring(xml_parsed) + + if not isinstance(model_xml, str): + model_xml = model_xml.decode(encoding="ascii") + + initial_pose = Pose() + initial_pose.position = Point() + initial_pose.position.x = float(x) + initial_pose.position.y = float(y) + initial_pose.position.z = float(z) + + q = quaternion_from_euler(roll, pitch, yaw) + initial_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + + request = SpawnEntity.Request() + + request.name = name + request.uri = object_xml + request.allow_renaming = False + stamped_pose = PoseStamped() + stamped_pose.pose = initial_pose + request.initial_pose = stamped_pose + + node.get_logger().info(f"Attempting to spawn entity {name}") + state = None + for _ in range(20): # sometimes spins forever if you don't set a timeout and retry, not sure why but this is necessary + # print(f'spawning {name}, attempt {i}') + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=1) + if future.result(): + print(future.result()) + state = future.result() + break + if state is None: + node.get_logger().error(f"Spawn service failed for {name}") + return False + + return True diff --git a/src/scenic/simulators/gazebo/utils/start_gazebo.py b/src/scenic/simulators/gazebo/utils/start_gazebo.py new file mode 100644 index 000000000..6d71cb58c --- /dev/null +++ b/src/scenic/simulators/gazebo/utils/start_gazebo.py @@ -0,0 +1,76 @@ +from simulation_interfaces.srv import SetSimulationState, ResetSimulation, StepSimulation +from simulation_interfaces.msg import SimulationState + +import rclpy + + +def PauseGazebo(node, client): + """ + Pauses Gazebo + """ + + request = SetSimulationState.Request() + + request.state = SimulationState() + request.state.state = SimulationState.STATE_PAUSED + + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + raise RuntimeError( + 'exception while calling service: %r' % future.exception()) + return + + +def UnpauseGazebo(node, client): + """ + Unpauses Gazebo + """ + request = SetSimulationState.Request() + + request.state = SimulationState() + request.state.state = SimulationState.STATE_PLAYING + + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + raise RuntimeError( + 'exception while calling service: %r' % future.exception()) + return + +def TakeStep(node, client): + """ + Steps the simulation forward once + """ + request = StepSimulation.Request() + # 1000 seems to be the max + request.steps = 1000 + + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + raise RuntimeError( + 'exception while calling service: %r' % future.exception()) + return + +def ResetGazeboWorld(node, client): + """ + Resets the Gazebo world. Simulation time is NOT reset + Probably the Reset function that you want + """ + request = ResetSimulation.Request() + + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + if future.result() is not None: + print('response: %r' % future.result()) + else: + raise RuntimeError( + 'exception while calling service: %r' % future.exception()) + return diff --git a/src/scenic/simulators/gazebo/utils/state_utils.py b/src/scenic/simulators/gazebo/utils/state_utils.py new file mode 100644 index 000000000..796f761a3 --- /dev/null +++ b/src/scenic/simulators/gazebo/utils/state_utils.py @@ -0,0 +1,75 @@ +from simulation_interfaces.srv import GetEntityState +from scenic.simulators.gazebo.utils.quaternion_utils import euler_from_quaternion + +import rclpy +import numpy as np + +def GetObjectPose(obj, node, client, frame="map"): + """ + String obj: the name of the object + String frame: the reference frame for the pose + Returns: dictionary containing pose information + """ + state = GetObjectGazeboState(obj, node, client, frame=frame) + if state: + pos = state.pose.position + ori = state.pose.orientation + ori = euler_from_quaternion(ori.x, ori.y, ori.z, ori.w) + return dict(x=pos.x, y=pos.y, z=pos.z, roll=ori[0], pitch=ori[1], yaw=ori[-1]) + + return None + + +def GetObjectState(obj, node, client, frame="map"): + """ + Probably what you want to call to get an object's current states + String obj: the name of the object + String frame: the reference frame for the pose + Returns: dictionary containing state information + """ + try: + state = GetObjectGazeboState(obj, node, client, frame) + if state: + pos = state.pose.position + ori = state.pose.orientation + ori = euler_from_quaternion(ori.x, ori.y, ori.z, ori.w) + linear = state.twist.linear + angular = state.twist.angular + state = dict( + x=pos.x, + y=pos.y, + z=pos.z, + roll=ori[0], + pitch=ori[1], + yaw=ori[-1], + speed=np.linalg.norm(np.array([linear.x, linear.y, linear.z])), + velocity=linear, + angularSpeed=np.linalg.norm(np.array([angular.x, angular.y, angular.z])), + angularVelocity=angular, + ) + return state + except Exception as e: + node.get_logger().error("GetObjectState failed!") + raise e + + +def GetObjectGazeboState(obj, node, client, frame="map"): + """ + String obj: the name of the object + String frame: the reference frame for the pose + Returns: gazebo_msgs.msg.ModelState + """ + request = GetEntityState.Request() + request.entity = obj + state = None + for _ in range(20): # sometimes spins forever if you don't set a timeout and retry, not sure why but this is necessary + # print(f'getting {obj} state, attempt {i}') + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=1) + if future.result(): + state = future.result().state + break + if state is None: + node.get_logger().info("Error getting entity state!") + raise RuntimeError('exception while calling service: %r' % future.exception()) + return state From c4a13760428600bf92fcde36922d4a964d223188 Mon Sep 17 00:00:00 2001 From: Liam Xu Date: Thu, 11 Jun 2026 23:10:32 -0700 Subject: [PATCH 2/2] add readme and example launch file --- src/scenic/simulators/gazebo/README.md | 22 ++++++++++++ .../simulators/gazebo/combined_launch.py | 36 +++++++++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 src/scenic/simulators/gazebo/README.md create mode 100644 src/scenic/simulators/gazebo/combined_launch.py diff --git a/src/scenic/simulators/gazebo/README.md b/src/scenic/simulators/gazebo/README.md new file mode 100644 index 000000000..90e3571d5 --- /dev/null +++ b/src/scenic/simulators/gazebo/README.md @@ -0,0 +1,22 @@ +# Scenic-Gazebo Interface +This is the code for the Scenic-Gazebo interface. + +## Requirements and Installation +- Gazebo Harmonic +- ROS 2 Jazzy +### ROS Packages +- ros-gz +- simulation-interfaces +- ros2launch +- libsdformat14 +- python3-sdformat14 + +## Instructions +This interface requires a running Gazebo simulation with the /gzserver endpoint up, which is launched with a ROS launch file. combined_launch.py is provided as a simple example of one. To use the interface, run: +- ros2 launch combined_launch.py +- scenic *some_scenario.scenic* -S + +The interface can spawn, delete, get object states, and pause, unpause, and reset the simulation. Any robot-specific code isn't included. + +## Notes +- python3-sdformat14 is a system package, to use it in a venv add include-system-site-packages = true to your venv config file \ No newline at end of file diff --git a/src/scenic/simulators/gazebo/combined_launch.py b/src/scenic/simulators/gazebo/combined_launch.py new file mode 100644 index 000000000..7033dd82a --- /dev/null +++ b/src/scenic/simulators/gazebo/combined_launch.py @@ -0,0 +1,36 @@ +# mostly copied from gz_server.launch.py in ros_gz_sim +"""Launch gz_server in a component container, and also launch the Gazebo GUI client.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration, TextSubstitution +from ros_gz_sim.actions import GzServer + + +def generate_launch_description(): + + declare_world_sdf_file_cmd = DeclareLaunchArgument( + 'world_sdf_file', default_value=TextSubstitution(text='empty.sdf'), + description='Path to the SDF world file') + + #starts server + gz_server_action = GzServer( + world_sdf_file=LaunchConfiguration('world_sdf_file'), + ) + + #starts gui + gz_sim_action = ExecuteProcess( + cmd=['gz', 'sim', '-r', LaunchConfiguration('world_sdf_file')], + output='screen' + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_sdf_file_cmd) + # Add the gz_server action + ld.add_action(gz_server_action) + ld.add_action(gz_sim_action) + + return ld