Add bounds to map config

Author: oysand

src/alitra/__init__.py

@@ -45,4 +45,5 @@
     Translation,
 )
 from alitra.frame_transform import FrameTransform
+from alitra.models.bounds import Bounds
 from alitra.models.map_config import MapConfig, load_map_config

src/alitra/models/bounds.py

@@ -0,0 +1,40 @@
+from dataclasses import dataclass
+
+from alitra.frame_dataclasses import Point
+
+
+# Point 1 and 2 should be on the diagonal corners of the bounding cube
+@dataclass
+class Bounds:
+    point1: Point
+    point2: Point
+
+    def __post_init__(self):
+        if self.point1.frame == self.point2.frame:
+            self.frame = self.point1.frame
+        else:
+            raise FrameException("The frames of the bounding points are not the same")
+        points: list = [self.point1, self.point2]
+        self.x_max = max(point.x for point in points)
+        self.x_min = min(point.x for point in points)
+        self.y_max = max(point.y for point in points)
+        self.y_min = min(point.y for point in points)
+        self.z_max = max(point.z for point in points)
+        self.z_min = min(point.z for point in points)
+
+    def point_within_bounds(self, point: Point) -> bool:
+        if not point.frame == self.frame:
+            raise FrameException(
+                f"The point is in {point.frame} frame and the bounds are in {self.frame} frame"
+            )
+        if point.x < self.x_min or point.x > self.x_max:
+            return False
+        if point.y < self.y_min or point.y > self.y_max:
+            return False
+        if point.z < self.z_min or point.z > self.z_max:
+            return False
+        return True
+
+
+class FrameException(Exception):
+    pass

src/alitra/models/map_config.py

@@ -5,13 +5,15 @@
 from dacite import from_dict
 
 from alitra.frame_dataclasses import PointList
+from alitra.models.bounds import Bounds
 
 
 @dataclass
 class MapConfig:
     map_name: str
     robot_reference_points: PointList
     asset_reference_points: PointList
+    bounds: Bounds = None
 
 
 def load_map_config(map_config_path: Path) -> MapConfig:

tests/models/test_map_config.py

@@ -1,8 +1,10 @@
+from cmath import exp
 from pathlib import Path
 
 import pytest
 
 from alitra.frame_dataclasses import Point, PointList
+from alitra.models.bounds import Bounds, FrameException
 from alitra.models.map_config import MapConfig, load_map_config
 
 expected_map_config = MapConfig(
@@ -24,6 +26,29 @@
         frame="asset",
     ),
 )
+expected_map_config_bounds = MapConfig(
+    map_name="test_map",
+    robot_reference_points=PointList(
+        points=[
+            Point(x=10, y=20, z=30, frame="robot"),
+            Point(x=40, y=50, z=60, frame="robot"),
+            Point(x=70, y=80, z=90, frame="robot"),
+        ],
+        frame="robot",
+    ),
+    asset_reference_points=PointList(
+        points=[
+            Point(x=11, y=21, z=31, frame="asset"),
+            Point(x=41, y=51, z=61, frame="asset"),
+            Point(x=71, y=81, z=91, frame="asset"),
+        ],
+        frame="asset",
+    ),
+    bounds=Bounds(
+        point1=Point(x=5, y=15, z=30, frame="asset"),
+        point2=Point(x=80, y=90, z=100, frame="asset"),
+    ),
+)
 
 
 def test_load_map_config():
@@ -36,3 +61,32 @@ def test_invalid_file_path():
     map_config_path = Path("./tests/test_data/test_map_config/no_file.json")
     with pytest.raises(Exception):
         load_map_config(map_config_path)
+
+
+def test_load_map_bounds():
+    map_config_path = Path(
+        "./tests/test_data/test_map_config/test_map_config_bounds.json"
+    )
+    map_config: MapConfig = load_map_config(map_config_path)
+    assert map_config == expected_map_config_bounds
+    assert map_config.bounds.x_min == expected_map_config_bounds.bounds.point1.x
+    assert map_config.bounds.x_min == expected_map_config_bounds.bounds.point1.x
+    assert map_config.bounds.y_min == expected_map_config_bounds.bounds.point1.y
+    assert map_config.bounds.y_max == expected_map_config_bounds.bounds.point2.y
+    assert map_config.bounds.z_max == expected_map_config_bounds.bounds.point2.z
+    assert map_config.bounds.z_max == expected_map_config_bounds.bounds.point2.z
+
+
+def test_points_within_bounds():
+    map_config_path = Path(
+        "./tests/test_data/test_map_config/test_map_config_bounds.json"
+    )
+    map_config: MapConfig = load_map_config(map_config_path)
+    point_within_bounds: Point = Point(x=10, y=40, z=50, frame="asset")
+    point_outside_bounds: Point = Point(x=2, y=40, z=50, frame="asset")
+    point_with_wrong_frame: Point = Point(x=2, y=40, z=50, frame="robot")
+
+    assert map_config.bounds.point_within_bounds(point=point_within_bounds) == True
+    assert map_config.bounds.point_within_bounds(point=point_outside_bounds) == False
+    with pytest.raises(FrameException):
+        map_config.bounds.point_within_bounds(point=point_with_wrong_frame)

tests/test_data/test_map_config/test_map_config_bounds.json

@@ -0,0 +1,63 @@
+{
+  "map_name": "test_map",
+  "robot_reference_points": {
+    "points": [
+      {
+        "x": 10,
+        "y": 20,
+        "z": 30,
+        "frame": "robot"
+      },
+      {
+        "x": 40,
+        "y": 50,
+        "z": 60,
+        "frame": "robot"
+      },
+      {
+        "x": 70,
+        "y": 80,
+        "z": 90,
+        "frame": "robot"
+      }
+    ],
+    "frame": "robot"
+  },
+  "asset_reference_points": {
+    "points": [
+      {
+        "x": 11,
+        "y": 21,
+        "z": 31,
+        "frame": "asset"
+      },
+      {
+        "x": 41,
+        "y": 51,
+        "z": 61,
+        "frame": "asset"
+      },
+      {
+        "x": 71,
+        "y": 81,
+        "z": 91,
+        "frame": "asset"
+      }
+    ],
+    "frame": "asset"
+  },
+  "bounds": {
+    "point1": {
+      "x": 5,
+      "y": 15,
+      "z": 30,
+      "frame": "asset"
+    },
+    "point2": {
+      "x": 80,
+      "y": 90,
+      "z": 100,
+      "frame": "asset"
+    }
+  }
+}