add baking pipeline capabilities, bugfixes for alpha values

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

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 = {}

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

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,30 +281,30 @@ 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})
 
         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)
         sdf_path = os.path.join(model_path, 'model.sdf')
         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)