diff --git a/rmf_building_map_tools/building_map/building.py b/rmf_building_map_tools/building_map/building.py index 2c3987502..3b1c2785e 100644 --- a/rmf_building_map_tools/building_map/building.py +++ b/rmf_building_map_tools/building_map/building.py @@ -164,13 +164,16 @@ def generate_nav_graphs(self): nav_graphs[f'{i}'] = g return nav_graphs - def generate_sdf_world(self, options): + def generate_sdf_world(self, options, baked_sdfs_postfix = set()): """ Return an etree of this Building in SDF starting from a template""" print(f'generator options: {options}') + use_baked_assets = False if 'gazebo' in options: template_name = 'gz_world.sdf' elif 'ignition' in options: template_name = 'ign_world.sdf' + if 'baked_assets' in options: + use_baked_assets = True else: raise RuntimeError("expected either gazebo or ignition in options") @@ -183,17 +186,37 @@ def generate_sdf_world(self, options): world = sdf.find('world') for level_name, level in self.levels.items(): - level.generate_sdf_models(world) # todo: a better name + # todo: a better name + if use_baked_assets: + level.generate_sdf_models(world, False, True) + # use the baked asset in our world file + for postfix in baked_sdfs_postfix: + baked_include_ele = SubElement(world, 'include') + name_ele = SubElement(baked_include_ele, 'name') + + uri_ele = SubElement(baked_include_ele, 'uri') + if postfix == '': + name_ele.text = level_name + uri_ele.text = f'model://{self.name}_{level_name}' + else: + name_ele.text = f'{level_name}_{postfix}' + uri_ele.text = f'model://{self.name}_{level_name}_{postfix}' + pose_ele = SubElement(baked_include_ele, 'pose') + pose_ele.text = f'0 0 {level.elevation} 0 0 0' + else: + level_include_ele = SubElement(world, 'include') + level_model_name = f'{self.name}_{level_name}' + name_ele = SubElement(level_include_ele, 'name') + name_ele.text = level_model_name + uri_ele = SubElement(level_include_ele, 'uri') + uri_ele.text = f'model://{level_model_name}' + pose_ele = SubElement(level_include_ele, 'pose') + pose_ele.text = f'0 0 {level.elevation} 0 0 0' + + level.generate_sdf_models(world, True, True) + level.generate_doors(world, options) - level_include_ele = SubElement(world, 'include') - level_model_name = f'{self.name}_{level_name}' - name_ele = SubElement(level_include_ele, 'name') - name_ele.text = level_model_name - uri_ele = SubElement(level_include_ele, 'uri') - uri_ele.text = f'model://{level_model_name}' - pose_ele = SubElement(level_include_ele, 'pose') - pose_ele.text = f'0 0 {level.elevation} 0 0 0' for lift_name, lift in self.lifts.items(): if not lift.level_doors: @@ -281,14 +304,64 @@ def generate_sdf_world(self, options): return sdf - def generate_sdf_models(self, models_path): + def generate_sdf_world_for_dae_export(self, export_world_name, options): + if 'gazebo' in options: + template_name = 'gz_world.sdf' + elif 'ignition' in options: + template_name = 'ign_world.sdf' + else: + raise RuntimeError("expected either gazebo or ignition in options") + + template_path = os.path.join( + get_package_share_directory('rmf_building_map_tools'), + f'templates/{template_name}') + tree = parse(template_path) + sdf = tree.getroot() + + world_ele = sdf.find('world') + + world_export_plugin_ele = SubElement( + world_ele, + 'plugin', + { + 'name': 'ignition::gazebo::systems::ColladaWorldExporter', + 'filename': 'ignition-gazebo-collada-world-exporter-system' + }) + + for level_name, level in self.levels.items(): + for model in level.models: + if model.lightmap == export_world_name: + model.generate( + world_ele, + level.transform, + level.elevation) + + level_include_ele = SubElement(world_ele, 'include') + if export_world_name == '': + level_model_name = f'{self.name}_{level_name}' + else: + level_model_name = f'{self.name}_{level_name}_{export_world_name}' + name_ele = SubElement(level_include_ele, 'name') + name_ele.text = level_model_name + uri_ele = SubElement(level_include_ele, 'uri') + uri_ele.text = f'model://{level_model_name}' + pose_ele = SubElement(level_include_ele, 'pose') + pose_ele.text = f'0 0 {level.elevation} 0 0 0' + + return sdf + + + def generate_sdf_models(self, models_path, filter_world = ''): for level_name, level in self.levels.items(): - model_name = f'{self.name}_{level_name}' + if filter_world != '': + model_name = f'{self.name}_{level_name}_{filter_world}' + else: + model_name = f'{self.name}_{level_name}' model_path = os.path.join(models_path, model_name) if not os.path.exists(model_path): os.makedirs(model_path) - level.generate_sdf_model(model_name, model_path) + level.generate_sdf_model(model_name, model_path, filter_world) def center(self): # todo: something smarter in the future. For now just the center diff --git a/rmf_building_map_tools/building_map/floor.py b/rmf_building_map_tools/building_map/floor.py index 4392e5681..c9e0692d1 100644 --- a/rmf_building_map_tools/building_map/floor.py +++ b/rmf_building_map_tools/building_map/floor.py @@ -34,6 +34,7 @@ def __init__(self, yaml_node): self.indoor = 0 if 'indoor' in self.params: self.indoor = self.params['indoor'].value + self.polygon = None def to_yaml(self): y = {} diff --git a/rmf_building_map_tools/building_map/generator.py b/rmf_building_map_tools/building_map/generator.py index deac1599f..12805e511 100644 --- a/rmf_building_map_tools/building_map/generator.py +++ b/rmf_building_map_tools/building_map/generator.py @@ -17,6 +17,14 @@ def parse_editor_yaml(self, input_filename): y = yaml.safe_load(f) return Building(y) + # Remove namespaces in models + def trim_model_namespaces(self, building): + for level_name, level in building.levels.items(): + for model in level.models: + if "/" in model.model_name: + model.model_name = \ + "/".join(model.model_name.split("/")[1:]) + def generate_sdf( self, input_filename, @@ -29,12 +37,8 @@ def generate_sdf( building = self.parse_editor_yaml(input_filename) # Remove namespaces in models - for level_name, level in building.levels.items(): - for model in level.models: - if "/" in model.model_name: - model.model_name = \ - "/".join(model.model_name.split("/")[1:]) - + self.trim_model_namespaces(building) + if not os.path.exists(output_models_dir): os.makedirs(output_models_dir) @@ -49,6 +53,82 @@ def generate_sdf( f.write(sdf_str) print(f'{len(sdf_str)} bytes written to {output_filename}') + def get_prebaked_worlds(self, building): + all_prebaked_worlds = set() + delimiter = ';' + + for level_name, level in building.levels.items(): + for floor in level.floors: + if 'lightmap' in floor.params: + floor_lightmap = floor.params['lightmap'] + splits = floor_lightmap.value.split(delimiter) + # print(floor_lightmap.value) + for split in splits: + all_prebaked_worlds.add(split) + + for wall in level.walls: + if 'lightmap' in wall.params: + splits = wall.params['lightmap'].value.split(';') + for split in splits: + all_prebaked_worlds.add(split) + + for model in level.models: + worlds_split = model.lightmap.split(delimiter) + # print(lightmaps_split) + for lightmap in worlds_split: + all_prebaked_worlds.add(lightmap) + + return all_prebaked_worlds + + def generate_baked_worlds(self, + input_filename, + output_worlds_dir, + output_baked_file, + output_models_dir + ): + building = self.parse_editor_yaml(input_filename) + self.trim_model_namespaces(building) + + all_prebaked_worlds = self.get_prebaked_worlds(building) + print(f'all_prebaked_worlds: {all_prebaked_worlds}') + + if not os.path.exists(output_models_dir): + os.makedirs(output_models_dir) + + if not os.path.exists(output_worlds_dir): + os.makedirs(output_worlds_dir) + + for prebaked_world_name in all_prebaked_worlds: + if prebaked_world_name == '': + export_world_file = output_worlds_dir + "/default.world" + else: + export_world_file = output_worlds_dir + "/" + prebaked_world_name + ".world" + + print(export_world_file) + + # output walls and floors specific to the lightmap + filter_world = prebaked_world_name + building.generate_sdf_models(output_models_dir, filter_world) + + # generate a top-level SDF for export + sdf = building.generate_sdf_world_for_dae_export(prebaked_world_name, 'ignition') + + indent_etree(sdf) + sdf_str = str(ElementToString(sdf), 'utf-8') + with open(export_world_file, 'w') as f: + f.write(sdf_str) + print(f'{len(sdf_str)} bytes written to {export_world_file}') + + # generate top level sdf + baked_sdf = building.generate_sdf_world(['ignition'] + ['baked_assets'], + all_prebaked_worlds) + + indent_etree(baked_sdf) + baked_sdf_str = str(ElementToString(baked_sdf), 'utf-8') + with open(output_baked_file, 'w') as f: + f.write(baked_sdf_str) + print(f'{len(baked_sdf_str)} bytes written to {output_baked_file}') + def generate_gazebo_sdf( self, input_filename, @@ -75,6 +155,16 @@ def generate_ignition_sdf( output_models_dir, options + ['ignition']) + def generate_ignition_sdf_with_baked_worlds( + self, + input_filename, + output_worlds_dir, + output_baked_file, + output_models_dir + ): + self.generate_baked_worlds( + input_filename, output_worlds_dir, output_baked_file, output_models_dir) + def generate_nav(self, input_filename, output_dir): building = self.parse_editor_yaml(input_filename) nav_graphs = building.generate_nav_graphs() diff --git a/rmf_building_map_tools/building_map/level.py b/rmf_building_map_tools/building_map/level.py index 3ed694481..f1b492be6 100644 --- a/rmf_building_map_tools/building_map/level.py +++ b/rmf_building_map_tools/building_map/level.py @@ -159,10 +159,19 @@ def parse_edge_sequence(self, sequence_yaml): edges.append(Edge(edge_yaml)) return edges - def generate_walls(self, model_ele, model_name, model_path): + def generate_walls(self, model_ele, model_name, model_path, filter_world): wall_params_list = [] + print(f'walls: {model_name}') # crude method to identify all unique params list in walls for wall in self.walls: + if 'lightmap' in wall.params: + lightmap_splits = wall.params['lightmap'].value.split(';') + v = wall.params['lightmap'].value + + # print(f'split : {lightmap_splits}') + if filter_world not in lightmap_splits: + continue + # check if param exists, if not use default val if "texture_name" not in wall.params: wall.params["texture_name"] = ParamValue( @@ -171,11 +180,13 @@ def generate_walls(self, model_ele, model_name, model_path): wall.params["alpha"] = ParamValue([ParamValue.DOUBLE, 1.0]) if wall.params not in wall_params_list: wall_params_list.append(wall.params) + print(f'Walls Generation, wall params list: {wall_params_list}') wall_cnt = 0 for wall_params in wall_params_list: wall_cnt += 1 + print("generate single_texture_walls walls") single_texture_walls = Wall(self.walls, wall_params) single_texture_walls.generate( model_ele, @@ -184,17 +195,19 @@ def generate_walls(self, model_ele, model_name, model_path): model_path, self.transformed_vertices) - def generate_sdf_models(self, world_ele): - for model in self.models: - model.generate( - world_ele, - self.transform, - self.elevation) + def generate_sdf_models(self, world_ele, add_models, add_robots): + if add_models: + for model in self.models: + model.generate( + world_ele, + self.transform, + self.elevation) # sniff around in our vertices and spawn robots if requested - for vertex_idx, vertex in enumerate(self.vertices): - if 'spawn_robot_type' in vertex.params: - self.generate_robot_at_vertex_idx(vertex_idx, world_ele) + if add_robots: + for vertex_idx, vertex in enumerate(self.vertices): + if 'spawn_robot_type' in vertex.params: + self.generate_robot_at_vertex_idx(vertex_idx, world_ele) def generate_doors(self, world_ele, options): for door_edge in self.doors: @@ -244,10 +257,12 @@ def generate_robot_at_vertex_idx(self, vertex_idx, world_ele): pose_ele = SubElement(include_ele, 'pose') pose_ele.text = f'{vertex.x} {vertex.y} {vertex.z} 0 0 {yaw}' - def generate_floors(self, world_ele, model_name, model_path): + def generate_floors(self, world_ele, model_name, model_path, filter_world): i = 0 for floor in self.floors: i += 1 + if 'lightmap' in floor.params and filter_world != floor.params['lightmap'].value: + continue floor.generate( world_ele, i, @@ -266,7 +281,7 @@ def generate_floors(self, world_ele, model_name, model_path): self.holes, self.lift_vert_lists) - def write_sdf(self, model_name, model_path): + def write_sdf(self, model_name, model_path, filter_world): sdf_ele = Element('sdf', {'version': '1.7'}) model_ele = SubElement(sdf_ele, 'model', {'name': model_name}) @@ -274,8 +289,8 @@ def write_sdf(self, model_name, model_path): static_ele = SubElement(model_ele, 'static') static_ele.text = 'true' - self.generate_floors(model_ele, model_name, model_path) - self.generate_walls(model_ele, model_name, model_path) + self.generate_floors(model_ele, model_name, model_path, filter_world) + self.generate_walls(model_ele, model_name, model_path, filter_world) sdf_tree = ElementTree(sdf_ele) indent_etree(sdf_ele) @@ -283,13 +298,13 @@ def write_sdf(self, model_name, model_path): sdf_tree.write(sdf_path, encoding='utf-8', xml_declaration=True) print(f' wrote {sdf_path}') - def generate_sdf_model(self, model_name, model_path): + def generate_sdf_model(self, model_name, model_path, filter_world): print(f'generating model of level {self.name} in {model_path}') config_fn = os.path.join(model_path, 'model.config') self.write_config(model_name, config_fn) print(f' wrote {config_fn}') - self.write_sdf(model_name, model_path) + self.write_sdf(model_name, model_path, filter_world) def write_config(self, model_name, path): config_ele = Element('model') @@ -475,7 +490,7 @@ def edge_heading(self, edge): return math.atan2(dy, dx) def center(self): - if not self.floors or self.floors[0].polygon is None: + if not self.floors or (len(self.floors) >= 1 and self.floors[0].polygon is None): return (0, 0) bounds = self.floors[0].polygon.bounds return ((bounds[0] + bounds[2]) / 2.0, (bounds[1] + bounds[3]) / 2.0) diff --git a/rmf_building_map_tools/building_map/material_utils.py b/rmf_building_map_tools/building_map/material_utils.py index 426c82388..8522e71ed 100644 --- a/rmf_building_map_tools/building_map/material_utils.py +++ b/rmf_building_map_tools/building_map/material_utils.py @@ -66,9 +66,9 @@ def add_pbr_material(visual_ele, model_name, obj_name, texture_filename = copy_texture(texture_name, meshes_path) material_ele = SubElement(visual_ele, 'material') diffuse_ele = SubElement(material_ele, 'diffuse') - diffuse_ele.text = '1.0 1.0 1.0' + diffuse_ele.text = '1.0 1.0 1.0 1.0' specular_ele = SubElement(material_ele, 'specular') - specular_ele.text = '0.1 0.1 0.1' # TODO check specular value + specular_ele.text = '0.1 0.1 0.1 1.0' # TODO check specular value pbr_ele = SubElement(material_ele, 'pbr') metal_ele = SubElement(pbr_ele, 'metal') metalness_ele = SubElement(metal_ele, 'metalness') diff --git a/rmf_building_map_tools/building_map/model.py b/rmf_building_map_tools/building_map/model.py index 6a61dd2cc..6ba992542 100644 --- a/rmf_building_map_tools/building_map/model.py +++ b/rmf_building_map_tools/building_map/model.py @@ -29,6 +29,9 @@ def __init__(self, name, yaml_node): self.static = True self.yaw = yaml_node['yaw'] + self.lightmap = '' + if 'lightmap' in yaml_node: + self.lightmap = yaml_node['lightmap'] def to_yaml(self): y = {} @@ -39,6 +42,7 @@ def to_yaml(self): y['name'] = self.name y['yaw'] = self.yaw y['static'] = self.static + y['lightmap'] = self.lightmap return y def generate(self, world_ele, transform, elevation): @@ -55,3 +59,13 @@ def generate(self, world_ele, transform, elevation): static_ele = SubElement(include_ele, 'static') static_ele.text = str(self.static) + + # is lightmapped, turn off shadows + if self.lightmap != '': + params_ele = SubElement(include_ele, 'experimental:params') + visual_ele = SubElement(params_ele, 'visual') + visual_ele.attrib['element_id'] = 'body::visual' + + cast_shadows_ele = SubElement(visual_ele, 'cast_shadows') + cast_shadows_ele.attrib['action'] = 'add' + cast_shadows_ele.text = '0' diff --git a/rmf_building_map_tools/building_map/wall.py b/rmf_building_map_tools/building_map/wall.py index f3b00e993..896c2ac16 100644 --- a/rmf_building_map_tools/building_map/wall.py +++ b/rmf_building_map_tools/building_map/wall.py @@ -19,7 +19,12 @@ def __init__(self, yaml_node, wall_params): self.walls = [] self.wall_cnt = 0 self.texture_name = wall_params['texture_name'] - self.alpha = wall_params['alpha'] # val 0.0-1.0 transparency of wall + + # alpha/transparency 0.0-1.0 + if wall_params['alpha'].type is ParamValue.DOUBLE: + self.alpha = float(wall_params['alpha'].value) + else: + self.alpha = float(1.0) self.pbr_textures = get_pbr_textures(wall_params) if 'texture_height' in wall_params: # check for new parameters self.texture_height = wall_params['texture_height'].value @@ -215,7 +220,7 @@ def generate_wall_visual_mesh(self, model_name, model_path, f.write('Ke 0.0 0.0 0.0\n') # emissive f.write('Ns 50.0\n') # specular highlight, 0..100 (?) f.write('Ni 1.0\n') # no idea what this is - f.write(f'd {self.alpha.value}\n') # alpha + f.write(f'd {self.alpha}\n') # alpha f.write('illum 2\n') # illumination model (enum) f.write(f'map_Kd {texture_filename}\n') diff --git a/rmf_building_map_tools/building_map_generator/_init_argparse.py b/rmf_building_map_tools/building_map_generator/_init_argparse.py index 84c795c03..6571644be 100644 --- a/rmf_building_map_tools/building_map_generator/_init_argparse.py +++ b/rmf_building_map_tools/building_map_generator/_init_argparse.py @@ -32,6 +32,23 @@ parents=[shared_parser] ) +dae_export_arg_parser = argparse.ArgumentParser(add_help=False) +dae_export_arg_parser.add_argument("INPUT", type=str, + help="Input building.yaml file to process") +dae_export_arg_parser.add_argument("OUTPUT_WORLD_DIR", type=str, + help="Output directory of worlds") +dae_export_arg_parser.add_argument("BAKED_WORLD_FILE", type=str, + help="Path of the file that uses the baked models") +dae_export_arg_parser.add_argument("OUTPUT_MODEL_DIR", type=str, + help="Path to output the map model files") +dae_export_arg_parser.add_argument("-o", "--options", type=str, nargs='*', default=[], + help="Generator options") +dae_export_parser = subparsers.add_parser( + 'ignition_dae_export', + help='Generate multiple .world files ready for dae exporting through Ignition', + parents=[dae_export_arg_parser] +) + nav_parser = subparsers.add_parser( 'nav', help='Generate nav map .yaml file', diff --git a/rmf_building_map_tools/building_map_generator/building_map_generator.py b/rmf_building_map_tools/building_map_generator/building_map_generator.py index d2304c5dd..5b12a551b 100755 --- a/rmf_building_map_tools/building_map_generator/building_map_generator.py +++ b/rmf_building_map_tools/building_map_generator/building_map_generator.py @@ -28,6 +28,14 @@ def main(): args.options ) + if args.command == "ignition_dae_export": + g.generate_ignition_sdf_with_baked_worlds( + args.INPUT, + args.OUTPUT_WORLD_DIR, + args.BAKED_WORLD_FILE, + args.OUTPUT_MODEL_DIR + ) + if args.command == "nav": g.generate_nav(args.INPUT, args.OUTPUT_DIR) diff --git a/rmf_traffic_editor/gui/building.cpp b/rmf_traffic_editor/gui/building.cpp index 222249833..c5bf4ecc0 100644 --- a/rmf_traffic_editor/gui/building.cpp +++ b/rmf_traffic_editor/gui/building.cpp @@ -336,6 +336,7 @@ QUuid Building::add_model( m.model_name = model_name; m.instance_name = model_name; // todo: add unique numeric suffix? m.is_static = true; + m.lightmap = ""; levels[level_idx].models.push_back(m); return levels[level_idx].models.rbegin()->uuid; } diff --git a/rmf_traffic_editor/gui/edge.cpp b/rmf_traffic_editor/gui/edge.cpp index b32916d93..5b1dbc1f3 100644 --- a/rmf_traffic_editor/gui/edge.cpp +++ b/rmf_traffic_editor/gui/edge.cpp @@ -122,6 +122,7 @@ void Edge::create_required_parameters() create_param_if_needed("texture_height", Param::DOUBLE, 2.5); create_param_if_needed("texture_width", Param::DOUBLE, 1.0); create_param_if_needed("texture_scale", Param::DOUBLE, 1.0); + create_param_if_needed("lightmap", Param::STRING, std::string("")); } else if (type == LANE) { @@ -141,6 +142,7 @@ void Edge::create_required_parameters() create_param_if_needed("motion_degrees", Param::DOUBLE, 90.0); // hinged create_param_if_needed("right_left_ratio", Param::DOUBLE, 1.0); // doubles create_param_if_needed("plugin", Param::STRING, std::string("normal")); + create_param_if_needed("lightmap", Param::STRING, std::string("")); } else if (type == HUMAN_LANE) { diff --git a/rmf_traffic_editor/gui/editor.cpp b/rmf_traffic_editor/gui/editor.cpp index 91c1f6b33..de2107ec1 100644 --- a/rmf_traffic_editor/gui/editor.cpp +++ b/rmf_traffic_editor/gui/editor.cpp @@ -1413,7 +1413,7 @@ void Editor::populate_property_editor(const Model& model) { property_editor->blockSignals(true); // otherwise we get tons of callbacks - property_editor->setRowCount(4); + property_editor->setRowCount(5); property_editor_set_row( 0, @@ -1439,6 +1439,12 @@ void Editor::populate_property_editor(const Model& model) model.is_static ? QString("true") : QString("false"), true); + property_editor_set_row( + 4, + "lightmap", + QString::fromStdString(model.lightmap), + true); + property_editor->blockSignals(false); // re-enable callbacks } diff --git a/rmf_traffic_editor/gui/model.cpp b/rmf_traffic_editor/gui/model.cpp index b69e42265..41e37636c 100644 --- a/rmf_traffic_editor/gui/model.cpp +++ b/rmf_traffic_editor/gui/model.cpp @@ -68,6 +68,11 @@ void Model::from_yaml(const YAML::Node& data, const string& level_name) is_static = data["static"].as(); else is_static = true; + + if (data["lightmap"]) + lightmap = data["lightmap"].as(); + else + lightmap = ""; } YAML::Node Model::to_yaml() const @@ -85,6 +90,7 @@ YAML::Node Model::to_yaml() const n["name"] = instance_name; n["model_name"] = model_name; n["static"] = is_static; + n["lightmap"] = lightmap; return n; } @@ -116,6 +122,10 @@ void Model::set_param(const std::string& name, const std::string& value) else is_static = false; } + else if (name == "lightmap") + { + lightmap = value; + } else if (name == "name") { instance_name = value; diff --git a/rmf_traffic_editor/gui/model.h b/rmf_traffic_editor/gui/model.h index fc3c65796..d7e328f1c 100644 --- a/rmf_traffic_editor/gui/model.h +++ b/rmf_traffic_editor/gui/model.h @@ -48,6 +48,7 @@ class Model bool selected = false; // only for visualization, not saved to YAML bool is_static = true; bool is_active = false; + std::string lightmap = ""; bool error_printed = false; std::string starting_level; // used when resetting a test scenario QGraphicsPixmapItem* pixmap_item = nullptr; diff --git a/rmf_traffic_editor/gui/polygon.cpp b/rmf_traffic_editor/gui/polygon.cpp index ec28f1763..98b7596af 100644 --- a/rmf_traffic_editor/gui/polygon.cpp +++ b/rmf_traffic_editor/gui/polygon.cpp @@ -125,6 +125,7 @@ void Polygon::create_required_parameters() Param::STRING, std::string("blue_linoleum")); create_param_if_needed("ceiling_scale", Param::DOUBLE, 1.0); + create_param_if_needed("lightmap", Param::STRING, std::string("")); } }