diff --git a/rmf_building_map_tools/building_map/building.py b/rmf_building_map_tools/building_map/building.py
index a0ab55db2..42ea3f0b0 100644
--- a/rmf_building_map_tools/building_map/building.py
+++ b/rmf_building_map_tools/building_map/building.py
@@ -503,7 +503,7 @@ def generate_sdf_world(self, options):
{'name': 'toggle_floors', 'filename': 'libtoggle_floors.so'})
elif 'ignition' in options:
- plugin_ele = gui_ele.find('.//plugin[@filename="GzScene3D"]')
+ plugin_ele = gui_ele.find('.//plugin[@filename="MinimalScene"]')
camera_pose_ele = plugin_ele.find('camera_pose')
camera_pose_ele.text = camera_pose
diff --git a/rmf_building_map_tools/building_map/doors/double_sliding_door.py b/rmf_building_map_tools/building_map/doors/double_sliding_door.py
index 7c984c961..00132f699 100644
--- a/rmf_building_map_tools/building_map/doors/double_sliding_door.py
+++ b/rmf_building_map_tools/building_map/doors/double_sliding_door.py
@@ -30,20 +30,23 @@ def generate(self, world_ele, options):
if not self.plugin == 'none':
plugin_ele = SubElement(self.model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Door')
plugin_params = {
'v_max_door': '0.2',
'a_max_door': '0.2',
'a_nom_door': '0.08',
'dx_min_door': '0.001',
- 'f_max_door': '100.0'
+ 'f_max_door': '100.0',
+ 'ros_interface': 'true'
}
for param_name, param_value in plugin_params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = param_value
- door_ele = SubElement(plugin_ele, 'door')
+ door_ele = SubElement(component_ele, 'door')
door_ele.set('name', self.name)
door_ele.set('type', 'DoubleSlidingDoor')
door_ele.set('left_joint_name', 'left_joint')
diff --git a/rmf_building_map_tools/building_map/doors/double_swing_door.py b/rmf_building_map_tools/building_map/doors/double_swing_door.py
index 3bd0e19f9..d768849f7 100644
--- a/rmf_building_map_tools/building_map/doors/double_swing_door.py
+++ b/rmf_building_map_tools/building_map/doors/double_swing_door.py
@@ -48,20 +48,23 @@ def generate(self, world_ele, options):
if not self.plugin == 'none':
plugin_ele = SubElement(self.model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Door')
plugin_params = {
'v_max_door': '0.5',
'a_max_door': '0.3',
'a_nom_door': '0.15',
'dx_min_door': '0.01',
- 'f_max_door': '500.0'
+ 'f_max_door': '500.0',
+ 'ros_interface': 'true'
}
for param_name, param_value in plugin_params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = param_value
- door_ele = SubElement(plugin_ele, 'door')
+ door_ele = SubElement(component_ele, 'door')
door_ele.set('name', self.name)
door_ele.set('type', 'DoubleSwingDoor')
door_ele.set('left_joint_name', 'left_joint')
diff --git a/rmf_building_map_tools/building_map/doors/sliding_door.py b/rmf_building_map_tools/building_map/doors/sliding_door.py
index 781db3b80..4c47b811a 100644
--- a/rmf_building_map_tools/building_map/doors/sliding_door.py
+++ b/rmf_building_map_tools/building_map/doors/sliding_door.py
@@ -16,23 +16,27 @@ def generate(self, world_ele, options):
if not self.plugin == 'none':
plugin_ele = SubElement(self.model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Door')
plugin_params = {
'v_max_door': '0.2',
'a_max_door': '0.2',
'a_nom_door': '0.08',
'dx_min_door': '0.001',
- 'f_max_door': '100.0'
+ 'f_max_door': '100.0',
+ 'ros_interface': 'true'
}
for param_name, param_value in plugin_params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = param_value
- door_ele = SubElement(plugin_ele, 'door')
+ door_ele = SubElement(component_ele, 'door')
door_ele.set('name', self.name)
door_ele.set('type', 'SlidingDoor')
door_ele.set('left_joint_name', 'empty_joint')
door_ele.set('right_joint_name', 'right_joint')
world_ele.append(self.model_ele)
+
diff --git a/rmf_building_map_tools/building_map/doors/swing_door.py b/rmf_building_map_tools/building_map/doors/swing_door.py
index eba38280a..166b5571d 100644
--- a/rmf_building_map_tools/building_map/doors/swing_door.py
+++ b/rmf_building_map_tools/building_map/doors/swing_door.py
@@ -25,23 +25,27 @@ def generate(self, world_ele, options):
if not self.plugin == 'none':
plugin_ele = SubElement(self.model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Door')
plugin_params = {
'v_max_door': '0.5',
'a_max_door': '0.3',
'a_nom_door': '0.15',
'dx_min_door': '0.01',
- 'f_max_door': '500.0'
+ 'f_max_door': '500.0',
+ 'ros_interface': 'true'
}
for param_name, param_value in plugin_params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = param_value
- door_ele = SubElement(plugin_ele, 'door')
+ door_ele = SubElement(component_ele, 'door')
door_ele.set('name', self.name)
door_ele.set('type', 'SwingDoor')
door_ele.set('left_joint_name', 'empty_joint')
door_ele.set('right_joint_name', 'right_joint')
world_ele.append(self.model_ele)
+
diff --git a/rmf_building_map_tools/building_map/lift.py b/rmf_building_map_tools/building_map/lift.py
index dd286b5e3..0ea8846b2 100644
--- a/rmf_building_map_tools/building_map/lift.py
+++ b/rmf_building_map_tools/building_map/lift.py
@@ -66,10 +66,11 @@ def generate_cabin_door(self, lift_model_ele, name):
door_pose.text = \
f'{x} {y} 0 0 0 {self.motion_axis_orientation}'
- self.generate_door_link_and_joint(door_model_ele, parent='platform')
+ self.generate_door_link(door_model_ele, parent='platform')
+ self.generate_joint(lift_model_ele, "platform", name)
if self.plugin:
- self.generate_door_plugin(door_model_ele, name)
+ self.generate_door_plugin(door_model_ele, name, True)
def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
model_ele = SubElement(world_ele, 'model')
@@ -82,7 +83,8 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
yaw_new = yaw + self.motion_axis_orientation
door_pose.text = f'{x_new} {y_new} {z} 0 0 {yaw_new}'
- self.generate_door_link_and_joint(model_ele)
+ self.generate_door_link(model_ele)
+ self.generate_joint(model_ele)
floor_thickness = 0.05
ramp_depth = self.gap * 2
@@ -97,9 +99,33 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
model_ele.append(joint('ramp_joint', 'fixed', 'world', 'ramp'))
if self.plugin:
- self.generate_door_plugin(model_ele, name)
+ self.generate_door_plugin(model_ele, name, False)
- def generate_door_link_and_joint(self, model_ele, parent='world'):
+ def generate_joint(
+ self, parent_element, parent_name="world", child_name=None):
+ if child_name is not None:
+ name_prefix = f'{child_name}_'
+ joint_child_prefix = f'{child_name}::'
+ else:
+ name_prefix = ''
+ joint_child_prefix = f''
+ parent_element.append(joint(f'{name_prefix}right_joint',
+ 'prismatic',
+ parent_name,
+ f'{joint_child_prefix}right_door',
+ joint_axis='x',
+ lower_limit=0,
+ upper_limit=self.width / 2))
+
+ parent_element.append(joint(f'{name_prefix}left_joint',
+ 'prismatic',
+ parent_name,
+ f'{joint_child_prefix}left_door',
+ joint_axis='x',
+ lower_limit=-self.width / 2,
+ upper_limit=0))
+
+ def generate_door_link(self, model_ele, parent='world'):
door_size = [self.width / 2, self.thickness, self.height]
right_door_pose = Element('pose')
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'):
material=lift_material(),
bitmask='0x02'))
- model_ele.append(joint('right_joint',
- 'prismatic',
- parent,
- 'right_door',
- joint_axis='x',
- lower_limit=0,
- upper_limit=self.width / 2))
-
left_door_pose = Element('pose')
left_door_pose.text = f'{-self.width / 4} 0 {self.height / 2} 0 0 0'
@@ -127,88 +145,25 @@ def generate_door_link_and_joint(self, model_ele, parent='world'):
material=lift_material(),
bitmask='0x02'))
- model_ele.append(joint('left_joint',
- 'prismatic',
- parent,
- 'left_door',
- joint_axis='x',
- lower_limit=-self.width / 2,
- upper_limit=0))
-
- def generate_door_plugin(self, model_ele, name):
+ def generate_door_plugin(self, model_ele, name, append_prefix=False):
+ if append_prefix is True:
+ prefix = f'{name}_'
+ else:
+ prefix = ''
plugin_ele = SubElement(model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Door')
for param_name, param_value in self.params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = f'{param_value}'
- door_ele = SubElement(plugin_ele, 'door')
- door_ele.set('left_joint_name', 'left_joint')
+ door_ele = SubElement(component_ele, 'door')
+ door_ele.set('left_joint_name', f'{prefix}left_joint')
door_ele.set('name', f'{name}')
- door_ele.set('right_joint_name', 'right_joint')
+ door_ele.set('right_joint_name', f'{prefix}right_joint')
door_ele.set('type', 'DoubleSlidingDoor')
- # TODO: remove this function once nesting model is supported in ignition.
- def generate_cabin_door_ign(self, lift_model_ele, name):
- # This is for cabin door generation for ignition gazebo as it doesn't
- # support nested models yet. Once ignition gazebo supports nested
- # models, this should be removed.
- (x, y) = self.cabin_door_pose
- yaw = self.motion_axis_orientation
- right_x = x + np.cos(yaw) * self.width/4
- left_x = x - np.cos(yaw) * self.width/4
- right_y = y + np.sin(yaw) * self.width/4
- left_y = y - np.sin(yaw) * self.width/4
-
- door_size = [self.width / 2, self.thickness, self.height]
- right_door_pose = Element('pose')
- right_door_pose.text = \
- f'{right_x} {right_y} {self.height / 2} 0 0 {yaw}'
-
- lift_model_ele.append(box_link(f'{name}_right_door',
- door_size,
- right_door_pose,
- material=lift_material(),
- bitmask='0x02'))
-
- lift_model_ele.append(joint(f'{name}_right_joint',
- 'prismatic',
- 'platform',
- f'{name}_right_door',
- joint_axis='x',
- lower_limit=0,
- upper_limit=self.width / 2))
-
- left_door_pose = Element('pose')
- left_door_pose.text = f'{left_x} {left_y} {self.height / 2} 0 0 {yaw}'
-
- lift_model_ele.append(box_link(f'{name}_left_door',
- door_size,
- left_door_pose,
- material=lift_material(),
- bitmask='0x02'))
-
- lift_model_ele.append(joint(f'{name}_left_joint',
- 'prismatic',
- 'platform',
- f'{name}_left_door',
- joint_axis='x',
- lower_limit=-self.width / 2,
- upper_limit=0))
-
- if self.plugin:
- plugin_ele = SubElement(lift_model_ele, 'plugin')
- plugin_ele.set('name', 'door')
- plugin_ele.set('filename', 'libdoor.so')
- for param_name, param_value in self.params.items():
- ele = SubElement(plugin_ele, param_name)
- ele.text = f'{param_value}'
- door_ele = SubElement(plugin_ele, 'door')
- door_ele.set('left_joint_name', f'{name}_left_joint')
- door_ele.set('name', f'{name}')
- door_ele.set('right_joint_name', f'{name}_right_joint')
- door_ele.set('type', 'DoubleSlidingDoor')
-
class Lift:
def __init__(self, yaml_node, name, transform, levels, coordinate_system):
@@ -429,28 +384,23 @@ def generate_cabin(self, world_ele, options):
joint_axis='z'))
# cabin doors
- # TODO: remove the if statement here once nesting model is supported
- # in ignition.
- if 'ignition' in options:
- for lift_door in self.doors:
- lift_door.generate_cabin_door_ign(
- lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
- else:
- for lift_door in self.doors:
- lift_door.generate_cabin_door(
- lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
+ for lift_door in self.doors:
+ lift_door.generate_cabin_door(
+ lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
# lift cabin plugin
if self.plugins:
plugin_ele = SubElement(lift_model_ele, 'plugin')
- plugin_ele.set('name', 'lift')
- plugin_ele.set('filename', 'liblift.so')
+ plugin_ele.set('name', 'register_component')
+ plugin_ele.set('filename', 'libregister_component.so')
+ component_ele = SubElement(plugin_ele, 'component')
+ component_ele.set('name', 'Lift')
- lift_name_ele = SubElement(plugin_ele, 'lift_name')
+ lift_name_ele = SubElement(component_ele, 'lift_name')
lift_name_ele.text = f'{self.name}'
for level_name, door_names in self.level_doors.items():
- floor_ele = SubElement(plugin_ele, 'floor')
+ floor_ele = SubElement(component_ele, 'floor')
floor_ele.set('name', f'{level_name}')
floor_ele.set(
'elevation', f'{self.level_elevation[level_name]}')
@@ -464,13 +414,13 @@ def generate_cabin(self, world_ele, options):
'shaft_door',
f'ShaftDoor_{self.name}_{level_name}_{door.name}')
- initial_floor_ele = SubElement(plugin_ele, 'initial_floor')
+ initial_floor_ele = SubElement(component_ele, 'initial_floor')
initial_floor_ele.text = f'{self.initial_floor_name}'
for param_name, param_value in self.params.items():
- ele = SubElement(plugin_ele, param_name)
+ ele = SubElement(component_ele, param_name)
ele.text = f'{param_value}'
- cabin_joint_ele = SubElement(plugin_ele, 'cabin_joint_name')
+ cabin_joint_ele = SubElement(component_ele, 'cabin_joint_name')
cabin_joint_ele.text = 'cabin_joint'
else:
static_lift_ele = SubElement(lift_model_ele, 'static')
@@ -479,3 +429,4 @@ def generate_cabin(self, world_ele, options):
# pose
model_pose = SubElement(lift_model_ele, 'pose')
model_pose.text = f'{self.x} {self.y} 0 0 0 {self.yaw}'
+
diff --git a/rmf_building_map_tools/building_map/templates/ign_world.sdf b/rmf_building_map_tools/building_map/templates/ign_world.sdf
index 81444b97f..d7c75108d 100644
--- a/rmf_building_map_tools/building_map/templates/ign_world.sdf
+++ b/rmf_building_map_tools/building_map/templates/ign_world.sdf
@@ -17,6 +17,14 @@
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
+
+
+
+
1 1 1
@@ -49,120 +57,225 @@
+
+
+ 3D View
+ false
+ docked
+
+ ogre2
+ scene
+ 0.4 0.4 0.4
+ 0.8 0.8 0.8
+ -6 0 6 0 0.5 0
+
-
-
-
- 3D View
- false
- docked
-
+
+
+
+ floating
+ 5
+ 5
+ false
+
+
- ogre2
- scene
- 0.4 0.4 0.4
- 0.8 0.8 0.8
- 6 0 6 0 0.5 3.14
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
-
-
-
- World control
- false
- false
- 72
- 121
- 1
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
+
- floating
-
-
-
-
-
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
+
- true
- true
- true
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
+
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
-
-
-
- World stats
- false
- false
- 110
- 290
- 1
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
+
+
+
+
+ false
+ 5
+ 5
+ floating
+ false
+
+
- floating
-
-
-
-
-
+
+
+
+ World control
+ false
+ false
+ 72
+ 1
+ floating
+
+
+
+
+
+ true
+ true
+ true
+ true
+
- true
- true
- true
- true
+
+
+
+ World stats
+ false
+ false
+ 110
+ 290
+ 1
+ floating
+
+
+
+
+
+ true
+ true
+ true
+ true
+
+
+
+
+
+ false
+ 0
+ 0
+ 250
+ 50
+ floating
+ false
+ #666666
+
+
+
+
+
+ false
+ 250
+ 0
+ 150
+ 50
+ floating
+ false
+ #666666
+
-
- Transform control
-
-
-
-
- false
- 230
- 50
- floating
- false
- #666666
-
+
+ false
+ 0
+ 50
+ 250
+ 50
+ floating
+ false
+ #777777
+
-
-
-
-
-
-
-
- false
- 200
- 50
- floating
- false
- #666666
-
+
+
+
+ false
+ 250
+ 50
+ 50
+ 50
+ floating
+ false
+ #777777
+
+
+
+
+
+
+ false
+ 300
+ 50
+ 100
+ 50
+ floating
+ false
+ #777777
+
-
- false
- docked
-
+
+ false
+ docked
+
-
- false
- docked
-
+
+ false
+ docked
+
-
-
-
@@ -181,3 +294,4 @@
+