From 117e2a95a219ebdb1f6a2ca3fd43e01dc3048bb6 Mon Sep 17 00:00:00 2001 From: filmak Date: Wed, 3 Sep 2025 13:06:01 +0200 Subject: [PATCH 01/21] lh2 add new packet type for homography matrix transfer --- dotbot/protocol.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 6668188f..c736ceea 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -37,6 +37,7 @@ class PayloadType(IntEnum): LH2_PROCESSED_DATA = 0x0C LH2_RAW_DATA = 0x0D RAW_DATA = 0x10 + LH2_CALIBRATION_HOMOGRAPHY = 0x11 DOTBOT_SIMULATOR_DATA = 0xFA @@ -305,6 +306,32 @@ class PayloadLH2Location(Payload): pos_y: int = 0 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=4*9), + ] + ) + + index: int = 0xFFFF + homography_matrix: list[PayloadFieldMetadata] = dataclasses.field( + default_factory=lambda: [ + PayloadFieldMetadata(name="h11", disp="h11", length = 4), + PayloadFieldMetadata(name="h12", disp="h12", length = 4), + PayloadFieldMetadata(name="h13", disp="h13", length = 4), + PayloadFieldMetadata(name="h21", disp="h21", length = 4), + PayloadFieldMetadata(name="h22", disp="h22", length = 4), + PayloadFieldMetadata(name="h23", disp="h23", length = 4), + PayloadFieldMetadata(name="h31", disp="h31", length = 4), + PayloadFieldMetadata(name="h32", disp="h32", length = 4), + PayloadFieldMetadata(name="h33", disp="h33", length = 4) + ] + ) + @dataclass class PayloadDotBotData(Payload): From 7d7932d271543972a1c03af910d30445281381d8 Mon Sep 17 00:00:00 2001 From: filmak Date: Wed, 3 Sep 2025 15:39:34 +0200 Subject: [PATCH 02/21] lh2 transmit homography to gateway after calibration is complete --- dotbot/controller.py | 16 ++++++++++++++++ dotbot/protocol.py | 18 +++--------------- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 6d3dcc1a..a2f9d000 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -19,6 +19,7 @@ import serial import uvicorn import websockets +import numpy as np from fastapi import WebSocket from haversine import Unit, haversine from pydantic import ValidationError @@ -43,6 +44,7 @@ SerialAdapter, ) from dotbot.lighthouse2 import LighthouseManager, LighthouseManagerState +from dotbot.lighthouse2 import CalibrationData from dotbot.logger import LOGGER from dotbot.models import ( MAX_POSITION_HISTORY_SIZE, @@ -75,6 +77,7 @@ PayloadLH2Location, PayloadLH2Waypoints, PayloadType, + PayloadLh2CalibrationHomography, ) from dotbot.serial_interface import SerialInterfaceException from dotbot.server import api @@ -164,9 +167,12 @@ def __init__(self, settings: ControllerSettings): self.adapter: GatewayAdapterBase = None self.websockets = [] self.lh2_manager = LighthouseManager() + self.lh2_calibration_data = CalibrationData(zeta=0.0, random_rodriguez=np.eye(3), normal = np.zeros(3), m = np.eye(3)) + self.api = api api.controller = self self.qrkey = None + self.subscriptions = [ SubscriptionModel( topic="/command/+/+/+/move_raw", callback=self.on_command_move_raw @@ -399,6 +405,16 @@ def on_lh2_start(self, topic, _): logger.info("Start calibration") self.lh2_manager.compute_calibration() + index_bytes = (0).to_bytes(4, "little", signed = False) + matrix_bytes = [int(n).to_bytes(4, "little", signed=True) for n in self.lh2_calibration_data.m.ravel()] + + logger.info("finish calibration - send to gateway") + payload = PayloadLh2CalibrationHomography( + index = index_bytes, + homography_matrix = matrix_bytes, + ) + + def on_request(self, payload): logger = LOGGER.bind(topic="/request") logger.info("Request received", **payload) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index c736ceea..77a82265 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -11,7 +11,7 @@ import typing from abc import ABC from binascii import hexlify -from dataclasses import dataclass +from dataclasses import dataclass, field from enum import IntEnum from typing import List @@ -317,20 +317,8 @@ class PayloadLh2CalibrationHomography(Payload): ] ) - index: int = 0xFFFF - homography_matrix: list[PayloadFieldMetadata] = dataclasses.field( - default_factory=lambda: [ - PayloadFieldMetadata(name="h11", disp="h11", length = 4), - PayloadFieldMetadata(name="h12", disp="h12", length = 4), - PayloadFieldMetadata(name="h13", disp="h13", length = 4), - PayloadFieldMetadata(name="h21", disp="h21", length = 4), - PayloadFieldMetadata(name="h22", disp="h22", length = 4), - PayloadFieldMetadata(name="h23", disp="h23", length = 4), - PayloadFieldMetadata(name="h31", disp="h31", length = 4), - PayloadFieldMetadata(name="h32", disp="h32", length = 4), - PayloadFieldMetadata(name="h33", disp="h33", length = 4) - ] - ) + index: bytes = field(default_factory = lambda: (0).to_bytes(4, "little", signed = False)) + homography_matrix: list[bytes] = field(default_factory=lambda: [ (0).to_bytes(4, "little", signed=True) for _ in range(9) ] ) @dataclass From 6c6f7be09d8c89b611953e199383e5504ecadce2 Mon Sep 17 00:00:00 2001 From: filmak Date: Thu, 4 Sep 2025 13:17:53 +0200 Subject: [PATCH 03/21] lh2 add debug print to verify homography matrix --- dotbot/controller.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index a2f9d000..54328744 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -406,7 +406,7 @@ def on_lh2_start(self, topic, _): self.lh2_manager.compute_calibration() index_bytes = (0).to_bytes(4, "little", signed = False) - matrix_bytes = [int(n).to_bytes(4, "little", signed=True) for n in self.lh2_calibration_data.m.ravel()] + matrix_bytes = [int(n*1e6).to_bytes(4, "little", signed=True) for n in self.lh2_calibration_data.m.ravel()] logger.info("finish calibration - send to gateway") payload = PayloadLh2CalibrationHomography( @@ -565,6 +565,15 @@ def handle_received_frame( # reload if a new dotbot comes in logger.info("New dotbot") notification_cmd = DotBotNotificationCommand.RELOAD + payload = PayloadCommandRgbLed(red=0, green=255, blue=0) + self.send_payload(int(source, 16), payload=payload) + + + + + + + if frame.packet.payload_type == PayloadType.ADVERTISEMENT: logger = logger.bind( From c3d2577a11d55b457f6a47b39b425202b8b5d656 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 4 Sep 2025 16:32:52 +0200 Subject: [PATCH 04/21] lh2: cleanup initial work --- dotbot/controller.py | 30 +++++++++++++----------------- dotbot/lighthouse2.py | 8 +++++++- dotbot/protocol.py | 7 ++++--- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 54328744..dca4026d 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -167,7 +167,6 @@ def __init__(self, settings: ControllerSettings): self.adapter: GatewayAdapterBase = None self.websockets = [] self.lh2_manager = LighthouseManager() - self.lh2_calibration_data = CalibrationData(zeta=0.0, random_rodriguez=np.eye(3), normal = np.zeros(3), m = np.eye(3)) self.api = api api.controller = self @@ -404,16 +403,7 @@ def on_lh2_start(self, topic, _): return logger.info("Start calibration") self.lh2_manager.compute_calibration() - - index_bytes = (0).to_bytes(4, "little", signed = False) - matrix_bytes = [int(n*1e6).to_bytes(4, "little", signed=True) for n in self.lh2_calibration_data.m.ravel()] - - logger.info("finish calibration - send to gateway") - payload = PayloadLh2CalibrationHomography( - index = index_bytes, - homography_matrix = matrix_bytes, - ) - + logger.info("Calibration complete") def on_request(self, payload): logger = LOGGER.bind(topic="/request") @@ -568,12 +558,18 @@ def handle_received_frame( payload = PayloadCommandRgbLed(red=0, green=255, blue=0) self.send_payload(int(source, 16), payload=payload) - - - - - - + # Send calibration to new dotbot if the localization system is calibrated + if frame.packet.payload_type == PayloadType.ADVERTISEMENT and self.lh2_manager.state == LighthouseManagerState.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.send_payload(int(source, 16), payload=payload) if frame.packet.payload_type == PayloadType.ADVERTISEMENT: logger = logger.bind( diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index ee09561c..f9f9dd19 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -116,17 +116,18 @@ 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) self.calibration_output_path = CALIBRATION_DIR / "calibration.out" self.calibration_data = self._load_calibration() + print(self.calibration_data.m.dtype) self.calibration_points = np.zeros( (2, len(self.reference_points), 2), dtype=np.float64 ) 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,9 +143,14 @@ 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 diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 77a82265..9192ca8a 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -306,6 +306,7 @@ class PayloadLH2Location(Payload): pos_y: int = 0 pos_z: int = 0 + @dataclass class PayloadLh2CalibrationHomography(Payload): """Dataclass that holds computed LH2 homography for a basestation indicated by index.""" @@ -313,12 +314,12 @@ class PayloadLh2CalibrationHomography(Payload): metadata: list[PayloadFieldMetadata] = dataclasses.field( default_factory=lambda: [ PayloadFieldMetadata(name="index", disp="idx"), - PayloadFieldMetadata(name="homography_matrix", disp="mat.", type_=bytes, length=4*9), + PayloadFieldMetadata(name="homography_matrix", disp="mat.", type_=bytes, length=36), ] ) - index: bytes = field(default_factory = lambda: (0).to_bytes(4, "little", signed = False)) - homography_matrix: list[bytes] = field(default_factory=lambda: [ (0).to_bytes(4, "little", signed=True) for _ in range(9) ] ) + index: int = 0 + homography_matrix: bytes = field(default_factory=lambda: bytearray) @dataclass From eedfd6813c8344c497f6f481918628edde61fa7f Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 4 Sep 2025 16:36:49 +0200 Subject: [PATCH 05/21] dotbot/protocol: register new lh2 calibration packet to supported parsers --- dotbot/protocol.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 9192ca8a..1fa04e5d 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -475,6 +475,7 @@ class PayloadRawData(Payload): PayloadType.LH2_WAYPOINTS: PayloadLH2Waypoints, PayloadType.GPS_WAYPOINTS: PayloadGPSWaypoints, PayloadType.RAW_DATA: PayloadRawData, + PayloadType.LH2_CALIBRATION_HOMOGRAPHY: PayloadLh2CalibrationHomography, } From 64931456ce9aa9cbecea158452d37d195adc9a32 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 4 Sep 2025 16:56:14 +0200 Subject: [PATCH 06/21] controller: fix homography calibration not being sent --- dotbot/controller.py | 2 ++ dotbot/protocol.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index dca4026d..6dec313e 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -560,6 +560,7 @@ def handle_received_frame( # Send calibration to new dotbot if the localization system is calibrated if frame.packet.payload_type == PayloadType.ADVERTISEMENT and self.lh2_manager.state == LighthouseManagerState.Calibrated: + self.dotbots.update({dotbot.address: dotbot}) # 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()]: @@ -569,6 +570,7 @@ def handle_received_frame( index=0, homography_matrix=matrix_bytes, ) + self.logger.info("Send calibration data", source=source, payload=payload) self.send_payload(int(source, 16), payload=payload) if frame.packet.payload_type == PayloadType.ADVERTISEMENT: diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 1fa04e5d..7fb3d689 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -36,8 +36,8 @@ class PayloadType(IntEnum): CMD_XGO_ACTION = 0x0B LH2_PROCESSED_DATA = 0x0C LH2_RAW_DATA = 0x0D + LH2_CALIBRATION_HOMOGRAPHY = 0x0E RAW_DATA = 0x10 - LH2_CALIBRATION_HOMOGRAPHY = 0x11 DOTBOT_SIMULATOR_DATA = 0xFA From 4dcad2ffc0f9f6ae1d56be211cfd563aedfc5b8f Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 9 Sep 2025 07:14:56 +0200 Subject: [PATCH 07/21] dotbot/lighthouse2: comment out debug message --- dotbot/lighthouse2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index f9f9dd19..90bc464b 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -122,7 +122,7 @@ def __init__(self): Path.mkdir(CALIBRATION_DIR, exist_ok=True) self.calibration_output_path = CALIBRATION_DIR / "calibration.out" self.calibration_data = self._load_calibration() - print(self.calibration_data.m.dtype) + # print(self.calibration_data.m.dtype) self.calibration_points = np.zeros( (2, len(self.reference_points), 2), dtype=np.float64 ) From 90cbd8c21bc8096502596e567d7366b4104451fb Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 9 Sep 2025 09:07:47 +0200 Subject: [PATCH 08/21] dotbot/controller: fix style --- dotbot/controller.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 6dec313e..1127f3da 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -19,7 +19,6 @@ import serial import uvicorn import websockets -import numpy as np from fastapi import WebSocket from haversine import Unit, haversine from pydantic import ValidationError @@ -44,7 +43,6 @@ SerialAdapter, ) from dotbot.lighthouse2 import LighthouseManager, LighthouseManagerState -from dotbot.lighthouse2 import CalibrationData from dotbot.logger import LOGGER from dotbot.models import ( MAX_POSITION_HISTORY_SIZE, @@ -74,10 +72,10 @@ PayloadCommandXgoAction, PayloadGPSPosition, PayloadGPSWaypoints, + PayloadLh2CalibrationHomography, PayloadLH2Location, PayloadLH2Waypoints, PayloadType, - PayloadLh2CalibrationHomography, ) from dotbot.serial_interface import SerialInterfaceException from dotbot.server import api @@ -559,18 +557,26 @@ def handle_received_frame( self.send_payload(int(source, 16), payload=payload) # Send calibration to new dotbot if the localization system is calibrated - if frame.packet.payload_type == PayloadType.ADVERTISEMENT and self.lh2_manager.state == LighthouseManagerState.Calibrated: + if ( + frame.packet.payload_type == PayloadType.ADVERTISEMENT + and self.lh2_manager.state == LighthouseManagerState.Calibrated + ): self.dotbots.update({dotbot.address: dotbot}) # 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()]: + 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, + index=0, homography_matrix=matrix_bytes, ) - self.logger.info("Send calibration data", source=source, payload=payload) + self.logger.info( + "Send calibration data", source=source, payload=payload + ) self.send_payload(int(source, 16), payload=payload) if frame.packet.payload_type == PayloadType.ADVERTISEMENT: From 994b8d5a774329598fffd5fb4e48422ed0ec0878 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 9 Sep 2025 09:27:42 +0200 Subject: [PATCH 09/21] dotbot/protocol: fix style --- dotbot/protocol.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 7fb3d689..382f2477 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -314,7 +314,9 @@ class PayloadLh2CalibrationHomography(Payload): metadata: list[PayloadFieldMetadata] = dataclasses.field( default_factory=lambda: [ PayloadFieldMetadata(name="index", disp="idx"), - PayloadFieldMetadata(name="homography_matrix", disp="mat.", type_=bytes, length=36), + PayloadFieldMetadata( + name="homography_matrix", disp="mat.", type_=bytes, length=36 + ), ] ) From bcac2b84c7eb83a160184183fd79887e5df7d4d1 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Tue, 9 Sep 2025 09:32:19 +0200 Subject: [PATCH 10/21] dotbot/protocol: fix ruff static check --- dotbot/protocol.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 382f2477..00da0e96 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -11,7 +11,7 @@ import typing from abc import ABC from binascii import hexlify -from dataclasses import dataclass, field +from dataclasses import dataclass from enum import IntEnum from typing import List @@ -321,7 +321,7 @@ class PayloadLh2CalibrationHomography(Payload): ) index: int = 0 - homography_matrix: bytes = field(default_factory=lambda: bytearray) + homography_matrix: bytes = dataclasses.field(default_factory=lambda: bytearray) @dataclass From 52061ec16f9ffd063282a05b9fae6e03d66876b4 Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 13:21:41 +0200 Subject: [PATCH 11/21] lh2 revise homography to avoid rodriguez transformation --- dotbot/lighthouse2.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index 90bc464b..5bff331a 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -122,7 +122,6 @@ def __init__(self): Path.mkdir(CALIBRATION_DIR, exist_ok=True) self.calibration_output_path = CALIBRATION_DIR / "calibration.out" self.calibration_data = self._load_calibration() - # print(self.calibration_data.m.dtype) self.calibration_points = np.zeros( (2, len(self.reference_points), 2), dtype=np.float64 ) @@ -152,6 +151,9 @@ def _load_calibration(self) -> Optional[CalibrationData]: calibration.m = calibration.m.astype(np.float32) self.logger.info("Lighthouse calibration loaded") self.state = LighthouseManagerState.Calibrated + + print(calibration.m) + return calibration def add_calibration_point(self, index): @@ -184,6 +186,7 @@ def add_calibration_point(self, index): self.state = LighthouseManagerState.CalibrationInProgress if all(self.calibration_points_available) is True: self.state = LighthouseManagerState.Ready + print(self.calibration_points) self.logger.info("Calibration point added", index=index, state=self.state) def compute_calibration(self): # pylint: disable=too-many-locals @@ -254,6 +257,13 @@ def compute_calibration(self): # pylint: disable=too-many-locals 5.0, ) + M,_ = cv2.findHomography( + camera_points, + np.array([self.reference_points], dtype=np.float64) + 0.5, + method = cv2.RANSAC, + ransacReprojThreshold=0.001 + ) + self.calibration_data = CalibrationData(zeta, random_rodriguez, n, M) with open(self.calibration_output_path, "wb") as output_file: @@ -286,6 +296,13 @@ def compute_position( ) pts_cam_new = np.hstack((camera_points, np.ones((len(camera_points), 1)))) + + reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new) + return DotBotLH2Position( + x = reprojected_points[0][0], y = 1 - reprojected_points[0][1], z=0.0 + ) + + scales = (1 / self.calibration_data.zeta) / np.matmul( self.calibration_data.normal, pts_cam_new.T ) @@ -295,9 +312,14 @@ def compute_position( 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) + + + return DotBotLH2Position( x=pts_meter_corners[0][0], y=1 - pts_meter_corners[0][1], z=0.0 ) From e55ccb182c211f68d46b4af2246115556ad54838 Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 14:32:39 +0200 Subject: [PATCH 12/21] bugfix: negotiate with numpy's demands, and remove debug prints --- dotbot/lighthouse2.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index 5bff331a..a8dcb74f 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -250,16 +250,18 @@ def compute_calibration(self): # pylint: disable=too-many-locals final_points = scales_matrix * pts_cam_new.T final_points = final_points.T - 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, - ) + #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, + #) + 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( - camera_points, - np.array([self.reference_points], dtype=np.float64) + 0.5, + camera_points_arr[0], + temporary_numpy_trash_heap_pt2, method = cv2.RANSAC, ransacReprojThreshold=0.001 ) @@ -297,9 +299,9 @@ def compute_position( pts_cam_new = np.hstack((camera_points, np.ones((len(camera_points), 1)))) - reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new) + reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new[0].T) return DotBotLH2Position( - x = reprojected_points[0][0], y = 1 - reprojected_points[0][1], z=0.0 + x = reprojected_points[0], y = reprojected_points[1], z=0.0 ) From 56cb795894b3d0ae57722d1b94dcdd2443dcfb7e Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 14:51:06 +0200 Subject: [PATCH 13/21] lh2 add correct scaling to correct homography reprojection --- dotbot/lighthouse2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index a8dcb74f..1e74ff23 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -301,7 +301,7 @@ def compute_position( reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new[0].T) return DotBotLH2Position( - x = reprojected_points[0], y = reprojected_points[1], z=0.0 + x = reprojected_points[0]/reprojected_points[2], y = reprojected_points[1]/reprojected_points[2], z=0.0 ) From 2de5b4441872e29585025108e5d8367a6a5c5a54 Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 14:57:18 +0200 Subject: [PATCH 14/21] lh2 bugfix: invert y-axis so that visualization is correct --- dotbot/lighthouse2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index 1e74ff23..ef22e300 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -301,7 +301,7 @@ def compute_position( reprojected_points = np.matmul(self.calibration_data.m, pts_cam_new[0].T) return DotBotLH2Position( - x = reprojected_points[0]/reprojected_points[2], y = reprojected_points[1]/reprojected_points[2], z=0.0 + x = reprojected_points[0]/reprojected_points[2], y = 1 - reprojected_points[1]/reprojected_points[2], z=0.0 ) From 175c4cb48d8691e2f2edb453c5297684a79ea67c Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 18:31:54 +0200 Subject: [PATCH 15/21] lh2 DOES NOT WORK: add code to handle reception of x,y pos from dotbot --- dotbot/adapter.py | 3 ++- dotbot/controller.py | 18 ++++++++++++++++++ dotbot/lighthouse2.py | 5 +---- 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/dotbot/adapter.py b/dotbot/adapter.py index 2b5cbf97..95345671 100644 --- a/dotbot/adapter.py +++ b/dotbot/adapter.py @@ -50,9 +50,10 @@ def __init__(self, port: str, baudrate: int): def on_byte_received(self, byte: bytes): self.hdlc_handler.handle_byte(byte) - if self.hdlc_handler.state == HDLCState.READY: + if self.hdlc_handler.state == HDLCState.READY: try: data = self.hdlc_handler.payload + #print(data, len(data)) try: frame = Frame.from_bytes(data) except (ValueError, ProtocolPayloadParserException) as exc: diff --git a/dotbot/controller.py b/dotbot/controller.py index 1127f3da..c6e13872 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -509,12 +509,18 @@ def handle_received_frame( ): # pylint:disable=too-many-branches,too-many-statements """Handle a received frame.""" # Controller is not interested by command messages received + + print("payload type: ", frame.packet.payload_type) + if frame.packet.payload_type in [ PayloadType.CMD_MOVE_RAW, PayloadType.CMD_RGB_LED, ]: return source = hexlify(int(frame.header.source).to_bytes(8, "big")).decode() + + print("source: ", source) + logger = self.logger.bind( source=source, payload_type=PayloadType(frame.packet.payload_type).name, @@ -600,6 +606,18 @@ def handle_received_frame( sail_angle=dotbot.sail_angle, ) + if frame.packet.payload_type in [PayloadType.LH2_LOCATION]: + new_position = DotBotLH2Position( + x=frame.packet.payload.pos_x / 1e6, + y=frame.packet.payload.pos_y / 1e6, + z=0, + ) + 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) + dotbot.lh2_position = self._compute_lh2_position(frame) if ( dotbot.lh2_position is not None diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index ef22e300..b7db6265 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -152,8 +152,6 @@ def _load_calibration(self) -> Optional[CalibrationData]: self.logger.info("Lighthouse calibration loaded") self.state = LighthouseManagerState.Calibrated - print(calibration.m) - return calibration def add_calibration_point(self, index): @@ -186,7 +184,6 @@ def add_calibration_point(self, index): self.state = LighthouseManagerState.CalibrationInProgress if all(self.calibration_points_available) is True: self.state = LighthouseManagerState.Ready - print(self.calibration_points) self.logger.info("Calibration point added", index=index, state=self.state) def compute_calibration(self): # pylint: disable=too-many-locals @@ -298,8 +295,8 @@ def compute_position( ) 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 ) From 80e0a8e35009a736f2442ac93f42229162dc56f5 Mon Sep 17 00:00:00 2001 From: filmak Date: Tue, 9 Sep 2025 19:42:25 +0200 Subject: [PATCH 16/21] lh2 add debug printing on received location packet --- dotbot/adapter.py | 2 +- dotbot/controller.py | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/dotbot/adapter.py b/dotbot/adapter.py index 95345671..e7489baf 100644 --- a/dotbot/adapter.py +++ b/dotbot/adapter.py @@ -53,9 +53,9 @@ def on_byte_received(self, byte: bytes): if self.hdlc_handler.state == HDLCState.READY: try: data = self.hdlc_handler.payload - #print(data, len(data)) try: frame = Frame.from_bytes(data) + #print(frame) except (ValueError, ProtocolPayloadParserException) as exc: LOGGER.error(f"Error parsing frame: {exc}") return diff --git a/dotbot/controller.py b/dotbot/controller.py index c6e13872..52760143 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -607,14 +607,17 @@ def handle_received_frame( ) if frame.packet.payload_type in [PayloadType.LH2_LOCATION]: + print("x pos: ", frame.packet.payload.pos_x) + print("y pos: ", frame.packet.payload.pos_y) new_position = DotBotLH2Position( x=frame.packet.payload.pos_x / 1e6, y=frame.packet.payload.pos_y / 1e6, - z=0, + z=0.0, ) dotbot.lh2_position = new_position dotbot.position_history.append(new_position) notification_cmd = DotBotNotificationCommand.UPDATE + print(f"dotbot.position_history: {len(dotbot.position_history)}") if len(dotbot.position_history) > MAX_POSITION_HISTORY_SIZE: dotbot.position_history.pop(0) From 4ef2198a9d98f1f75512e042532da8c655efeaf8 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Wed, 10 Sep 2025 07:12:59 +0200 Subject: [PATCH 17/21] protocol: controller: cleanup handling of LH2 messages --- dotbot/adapter.py | 3 +-- dotbot/controller.py | 41 +++++++--------------------------- dotbot/lighthouse2.py | 42 ++++++++--------------------------- dotbot/protocol.py | 4 +--- dotbot/tests/test_protocol.py | 21 +++--------------- 5 files changed, 22 insertions(+), 89 deletions(-) diff --git a/dotbot/adapter.py b/dotbot/adapter.py index e7489baf..2b5cbf97 100644 --- a/dotbot/adapter.py +++ b/dotbot/adapter.py @@ -50,12 +50,11 @@ def __init__(self, port: str, baudrate: int): def on_byte_received(self, byte: bytes): self.hdlc_handler.handle_byte(byte) - if self.hdlc_handler.state == HDLCState.READY: + if self.hdlc_handler.state == HDLCState.READY: try: data = self.hdlc_handler.payload try: frame = Frame.from_bytes(data) - #print(frame) except (ValueError, ProtocolPayloadParserException) as exc: LOGGER.error(f"Error parsing frame: {exc}") return diff --git a/dotbot/controller.py b/dotbot/controller.py index 52760143..b945a7ab 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -509,9 +509,9 @@ def handle_received_frame( ): # pylint:disable=too-many-branches,too-many-statements """Handle a received frame.""" # Controller is not interested by command messages received - + print("payload type: ", frame.packet.payload_type) - + if frame.packet.payload_type in [ PayloadType.CMD_MOVE_RAW, PayloadType.CMD_RGB_LED, @@ -606,7 +606,7 @@ def handle_received_frame( sail_angle=dotbot.sail_angle, ) - if frame.packet.payload_type in [PayloadType.LH2_LOCATION]: + if frame.packet.payload_type == PayloadType.LH2_LOCATION: print("x pos: ", frame.packet.payload.pos_x) print("y pos: ", frame.packet.payload.pos_y) new_position = DotBotLH2Position( @@ -621,36 +621,11 @@ def handle_received_frame( if len(dotbot.position_history) > MAX_POSITION_HISTORY_SIZE: dotbot.position_history.pop(0) - 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 - ): - new_position = DotBotLH2Position( - x=dotbot.lh2_position.x, - y=dotbot.lh2_position.y, - z=dotbot.lh2_position.z, - ) - 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 - 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.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_RAW_DATA: + self.lh2_manager.last_raw_data = frame.packet.payload + + if frame.packet.payload_type == PayloadType.DOTBOT_DATA: + logger.error("Unsupported DOTBOT_DATA payload type") if frame.packet.payload_type == PayloadType.LH2_PROCESSED_DATA: logger.info( diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index b7db6265..ff7be0f7 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -247,20 +247,16 @@ def compute_calibration(self): # pylint: disable=too-many-locals final_points = scales_matrix * pts_cam_new.T final_points = final_points.T - #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, - #) - temporary_numpy_trash_heap = np.array([self.reference_points], dtype=np.float64) + 0.5 + 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( + M, _ = cv2.findHomography( camera_points_arr[0], temporary_numpy_trash_heap_pt2, - method = cv2.RANSAC, - ransacReprojThreshold=0.001 + method=cv2.RANSAC, + ransacReprojThreshold=0.001, ) self.calibration_data = CalibrationData(zeta, random_rodriguez, n, M) @@ -298,27 +294,7 @@ def compute_position( 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 - ) - - - 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) - - - - 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, ) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 00da0e96..717ce31a 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -24,7 +24,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 +35,6 @@ 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 @@ -465,7 +464,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, diff --git a/dotbot/tests/test_protocol.py b/dotbot/tests/test_protocol.py index eab38f93..6dcc07ca 100644 --- a/dotbot/tests/test_protocol.py +++ b/dotbot/tests/test_protocol.py @@ -132,22 +132,7 @@ def test_parse_header(bytes_, expected): id="PayloadCommandXgoAction", ), pytest.param( - b"\x04\x02\x11\x11\x11\x11\x11\x22\x22\x11\x12\x12\x12\x12\x12\x12\x12\x12\x02" - b"\x12\x34\x56\x78\x9a\xbc\xde\xf1\x01\x02", - Header( - version=4, - type_=2, - destination=0x1122221111111111, - source=0x1212121212121212, - ), - PayloadType.LH2_RAW_LOCATION, - PayloadLh2RawLocation( - bits=0xF1DEBC9A78563412, polynomial_index=0x01, offset=0x02 - ), - id="PayloadLH2RawLocation", - ), - pytest.param( - b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x0d" + b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x02" b"\x02" b"\x12\x34\x56\x78\x9a\xbc\xde\xf1\x01\x02" b"\x12\x34\x56\x78\x9a\xbc\xde\xf1\x02\x04", @@ -427,7 +412,7 @@ def test_frame_parser(bytes_, header, payload_type, payload): ) ), ), - b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x0d\x02" + b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x02\x02" b"\xf1\xde\xbc\x9a\x78\x56\x34\x12\x01\x02" b"\xf1\xde\xbc\x9a\x78\x56\x34\x12\x01\x02", id="PayloadLH2RawData", @@ -732,7 +717,7 @@ def test_payload_to_bytes(payload, expected): ( " +------+------+--------------------+--------------------+------+\n" " LH2_RAW_DATA | ver. | type | dst | src | type |\n" - " (40 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x0d |\n" + " (40 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x02 |\n" " +------+------+--------------------+--------------------+------+\n" " +------+--------------------+------+------+--------------------+------+------+\n" " | len | bits | poly | off. | bits | poly | off. |\n" From ca271273cd9fd86a1e88dfddd056ff460db22ff1 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Wed, 10 Sep 2025 07:25:22 +0200 Subject: [PATCH 18/21] dotbot/lighthouse2: set last_raw_data to None once used --- dotbot/lighthouse2.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index ff7be0f7..79767f5e 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -180,6 +180,7 @@ def add_calibration_point(self, index): dtype=np.float64, ) + self.last_raw_data = None if all(self.calibration_points_available) is False: self.state = LighthouseManagerState.CalibrationInProgress if all(self.calibration_points_available) is True: From 249238256e9c9c814187993a322a6fa962f4c0db Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Wed, 10 Sep 2025 10:46:19 +0200 Subject: [PATCH 19/21] protocol: refactor advertisement and dotbot_data packet types --- dotbot/controller.py | 25 +++++----- dotbot/models.py | 1 + dotbot/protocol.py | 15 +++--- dotbot/tests/test_protocol.py | 88 ++++++++++------------------------- 4 files changed, 47 insertions(+), 82 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index b945a7ab..028e3913 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -562,11 +562,20 @@ def handle_received_frame( payload = PayloadCommandRgbLed(red=0, green=255, blue=0) self.send_payload(int(source, 16), payload=payload) - # Send calibration to new dotbot if the localization system is calibrated + if frame.packet.payload_type == PayloadType.ADVERTISEMENT: + logger = logger.bind( + 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) + logger.info("Advertisement received") + # Send calibration to dotbot if it's not calibrated and the localization system has calibration if ( - frame.packet.payload_type == PayloadType.ADVERTISEMENT + dotbot.calibrated is False and self.lh2_manager.state == LighthouseManagerState.Calibrated ): + # Send calibration to new dotbot if the localization system is calibrated self.dotbots.update({dotbot.address: dotbot}) # Check if robot has lighthouse calibration matrix_bytes = bytearray() @@ -585,12 +594,6 @@ def handle_received_frame( ) self.send_payload(int(source, 16), payload=payload) - if frame.packet.payload_type == PayloadType.ADVERTISEMENT: - logger = logger.bind( - application=ApplicationType(frame.packet.payload.application).name - ) - dotbot.application = ApplicationType(frame.packet.payload.application) - if ( frame.packet.payload_type in [PayloadType.DOTBOT_DATA, PayloadType.SAILBOT_DATA] @@ -606,7 +609,7 @@ def handle_received_frame( sail_angle=dotbot.sail_angle, ) - if frame.packet.payload_type == PayloadType.LH2_LOCATION: + if frame.packet.payload_type == PayloadType.DOTBOT_DATA: print("x pos: ", frame.packet.payload.pos_x) print("y pos: ", frame.packet.payload.pos_y) new_position = DotBotLH2Position( @@ -614,6 +617,7 @@ def handle_received_frame( y=frame.packet.payload.pos_y / 1e6, z=0.0, ) + dotbot.direction = frame.packet.payload.direction dotbot.lh2_position = new_position dotbot.position_history.append(new_position) notification_cmd = DotBotNotificationCommand.UPDATE @@ -624,9 +628,6 @@ def handle_received_frame( if frame.packet.payload_type == PayloadType.LH2_RAW_DATA: self.lh2_manager.last_raw_data = frame.packet.payload - if frame.packet.payload_type == PayloadType.DOTBOT_DATA: - logger.error("Unsupported DOTBOT_DATA payload type") - if frame.packet.payload_type == PayloadType.LH2_PROCESSED_DATA: logger.info( "lh2-processed", diff --git a/dotbot/models.py b/dotbot/models.py index 1bc97bdd..d37eb811 100644 --- a/dotbot/models.py +++ b/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 diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 717ce31a..115d5aa4 100644 --- a/dotbot/protocol.py +++ b/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 @@ -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 @@ -330,16 +331,16 @@ class PayloadDotBotData(Payload): 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 diff --git a/dotbot/tests/test_protocol.py b/dotbot/tests/test_protocol.py index 6dcc07ca..7e08adc4 100644 --- a/dotbot/tests/test_protocol.py +++ b/dotbot/tests/test_protocol.py @@ -84,7 +84,7 @@ def test_parse_header(bytes_, expected): "bytes_,header,payload_type,payload", [ pytest.param( - b"\x04\x02\x11\x11\x11\x11\x11\x22\x22\x11\x12\x12\x12\x12\x12\x12\x12\x12\x04\x01", + b"\x04\x02\x11\x11\x11\x11\x11\x22\x22\x11\x12\x12\x12\x12\x12\x12\x12\x12\x04\x01\x01", Header( version=4, type_=2, @@ -92,7 +92,7 @@ def test_parse_header(bytes_, expected): source=0x1212121212121212, ), PayloadType.ADVERTISEMENT, - PayloadAdvertisement(application=ApplicationType.SailBot), + PayloadAdvertisement(application=ApplicationType.SailBot, calibrated=True), id="PayloadAdvertisement", ), pytest.param( @@ -442,10 +442,12 @@ def test_frame_parser(bytes_, header, payload_type, payload): source=0x1222122212221221, ), Packet.from_payload( - PayloadAdvertisement(application=ApplicationType.SailBot) + PayloadAdvertisement( + application=ApplicationType.SailBot, calibrated=False + ) ), ), - b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x04\x01", + b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x04\x01\x00", id="PayloadAdvertisement", ), pytest.param( @@ -473,28 +475,12 @@ def test_frame_parser(bytes_, header, payload_type, payload): source=0x1222122212221221, ), Packet.from_payload( - PayloadDotBotData( - direction=45, - count=2, - locations=[ - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, - polynomial_index=0x01, - offset=0x02, - ), - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, - polynomial_index=0x01, - offset=0x02, - ), - ], - ) + PayloadDotBotData(direction=45, pos_x=1000, pos_y=1000, pos_z=2) ), ), b"\x04\x02\x88\x77\x66\x55\x44\x33\x22\x11\x21\x12\x22\x12\x22\x12\x22\x12\x06" - b"-\x00\x02" - b"\xf1\xde\xbc\x9a\x78\x56\x34\x12\x01\x02" - b"\xf1\xde\xbc\x9a\x78\x56\x34\x12\x01\x02", + b"-\x00" + b"\xe8\x03\x00\x00\xe8\x03\x00\x00\x02\x00\x00\x00", id="PayloadDotBotData", ), pytest.param( @@ -761,14 +747,16 @@ def test_payload_to_bytes(payload, expected): source=0x1222122212221221, ), Packet.from_payload( - PayloadAdvertisement(application=ApplicationType.SailBot) + PayloadAdvertisement( + application=ApplicationType.SailBot, calibrated=False + ) ), ), ( - " +------+------+--------------------+--------------------+------+------+\n" - " ADVERTISEMENT | ver. | type | dst | src | type | app |\n" - " (20 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x04 | 0x01 |\n" - " +------+------+--------------------+--------------------+------+------+\n" + " +------+------+--------------------+--------------------+------+------+------+\n" + " ADVERTISEMENT | ver. | type | dst | src | type | app | cal. |\n" + " (21 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x04 | 0x01 | 0x00 |\n" + " +------+------+--------------------+--------------------+------+------+------+\n" "\n" ), id="Advertisement", @@ -807,33 +795,18 @@ def test_payload_to_bytes(payload, expected): source=0x1222122212221221, ), Packet.from_payload( - PayloadDotBotData( - direction=45, - count=2, - locations=[ - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, - polynomial_index=0x01, - offset=0x02, - ), - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, - polynomial_index=0x01, - offset=0x02, - ), - ], - ) + PayloadDotBotData(direction=45, pos_x=1000, pos_y=1000, pos_z=2) ), ), ( " +------+------+--------------------+--------------------+------+\n" " DOTBOT_DATA | ver. | type | dst | src | type |\n" - " (42 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x06 |\n" + " (33 Bytes) | 0x04 | 0x02 | 0x1122334455667788 | 0x1222122212221221 | 0x06 |\n" " +------+------+--------------------+--------------------+------+\n" - " +--------+------+--------------------+------+------+--------------------+------+------+\n" - " | dir. | len | bits | poly | off. | bits | poly | off. |\n" - " | 0x002d | 0x02 | 0x123456789abcdef1 | 0x01 | 0x02 | 0x123456789abcdef1 | 0x01 | 0x02 |\n" - " +--------+------+--------------------+------+------+--------------------+------+------+\n" + " +--------+------------+------------+------------+\n" + " | dir. | x | y | z |\n" + " | 0x002d | 0x000003e8 | 0x000003e8 | 0x00000002 |\n" + " +--------+------------+------------+------------+\n" "\n" ), id="DotBotData", @@ -1030,25 +1003,14 @@ class PayloadMissingMetadata(Payload): "payload,bytes_", [ pytest.param( - PayloadAdvertisement(application=ApplicationType.DotBot), + PayloadAdvertisement(application=ApplicationType.DotBot, calibrated=False), b"", id="PayloadAdvertisement", ), pytest.param( - PayloadDotBotData( - direction=45, - count=2, - locations=[ - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, polynomial_index=0x01, offset=0x02 - ), - PayloadLh2RawLocation( - bits=0x123456789ABCDEF1, polynomial_index=0x01, offset=0x02 - ), - ], - ), + PayloadDotBotData(direction=45, pos_x=1000, pos_y=1000, pos_z=2), b"-\x00\x02" b"\xf1\xde\xbc\x9a\x78\x56\x34\x12\x01\x02", - id="PayloadLh2RawLocation", + id="PayloadDotBotData", ), ], ) From cd5e56a6330c581a01712c42573ebce7ae2cc720 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 11 Sep 2025 11:12:47 +0200 Subject: [PATCH 20/21] pyproject.toml: revert some dependencies updates --- pyproject.toml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 25f24621..713a3aae 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,16 +41,16 @@ dependencies = [ "click >= 8.1.7", "fastapi >= 0.115.0", "haversine >= 2.8.0", - "httpx >= 0.28.1", + "httpx >= 0.27.2", "numpy >= 2.1.1", "opencv-python >= 4.12.0.88", "pygame >= 2.6.1", "pynput >= 1.7.6", "pyserial >= 3.5", - "qrkey >= 0.10.0", + "qrkey == 0.9.1", "structlog >= 24.4.0", - "uvicorn >= 0.35.0", - "websockets >= 15.0.1", + "uvicorn >= 0.32.0", + "websockets >= 13.1.0", "gmqtt >= 0.7.0", "marilib-pkg >= 0.6.0", ] From 9b8d5662291b5dcad63e6d76738acdea24d4fe83 Mon Sep 17 00:00:00 2001 From: Alexandre Abadie Date: Thu, 11 Sep 2025 11:14:05 +0200 Subject: [PATCH 21/21] dotbot/controller: cleanup and improving logged messages --- dotbot/controller.py | 39 +++++++++++++++++++++++---------------- dotbot/lighthouse2.py | 2 +- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 028e3913..0f85f15f 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -509,18 +509,12 @@ def handle_received_frame( ): # pylint:disable=too-many-branches,too-many-statements """Handle a received frame.""" # Controller is not interested by command messages received - - print("payload type: ", frame.packet.payload_type) - if frame.packet.payload_type in [ PayloadType.CMD_MOVE_RAW, PayloadType.CMD_RGB_LED, ]: return source = hexlify(int(frame.header.source).to_bytes(8, "big")).decode() - - print("source: ", source) - logger = self.logger.bind( source=source, payload_type=PayloadType(frame.packet.payload_type).name, @@ -559,8 +553,6 @@ def handle_received_frame( # reload if a new dotbot comes in logger.info("New dotbot") notification_cmd = DotBotNotificationCommand.RELOAD - payload = PayloadCommandRgbLed(red=0, green=255, blue=0) - self.send_payload(int(source, 16), payload=payload) if frame.packet.payload_type == PayloadType.ADVERTISEMENT: logger = logger.bind( @@ -569,14 +561,14 @@ def handle_received_frame( ) dotbot.application = ApplicationType(frame.packet.payload.application) dotbot.calibrated = bool(frame.packet.payload.calibrated) - logger.info("Advertisement received") + 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 - self.dotbots.update({dotbot.address: dotbot}) # Check if robot has lighthouse calibration matrix_bytes = bytearray() for bytes_block in [ @@ -589,9 +581,8 @@ def handle_received_frame( index=0, homography_matrix=matrix_bytes, ) - self.logger.info( - "Send calibration data", source=source, payload=payload - ) + self.logger.info("Send calibration data", payload=payload) + self.dotbots.update({dotbot.address: dotbot}) self.send_payload(int(source, 16), payload=payload) if ( @@ -610,8 +601,6 @@ def handle_received_frame( ) if frame.packet.payload_type == PayloadType.DOTBOT_DATA: - print("x pos: ", frame.packet.payload.pos_x) - print("y pos: ", frame.packet.payload.pos_y) new_position = DotBotLH2Position( x=frame.packet.payload.pos_x / 1e6, y=frame.packet.payload.pos_y / 1e6, @@ -621,12 +610,30 @@ def handle_received_frame( dotbot.lh2_position = new_position dotbot.position_history.append(new_position) notification_cmd = DotBotNotificationCommand.UPDATE - print(f"dotbot.position_history: {len(dotbot.position_history)}") if len(dotbot.position_history) > MAX_POSITION_HISTORY_SIZE: dotbot.position_history.pop(0) + 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, + ) if frame.packet.payload_type == PayloadType.LH2_PROCESSED_DATA: logger.info( diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index 79767f5e..5a4020dd 100644 --- a/dotbot/lighthouse2.py +++ b/dotbot/lighthouse2.py @@ -180,7 +180,7 @@ def add_calibration_point(self, index): dtype=np.float64, ) - self.last_raw_data = None + 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: