dotbot: adapt for multi-lighthouse support and position coordinates in mm

doc/rest.md

@@ -60,7 +60,7 @@ If a DotBot is connected, this script should give an output similar to:
     "mode": 0,
     "last_seen": 1701244665.8099585,
     "waypoints": [],
-    "waypoints_threshold": 40,
+    "waypoints_threshold": 50,
     "position_history": []
   }
 ]

dotbot/__init__.py

@@ -15,6 +15,7 @@
 CONTROLLER_ADAPTER_DEFAULT = "serial"
 MQTT_HOST_DEFAULT = "localhost"
 MQTT_PORT_DEFAULT = 1883
+MAP_SIZE_DEFAULT = "2000x2000"  # in mm unit
 SIMULATOR_INIT_STATE_PATH_DEFAULT = "simulator_init_state.toml"
 
 

dotbot/controller.py

@@ -8,6 +8,7 @@
 """Interface of the Dotbot controller."""
 
 import asyncio
+import dataclasses
 import json
 import math
 import os
@@ -33,6 +34,7 @@
     CONTROLLER_HTTP_PORT_DEFAULT,
     DOTBOT_ADDRESS_DEFAULT,
     GATEWAY_ADDRESS_DEFAULT,
+    MAP_SIZE_DEFAULT,
     MQTT_HOST_DEFAULT,
     MQTT_PORT_DEFAULT,
     NETWORK_ID_DEFAULT,
@@ -53,6 +55,7 @@
     MAX_POSITION_HISTORY_SIZE,
     DotBotGPSPosition,
     DotBotLH2Position,
+    DotBotMapSizeModel,
     DotBotModel,
     DotBotMoveRawCommandModel,
     DotBotNotificationCommand,
@@ -92,20 +95,31 @@
 CONTROLLERS = {}
 INACTIVE_DELAY = 5  # seconds
 LOST_DELAY = 60  # seconds
-LH2_POSITION_DISTANCE_THRESHOLD = 0.01
+LH2_POSITION_DISTANCE_THRESHOLD = 20  # mm
 GPS_POSITION_DISTANCE_THRESHOLD = 5  # meters
 CALIBRATION_PATH = Path.home() / ".dotbot" / "calibration.out"
 
 
-def load_calibration() -> PayloadLh2CalibrationHomography:
+@dataclass
+class CalibrationHomography:
+    """Dataclass that holds computed LH2 homography for a basestation indicated by index."""
+
+    homography_matrix: bytes = dataclasses.field(default_factory=lambda: bytearray)
+
+
+def load_calibration() -> list[CalibrationHomography]:
     if not os.path.exists(CALIBRATION_PATH):
-        return None
+        return []
     with open(CALIBRATION_PATH, "rb") as calibration_file:
-        index = int.from_bytes(calibration_file.read(4), "little", signed=False)
-        homography_matrix = calibration_file.read(36)
-    return PayloadLh2CalibrationHomography(
-        index=index, homography_matrix=homography_matrix
-    )
+        homographies: list[CalibrationHomography] = []
+        homographies_num = int.from_bytes(
+            calibration_file.read(1), "little", signed=False
+        )
+        for _ in range(homographies_num):
+            homographies.append(
+                CalibrationHomography(homography_matrix=calibration_file.read(36))
+            )
+    return homographies
 
 
 class ControllerException(Exception):
@@ -126,6 +140,7 @@ class ControllerSettings:
     gw_address: str = GATEWAY_ADDRESS_DEFAULT
     network_id: str = NETWORK_ID_DEFAULT
     controller_http_port: int = CONTROLLER_HTTP_PORT_DEFAULT
+    map_size: str = MAP_SIZE_DEFAULT
     webbrowser: bool = False
     verbose: bool = False
     log_level: str = "info"
@@ -193,8 +208,12 @@ def __init__(self, settings: ControllerSettings):
         self.settings = settings
         self.adapter: GatewayAdapterBase = None
         self.websockets = []
-        self.lh2_calibration = load_calibration()
+        self.lh2_calibration: list[CalibrationHomography] = load_calibration()
         self.api = api
+        self.map_size = DotBotMapSizeModel(
+            width=int(settings.map_size.split("x")[0]),
+            height=int(settings.map_size.split("x")[1]),
+        )
         api.controller = self
         self.qrkey = None
 
@@ -358,9 +377,9 @@ def on_command_waypoints(self, topic, payload):
                 count=len(command.waypoints),
                 waypoints=[
                     PayloadLH2Location(
-                        pos_x=int(waypoint.x * 1e6),
-                        pos_y=int(waypoint.y * 1e6),
-                        pos_z=int(waypoint.z * 1e6),
+                        pos_x=int(waypoint.x),
+                        pos_y=int(waypoint.y),
+                        pos_z=int(waypoint.z),
                     )
                     for waypoint in command.waypoints
                 ],
@@ -423,6 +442,14 @@ def on_request(self, payload):
                 data=data,
             ).model_dump(exclude_none=True)
             self.qrkey.publish(reply_topic, message)
+        elif request.request == DotBotRequestType.MAP_SIZE:
+            logger.info("Publish map size")
+            data = self.map_size.model_dump(exclude_none=True)
+            message = DotBotReplyModel(
+                request=DotBotRequestType.MAP_SIZE,
+                data=data,
+            ).model_dump(exclude_none=True)
+            self.qrkey.publish(reply_topic, message)
         else:
             logger.warning("Unsupported request command")
 
@@ -547,21 +574,37 @@ def handle_received_frame(
 
         if frame.packet.payload_type == PayloadType.DOTBOT_ADVERTISEMENT:
             logger = logger.bind(application=ApplicationType.DotBot.name)
-            dotbot.calibrated = bool(frame.packet.payload.calibrated)
-            logger.info("Advertisement received", calibrated=bool(dotbot.calibrated))
+            dotbot.calibrated = int(frame.packet.payload.calibrated)
+            logger.info("Advertisement received", calibrated=hex(dotbot.calibrated))
             # Send calibration to dotbot if it's not calibrated and the localization system has calibration
             need_update = False
-            if dotbot.calibrated is False and self.lh2_calibration is not None:
+            is_fully_calibrated = all(
+                [
+                    dotbot.calibrated >> index & 0x01
+                    for index in range(len(self.lh2_calibration))
+                ]
+            )
+            if is_fully_calibrated is False and self.lh2_calibration:
                 # Send calibration to new dotbot if the localization system is calibrated
                 self.logger.info("Send calibration data", payload=self.lh2_calibration)
                 self.dotbots.update({dotbot.address: dotbot})
-                self.send_payload(int(source, 16), payload=self.lh2_calibration)
-            elif dotbot.calibrated is True:
+                for index, homography in enumerate(self.lh2_calibration):
+                    self.logger.info(
+                        "Sending calibration homography",
+                        index=index,
+                        matrix=homography.homography_matrix,
+                    )
+                    payload = PayloadLh2CalibrationHomography(
+                        index=index,
+                        homography_matrix=homography.homography_matrix,
+                    )
+                    self.send_payload(int(source, 16), payload=payload)
+            elif is_fully_calibrated is True:
                 if frame.packet.payload.direction != 0xFFFF:
                     dotbot.direction = frame.packet.payload.direction
                 new_position = DotBotLH2Position(
-                    x=frame.packet.payload.pos_x / 1e6,
-                    y=frame.packet.payload.pos_y / 1e6,
+                    x=frame.packet.payload.pos_x,
+                    y=frame.packet.payload.pos_y,
                     z=0.0,
                 )
                 if new_position.x != 0xFFFFFFFF and new_position.y != 0xFFFFFFFF:

dotbot/controller_app.py

@@ -19,6 +19,7 @@
     CONTROLLER_HTTP_PORT_DEFAULT,
     DOTBOT_ADDRESS_DEFAULT,
     GATEWAY_ADDRESS_DEFAULT,
+    MAP_SIZE_DEFAULT,
     MQTT_HOST_DEFAULT,
     MQTT_PORT_DEFAULT,
     NETWORK_ID_DEFAULT,
@@ -120,6 +121,12 @@
     type=click.Path(exists=True, dir_okay=False),
     help="Path to a .toml configuration file.",
 )
+@click.option(
+    "-m",
+    "--map-size",
+    type=str,
+    help=f"Map size in mm. Defaults to '{MAP_SIZE_DEFAULT}'",
+)
 def main(
     adapter,
     port,
@@ -131,6 +138,7 @@ def main(
     gw_address,
     network_id,
     controller_http_port,
+    map_size,
     webbrowser,
     verbose,
     log_level,
@@ -153,6 +161,7 @@ def main(
         "gw_address": gw_address,
         "network_id": network_id,
         "controller_http_port": controller_http_port,
+        "map_size": map_size,
         "webbrowser": webbrowser,
         "verbose": verbose,
         "log_level": log_level,

dotbot/dotbot_simulator.py

@@ -23,9 +23,10 @@
 from dotbot.logger import LOGGER
 from dotbot.protocol import PayloadDotBotAdvertisement, PayloadType
 
-R = 1
-L = 2
-SIMULATOR_STEP_DELTA_T = 0.002
+R = 50
+L = 75
+MOTOR_SPEED = 70
+SIMULATOR_STEP_DELTA_T = 0.002  # 2 ms
 
 INITIAL_BATTERY_VOLTAGE = 3000  # mV
 MAX_BATTERY_DURATION = 300  # 5 minutes
@@ -61,8 +62,8 @@ class SimulatedDotBotSettings(BaseModel):
     pos_x: int
     pos_y: int
     theta: float
-    calibrated: bool = True
-    motor_left_error: float = 0.5
+    calibrated: int = 0xFF
+    motor_left_error: float = 0
     motor_right_error: float = 0
 
 
@@ -112,9 +113,9 @@ def _diff_drive_bot(self, x_pos_old, y_pos_old, theta_old, v_right, v_left):
         """Execute state space model of a rigid differential drive robot."""
         v_right_real = v_right * (1 - self.motor_right_error)
         v_left_real = v_left * (1 - self.motor_left_error)
-        x_dot = R / 2 * (v_right_real + v_left_real) * cos(theta_old - pi) * 50000
-        y_dot = R / 2 * (v_right_real + v_left_real) * sin(theta_old - pi) * 50000
-        theta_dot = R / L * (-v_right_real + v_left_real)
+        x_dot = (R / 2) * ((v_left_real + v_right_real) / L) * cos(theta_old - pi) * 100
+        y_dot = (R / 2) * ((v_left_real + v_right_real) / L) * sin(theta_old - pi) * 100
+        theta_dot = R / L * (v_left_real - v_right_real)
 
         x_pos = x_pos_old + x_dot * SIMULATOR_STEP_DELTA_T
         y_pos = y_pos_old + y_dot * SIMULATOR_STEP_DELTA_T
@@ -166,9 +167,9 @@ def update(self, dt: float):
                     error_angle=error_angle,
                 )
 
-                angular_speed = error_angle * 200
-                self.v_left = 100 + angular_speed
-                self.v_right = 100 - angular_speed
+                angular_speed = error_angle * MOTOR_SPEED
+                self.v_left = MOTOR_SPEED + angular_speed
+                self.v_right = MOTOR_SPEED - angular_speed
 
                 if self.v_left > 100:
                     self.v_left = 100
@@ -223,7 +224,7 @@ def handle_frame(self, frame: Frame):
                 self.v_left = 0
                 self.v_right = 0
                 self.controller_mode = DotBotSimulatorMode.MANUAL
-                self.waypoint_threshold = frame.packet.payload.threshold * 1000
+                self.waypoint_threshold = frame.packet.payload.threshold
                 self.waypoints = frame.packet.payload.waypoints
                 if self.waypoints:
                     self.controller_mode = DotBotSimulatorMode.AUTOMATIC

dotbot/examples/charging_station.py

@@ -22,23 +22,23 @@
 from dotbot.rest import RestClient, rest_client
 from dotbot.websocket import DotBotWsClient
 
-THRESHOLD = 30  # Acceptable distance error to consider a waypoint reached
+THRESHOLD = 50  # Acceptable distance error to consider a waypoint reached
 DT = 0.05  # Control loop period (seconds)
 
 # TODO: Measure these values for real dotbots
-BOT_RADIUS = 0.03  # Physical radius of a DotBot (unit), used for collision avoidance
-MAX_SPEED = 0.075  # Maximum allowed linear speed of a bot
+BOT_RADIUS = 40  # Physical radius of a DotBot (unit), used for collision avoidance
+MAX_SPEED = 300  # Maximum allowed linear speed of a bot (mm/s)
 
 (QUEUE_HEAD_X, QUEUE_HEAD_Y) = (
-    0.1,
-    0.8,
+    500,
+    1500,
 )  # World-frame (X, Y) position of the charging queue head
 QUEUE_SPACING = (
-    0.1  # Spacing between consecutive bots in the charging queue (along X axis)
+    200  # Spacing between consecutive bots in the charging queue (along X axis)
 )
 
-(PARK_X, PARK_Y) = (0.8, 0.1)  # World-frame (X, Y) position of the parking area origin
-PARK_SPACING = 0.1  # Spacing between parked bots (along Y axis)
+(PARK_X, PARK_Y) = (1500, 500)  # World-frame (X, Y) position of the parking area origin
+PARK_SPACING = 200  # Spacing between parked bots (along Y axis)
 
 
 async def queue_robots(
@@ -236,8 +236,8 @@ def assign_charge_goals(
     # Send the first one to the charger
     head = ordered[0]
     goals[head.address] = {
-        "x": 0.2,
-        "y": 0.2,
+        "x": 200,
+        "y": 200,
     }
 
     # Remaining bots shift left in the queue
@@ -257,9 +257,8 @@ def preferred_vel(dotbot: DotBotModel, goal: Vec2 | None) -> Vec2:
     dy = goal["y"] - dotbot.lh2_position.y
     dist = math.sqrt(dx * dx + dy * dy)
 
-    dist1000 = dist * 1000
     # If close to goal, stop
-    if dist1000 < THRESHOLD:
+    if dist < THRESHOLD:
         return Vec2(x=0, y=0)
 
     # Right-hand rule bias

dotbot/examples/charging_station_init_state.toml

@@ -1,6 +1,6 @@
 [[dotbots]]
 address = "BADCAFE111111111" # DotBot unique address
-calibrated = true # optional, defaults to true
+calibrated = 0x01 # optional, defaults to only first lighthouse calibrated
 pos_x = 400_000 # [0, 1_000_000]
 pos_y = 200_000 # [0, 1_000_000]
 theta = 0.0 # [0.0, 2pi]

dotbot/frontend/config-overrides.js

@@ -19,5 +19,8 @@ module.exports = function override(config) {
             Buffer: ['buffer', 'Buffer']
         })
     ])
+    config.watchOptions = {
+        ignored: /node_modules/,
+    };
     return config;
 }

dotbot/frontend/src/App.js

@@ -14,6 +14,7 @@ const log = logger.child({module: 'app'});
 const App = () => {
   const [searchParams, setSearchParams] = useSearchParams();
   const [message, setMessage] = useState(null);
+  const [areaSize, setAreaSize] = useState({height: 2000, width: 2000});
   const [dotbots, setDotbots] = useState([]);
 
   const [ready, clientId, mqttData, setMqttData, publish, publishCommand, sendRequest] = useQrKey({
@@ -30,6 +31,8 @@ const App = () => {
       // Received the list of dotbots
       if (payload.request === RequestType.DotBots) {
         setDotbots(payload.data);
+      } else if (payload.request === RequestType.AreaSize) {
+        setAreaSize(payload.data);
       }
     } else if (message.topic === `/notify`) {
       // Process notifications
@@ -45,7 +48,9 @@ const App = () => {
                 x: payload.data.lh2_position.x,
                 y: payload.data.lh2_position.y
               };
-              if (dotbotsTmp[idx].lh2_position && (dotbotsTmp[idx].position_history.length === 0 || lh2_distance(dotbotsTmp[idx].lh2_position, newPosition) > lh2_distance_threshold)) {
+              console.log('distance threshold:', lh2_distance_threshold, lh2_distance(dotbotsTmp[idx].lh2_position, newPosition));
+              if (dotbotsTmp[idx].lh2_position && (dotbotsTmp[idx].position_history.length === 0 || lh2_distance(dotbotsTmp[idx].lh2_position, newPosition) >= lh2_distance_threshold)) {
+                console.log('Adding to position history');
                 dotbotsTmp[idx].position_history.push(newPosition);
               }
               dotbotsTmp[idx].lh2_position = newPosition;
@@ -72,13 +77,14 @@ const App = () => {
       }
     }
     setMessage(null);
-  },[clientId, dotbots, setDotbots, sendRequest, message, setMessage]
+  },[clientId, dotbots, setDotbots, setAreaSize, sendRequest, message, setMessage]
   );
 
   useEffect(() => {
     if (clientId) {
       // Ask for the list of dotbots at startup
       setTimeout(sendRequest, 100, ({request: RequestType.DotBots, reply: `${clientId}`}));
+      setTimeout(sendRequest, 200, ({request: RequestType.AreaSize, reply: `${clientId}`}));
     }
   }, [sendRequest, clientId]
   );
@@ -98,6 +104,7 @@ const App = () => {
       <div id="dotbots">
         <DotBots
           dotbots={dotbots}
+          areaSize={areaSize}
           updateDotbots={setDotbots}
           publishCommand={publishCommand}
           publish={publish}