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