From 425fb95617ce28c50e80ce7e5b7795b432d4d0b8 Mon Sep 17 00:00:00 2001 From: kevinskwk Date: Wed, 22 Jul 2020 18:42:47 +0800 Subject: [PATCH] cleanups for nested model in ignition --- building_map_tools/building_map/building.py | 2 +- building_map_tools/building_map/lift.py | 75 +------------------ .../building_ignition_plugins/src/door.cpp | 11 +-- 3 files changed, 9 insertions(+), 79 deletions(-) diff --git a/building_map_tools/building_map/building.py b/building_map_tools/building_map/building.py index 5ed3a4856..5169459da 100644 --- a/building_map_tools/building_map/building.py +++ b/building_map_tools/building_map/building.py @@ -213,7 +213,7 @@ def generate_sdf_world(self, options): for lift_name, lift in self.lifts.items(): lift.generate_shaft_doors(world) - lift.generate_cabin(world, options) + lift.generate_cabin(world) gui_ele = world.find('gui') c = self.center() diff --git a/building_map_tools/building_map/lift.py b/building_map_tools/building_map/lift.py index 45c9193a3..84d36f042 100644 --- a/building_map_tools/building_map/lift.py +++ b/building_map_tools/building_map/lift.py @@ -135,66 +135,6 @@ def generate_door_plugin(self, model_ele, name): door_ele.set('right_joint_name', '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)) - - 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): @@ -303,7 +243,7 @@ def generate_wall(self, side, pair, name, platform): platform.append(visual(name, pose, dims, lift_material())) platform.append(collision(name, pose, dims, '0x01')) - def generate_cabin(self, world_ele, options): + def generate_cabin(self, world_ele): # materials missing for now lift_model_name = f'{self.name}' lift_model_ele = SubElement(world_ele, 'model') @@ -347,16 +287,9 @@ 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 plugin_ele = SubElement(lift_model_ele, 'plugin') diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 4f1c67f4d..246f54268 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -65,21 +65,18 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // TODO proper rclcpp init (only once and pass args) auto model = Model(entity); char const** argv = NULL; - std::string name; - auto door_ele = sdf->GetElementImpl("door"); - get_sdf_attribute_required(door_ele, "name", name); if (!rclcpp::is_initialized()) rclcpp::init(0, argv); - std::string plugin_name("plugin_" + name); + std::string plugin_name("plugin_" + model.Name(ecm)); ignwarn << "Initializing plugin with name " << plugin_name << std::endl; _ros_node = std::make_shared(plugin_name); RCLCPP_INFO(_ros_node->get_logger(), "Loading DoorPlugin for [%s]", - name.c_str()); + model.Name(ecm).c_str()); _door_common = DoorCommon::make( - name, + model.Name(ecm), _ros_node, sdf); @@ -104,7 +101,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin RCLCPP_INFO(_ros_node->get_logger(), "Finished loading [%s]", - name.c_str()); + model.Name(ecm).c_str()); } void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override