Merge pull request #169 from aabadie/lightouse_export_calibration_in_header

Add lighthouse calibration tool

Author: aabadie

dotbot/calibration_exporter.py

@@ -0,0 +1,76 @@
+import logging
+import os
+import sys
+from pathlib import Path
+
+import click
+import structlog
+
+from dotbot.lighthouse2 import LighthouseManager, LighthouseManagerState
+from dotbot.protocol import PayloadLh2CalibrationHomography
+
+CALIBRATION_HEADER_FILENAME = Path("lh2_calibration.h")
+CALIBRATION_HEADER_HEADER = """// Auto-generated file, do not edit!
+#ifndef __LH2_CALIBRATION_H
+#define __LH2_CALIBRATION_H
+
+#include "localization.h"
+
+#define LH2_CALIBRATION_IS_VALID    (1)
+
+static int32_t swrmt_homography[3][3] = {
+"""
+
+CALIBRATION_HEADER_FOOTER = """};
+
+#endif // __LH2_CALIBRATION_H
+"""
+
+
+def export_calibration(calibration: PayloadLh2CalibrationHomography) -> str:
+    """Export the calibration file to a user-defined location."""
+    # Store homography matrix as C header to use in SwarmIT bootloader
+    output = CALIBRATION_HEADER_HEADER
+    matrix_int = [
+        int.from_bytes(calibration.homography_matrix[i : i + 4], "little", signed=True)
+        for i in range(0, 36, 4)
+    ]
+    matrix = [matrix_int[i : i + 3] for i in range(0, 9, 3)]
+    for row in matrix:
+        output += "    {" + ", ".join(str(v) for v in row) + "},\n"
+    output += CALIBRATION_HEADER_FOOTER
+    return output
+
+
+@click.command()
+@click.argument("output_path", nargs=1)
+def main(output_path):
+    """Export DotBot calibration data to a file."""
+    # Disable logging
+    structlog.configure(
+        wrapper_class=structlog.make_filtering_bound_logger(logging.CRITICAL),
+    )
+
+    if not os.path.exists(output_path):
+        print(f"Error: '{output_path}' doesn't exist.", file=sys.stderr)
+        sys.exit(1)
+
+    lh2_manager = LighthouseManager()
+    if lh2_manager.state != LighthouseManagerState.Calibrated:
+        print("Error: LighthouseManager is not calibrated.", file=sys.stderr)
+        sys.exit(1)
+
+    try:
+        output = export_calibration(lh2_manager.calibration)
+        header_path = Path(output_path) / CALIBRATION_HEADER_FILENAME
+        with open(header_path, "w") as header_file:
+            header_file.write(output)
+        print(output)
+        print(f"Calibration data exported to '{header_path}'")
+    except Exception as e:
+        print(f"Error exporting calibration data: {e}", file=sys.stderr)
+        sys.exit(1)
+
+
+if __name__ == "__main__":
+    main()

dotbot/controller.py

@@ -14,7 +14,7 @@
 import webbrowser
 from binascii import hexlify
 from dataclasses import dataclass
-from typing import Dict, List, Optional
+from typing import Dict, List
 
 import serial
 import uvicorn
@@ -72,7 +72,6 @@
     PayloadCommandXgoAction,
     PayloadGPSPosition,
     PayloadGPSWaypoints,
-    PayloadLh2CalibrationHomography,
     PayloadLH2Location,
     PayloadLH2Waypoints,
     PayloadType,
@@ -493,17 +492,6 @@ async def _dotbots_status_refresh(self):
                 )
             await asyncio.sleep(1)
 
-    def _compute_lh2_position(self, frame: Frame) -> Optional[DotBotLH2Position]:
-        if frame.packet.payload_type not in (
-            PayloadType.LH2_RAW_DATA,
-            PayloadType.DOTBOT_DATA,
-        ):
-            return None
-        self.lh2_manager.last_raw_data = frame.packet.payload
-        if self.lh2_manager.state != LighthouseManagerState.Calibrated:
-            return None
-        return self.lh2_manager.compute_position(frame.packet.payload)
-
     def handle_received_frame(
         self, frame: Frame
     ):  # pylint:disable=too-many-branches,too-many-statements
@@ -569,21 +557,11 @@ def handle_received_frame(
                 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=self.lh2_manager.calibration
                 )
-                self.logger.info("Send calibration data", payload=payload)
                 self.dotbots.update({dotbot.address: dotbot})
-                self.send_payload(int(source, 16), payload=payload)
+                self.send_payload(int(source, 16), payload=self.lh2_manager.calibration)
 
         if (
             frame.packet.payload_type

dotbot/lighthouse2.py

@@ -22,8 +22,8 @@
 import numpy as np
 
 from dotbot.logger import LOGGER
-from dotbot.models import DotBotCalibrationStateModel, DotBotLH2Position
-from dotbot.protocol import PayloadLh2RawData
+from dotbot.models import DotBotCalibrationStateModel
+from dotbot.protocol import PayloadLh2CalibrationHomography, PayloadLh2RawData
 
 if sys.platform == "win32":
     LIB_EXT = "dll"
@@ -121,7 +121,7 @@ def __init__(self):
         self.reference_points = REFERENCE_POINTS_DEFAULT
         Path.mkdir(CALIBRATION_DIR, exist_ok=True)
         self.calibration_output_path = CALIBRATION_DIR / "calibration.out"
-        self.calibration_data = self._load_calibration()
+        self.calibration: PayloadLh2CalibrationHomography = self._load_calibration()
         self.calibration_points = np.zeros(
             (2, len(self.reference_points), 2), dtype=np.float64
         )
@@ -140,18 +140,14 @@ def state_model(self) -> DotBotCalibrationStateModel:
             return DotBotCalibrationStateModel(state="done")
         return DotBotCalibrationStateModel(state="unknown")
 
-    def _load_calibration(self) -> Optional[CalibrationData]:
+    def _load_calibration(self) -> Optional[PayloadLh2CalibrationHomography]:
         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):
@@ -260,42 +256,23 @@ def compute_calibration(self):  # pylint: disable=too-many-locals
             ransacReprojThreshold=0.001,
         )
 
-        self.calibration_data = CalibrationData(zeta, random_rodriguez, n, M)
+        calibration_data = CalibrationData(zeta, random_rodriguez, n, M)
+        matrix_bytes = bytearray()
+        for bytes_block in [
+            int(n * 1e6).to_bytes(4, "little", signed=True)
+            for n in calibration_data.m.ravel()
+        ]:
+            matrix_bytes += bytes_block
+
+        # Prepare homography matrix and send it to the robot
+        self.calibration = PayloadLh2CalibrationHomography(
+            index=0,
+            homography_matrix=matrix_bytes,
+        )
 
+        # Store calibration data as pickle for later reload
         with open(self.calibration_output_path, "wb") as output_file:
-            pickle.dump(self.calibration_data, output_file)
+            pickle.dump(self.calibration, output_file)
 
         self.state = LighthouseManagerState.Calibrated
-        self.logger.info("Calibration done", data=self.calibration_data)
-
-    def compute_position(
-        self, raw_data: PayloadLh2RawData
-    ) -> Optional[DotBotLH2Position]:
-        """Compute the position coordinates from LH2 raw data and available calibration."""
-        if self.state != LighthouseManagerState.Calibrated:
-            return None
-
-        if any(raw_data.locations[index].bits == 0 for index in range(2)):
-            return None
-
-        counts = lh2_raw_data_to_counts(raw_data)
-        camera_points = np.asarray(
-            [
-                calculate_camera_point(
-                    counts[0], counts[1], raw_data.locations[0].polynomial_index
-                ),
-                calculate_camera_point(
-                    counts[0], counts[1], raw_data.locations[1].polynomial_index
-                ),
-            ],
-            dtype=np.float64,
-        )
-
-        pts_cam_new = np.hstack((camera_points, np.ones((len(camera_points), 1))))
-        reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new[0].T)
-
-        return DotBotLH2Position(
-            x=reprojected_points[0] / reprojected_points[2],
-            y=1 - reprojected_points[1] / reprojected_points[2],
-            z=0.0,
-        )
+        self.logger.info("Calibration done", data=self.calibration)

pyproject.toml

@@ -76,6 +76,7 @@ dotbot-controller = "dotbot.controller_app:main"
 dotbot-edge-gateway = "dotbot.edge_gateway_app:main"
 dotbot-keyboard = "dotbot.keyboard:main"
 dotbot-joystick = "dotbot.joystick:main"
+dotbot-calibration-exporter = "dotbot.calibration_exporter:main"
 
 [tool.ruff]
 lint.select = ["E", "F"]

setup.cfg

@@ -8,7 +8,7 @@ addopts = -vv -s
           --cov-report=term-missing
           --cov-report=xml
 testpaths = dotbot
-asyncio_default_fixture_loop_scope = "function"
+asyncio_default_fixture_loop_scope = function
 
 [tool.black]
 line-length = 79

tox.ini

@@ -49,7 +49,8 @@ allowlist_externals=
     /bin/bash
     /usr/bin/bash
 commands=
-    bash -exc "dotbot-controller --help > /dev/null"
+    bash -exc "dotbot-controller --help"
+    bash -exc "dotbot-calibration-exporter --help"
 
 [testenv:web]
 allowlist_externals=