diff --git a/dotbot/controller.py b/dotbot/controller.py index 6d3dcc1a..0f85f15f 100644 --- a/dotbot/controller.py +++ b/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( diff --git a/dotbot/lighthouse2.py b/dotbot/lighthouse2.py index ee09561c..5a4020dd 100644 --- a/dotbot/lighthouse2.py +++ b/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, ) 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 6668188f..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 @@ -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,6 +307,23 @@ 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.""" @@ -313,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 @@ -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, } diff --git a/dotbot/tests/test_protocol.py b/dotbot/tests/test_protocol.py index eab38f93..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( @@ -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", @@ -457,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( @@ -488,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( @@ -732,7 +703,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" @@ -776,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", @@ -822,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", @@ -1045,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", ), ], ) 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", ]