Skip to content

Commit d46fba0

Browse files
luca-della-vedovaluca-della-vedova
committed
ECS refactor for lifts and doors
Signed-off-by: Luca Della Vedova <[email protected]>
1 parent 500a868 commit d46fba0

File tree

7 files changed

+293
-214
lines changed

7 files changed

+293
-214
lines changed

‎rmf_building_map_tools/building_map/building.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -503,7 +503,7 @@ def generate_sdf_world(self, options):
503503
{'name': 'toggle_floors', 'filename': 'libtoggle_floors.so'})
504504

505505
elif 'ignition' in options:
506-
plugin_ele = gui_ele.find('.//plugin[@filename="GzScene3D"]')
506+
plugin_ele = gui_ele.find('.//plugin[@filename="MinimalScene"]')
507507
camera_pose_ele = plugin_ele.find('camera_pose')
508508
camera_pose_ele.text = camera_pose
509509

‎rmf_building_map_tools/building_map/doors/double_sliding_door.py

+8-5
Original file line numberDiff line numberDiff line change
@@ -30,20 +30,23 @@ def generate(self, world_ele, options):
3030

3131
if not self.plugin == 'none':
3232
plugin_ele = SubElement(self.model_ele, 'plugin')
33-
plugin_ele.set('name', 'door')
34-
plugin_ele.set('filename', 'libdoor.so')
33+
plugin_ele.set('name', 'register_component')
34+
plugin_ele.set('filename', 'libregister_component.so')
35+
component_ele = SubElement(plugin_ele, 'component')
36+
component_ele.set('name', 'Door')
3537
plugin_params = {
3638
'v_max_door': '0.2',
3739
'a_max_door': '0.2',
3840
'a_nom_door': '0.08',
3941
'dx_min_door': '0.001',
40-
'f_max_door': '100.0'
42+
'f_max_door': '100.0',
43+
'ros_interface': 'true'
4144
}
4245
for param_name, param_value in plugin_params.items():
43-
ele = SubElement(plugin_ele, param_name)
46+
ele = SubElement(component_ele, param_name)
4447
ele.text = param_value
4548

46-
door_ele = SubElement(plugin_ele, 'door')
49+
door_ele = SubElement(component_ele, 'door')
4750
door_ele.set('name', self.name)
4851
door_ele.set('type', 'DoubleSlidingDoor')
4952
door_ele.set('left_joint_name', 'left_joint')

‎rmf_building_map_tools/building_map/doors/double_swing_door.py

+8-5
Original file line numberDiff line numberDiff line change
@@ -48,20 +48,23 @@ def generate(self, world_ele, options):
4848

4949
if not self.plugin == 'none':
5050
plugin_ele = SubElement(self.model_ele, 'plugin')
51-
plugin_ele.set('name', 'door')
52-
plugin_ele.set('filename', 'libdoor.so')
51+
plugin_ele.set('name', 'register_component')
52+
plugin_ele.set('filename', 'libregister_component.so')
53+
component_ele = SubElement(plugin_ele, 'component')
54+
component_ele.set('name', 'Door')
5355
plugin_params = {
5456
'v_max_door': '0.5',
5557
'a_max_door': '0.3',
5658
'a_nom_door': '0.15',
5759
'dx_min_door': '0.01',
58-
'f_max_door': '500.0'
60+
'f_max_door': '500.0',
61+
'ros_interface': 'true'
5962
}
6063
for param_name, param_value in plugin_params.items():
61-
ele = SubElement(plugin_ele, param_name)
64+
ele = SubElement(component_ele, param_name)
6265
ele.text = param_value
6366

64-
door_ele = SubElement(plugin_ele, 'door')
67+
door_ele = SubElement(component_ele, 'door')
6568
door_ele.set('name', self.name)
6669
door_ele.set('type', 'DoubleSwingDoor')
6770
door_ele.set('left_joint_name', 'left_joint')

‎rmf_building_map_tools/building_map/doors/sliding_door.py

+9-5
Original file line numberDiff line numberDiff line change
@@ -16,23 +16,27 @@ def generate(self, world_ele, options):
1616

1717
if not self.plugin == 'none':
1818
plugin_ele = SubElement(self.model_ele, 'plugin')
19-
plugin_ele.set('name', 'door')
20-
plugin_ele.set('filename', 'libdoor.so')
19+
plugin_ele.set('name', 'register_component')
20+
plugin_ele.set('filename', 'libregister_component.so')
21+
component_ele = SubElement(plugin_ele, 'component')
22+
component_ele.set('name', 'Door')
2123
plugin_params = {
2224
'v_max_door': '0.2',
2325
'a_max_door': '0.2',
2426
'a_nom_door': '0.08',
2527
'dx_min_door': '0.001',
26-
'f_max_door': '100.0'
28+
'f_max_door': '100.0',
29+
'ros_interface': 'true'
2730
}
2831
for param_name, param_value in plugin_params.items():
29-
ele = SubElement(plugin_ele, param_name)
32+
ele = SubElement(component_ele, param_name)
3033
ele.text = param_value
3134

32-
door_ele = SubElement(plugin_ele, 'door')
35+
door_ele = SubElement(component_ele, 'door')
3336
door_ele.set('name', self.name)
3437
door_ele.set('type', 'SlidingDoor')
3538
door_ele.set('left_joint_name', 'empty_joint')
3639
door_ele.set('right_joint_name', 'right_joint')
3740

3841
world_ele.append(self.model_ele)
42+

‎rmf_building_map_tools/building_map/doors/swing_door.py

+9-5
Original file line numberDiff line numberDiff line change
@@ -25,23 +25,27 @@ def generate(self, world_ele, options):
2525

2626
if not self.plugin == 'none':
2727
plugin_ele = SubElement(self.model_ele, 'plugin')
28-
plugin_ele.set('name', 'door')
29-
plugin_ele.set('filename', 'libdoor.so')
28+
plugin_ele.set('name', 'register_component')
29+
plugin_ele.set('filename', 'libregister_component.so')
30+
component_ele = SubElement(plugin_ele, 'component')
31+
component_ele.set('name', 'Door')
3032
plugin_params = {
3133
'v_max_door': '0.5',
3234
'a_max_door': '0.3',
3335
'a_nom_door': '0.15',
3436
'dx_min_door': '0.01',
35-
'f_max_door': '500.0'
37+
'f_max_door': '500.0',
38+
'ros_interface': 'true'
3639
}
3740
for param_name, param_value in plugin_params.items():
38-
ele = SubElement(plugin_ele, param_name)
41+
ele = SubElement(component_ele, param_name)
3942
ele.text = param_value
4043

41-
door_ele = SubElement(plugin_ele, 'door')
44+
door_ele = SubElement(component_ele, 'door')
4245
door_ele.set('name', self.name)
4346
door_ele.set('type', 'SwingDoor')
4447
door_ele.set('left_joint_name', 'empty_joint')
4548
door_ele.set('right_joint_name', 'right_joint')
4649

4750
world_ele.append(self.model_ele)
51+

‎rmf_building_map_tools/building_map/lift.py

+57-106
Original file line numberDiff line numberDiff line change
@@ -66,10 +66,11 @@ def generate_cabin_door(self, lift_model_ele, name):
6666
door_pose.text = \
6767
f'{x} {y} 0 0 0 {self.motion_axis_orientation}'
6868

69-
self.generate_door_link_and_joint(door_model_ele, parent='platform')
69+
self.generate_door_link(door_model_ele, parent='platform')
70+
self.generate_joint(lift_model_ele, "platform", name)
7071

7172
if self.plugin:
72-
self.generate_door_plugin(door_model_ele, name)
73+
self.generate_door_plugin(door_model_ele, name, True)
7374

7475
def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
7576
model_ele = SubElement(world_ele, 'model')
@@ -82,7 +83,8 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
8283
yaw_new = yaw + self.motion_axis_orientation
8384
door_pose.text = f'{x_new} {y_new} {z} 0 0 {yaw_new}'
8485

85-
self.generate_door_link_and_joint(model_ele)
86+
self.generate_door_link(model_ele)
87+
self.generate_joint(model_ele)
8688

8789
floor_thickness = 0.05
8890
ramp_depth = self.gap * 2
@@ -97,9 +99,33 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
9799
model_ele.append(joint('ramp_joint', 'fixed', 'world', 'ramp'))
98100

99101
if self.plugin:
100-
self.generate_door_plugin(model_ele, name)
102+
self.generate_door_plugin(model_ele, name, False)
101103

102-
def generate_door_link_and_joint(self, model_ele, parent='world'):
104+
def generate_joint(
105+
self, parent_element, parent_name="world", child_name=None):
106+
if child_name is not None:
107+
name_prefix = f'{child_name}_'
108+
joint_child_prefix = f'{child_name}::'
109+
else:
110+
name_prefix = ''
111+
joint_child_prefix = f''
112+
parent_element.append(joint(f'{name_prefix}right_joint',
113+
'prismatic',
114+
parent_name,
115+
f'{joint_child_prefix}right_door',
116+
joint_axis='x',
117+
lower_limit=0,
118+
upper_limit=self.width / 2))
119+
120+
parent_element.append(joint(f'{name_prefix}left_joint',
121+
'prismatic',
122+
parent_name,
123+
f'{joint_child_prefix}left_door',
124+
joint_axis='x',
125+
lower_limit=-self.width / 2,
126+
upper_limit=0))
127+
128+
def generate_door_link(self, model_ele, parent='world'):
103129
door_size = [self.width / 2, self.thickness, self.height]
104130
right_door_pose = Element('pose')
105131
right_door_pose.text = f'{self.width / 4} 0 {self.height / 2} 0 0 0'
@@ -110,14 +136,6 @@ def generate_door_link_and_joint(self, model_ele, parent='world'):
110136
material=lift_material(),
111137
bitmask='0x02'))
112138

113-
model_ele.append(joint('right_joint',
114-
'prismatic',
115-
parent,
116-
'right_door',
117-
joint_axis='x',
118-
lower_limit=0,
119-
upper_limit=self.width / 2))
120-
121139
left_door_pose = Element('pose')
122140
left_door_pose.text = f'{-self.width / 4} 0 {self.height / 2} 0 0 0'
123141

@@ -127,88 +145,25 @@ def generate_door_link_and_joint(self, model_ele, parent='world'):
127145
material=lift_material(),
128146
bitmask='0x02'))
129147

130-
model_ele.append(joint('left_joint',
131-
'prismatic',
132-
parent,
133-
'left_door',
134-
joint_axis='x',
135-
lower_limit=-self.width / 2,
136-
upper_limit=0))
137-
138-
def generate_door_plugin(self, model_ele, name):
148+
def generate_door_plugin(self, model_ele, name, append_prefix=False):
149+
if append_prefix is True:
150+
prefix = f'{name}_'
151+
else:
152+
prefix = ''
139153
plugin_ele = SubElement(model_ele, 'plugin')
140-
plugin_ele.set('name', 'door')
141-
plugin_ele.set('filename', 'libdoor.so')
154+
plugin_ele.set('name', 'register_component')
155+
plugin_ele.set('filename', 'libregister_component.so')
156+
component_ele = SubElement(plugin_ele, 'component')
157+
component_ele.set('name', 'Door')
142158
for param_name, param_value in self.params.items():
143-
ele = SubElement(plugin_ele, param_name)
159+
ele = SubElement(component_ele, param_name)
144160
ele.text = f'{param_value}'
145-
door_ele = SubElement(plugin_ele, 'door')
146-
door_ele.set('left_joint_name', 'left_joint')
161+
door_ele = SubElement(component_ele, 'door')
162+
door_ele.set('left_joint_name', f'{prefix}left_joint')
147163
door_ele.set('name', f'{name}')
148-
door_ele.set('right_joint_name', 'right_joint')
164+
door_ele.set('right_joint_name', f'{prefix}right_joint')
149165
door_ele.set('type', 'DoubleSlidingDoor')
150166

151-
# TODO: remove this function once nesting model is supported in ignition.
152-
def generate_cabin_door_ign(self, lift_model_ele, name):
153-
# This is for cabin door generation for ignition gazebo as it doesn't
154-
# support nested models yet. Once ignition gazebo supports nested
155-
# models, this should be removed.
156-
(x, y) = self.cabin_door_pose
157-
yaw = self.motion_axis_orientation
158-
right_x = x + np.cos(yaw) * self.width/4
159-
left_x = x - np.cos(yaw) * self.width/4
160-
right_y = y + np.sin(yaw) * self.width/4
161-
left_y = y - np.sin(yaw) * self.width/4
162-
163-
door_size = [self.width / 2, self.thickness, self.height]
164-
right_door_pose = Element('pose')
165-
right_door_pose.text = \
166-
f'{right_x} {right_y} {self.height / 2} 0 0 {yaw}'
167-
168-
lift_model_ele.append(box_link(f'{name}_right_door',
169-
door_size,
170-
right_door_pose,
171-
material=lift_material(),
172-
bitmask='0x02'))
173-
174-
lift_model_ele.append(joint(f'{name}_right_joint',
175-
'prismatic',
176-
'platform',
177-
f'{name}_right_door',
178-
joint_axis='x',
179-
lower_limit=0,
180-
upper_limit=self.width / 2))
181-
182-
left_door_pose = Element('pose')
183-
left_door_pose.text = f'{left_x} {left_y} {self.height / 2} 0 0 {yaw}'
184-
185-
lift_model_ele.append(box_link(f'{name}_left_door',
186-
door_size,
187-
left_door_pose,
188-
material=lift_material(),
189-
bitmask='0x02'))
190-
191-
lift_model_ele.append(joint(f'{name}_left_joint',
192-
'prismatic',
193-
'platform',
194-
f'{name}_left_door',
195-
joint_axis='x',
196-
lower_limit=-self.width / 2,
197-
upper_limit=0))
198-
199-
if self.plugin:
200-
plugin_ele = SubElement(lift_model_ele, 'plugin')
201-
plugin_ele.set('name', 'door')
202-
plugin_ele.set('filename', 'libdoor.so')
203-
for param_name, param_value in self.params.items():
204-
ele = SubElement(plugin_ele, param_name)
205-
ele.text = f'{param_value}'
206-
door_ele = SubElement(plugin_ele, 'door')
207-
door_ele.set('left_joint_name', f'{name}_left_joint')
208-
door_ele.set('name', f'{name}')
209-
door_ele.set('right_joint_name', f'{name}_right_joint')
210-
door_ele.set('type', 'DoubleSlidingDoor')
211-
212167

213168
class Lift:
214169
def __init__(self, yaml_node, name, transform, levels, coordinate_system):
@@ -429,28 +384,23 @@ def generate_cabin(self, world_ele, options):
429384
joint_axis='z'))
430385

431386
# cabin doors
432-
# TODO: remove the if statement here once nesting model is supported
433-
# in ignition.
434-
if 'ignition' in options:
435-
for lift_door in self.doors:
436-
lift_door.generate_cabin_door_ign(
437-
lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
438-
else:
439-
for lift_door in self.doors:
440-
lift_door.generate_cabin_door(
441-
lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
387+
for lift_door in self.doors:
388+
lift_door.generate_cabin_door(
389+
lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
442390

443391
# lift cabin plugin
444392
if self.plugins:
445393
plugin_ele = SubElement(lift_model_ele, 'plugin')
446-
plugin_ele.set('name', 'lift')
447-
plugin_ele.set('filename', 'liblift.so')
394+
plugin_ele.set('name', 'register_component')
395+
plugin_ele.set('filename', 'libregister_component.so')
396+
component_ele = SubElement(plugin_ele, 'component')
397+
component_ele.set('name', 'Lift')
448398

449-
lift_name_ele = SubElement(plugin_ele, 'lift_name')
399+
lift_name_ele = SubElement(component_ele, 'lift_name')
450400
lift_name_ele.text = f'{self.name}'
451401

452402
for level_name, door_names in self.level_doors.items():
453-
floor_ele = SubElement(plugin_ele, 'floor')
403+
floor_ele = SubElement(component_ele, 'floor')
454404
floor_ele.set('name', f'{level_name}')
455405
floor_ele.set(
456406
'elevation', f'{self.level_elevation[level_name]}')
@@ -464,13 +414,13 @@ def generate_cabin(self, world_ele, options):
464414
'shaft_door',
465415
f'ShaftDoor_{self.name}_{level_name}_{door.name}')
466416

467-
initial_floor_ele = SubElement(plugin_ele, 'initial_floor')
417+
initial_floor_ele = SubElement(component_ele, 'initial_floor')
468418
initial_floor_ele.text = f'{self.initial_floor_name}'
469419
for param_name, param_value in self.params.items():
470-
ele = SubElement(plugin_ele, param_name)
420+
ele = SubElement(component_ele, param_name)
471421
ele.text = f'{param_value}'
472422

473-
cabin_joint_ele = SubElement(plugin_ele, 'cabin_joint_name')
423+
cabin_joint_ele = SubElement(component_ele, 'cabin_joint_name')
474424
cabin_joint_ele.text = 'cabin_joint'
475425
else:
476426
static_lift_ele = SubElement(lift_model_ele, 'static')
@@ -479,3 +429,4 @@ def generate_cabin(self, world_ele, options):
479429
# pose
480430
model_pose = SubElement(lift_model_ele, 'pose')
481431
model_pose.text = f'{self.x} {self.y} 0 0 0 {self.yaw}'
432+

0 commit comments

Comments
 (0)