from simulation.utils.road.sections import Pedestrian
[docs]def draw(name, obst):
if type(obst) == Pedestrian:
return pedestrian_model(
name,
obst.center.x,
obst.center.y,
obst.orientation,
)
else:
return obstacle_model(
name,
obst.center.x,
obst.center.y,
obst.depth,
obst.width,
obst.height,
obst.orientation,
)
[docs]def pedestrian_model(name, x, y, orientation):
return """
<model name='{name}'>
<plugin filename="libmodel_plugin_link.so" name="model_plugin_link"/>
<link name='link'>
<visual name='visual'>
<geometry>
<mesh>
<uri>file://meshes/{mesh}.dae</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
</visual>
<collision name='collision'>
<pose>0 0 0.075 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.1 0.15</size>
</box>
</geometry>
</collision>
</link>
<pose>{x} {y} {z} 0 0 {orientation}</pose>
</model>
""".format(
x=x,
y=y,
z=0,
orientation=orientation,
name=name,
mesh="pedestrian",
)
# large masses make model instable
[docs]def obstacle_model(name, x, y, length, width, height, orientation):
return """
<model name='{name}'>
<plugin filename="libmodel_plugin_link.so" name="model_plugin_link"/>
<pose>{x} {y} {z} 0 0 {orientation}</pose>
<link name='link'>
<collision name='collision'>
<geometry>
<box>
<size>{length} {width} {height}</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>{length} {width} {height}</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/White</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
</link>
</model>
""".format(
name=name,
x=x,
y=y,
z=height / 2 + 0.1,
length=length,
width=width,
height=height,
orientation=orientation,
)