def __init__(self,
world: scenario.World,
name: str = 'object',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
model_paths: str = None,
owner: str = 'GoogleResearch',
collection: str = 'Google Scanned Objects',
server: str = 'https://fuel.ignitionrobotics.org',
server_version: str = '1.0',
unique_cache: bool = False,
reset_collection: bool = False,
np_random=None):
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
model_collection_randomizer = ModelCollectionRandomizer(
model_paths=model_paths,
owner=owner,
collection=collection,
server=server,
server_version=server_version,
unique_cache=unique_cache,
reset_collection=reset_collection,
np_random=np_random)
# Note: using default arguments here
modified_sdf_file = model_collection_randomizer.random_model()
# Insert the model
ok_model = world.to_gazebo().insert_model(modified_sdf_file,
initial_pose, model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
def test_world_physics_plugin(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
dt = gazebo.step_size()
assert world.time() == 0
# Insert a cube
cube_urdf = utils.get_cube_urdf()
cube_name = "my_cube"
cube_pose = core.Pose([0, 0, 1.0], [1, 0, 0, 0])
assert world.insert_model(cube_urdf, cube_pose, cube_name)
assert cube_name in world.model_names()
cube = world.get_model(cube_name)
# There's no physics, the cube should not move
for _ in range(10):
gazebo.run()
assert cube.base_position() == cube_pose.position
assert world.time() == 0
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# After the first step, the physics catches up with time
gazebo.run()
assert world.time() == pytest.approx(11 * dt)
# The cube should start falling
assert cube.base_position()[2] < cube_pose.position[2]
gazebo.run()
assert world.time() == pytest.approx(12 * dt)
# Paused steps do not increase the time
gazebo.run(paused=True)
assert world.time() == pytest.approx(12 * dt)
def insert_bucket(world: scenario_gazebo.World) -> scenario_gazebo.Model:
# Insert objects from Fuel
uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}"
# Download the cube SDF file
bucket_sdf = scenario_gazebo.get_model_file_from_fuel(uri=uri(
org="GoogleResearch",
name="Threshold_Basket_Natural_Finish_Fabric_Liner_Small"),
use_cache=False)
# Assign a custom name to the model
model_name = "bucket"
# Insert the model
assert world.insert_model(
bucket_sdf, scenario_core.Pose([0.68, 0, 1.02], [1., 0, 0, 1]),
model_name)
# Return the model
return world.get_model(model_name=model_name)
def insert_model(self,
model_template: str,
position: Tuple[float, float, float],
orientation: Tuple[float, float, float,
float] = (0, 0, 0, 1),
velocity: Tuple[float, float, float] = (0, 0, 0),
angular_velocity: Tuple[float, float, float] = (0, 0, 0),
*,
name_prefix="",
**template_parameters):
"""Spawn the model into the simulation world"""
if model_template not in self.model_cache:
with open(model_template, "r") as file:
self.model_cache[model_template] = file.read()
if isinstance(velocity, np.ndarray):
velocity = velocity.tolist()
if isinstance(angular_velocity, np.ndarray):
angular_velocity = angular_velocity.tolist()
world = self.world
model = self.model_cache[model_template].format(**template_parameters)
model_name = gym_ignition.utils.scenario.get_unique_model_name(
world=world, model_name=name_prefix)
pose = scenario_core.Pose(position, orientation)
assert world.insert_model(model, pose, model_name)
obj = world.get_model(model_name)
velocity = scenario_core.Array3d(velocity)
assert obj.to_gazebo().reset_base_world_linear_velocity(velocity)
angular_velocity = scenario_core.Array3d(angular_velocity)
assert obj.to_gazebo().reset_base_world_angular_velocity(velocity)
return obj
def insert_cube_in_operating_area(world: scenario_gazebo.World) -> scenario_gazebo.Model:
# Insert objects from Fuel
uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}"
# Download the cube SDF file
cube_sdf = scenario_gazebo.get_model_file_from_fuel(
uri=uri(org="openrobotics", name="wood cube 5cm"), use_cache=False)
# Sample a random position
random_position = np.random.uniform(low=[0.2, -0.3, 1.01], high=[0.4, 0.3, 1.01])
# Get a unique name
model_name = gym_ignition.utils.scenario.get_unique_model_name(
world=world, model_name="cube")
# Insert the model
assert world.insert_model(
cube_sdf, scenario_core.Pose(random_position, [1., 0, 0, 0]), model_name)
# Return the model
return world.get_model(model_name=model_name)
def __init__(
self,
world: scenario_core.World,
position: List[float],
orientation: List[float],
model_file: str = None,
):
# Get a unique model name
model_name = get_unique_model_name(world, "icub")
# Initial pose
initial_pose = scenario_core.Pose(position, orientation)
# Get the model file (URDF or SDF) and allow to override it
if model_file is None:
model_file = self.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initialize base class
super().__init__(model=model)
# Store the initial positions
q0 = list(self.initial_positions.values())
joint_names = list(self.initial_positions.keys())
assert self.dofs() == len(q0) == len(joint_names)
ok_q0 = self.to_gazebo().reset_joint_positions(q0, joint_names)
assert ok_q0, "Failed to set initial position"
def test_download_model_from_fuel(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
# Get the default world
world = gazebo.get_world()
# Download a model from Fuel (testing a name with spaces)
model_name = "Electrical Box"
model_sdf = scenario.get_model_file_from_fuel(
f"https://fuel.ignitionrobotics.org/openrobotics/models/{model_name}", False
)
assert model_sdf
assert world.insert_model(model_sdf, core.Pose_identity())
assert model_name in world.model_names()
# Insert another model changing its name
other_model_name = "my_box"
other_model_pose = core.Pose([3.0, 0.0, 0.0], [1.0, 0, 0, 0])
assert world.insert_model(model_sdf, other_model_pose, other_model_name)
assert other_model_name in world.model_names()
assert gazebo.run()
def __init__(self,
world: scenario.World,
name: str = 'plane',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
size: List[float] = (1.0, 1.0),
direction: List[float] = (0.0, 0.0, 1.0),
collision: bool = True,
friction: float = 1.0,
visual: bool = True):
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
{
f"""
<collision name="{model_name}_collision">
<geometry>
<plane>
<normal>{direction[0]} {direction[1]} {direction[2]}</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
<mu2>{friction}</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
""" if collision else ""
}
{
f"""
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>{direction[0]} {direction[1]} {direction[2]}</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
""" if visual else ""
}
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file,
initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
def test_model_base_velocity(default_world: Tuple[scenario.GazeboSimulator,
scenario.World]):
# Get the simulator and the world
gazebo, world = default_world
model_urdf = gym_ignition_models.get_model_file("pendulum")
# Insert the first pendulum
model1_name = "pendulum1"
model1_pose = core.Pose([0, 1.0, 1.0], [1.0, 0, 0, 0])
assert world.insert_model(model_urdf, model1_pose, model1_name)
model1 = world.get_model(model1_name).to_gazebo()
assert model1.valid()
assert model1_name in gazebo.get_world().model_names()
assert model1.base_frame() == "support"
# Insert the second pendulum
model2_name = "pendulum2"
model2_pose = core.Pose([0, -1.0, 1.0], [1.0, 0, 0, 0])
assert world.insert_model(model_urdf, model2_pose, model2_name)
model2 = world.get_model(model2_name).to_gazebo()
assert model2.valid()
assert model2_name in gazebo.get_world().model_names()
assert model2.base_frame() == "support"
# Target velocity
lin_velocity = [0, 0, 5.0]
ang_velocity = [0.1, 0.5, -3.0]
# Reset both linear and angular of pendulum1 by calling the combined method
assert model1.reset_base_world_velocity(lin_velocity, ang_velocity)
assert model1.base_world_linear_velocity() == pytest.approx([0, 0, 0])
assert model1.base_world_angular_velocity() == pytest.approx([0, 0, 0])
# Reset both linear and angular of pendulum2 by calling the individual method
assert model2.reset_base_world_linear_velocity(lin_velocity)
assert model2.reset_base_world_angular_velocity(ang_velocity)
assert model2.base_world_linear_velocity() == pytest.approx([0, 0, 0])
assert model2.base_world_angular_velocity() == pytest.approx([0, 0, 0])
# Run the simulation
gazebo.run()
# Check model1 velocity
assert model1.base_world_linear_velocity() == pytest.approx(lin_velocity,
abs=0.01)
assert model1.base_world_angular_velocity() == pytest.approx(ang_velocity,
abs=0.01)
# Check model2 velocity
assert model2.base_world_linear_velocity() == pytest.approx(lin_velocity,
abs=0.01)
assert model2.base_world_angular_velocity() == pytest.approx(ang_velocity,
abs=0.01)
# The velocity of the base link must be the same
assert model1.get_link("support").world_linear_velocity() == pytest.approx(
lin_velocity, abs=0.01)
assert model1.get_link(
"support").world_angular_velocity() == pytest.approx(ang_velocity,
abs=0.01)
# The velocity of the base link must be the same
assert model2.get_link("support").world_linear_velocity() == pytest.approx(
lin_velocity, abs=0.01)
assert model2.get_link(
"support").world_angular_velocity() == pytest.approx(ang_velocity,
abs=0.01)
def test_world_api(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
gravity = [0, 0, 10.0]
assert world.set_gravity(gravity)
assert world.gravity() == pytest.approx(gravity)
assert len(world.model_names()) == 0
# Insert a model from an non existent file
assert not world.insert_model("")
# Insert first cube with default name and pose
cube_urdf = utils.get_cube_urdf()
assert world.insert_model(cube_urdf)
assert len(world.model_names()) == 1
default_model_name = scenario.get_model_name_from_sdf(cube_urdf)
assert default_model_name in world.model_names()
cube1 = world.get_model(default_model_name)
# TODO: assert cube1
assert cube1.name() == default_model_name
assert cube1.base_position() == pytest.approx([0, 0, 0])
assert cube1.base_orientation() == pytest.approx([1, 0, 0, 0])
# Inserting a model with the same name should fail
assert not world.insert_model(cube_urdf)
assert len(world.model_names()) == 1
# Insert second cube with custom name and pose
custom_model_name = "other_cube"
assert custom_model_name != default_model_name
custom_model_pose = core.Pose([1, 1, 0], [0, 0, 0, 1])
assert world.insert_model(cube_urdf, custom_model_pose, custom_model_name)
assert custom_model_name in world.model_names()
assert len(world.model_names()) == 2
cube2 = world.get_model(custom_model_name)
assert cube1 != cube2
# TODO: assert cube2
assert cube2.name() == custom_model_name
assert cube2.base_position() == pytest.approx(custom_model_pose.position)
assert cube2.base_orientation() == pytest.approx(
custom_model_pose.orientation)
# insert a cube from urdf string
cube_3_pose = core.Pose([1, 0, 0], [0, 0, 0, 1])
assert world.insert_model_from_string(utils.get_cube_urdf_string(),
cube_3_pose, "cube3")
assert "cube3" in world.model_names()
# insert a cube from sdf string
cube_4_pose = core.Pose([2, 0, 0], [0, 0, 0, 1])
assert world.insert_model_from_string(utils.get_cube_sdf_string(),
cube_4_pose, "cube4")
assert "cube4" in world.model_names()
# Remove the first model (requires either a paused or unpaused step)
assert world.remove_model(default_model_name)
assert len(world.model_names()) == 4
gazebo.run(paused=True)
assert len(world.model_names()) == 3
# Without the physics system, the time should not increase
gazebo.run()
gazebo.run()
gazebo.run()
assert world.time() == 0.0
def test_cube_multiple_contacts(gazebo: scenario.GazeboSimulator,
get_model_str: Callable):
assert gazebo.initialize()
world = gazebo.get_world().to_gazebo()
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Insert the ground plane
assert world.insert_model(gym_ignition_models.get_model_file("ground_plane"))
assert len(world.model_names()) == 1
# Insert two cubes side to side with a 10cm gap
cube_urdf = misc.string_to_file(get_model_str())
assert world.insert_model(cube_urdf,
core.Pose([0, -0.15, 0.101], [1., 0, 0, 0]),
"cube1")
assert world.insert_model(cube_urdf,
core.Pose([0, 0.15, 0.101], [1., 0, 0, 0]),
"cube2")
assert len(world.model_names()) == 3
# Get the cubes
cube1 = world.get_model("cube1")
cube2 = world.get_model("cube2")
# Enable contact detection
assert not cube1.contacts_enabled()
assert cube1.enable_contacts(enable=True)
assert cube1.contacts_enabled()
# Enable contact detection
assert not cube2.contacts_enabled()
assert cube2.enable_contacts(enable=True)
assert cube2.contacts_enabled()
# The cubes were inserted floating in the air with a 1mm gap wrt to ground.
# There should be no contacts.
gazebo.run(paused=True)
assert not cube1.get_link("cube").in_contact()
assert not cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 0
assert len(cube2.contacts()) == 0
# Make the cubes fall for 50ms
for _ in range(50):
gazebo.run()
assert cube1.get_link("cube").in_contact()
assert cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 1
assert len(cube2.contacts()) == 1
# Now we make another cube fall above the gap. It will touch both cubes.
assert world.insert_model(cube_urdf,
core.Pose([0, 0, 0.301], [1., 0, 0, 0]),
"cube3")
assert len(world.model_names()) == 4
cube3 = world.get_model("cube3")
assert not cube3.contacts_enabled()
assert cube3.enable_contacts(enable=True)
assert cube3.contacts_enabled()
gazebo.run(paused=True)
assert not cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 0
# Make the cube fall for 50ms
for _ in range(50):
gazebo.run()
# There will be two contacts, respectively with cube1 and cube2.
# Contacts are related to the link, not the collision elements. In the case of two
# collisions elements associated to the same link, contacts are merged together.
assert cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 2
contact1 = cube3.contacts()[0]
contact2 = cube3.contacts()[1]
assert contact1.body_a == "cube3::cube"
assert contact2.body_a == "cube3::cube"
assert contact1.body_b == "cube1::cube"
assert contact2.body_b == "cube2::cube"
# Calculate the total contact wrench of cube3
assert cube3.get_link("cube").contact_wrench() == pytest.approx([0, 0, 50, 0, 0, 0],
abs=1.1)
# Calculate the total contact force of the cubes below.
# They will have 1.5 their weight from below and -0.5 from above.
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
# Check the normals and the sign of the forces
for contact in cube2.contacts():
if contact.body_b == "cube3::cube":
for point in contact.points:
assert point.force[2] < 0
assert point.normal == pytest.approx([0, 0, -1], abs=0.001)
if contact.body_b == "ground_plane::link":
for point in contact.points:
assert point.force[2] > 0
assert point.normal == pytest.approx([0, 0, 1], abs=0.001)
def __init__(self,
world: scenario.World,
position: List[float] = (0.0, 0.0, 0.0),
orientation: List[float] = (1.0, 0, 0, 0),
model_file: str = None):
# Get a unique model name
model_name = get_unique_model_name(world, "panda")
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Get the default model description (URDF or SDF) allowing to pass a custom model
if model_file is None:
model_file = Panda.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file,
initial_pose,
model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initial joint configuration
model.to_gazebo().reset_joint_positions(
[0, -0.785,0, -2.356, 0, 1.571, 0.785],
[name for name in model.joint_names() if "panda_joint" in name])
# From:
# https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml
pid_gains_1000hz = {
'panda_joint1': scenario.PID(50, 0, 20),
'panda_joint2': scenario.PID(10000, 0, 500),
'panda_joint3': scenario.PID(100, 0, 10),
'panda_joint4': scenario.PID(1000, 0, 50),
'panda_joint5': scenario.PID(100, 0, 10),
'panda_joint6': scenario.PID(100, 0, 10),
'panda_joint7': scenario.PID(10, 0.5, 0.1),
'panda_finger_joint1': scenario.PID(100, 0, 50),
'panda_finger_joint2': scenario.PID(100, 0, 50),
}
# Check that all joints have gains
if not set(model.joint_names()) == set(pid_gains_1000hz.keys()):
raise ValueError("The number of PIDs does not match the number of joints")
# Set the PID gains
for joint_name, pid in pid_gains_1000hz.items():
if not model.get_joint(joint_name).set_pid(pid=pid):
raise RuntimeError(f"Failed to set the PID of joint '{joint_name}'")
# Set the default PID update period
assert model.set_controller_period(1000.0)
# Initialize base class
super().__init__(model=model)
def __init__(self,
world: scenario.World,
name: str = 'ground',
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
size: List[float] = (1.0, 1.0),
collision_thickness: float = 0.05,
friction: float = 5.0,
texture_dir: str = None,
np_random=None):
if np_random is None:
np_random = np.random.default_rng()
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Find random PBR texture
albedo_map = None
normal_map = None
roughness_map = None
metalness_map = None
if texture_dir is not None:
# Get list of the available textures
textures = os.listdir(texture_dir)
# Keep only texture directories if texture_dir is a git repo (ugly fix)
try:
textures.remove('.git')
except:
pass
try:
textures.remove('README.md')
except:
pass
# Choose a random texture from these
random_texture_dir = os.path.join(texture_dir,
np_random.choice(textures))
# List all files
texture_files = os.listdir(random_texture_dir)
# Extract the appropriate files
for texture in texture_files:
texture_lower = texture.lower()
if 'basecolor' in texture_lower or 'albedo' in texture_lower:
albedo_map = os.path.join(random_texture_dir, texture)
elif 'normal' in texture_lower:
normal_map = os.path.join(random_texture_dir, texture)
elif 'roughness' in texture_lower:
roughness_map = os.path.join(random_texture_dir, texture)
elif 'specular' in texture_lower or 'metalness' in texture_lower:
metalness_map = os.path.join(random_texture_dir, texture)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<collision name="{model_name}_collision">
<pose>0 0 {-collision_thickness/2} 0 0 0</pose>
<geometry>
<box>
<size>{size[0]} {size[1]} {collision_thickness}</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
<mu2>{friction}</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
{"<albedo_map>%s</albedo_map>"
% albedo_map if albedo_map is not None else ""}
{"<normal_map>%s</normal_map>"
% normal_map if normal_map is not None else ""}
{"<roughness_map>%s</roughness_map>"
% roughness_map if roughness_map is not None else ""}
{"<metalness_map>%s</metalness_map>"
% metalness_map if metalness_map is not None else ""}
</metal>
</pbr>
</material>
</visual>
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
def __init__(self,
world: scenario.World,
name: Union[str, None] = None,
position: List[float] = (0, 0, 0),
orientation: List[float] = (1, 0, 0, 0),
camera_type: str = 'rgbd_camera',
width: int = 212,
height: int = 120,
update_rate: int = 15,
horizontal_fov: float = 1.567821,
vertical_fov: float = 1.022238,
clip_color: List[float] = (0.01, 1000.0),
clip_depth: List[float] = (0.01, 10.0),
noise_mean: float = None,
noise_stddev: float = None,
ros2_bridge_color: bool = False,
ros2_bridge_depth: bool = False,
ros2_bridge_points: bool = False,
visibility_mask: int = 2):
# Get a unique model name
if name is not None:
model_name = get_unique_model_name(world, name)
else:
model_name = get_unique_model_name(world, camera_type)
self._model_name = model_name
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Create SDF string for the model
sdf = \
f'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<sensor name="camera" type="{camera_type}">
<topic>{model_name}</topic>
<always_on>1</always_on>
<update_rate>{update_rate}</update_rate>
<camera name="{model_name}_camera">
<image>
<width>{width}</width>
<height>{height}</height>
</image>
<horizontal_fov>{horizontal_fov}</horizontal_fov>
<vertical_fov>{vertical_fov}</vertical_fov>
<clip>
<near>{clip_color[0]}</near>
<far>{clip_color[1]}</far>
</clip>
{
f"""<depth_camera>
<clip>
<near>{clip_depth[0]}</near>
<far>{clip_depth[1]}</far>
</clip>
</depth_camera>""" if self.is_rgbd() else ""
}
{
f"""<noise>
<type>gaussian</type>
<mean>{noise_mean}</mean>
<stddev>{noise_stddev}</stddev>
</noise>""" if noise_mean is not None and noise_stddev is not None else ""
}
<visibility_mask>{visibility_mask}</visibility_mask>
</camera>
</sensor>
</link>
</model>
</sdf>'''
# Convert it into a file
sdf_file = misc.string_to_file(sdf)
# Insert the model
ok_model = world.to_gazebo().insert_model(sdf_file, initial_pose,
model_name)
if not ok_model:
raise RuntimeError('Failed to insert ' + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)
if ros2_bridge_color or ros2_bridge_depth or ros2_bridge_points:
from threading import Thread
# Note: This unfortunately hinders use of SIGINT
threads = []
if ros2_bridge_color:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.color_topic(),
'sensor_msgs/msg/Image',
'ignition.msgs.Image'),
daemon=True)
threads.append(thread)
if ros2_bridge_depth:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.depth_topic(),
'sensor_msgs/msg/Image',
'ignition.msgs.Image'),
daemon=True)
threads.append(thread)
if ros2_bridge_points:
thread = Thread(target=self.construct_ros2_bridge,
args=(self.points_topic(),
'sensor_msgs/msg/PointCloud2',
'ignition.msgs.PointCloudPacked'),
daemon=True)
threads.append(thread)
for thread in threads:
thread.start()
