Add 3d mesh and occ. grid generation from DSL repo

README.md

@@ -2,6 +2,27 @@
 
 ## Installation
 
+Install all the requirements:
+
+```shell
+sudo apt-get install blender python3-pip python3-venv -y
+```
+
+First, create a virtual environment and activate it: 
+
+```shell
+python -m venv .venv
+source .venv/bin/activate
+```
+
+For Blender to regonize the virtual environment, add it to your `PYTHONPATH`:
+
+```shell
+export PYTHONPATH=<Path to .venv directory>/lib/python3.11/site-packages   
+```
+
+From the root directory of the repo, install the python packages by running: 
+
 ```shell
 pip install -e .
 ```
@@ -26,6 +47,49 @@ Where the input folder must contain:
     - `object-door-states.json`
 - any object instance models, e.g. `object-door-instance-X.json` where `X` is a unique numeric ID.
 
+### Generating 3D meshes and occupancy grid maps
+
+> [!WARNING]
+> The generation of 3D meshes and occupancy grid maps is currently being moved to the [scenery_builder](https://github.com/secorolab/scenery_builder) repository. The instructions below may not work and/or may be outdated.
+
+This tool is currently in active development. To use the tool you can execute the following command: 
+
+```
+blender --background --python src/exsce_floorplan/exsce_floorplan.py --python-use-system-env -- <model_path>
+```
+
+Optionally, you can remove the `--background` flag to see directly the result opened in Blender.
+
+***Note**: The `--` before `<model_path>` is intentional and important.*
+
+#### Example
+
+![3D asset generated from the environment description](images/hospital_no_brackground.png)
+
+An example model for a building is available [here](../models/examples/hospital.floorplan). To generate the 3D mesh and occupancy grid:
+
+
+```
+blender --background --python src/exsce_floorplan/exsce_floorplan.py --python-use-system-env -- models/examples/hospital.floorplan
+```
+
+That should generate the following files:
+
+```
+.
+├── map
+│   ├── hospital.pgm
+│   └── hospital.yaml
+└── mesh
+    └── hospital.stl
+```
+
+The output path for the generated models in configurable (see [confg/setup.cfg](../config/setup.cfg) and note they are relative paths from where you're calling the command).
+
+The `.stl` mesh can now be used to specify the Gazebo models and included in a Gazebo world. See, for example, [this tutorial](https://classic.gazebosim.org/tutorials?tut=import_mesh&cat=build_robot).
+
+
+
 ## Task generator
 
 It uses the FloorPlan insets to generate a task specification.

src/fpm/generators/mesh.py

@@ -0,0 +1,77 @@
+import os
+
+import bpy
+import configparser
+from pathlib import Path
+
+from fpm.transformations.blender import (
+    boolean_operation_difference,
+    clear_scene,
+    create_mesh,
+    create_collection,
+    export,
+)
+
+
+class FloorPlan(object):
+    """
+    Floor plan model interpreter
+    """
+
+    def __init__(self, model):
+        # instanciate all walls (boundary lines for each space)
+        self.model = model
+        self.spaces = model.spaces
+        self.wall_openings = model.wall_openings
+
+        # config file
+        config = configparser.ConfigParser()
+        path_to_file = Path(
+            os.path.dirname(os.path.abspath(__file__))
+        ).parent.parent.parent
+        config.read(os.path.join(path_to_file, "config", "setup.cfg"))
+
+        self.output_3d_file = config["model"]["output_folder"]
+        self.format_3d_file = config["model"]["format"]
+
+        if "{{model_name}}" in self.output_3d_file:
+            self.output_3d_file = self.output_3d_file.replace(
+                "{{model_name}}", model.name
+            )
+            print(self.output_3d_file)
+            if not os.path.exists(self.output_3d_file):
+                os.makedirs(self.output_3d_file)
+
+    def model_to_3d_transformation(self):
+
+        building = create_collection(self.model.name)
+        # clear the blender scene
+        clear_scene()
+
+        # create wall spaces
+        for space in self.spaces:
+            for i, wall in enumerate(space.walls):
+                vertices, faces = wall.generate_3d_structure()
+                create_mesh(building, wall.name, vertices, faces)
+
+            for feature in space.floor_features:
+                vertices, faces = feature.generate_3d_structure()
+                create_mesh(building, feature.name, vertices, faces)
+
+        # create wall openings
+        for wall_opening in self.wall_openings:
+
+            vertices, faces = wall_opening.generate_3d_structure()
+            create_mesh(building, wall_opening.name, vertices, faces)
+
+            # boolean operation for walls and opening
+            boolean_operation_difference(wall_opening.wall_a.name, wall_opening.name)
+            if not wall_opening.wall_b is None:
+                boolean_operation_difference(
+                    wall_opening.wall_b.name, wall_opening.name
+                )
+
+            bpy.data.objects[wall_opening.name].select_set(True)
+            bpy.ops.object.delete()
+
+        export(self.format_3d_file, self.output_3d_file, self.model.name)

src/fpm/generators/occ_grid.py

@@ -0,0 +1,162 @@
+import os
+import io
+
+import yaml
+import numpy as np
+from PIL import Image, ImageDraw, ImageOps
+
+import configparser
+from pathlib import Path
+
+
+class FloorPlan(object):
+    """
+    Floor plan model interpreter
+    """
+
+    def __init__(self, model):
+        # instanciate all walls (boundary lines for each space)
+        self.model = model
+        self.spaces = model.spaces
+        self.wall_openings = model.wall_openings
+
+        # config file
+        config = configparser.ConfigParser()
+        path_to_file = Path(
+            os.path.dirname(os.path.abspath(__file__))
+        ).parent.parent.parent
+        config.read(os.path.join(path_to_file, "config", "setup.cfg"))
+
+        self.map_yaml_resolution = config.getfloat("map_yaml", "resolution")
+        self.map_yaml_occupied_thresh = config.getfloat("map_yaml", "occupied_thresh")
+        self.map_yaml_free_thresh = config.getfloat("map_yaml", "free_thresh")
+        self.map_yaml_negate = config.getint("map_yaml", "negate")
+
+        self.map_unknown = config.getint("map", "unknown")
+        self.map_occupied = config.getint("map", "occupied")
+        self.map_free = config.getint("map", "free")
+        self.map_laser_height = config.getfloat("map", "laser_height")
+        self.map_output_folder = config["map"]["output_folder"]
+        self.map_border = config.getint("map", "border")
+
+        if "{{model_name}}" in self.map_output_folder:
+            self.map_output_folder = self.map_output_folder.replace(
+                "{{model_name}}", model.name
+            )
+            if not os.path.exists(self.map_output_folder):
+                os.makedirs(self.map_output_folder)
+
+    def model_to_occupancy_grid_transformation(self):
+
+        unknown = self.map_unknown
+        occupied = self.map_occupied
+        free = self.map_free
+        res = self.map_yaml_resolution
+        border = self.map_border
+        laser_height = self.map_laser_height
+
+        points = []
+        directions = []
+
+        for space in self.spaces:
+            shape = space.get_shape()
+            shape_points = shape.get_points()
+            points.append(shape_points)
+
+            directions.append(
+                [
+                    np.amax(shape_points[:, 1]),  # north
+                    np.amin(shape_points[:, 1]),  # south
+                    np.amax(shape_points[:, 0]),  # east
+                    np.amin(shape_points[:, 0]),  # west
+                ]
+            )
+
+        directions = np.array(directions)
+        north = np.amax(directions[:, 0])
+        south = np.amin(directions[:, 1])
+        east = np.amax(directions[:, 2])
+        west = np.amin(directions[:, 3])
+
+        # Create canvas
+        floor = (
+            int(abs(east - west) / res) + border,
+            int(abs(north - south) / res) + border,
+        )
+
+        im = Image.new("L", floor, unknown)
+        draw = ImageDraw.Draw(im)
+
+        center = [
+            -float(abs(west) + border * res / 2),
+            -float(abs(south) + border * res / 2),
+            0,
+        ]
+
+        for shape in points:
+            shape[:, 0] = (shape[:, 0] + abs(west)) / res
+            shape[:, 1] = (shape[:, 1] + abs(south)) / res
+            shape += border / 2
+            shape = shape.astype(int)
+
+            draw.polygon(shape[:, 0:2].flatten().tolist(), fill=free)
+
+        for space in self.spaces:
+            for wall in space.walls:
+                points, _ = wall.generate_3d_structure()
+
+                shape = points[0 : int(len(points) / 2), 0:2]
+                shape[:, 0] = (shape[:, 0] + abs(west)) / res
+                shape[:, 1] = (shape[:, 1] + abs(south)) / res
+                shape += border / 2
+                shape = shape.astype(int)
+
+                draw.polygon(shape[:, 0:2].flatten().tolist(), fill=occupied)
+
+        name_yaml = "{}.yaml".format(self.model.name)
+        name_image = "{}.pgm".format(self.model.name)
+
+        with io.open(
+            os.path.join(self.map_output_folder, name_yaml), "w", encoding="utf8"
+        ) as outfile:
+            pgm_config = {
+                "resolution": res,
+                "origin": center,
+                "occupied_thresh": self.map_yaml_occupied_thresh,
+                "free_thresh": self.map_yaml_free_thresh,
+                "negate": self.map_yaml_negate,
+                "image": name_image,
+            }
+            yaml.dump(pgm_config, outfile, default_flow_style=False, allow_unicode=True)
+
+        for wall_opening in self.wall_openings:
+
+            shape = wall_opening.generate_2d_structure(laser_height)
+
+            if shape is None:
+                continue
+
+            shape[:, 0] = (shape[:, 0] + abs(west)) / res
+            shape[:, 1] = (shape[:, 1] + abs(south)) / res
+            shape += border / 2
+            shape = shape.astype(int)
+
+            draw.polygon(shape[:, 0:2].flatten().tolist(), fill=free)
+
+        for space in self.spaces:
+            for feature in space.floor_features:
+                points, _ = feature.generate_3d_structure()
+
+                if points[int(len(points) / 2) :, 2][0] < laser_height:
+                    continue
+
+                shape = points[0 : int(len(points) / 2), 0:2]
+                shape[:, 0] = (shape[:, 0] + abs(west)) / res
+                shape[:, 1] = (shape[:, 1] + abs(south)) / res
+                shape += border / 2
+                shape = shape.astype(int)
+
+                draw.polygon(shape[:, 0:2].flatten().tolist(), fill=occupied)
+
+        im = ImageOps.flip(im)
+        im.save(os.path.join(self.map_output_folder, name_image), quality=95)

src/fpm/transformations/blender.py

@@ -0,0 +1,64 @@
+import bpy
+import bmesh
+import os
+
+
+def create_mesh(collection, name, vertices, faces):
+    """Creates a mesh"""
+
+    me = bpy.data.meshes.new(name)
+    me.from_pydata(vertices, [], faces)
+    me.update()
+
+    bm = bmesh.new()
+    bm.from_mesh(me, face_normals=True)
+
+    bmesh.ops.recalc_face_normals(bm, faces=bm.faces)
+
+    bm.to_mesh(me)
+    bm.free()
+    me.update()
+
+    obj = bpy.data.objects.new(name, me)
+    collection.objects.link(obj)
+
+
+def create_collection(name):
+    """Creates an object collection"""
+
+    collection = bpy.data.collections.new(name)
+    bpy.context.scene.collection.children.link(collection)
+    return collection
+
+
+def clear_scene():
+    """Clears the scene from all objects (often the default objects: a cube mesh, a light source, and a camera)"""
+
+    for obj in bpy.context.scene.objects:
+        obj.select_set(True)
+        bpy.ops.object.delete()
+
+
+def boolean_operation_difference(obj_name, cutter_name):
+    """Performs a the difference boolean operation"""
+
+    # select the object
+    obj = bpy.data.objects[obj_name]
+    # configure modifier
+    boolean = obj.modifiers.new(name="boolean", type="BOOLEAN")
+    boolean.object = bpy.data.objects[cutter_name]
+    boolean.operation = "DIFFERENCE"
+    # apply modifier
+    bpy.context.view_layer.objects.active = obj
+    bpy.ops.object.modifier_apply(modifier="boolean")
+
+
+def export(_format, path, name):
+    """Exports scene into a mesh with the specified format, path, and name"""
+
+    if _format == "stl":
+        name = "{name}.stl".format(name=name)
+        bpy.ops.export_mesh.stl(filepath=os.path.join(path, name))
+    elif _format == "dae":
+        name = "{name}.dae".format(name=name)
+        bpy.ops.wm.collada_export(filepath=os.path.join(path, name))