ECS refactor for lifts and doors

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

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
 

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')

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')

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)
+

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)
+

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}'
+