Skip to content

Commit 30f18b2

Browse files
IranaphorIranaphor
authored
Merge pull request #1 from LCAS/gazebo_generation
Gazebo generation
2 parents e79ed95 + 32d07b0 commit 30f18b2

File tree

16 files changed

+3010
-1
lines changed

16 files changed

+3010
-1
lines changed

‎environment_common/convertors/objects_to_gazebo.py

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
import sys, os
2+
from ament_index_python.packages import get_package_share_directory, get_package_prefix
3+
4+
import yaml
5+
from pprint import pprint
6+
7+
import environment_common
8+
import environment_common.convertors
9+
from environment_common.convertors.templating.gazebo import GazeboTemplates
10+
from environment_common.convertors.templating.gazebo_models.environmental import Environment
11+
from environment_common.convertors.templating.gazebo_models.meta import Meta
12+
13+
14+
def run(args=None):
15+
16+
17+
# Load objects.yaml file
18+
objects_path = os.path.join(args['src'], 'config', 'world', 'objects.yaml')
19+
if not os.path.isfile(objects_path):
20+
objects_path = os.path.join(args['src'], 'config', 'world', 'objects_autogen.yaml')
21+
with open(objects_path) as f:
22+
data = f.read()
23+
objects = yaml.safe_load(data)
24+
25+
26+
# Pull in custom objects from references
27+
custom_components = []
28+
custom_objects_path = os.path.join(args['src'], 'config', 'world', 'custom_models')
29+
for obj in objects['components']:
30+
31+
# Ensure anchor exists
32+
if 'anchor' not in obj:
33+
obj['anchor'] = {}
34+
if 'position' not in obj['anchor']:
35+
obj['anchor']['position'] = {'x': 0.0, 'y': 0.0}
36+
if 'x' not in obj['anchor']['position']:
37+
obj['anchor']['position']['x'] = 0.0
38+
if 'y' not in obj['anchor']['position']:
39+
obj['anchor']['position']['y'] = 0.0
40+
if 'orientation' not in obj['anchor']:
41+
obj['anchor']['orientation'] = {'yaw': 0.0}
42+
if 'yaw' not in obj['anchor']['orientation']:
43+
obj['anchor']['orientation']['yaw'] = 0.0
44+
45+
# Copy in custom objects and apply the anchor to their components
46+
if obj['type']['object'] == 'custom':
47+
if obj['type']['reference']:
48+
filepath = f"{custom_objects_path}/{obj['type']['reference']}"
49+
with open(filepath) as f:
50+
data = f.read()
51+
custom_object = yaml.safe_load(data)
52+
print(f"Loading in {obj['type']['reference']}")
53+
for cc in custom_object['components']:
54+
cc['anchor'] = obj['anchor']
55+
custom_components += custom_object['components']
56+
57+
objects['components'] += custom_components
58+
59+
print('====')
60+
61+
# Remove reference objects
62+
filtered_components = []
63+
for object in objects['components']:
64+
if 'type' not in object:
65+
filtered_components += [object]
66+
continue
67+
if object['type']['object'] != 'custom':
68+
filtered_components += [object]
69+
continue
70+
objects['components'] = filtered_components
71+
72+
73+
# Set default values for fields
74+
for obj in objects['components']:
75+
if 'size' not in obj:
76+
obj['size'] = {'x': 1.0, 'y': 1.0, 'z': 1.0}
77+
if 'position' not in obj:
78+
obj['position'] = {'x': 0.0, 'y': 0.0, 'z': 0.0}
79+
if 'orientation' not in obj:
80+
obj['orientation'] = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
81+
82+
if 'material' in obj and 'uri' not in obj['material']:
83+
uri = 'file://media/materials/scripts/gazebo.material'
84+
obj['material']['uri'] = uri
85+
obj['material']['name'] = 'Gazebo/'+obj['material']['name']
86+
87+
if 'material' not in obj:
88+
uri = 'file://media/materials/scripts/gazebo.material'
89+
name = 'Gazebo/Grey'
90+
obj['material'] = {'name': name, 'uri': uri}
91+
92+
93+
# Construct gazebo xml string
94+
gazebo = Meta.get(objects)
95+
96+
97+
# Save gazebo.world file
98+
gazebo_path = os.path.join(args['src'], 'config', 'world', 'gazebo_autogen.world.xml')
99+
with open(gazebo_path, 'w') as f:
100+
f.write(gazebo)
101+
102+
103+
def main(args=None):
104+
e = 'environment_template'
105+
src = '/'.join(get_package_prefix(e).split('/')[:-2]) + f'/src/{e}'
106+
args = {'src': src}
107+
run(args)
108+
109+
110+
if __name__ == '__main__':
111+
main()
112+

‎environment_common/convertors/templating/gazebo.py

Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
2+
class GazeboTemplates:
3+
4+
opening = """
5+
<sdf version='1.7'>
6+
<world name='default'>"""
7+
8+
light = """
9+
<light name='sun' type='directional'>
10+
<cast_shadows>1</cast_shadows>
11+
<pose>0 0 10 0 -0 0</pose>
12+
<diffuse>0.8 0.8 0.8 1</diffuse>
13+
<specular>0.2 0.2 0.2 1</specular>
14+
<attenuation>
15+
<range>1000</range>
16+
<constant>0.9</constant>
17+
<linear>0.01</linear>
18+
<quadratic>0.001</quadratic>
19+
</attenuation>
20+
<direction>-0.5 0.1 -0.9</direction>
21+
<spot>
22+
<inner_angle>0</inner_angle>
23+
<outer_angle>0</outer_angle>
24+
<falloff>0</falloff>
25+
</spot>
26+
</light>"""
27+
28+
gravity = """
29+
<gravity>0 0 -9.8</gravity>"""
30+
31+
magnetic_field="""
32+
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>"""
33+
34+
atmosphere="""
35+
<atmosphere type='adiabatic'/>"""
36+
37+
physics="""
38+
<physics type='ode'>
39+
<max_step_size>0.001</max_step_size>
40+
<real_time_factor>1</real_time_factor>
41+
<real_time_update_rate>1000</real_time_update_rate>
42+
</physics>"""
43+
44+
scene="""
45+
<scene>
46+
<ambient>0.4 0.4 0.4 1</ambient>
47+
<background>0.7 0.7 0.7 1</background>
48+
<shadows>1</shadows>
49+
</scene>"""
50+
51+
audio="""
52+
<audio>
53+
<device>default</device>
54+
</audio>"""
55+
56+
wind="""
57+
<wind/>"""
58+
59+
coordinates="""
60+
<spherical_coordinates>
61+
<surface_model>EARTH_WGS84</surface_model>
62+
<latitude_deg>0</latitude_deg>
63+
<longitude_deg>0</longitude_deg>
64+
<elevation>0</elevation>
65+
<heading_deg>0</heading_deg>
66+
</spherical_coordinates>"""
67+
68+
gui="""
69+
<gui fullscreen='0'>
70+
<camera name='user_camera'>
71+
<pose>8.64986 11.0927 27.9008 -0 1.19964 -2.775</pose>
72+
<view_controller>orbit</view_controller>
73+
<projection_type>perspective</projection_type>
74+
</camera>
75+
</gui>"""
76+
77+
quick_opening = opening + light + gravity + magnetic_field + atmosphere + physics + scene + audio + wind + coordinates + gui
78+
79+
# GROUND DEFINITIONS
80+
ground = """
81+
<model name='ground_plane'>
82+
<static>1</static>
83+
<link name='link'>
84+
<collision name='collision'>
85+
<geometry>
86+
<plane>
87+
<normal>0 0 1</normal>
88+
<size>%s %s</size>
89+
</plane>
90+
</geometry>
91+
<surface>
92+
<contact>
93+
<collide_bitmask>65535</collide_bitmask>
94+
<ode/>
95+
</contact>
96+
<friction>
97+
<ode>
98+
<mu>100</mu>
99+
<mu2>50</mu2>
100+
</ode>
101+
<torsional>
102+
<ode/>
103+
</torsional>
104+
</friction>
105+
<bounce/>
106+
</surface>
107+
<max_contacts>10</max_contacts>
108+
</collision>
109+
<visual name='visual'>
110+
<cast_shadows>0</cast_shadows>
111+
<geometry>
112+
<plane>
113+
<normal>0 0 1</normal>
114+
<size>%s %s</size>
115+
</plane>
116+
</geometry>
117+
<material>
118+
<script>
119+
<uri>file://media/materials/scripts/gazebo.material</uri>
120+
<name>Gazebo/Grey</name>
121+
</script>
122+
</material>
123+
</visual>
124+
<self_collide>0</self_collide>
125+
<enable_wind>0</enable_wind>
126+
<kinematic>0</kinematic>
127+
</link>
128+
</model>"""
129+
def get_ground(w, h):
130+
return GazeboTemplates.ground % (w, h, w, h)
131+
132+
# MODEL DEFINITIONS
133+
model_opening="""
134+
<model name='%s'>
135+
<pose>%s %s %s %s %s %s</pose>
136+
<static>1</static>"""
137+
model_closing="""
138+
</model>"""
139+
140+
# STATE DEFINITIONS
141+
state_opening="""
142+
<state world_name='default'>
143+
<sim_time>155 86000000</sim_time>
144+
<real_time>100 237517715</real_time>
145+
<wall_time>1708451611 185616378</wall_time>
146+
<iterations>99859</iterations>
147+
<light name='sun'>
148+
<pose>0 0 10 0 -0 0</pose>
149+
</light>"""
150+
151+
state_model="""
152+
<model name='%s'>
153+
<pose>%s %s %s %s %s %s</pose>
154+
<scale>1 1 1</scale>
155+
<link name='%s'>
156+
<pose>%s %s %s %s %s %s</pose>
157+
<velocity>0 0 0 0 -0 0</velocity>
158+
<acceleration>0 0 0 0 -0 0</acceleration>
159+
<wrench>0 0 0 0 -0 0</wrench>
160+
</link>
161+
</model>"""
162+
163+
state_model_opening_2="""
164+
<model name='cinder_block'>
165+
<pose>-4.25139 1.07298 -6e-06 -1e-06 2.8e-05 -0</pose>
166+
<scale>1 1 1</scale>
167+
<link name='link'>
168+
<pose>-4.25139 1.07298 -6e-06 -1e-06 2.8e-05 -0</pose>
169+
<velocity>0 0 0 0 -0 0</velocity>
170+
<acceleration>-0 0 -0 0 -0 0</acceleration>
171+
<wrench>-0 0 -0 0 -0 0</wrench>
172+
</link>
173+
</model>"""
174+
175+
state_model_opening_3="""
176+
<model name='ground_plane'>
177+
<pose>0 0 0 0 -0 0</pose>
178+
<scale>1 1 1</scale>
179+
<link name='link'>
180+
<pose>0 0 0 0 -0 0</pose>
181+
<velocity>0 0 0 0 -0 0</velocity>
182+
<acceleration>0 0 0 0 -0 0</acceleration>
183+
<wrench>0 0 0 0 -0 0</wrench>
184+
</link>
185+
</model>"""
186+
187+
state_closing="""
188+
</state>"""
189+
190+
closing="""
191+
</world>
192+
</sdf>"""
193+
194+
195+
196+

‎environment_common/convertors/templating/gazebo_models/environmental.py

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
#######################
2+
3+
light = """
4+
<light name='sun' type='directional'>
5+
<cast_shadows>1</cast_shadows>
6+
<pose>0 0 10 0 -0 0</pose>
7+
<diffuse>0.8 0.8 0.8 1</diffuse>
8+
<specular>0.2 0.2 0.2 1</specular>
9+
<attenuation>
10+
<range>1000</range>
11+
<constant>0.9</constant>
12+
<linear>0.01</linear>
13+
<quadratic>0.001</quadratic>
14+
</attenuation>
15+
<direction>-0.5 0.1 -0.9</direction>
16+
<spot>
17+
<inner_angle>0</inner_angle>
18+
<outer_angle>0</outer_angle>
19+
<falloff>0</falloff>
20+
</spot>
21+
</light>"""
22+
23+
scene = """
24+
<scene>
25+
<ambient>0.4 0.4 0.4 1</ambient>
26+
<background>0.7 0.7 0.7 1</background>
27+
<shadows>1</shadows>
28+
</scene>"""
29+
30+
31+
downtunnelview = """
32+
<gui fullscreen='0'>
33+
<camera name='user_camera'>
34+
<pose>0.3339 35.8481 3.06376 0 0.001799 -1.55494</pose>
35+
<view_controller>orbit</view_controller>
36+
<projection_type>perspective</projection_type>
37+
</camera>
38+
</gui>
39+
"""
40+
41+
downtunnelortho = """
42+
<gui fullscreen='0'>
43+
<camera name='user_camera'>
44+
<pose>0.063391 35.9803 1.73163 0 0 -1.5708</pose>
45+
<view_controller>ortho</view_controller>
46+
<projection_type>orthographic</projection_type>
47+
</camera>
48+
</gui>
49+
"""
50+
51+
outview = """
52+
<gui fullscreen='0'>
53+
<camera name='user_camera'>
54+
<pose>36.8821 18.6508 45.751 -0 0.877799 -2.81094</pose>
55+
<view_controller>orbit</view_controller>
56+
<projection_type>perspective</projection_type>
57+
</camera>
58+
</gui>
59+
"""
60+
61+
view = downtunnelortho
62+
63+
64+
#######################
65+
66+
class Environment:
67+
def get(data):
68+
xml = light
69+
xml += scene
70+
xml += view
71+
return xml
72+
73+

0 commit comments

Comments
 (0)