ECS refactor for lifts and doors

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

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