Lh2 on dotbot

dotbot/controller.py

@@ -72,6 +72,7 @@
     PayloadCommandXgoAction,
     PayloadGPSPosition,
     PayloadGPSWaypoints,
+    PayloadLh2CalibrationHomography,
     PayloadLH2Location,
     PayloadLH2Waypoints,
     PayloadType,
@@ -164,9 +165,11 @@ def __init__(self, settings: ControllerSettings):
         self.adapter: GatewayAdapterBase = None
         self.websockets = []
         self.lh2_manager = LighthouseManager()
+
         self.api = api
         api.controller = self
         self.qrkey = None
+
         self.subscriptions = [
             SubscriptionModel(
                 topic="/command/+/+/+/move_raw", callback=self.on_command_move_raw
@@ -398,6 +401,7 @@ def on_lh2_start(self, topic, _):
             return
         logger.info("Start calibration")
         self.lh2_manager.compute_calibration()
+        logger.info("Calibration complete")
 
     def on_request(self, payload):
         logger = LOGGER.bind(topic="/request")
@@ -552,9 +556,34 @@ def handle_received_frame(
 
         if frame.packet.payload_type == PayloadType.ADVERTISEMENT:
             logger = logger.bind(
-                application=ApplicationType(frame.packet.payload.application).name
+                application=ApplicationType(frame.packet.payload.application).name,
+                calibrated=bool(frame.packet.payload.calibrated),
             )
             dotbot.application = ApplicationType(frame.packet.payload.application)
+            dotbot.calibrated = bool(frame.packet.payload.calibrated)
+            self.dotbots.update({dotbot.address: dotbot})
+            logger.debug("Advertisement received")
+            # Send calibration to dotbot if it's not calibrated and the localization system has calibration
+            if (
+                dotbot.calibrated is False
+                and self.lh2_manager.state == LighthouseManagerState.Calibrated
+            ):
+                # Send calibration to new dotbot if the localization system is calibrated
+                # Check if robot has lighthouse calibration
+                matrix_bytes = bytearray()
+                for bytes_block in [
+                    int(n * 1e6).to_bytes(4, "little", signed=True)
+                    for n in self.lh2_manager.calibration_data.m.ravel()
+                ]:
+                    matrix_bytes += bytes_block
+                # Prepare homography matrix and send it to the robot
+                payload = PayloadLh2CalibrationHomography(
+                    index=0,
+                    homography_matrix=matrix_bytes,
+                )
+                self.logger.info("Send calibration data", payload=payload)
+                self.dotbots.update({dotbot.address: dotbot})
+                self.send_payload(int(source, 16), payload=payload)
 
         if (
             frame.packet.payload_type
@@ -571,36 +600,40 @@ def handle_received_frame(
                 sail_angle=dotbot.sail_angle,
             )
 
-        dotbot.lh2_position = self._compute_lh2_position(frame)
-        if (
-            dotbot.lh2_position is not None
-            and 0 <= dotbot.lh2_position.x <= 1
-            and 0 <= dotbot.lh2_position.y <= 1
-        ):
+        if frame.packet.payload_type == PayloadType.DOTBOT_DATA:
             new_position = DotBotLH2Position(
-                x=dotbot.lh2_position.x,
-                y=dotbot.lh2_position.y,
-                z=dotbot.lh2_position.z,
+                x=frame.packet.payload.pos_x / 1e6,
+                y=frame.packet.payload.pos_y / 1e6,
+                z=0.0,
             )
-            logger.info("lh2-raw", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y)
-            if (
-                not dotbot.position_history
-                or lh2_distance(dotbot.position_history[-1], new_position)
-                >= LH2_POSITION_DISTANCE_THRESHOLD
-            ):
-                dotbot.position_history.append(new_position)
-                notification_cmd = DotBotNotificationCommand.UPDATE
+            dotbot.direction = frame.packet.payload.direction
+            dotbot.lh2_position = new_position
+            dotbot.position_history.append(new_position)
+            notification_cmd = DotBotNotificationCommand.UPDATE
             if len(dotbot.position_history) > MAX_POSITION_HISTORY_SIZE:
                 dotbot.position_history.pop(0)
-            # Send the computed position back to the dotbot
-            payload = PayloadLH2Location(
-                pos_x=int(dotbot.lh2_position.x * 1e6),
-                pos_y=int(dotbot.lh2_position.y * 1e6),
-                pos_z=int(dotbot.lh2_position.z * 1e6),
+            self.logger.info(
+                "Received DotBot Data",
+                direction=dotbot.direction,
+                X=new_position.x,
+                Y=new_position.y,
+            )
+
+        if frame.packet.payload_type == PayloadType.LH2_RAW_DATA:
+            self.lh2_manager.last_raw_data = frame.packet.payload
+            self.logger.debug(
+                "Received LH2 Raw Data",
+                location_1_bits=self.lh2_manager.last_raw_data.locations[0].bits,
+                location_1_index=self.lh2_manager.last_raw_data.locations[
+                    0
+                ].polynomial_index,
+                location_1_offset=self.lh2_manager.last_raw_data.locations[0].offset,
+                location_2_bits=self.lh2_manager.last_raw_data.locations[1].bits,
+                location_2_index=self.lh2_manager.last_raw_data.locations[
+                    1
+                ].polynomial_index,
+                location_2_offset=self.lh2_manager.last_raw_data.locations[1].offset,
             )
-            self.send_payload(int(source, 16), payload=payload)
-        elif frame.packet.payload_type == PayloadType.DOTBOT_DATA:
-            logger.warning("lh2: invalid position")
 
         if frame.packet.payload_type == PayloadType.LH2_PROCESSED_DATA:
             logger.info(

dotbot/lighthouse2.py

@@ -116,6 +116,7 @@ class LighthouseManager:
     """Class to manage the LightHouse positionning state and workflow."""
 
     def __init__(self):
+        self.logger = LOGGER.bind(context=__name__)
         self.state = LighthouseManagerState.NotCalibrated
         self.reference_points = REFERENCE_POINTS_DEFAULT
         Path.mkdir(CALIBRATION_DIR, exist_ok=True)
@@ -126,7 +127,6 @@ def __init__(self):
         )
         self.calibration_points_available = [False] * len(self.reference_points)
         self.last_raw_data = None
-        self.logger = LOGGER.bind(context=__name__)
         self.logger.info("Lighthouse initialized")
 
     @property
@@ -142,10 +142,16 @@ def state_model(self) -> DotBotCalibrationStateModel:
 
     def _load_calibration(self) -> Optional[CalibrationData]:
         if not os.path.exists(self.calibration_output_path):
+            self.logger.info("No calibration file found")
             return None
         with open(self.calibration_output_path, "rb") as calibration_file:
             calibration = pickle.load(calibration_file)
+        # for compatibility with existing calibration data type, cast
+        # homography matrix to float32
+        calibration.m = calibration.m.astype(np.float32)
+        self.logger.info("Lighthouse calibration loaded")
         self.state = LighthouseManagerState.Calibrated
+
         return calibration
 
     def add_calibration_point(self, index):
@@ -174,6 +180,7 @@ def add_calibration_point(self, index):
             dtype=np.float64,
         )
 
+        self.last_raw_data: PayloadLh2RawData = None
         if all(self.calibration_points_available) is False:
             self.state = LighthouseManagerState.CalibrationInProgress
         if all(self.calibration_points_available) is True:
@@ -241,11 +248,16 @@ def compute_calibration(self):  # pylint: disable=too-many-locals
         final_points = scales_matrix * pts_cam_new.T
         final_points = final_points.T
 
+        temporary_numpy_trash_heap = (
+            np.array([self.reference_points], dtype=np.float64) + 0.5
+        )
+        temporary_numpy_trash_heap_pt2 = temporary_numpy_trash_heap.squeeze()
+
         M, _ = cv2.findHomography(
-            final_points.dot(random_rodriguez.T)[:, 0:2],
-            np.array([self.reference_points], dtype=np.float64) + 0.5,
-            cv2.RANSAC,
-            5.0,
+            camera_points_arr[0],
+            temporary_numpy_trash_heap_pt2,
+            method=cv2.RANSAC,
+            ransacReprojThreshold=0.001,
         )
 
         self.calibration_data = CalibrationData(zeta, random_rodriguez, n, M)
@@ -280,18 +292,10 @@ def compute_position(
         )
 
         pts_cam_new = np.hstack((camera_points, np.ones((len(camera_points), 1))))
-        scales = (1 / self.calibration_data.zeta) / np.matmul(
-            self.calibration_data.normal, pts_cam_new.T
-        )
-        scales_matrix = np.vstack((scales, scales, scales))
-        final_points = scales_matrix * pts_cam_new.T
-        final_points = final_points.T
-        corners_planar = final_points.dot(self.calibration_data.random_rodriguez.T)[
-            :, 0:2
-        ][1].reshape(1, 1, 2)
-        pts_meter_corners = cv2.perspectiveTransform(
-            corners_planar, self.calibration_data.m
-        ).reshape(-1, 2)
+        reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new[0].T)
+
         return DotBotLH2Position(
-            x=pts_meter_corners[0][0], y=1 - pts_meter_corners[0][1], z=0.0
+            x=reprojected_points[0] / reprojected_points[2],
+            y=1 - reprojected_points[1] / reprojected_points[2],
+            z=0.0,
         )

dotbot/models.py

@@ -182,3 +182,4 @@ class DotBotModel(BaseModel):
     waypoints: List[Union[DotBotLH2Position, DotBotGPSPosition]] = []
     waypoints_threshold: int = 40
     position_history: List[Union[DotBotLH2Position, DotBotGPSPosition]] = []
+    calibrated: bool = False

dotbot/protocol.py

@@ -13,7 +13,6 @@
 from binascii import hexlify
 from dataclasses import dataclass
 from enum import IntEnum
-from typing import List
 
 PROTOCOL_VERSION = 1
 PAYLOAD_RESERVED_THRESHOLD = 0x80
@@ -24,7 +23,7 @@ class PayloadType(IntEnum):
 
     CMD_MOVE_RAW = 0x00
     CMD_RGB_LED = 0x01
-    LH2_RAW_LOCATION = 0x02
+    LH2_RAW_DATA = 0x02
     LH2_LOCATION = 0x03
     ADVERTISEMENT = 0x04
     GPS_POSITION = 0x05
@@ -35,7 +34,7 @@ class PayloadType(IntEnum):
     SAILBOT_DATA = 0x0A
     CMD_XGO_ACTION = 0x0B
     LH2_PROCESSED_DATA = 0x0C
-    LH2_RAW_DATA = 0x0D
+    LH2_CALIBRATION_HOMOGRAPHY = 0x0E
     RAW_DATA = 0x10
     DOTBOT_SIMULATOR_DATA = 0xFA
 
@@ -184,10 +183,12 @@ class PayloadAdvertisement(Payload):
     metadata: list[PayloadFieldMetadata] = dataclasses.field(
         default_factory=lambda: [
             PayloadFieldMetadata(name="application", disp="app"),
+            PayloadFieldMetadata(name="calibrated", disp="cal."),
         ]
     )
 
     application: ApplicationType = ApplicationType.DotBot
+    calibrated: bool = False
 
 
 @dataclass
@@ -306,23 +307,40 @@ class PayloadLH2Location(Payload):
     pos_z: int = 0
 
 
+@dataclass
+class PayloadLh2CalibrationHomography(Payload):
+    """Dataclass that holds computed LH2 homography for a basestation indicated by index."""
+
+    metadata: list[PayloadFieldMetadata] = dataclasses.field(
+        default_factory=lambda: [
+            PayloadFieldMetadata(name="index", disp="idx"),
+            PayloadFieldMetadata(
+                name="homography_matrix", disp="mat.", type_=bytes, length=36
+            ),
+        ]
+    )
+
+    index: int = 0
+    homography_matrix: bytes = dataclasses.field(default_factory=lambda: bytearray)
+
+
 @dataclass
 class PayloadDotBotData(Payload):
     """Dataclass that holds direction and LH2 raw data from DotBot application."""
 
     metadata: list[PayloadFieldMetadata] = dataclasses.field(
         default_factory=lambda: [
             PayloadFieldMetadata(name="direction", disp="dir.", length=2, signed=True),
-            PayloadFieldMetadata(name="count", disp="len"),
-            PayloadFieldMetadata(name="locations", type_=list, length=0),
+            PayloadFieldMetadata(name="pos_x", disp="x", length=4),
+            PayloadFieldMetadata(name="pos_y", disp="y", length=4),
+            PayloadFieldMetadata(name="pos_z", disp="z", length=4),
         ]
     )
 
     direction: int = 0xFFFF
-    count: int = 0
-    locations: List[PayloadLh2RawLocation] = dataclasses.field(
-        default_factory=lambda: []
-    )
+    pos_x: int = 0
+    pos_y: int = 0
+    pos_z: int = 0
 
 
 @dataclass
@@ -447,7 +465,6 @@ class PayloadRawData(Payload):
     PayloadType.CMD_MOVE_RAW: PayloadCommandMoveRaw,
     PayloadType.CMD_RGB_LED: PayloadCommandRgbLed,
     PayloadType.CMD_XGO_ACTION: PayloadCommandXgoAction,
-    PayloadType.LH2_RAW_LOCATION: PayloadLh2RawLocation,
     PayloadType.LH2_PROCESSED_DATA: PayloadLh2ProcessedLocation,
     PayloadType.LH2_RAW_DATA: PayloadLh2RawData,
     PayloadType.LH2_LOCATION: PayloadLH2Location,
@@ -459,6 +476,7 @@ class PayloadRawData(Payload):
     PayloadType.LH2_WAYPOINTS: PayloadLH2Waypoints,
     PayloadType.GPS_WAYPOINTS: PayloadGPSWaypoints,
     PayloadType.RAW_DATA: PayloadRawData,
+    PayloadType.LH2_CALIBRATION_HOMOGRAPHY: PayloadLh2CalibrationHomography,
 }