diff --git a/dotbot/adapter.py b/dotbot/adapter.py index 3742a269..e715537a 100644 --- a/dotbot/adapter.py +++ b/dotbot/adapter.py @@ -287,12 +287,25 @@ class DotBotSimulatorAdapter(SimulatorAdapterBase): def __init__( self, simulator_init_state: str = SIMULATOR_INIT_STATE_DEFAULT, + bots: int | None = None, + layout: str = "grid", + seed: int = 0, + map_size: str | None = None, ): self.simulator_init_state = simulator_init_state + self.bots = bots + self.layout = layout + self.seed = seed + self.map_size = map_size def create_simulator(self, on_frame_received: callable): return DotBotSimulatorCommunicationInterface( - on_frame_received, self.simulator_init_state + on_frame_received, + self.simulator_init_state, + bots=self.bots, + layout=self.layout, + seed=self.seed, + map_size=self.map_size, ) diff --git a/dotbot/cli/swarm.py b/dotbot/cli/swarm.py index 0eb89808..1f5c4d5d 100644 --- a/dotbot/cli/swarm.py +++ b/dotbot/cli/swarm.py @@ -50,6 +50,16 @@ def _with_config_injection(swarmit_group): `--help` and subcommand help flow straight through. """ + # Bridge to the Python rung: `dotbot swarm` operates the fleet; *driving* + # it (motion, LEDs, positions) is the Swarm API's job. Say so where an + # operator looks first. + if not swarmit_group.epilog: + swarmit_group.epilog = ( + "To drive the swarm from Python (motion, LEDs, positions), start a " + "controller (`dotbot run controller`) and use " + "`from dotbot.swarm import Swarm`." + ) + @click.command( name="swarm", help=_HELP, diff --git a/dotbot/controller.py b/dotbot/controller.py index 043862d2..7eaa1ec2 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -21,9 +21,7 @@ from typing import Dict, List, Optional import serial -import starlette import uvicorn -import websockets from dotbot_utils.protocol import Frame, Payload from dotbot_utils.serial_interface import SerialInterfaceException from fastapi import WebSocket @@ -135,6 +133,9 @@ class ControllerSettings: log_output: str = os.path.join(os.getcwd(), "pydotbot.log") csv_data_output: Optional[str] = None simulator_init_state: str = SIMULATOR_INIT_STATE_DEFAULT + simulator_bots: Optional[int] = None + simulator_layout: str = "grid" + simulator_seed: int = 0 def lh2_distance(last: DotBotLH2Position, new: DotBotLH2Position) -> float: @@ -197,6 +198,7 @@ def __init__(self, settings: ControllerSettings): self.settings = settings self.adapter: GatewayAdapterBase = None self.websockets = [] + self._ws_send_locks = {} self.lh2_calibration: list[CalibrationHomography] = load_calibration() self.api = api self.map_size = DotBotMapSizeModel( @@ -559,20 +561,35 @@ def handle_received_frame( asyncio.create_task(self.notify_clients(notification)) async def _ws_send_safe(self, websocket: WebSocket, msg: str): - """Safely send a message to a websocket client.""" + """Safely send a message to a websocket client. + + Writes to one connection are serialized with a per-connection lock: + notify_clients tasks run concurrently (one per received frame), and + two coroutines writing/draining the same websocket trips an + AssertionError deep in the websockets protocol, which crashed the + controller on a busy real fleet. The broad except is deliberate for + the same reason - a failing client gets dropped, never the controller. + """ + lock = self._ws_send_locks.setdefault(id(websocket), asyncio.Lock()) try: - await websocket.send_text(msg) - except ( - websockets.exceptions.ConnectionClosedError, - RuntimeError, - starlette.websockets.WebSocketDisconnect, - ) as exc: + async with lock: + if websocket not in self.websockets: + # Dropped by a concurrent sender while we waited. Sends are + # membership-gated, so popping the entry here is safe even + # with senders still queued on the old lock object. + self._ws_send_locks.pop(id(websocket), None) + return + await websocket.send_text(msg) + except Exception as exc: # noqa: BLE001 self.logger.warning( "Failed to send message to websocket client", error=str(exc), ) if websocket in self.websockets: self.websockets.remove(websocket) + # The lock entry is NOT popped here: senders already queued on it + # must keep serializing through the same object. It is cleaned up + # on the websocket-disconnect path instead. async def notify_clients(self, notification): """Send a message to all clients connected.""" @@ -699,6 +716,10 @@ async def _start_adapter(self): elif self.settings.adapter == "dotbot-simulator": self.adapter = DotBotSimulatorAdapter( self.settings.simulator_init_state, + bots=self.settings.simulator_bots, + layout=self.settings.simulator_layout, + seed=self.settings.simulator_seed, + map_size=self.settings.map_size, ) elif self.settings.adapter == "sailbot-simulator": self.adapter = SailBotSimulatorAdapter() diff --git a/dotbot/controller_app.py b/dotbot/controller_app.py index 117f8377..e9e23b40 100644 --- a/dotbot/controller_app.py +++ b/dotbot/controller_app.py @@ -226,6 +226,27 @@ def _maybe_scaffold_sim_state(explicit_init_state): type=click.Path(dir_okay=False), help=f"Path to the simulator initial state .toml file. Defaults to '{SIMULATOR_INIT_STATE_DEFAULT}'.", ) +@click.option( + "--bots", + "simulator_bots", + type=click.IntRange(1, 100), + default=None, + help="Simulator: spawn this many bots (1-100) in a generated --layout, instead of an init-state file.", +) +@click.option( + "--layout", + "simulator_layout", + type=click.Choice(["grid", "circle", "line", "random"]), + default=None, + help="Simulator: how --bots are arranged (default: grid).", +) +@click.option( + "--seed", + "simulator_seed", + type=int, + default=None, + help="Simulator: random seed for `--layout random`.", +) @click.pass_context def main( ctx, @@ -237,6 +258,9 @@ def main( map_size, background_map, simulator_init_state, + simulator_bots, + simulator_layout, + simulator_seed, headless, verbose, log_level, @@ -285,7 +309,10 @@ def main( # None, so fold in any config value), offer to scaffold an editable # world file in the cwd. resolve_init_state_path then picks up the # freshly-written file (or the packaged world if declined/non-tty). - if conn_settings.get("adapter", "").endswith("simulator"): + if ( + conn_settings.get("adapter", "").endswith("simulator") + and simulator_bots is None + ): _maybe_scaffold_sim_state( simulator_init_state or file_data.get("simulator_init_state") ) @@ -296,6 +323,9 @@ def main( "map_size": map_size, "background_map": background_map, "simulator_init_state": simulator_init_state, + "simulator_bots": simulator_bots, + "simulator_layout": simulator_layout, + "simulator_seed": simulator_seed, "headless": True if headless else None, "verbose": verbose, "log_level": log_level, diff --git a/dotbot/dotbot_simulator.py b/dotbot/dotbot_simulator.py index a9f2f58d..4d710bc0 100644 --- a/dotbot/dotbot_simulator.py +++ b/dotbot/dotbot_simulator.py @@ -174,6 +174,44 @@ class InitStateToml(BaseModel): network: SimulatedNetworkSettings = SimulatedNetworkSettings() +def _parse_map_size(map_size: str | None) -> tuple[int, int] | None: + """Parse a 'WIDTHxHEIGHT' map size (in mm) into (width, height), or None if + it can't be parsed (so the layout falls back to its default arena).""" + if not map_size: + return None + try: + width, height = map_size.lower().split("x") + return int(width), int(height) + except (ValueError, AttributeError): + return None + + +def generate_fleet( + n: int, layout: str = "grid", seed: int = 0, map_size: str | None = None +) -> List[SimulatedDotBotSettings]: + """Build `n` simulated bots placed in a named layout, sized to `map_size` + (so the fleet fills the whole map), with sequential auto-generated + addresses. The `random` layout also gives each bot a random heading; the + structured layouts keep the default heading so rows/circles stay aligned. + Backs `dotbot run simulator --bots N --layout`.""" + from dotbot import patterns + + dims = _parse_map_size(map_size) + kwargs = {"width": dims[0], "height": dims[1]} if dims else {} + positions = patterns.layout(n, layout, seed=seed, **kwargs) + # Separate seeded stream so headings don't perturb the placement. + heading_rng = random.Random(seed + 7919) if layout == "random" else None + fleet = [] + for i, (x, y) in enumerate(positions): + extra = {"direction": heading_rng.randrange(360)} if heading_rng else {} + fleet.append( + SimulatedDotBotSettings( + address=f"{i + 1:016x}", pos_x=int(x), pos_y=int(y), **extra + ) + ) + return fleet + + class DotBotSimulator: """Simulator class for the dotbot.""" @@ -190,7 +228,10 @@ def __init__(self, settings: SimulatedDotBotSettings, tx_queue: queue.Queue): self.pwm_left = 0 self.pwm_right = 0 - self.direction = settings.direction + # Map the "unset direction" sentinel to north (0), same as theta above; + # otherwise -1000 reaches the control loop as a bogus heading and the + # bot can never settle on a waypoint. + self.direction = settings.direction if settings.direction != -1000 else 0 # Accumulated encoder deltas between control-loop calls (control runs at # SIMULATOR_UPDATE_INTERVAL_S, physics at SIMULATOR_STEP_DELTA_T — multiple @@ -604,7 +645,7 @@ def rx_frame(self): if frame is None: break with self._lock: - if self.address == hex(frame.header.destination)[2:]: + if self.address == f"{frame.header.destination:016x}": if frame.payload_type == PayloadType.CMD_MOVE_RAW: self.controller_mode = ControlModeType.MANUAL self.waypoint_index = 0 @@ -769,14 +810,27 @@ def resolve_init_state_path(path: str) -> str: class DotBotSimulatorCommunicationInterface: """Bidirectional serial interface to control simulated robots""" - def __init__(self, on_frame_received: Callable, simulator_init_state: str): + def __init__( + self, + on_frame_received: Callable, + simulator_init_state: str, + bots: int | None = None, + layout: str = "grid", + seed: int = 0, + map_size: str | None = None, + ): self.queue = queue.Queue() self.on_frame_received = on_frame_received self._stp_event = threading.Event() self.main_thread = threading.Thread(target=self.run, daemon=True) - init_state = InitStateToml( - **toml.load(resolve_init_state_path(simulator_init_state)) - ) + if bots: + init_state = InitStateToml( + dotbots=generate_fleet(bots, layout, seed, map_size) + ) + else: + init_state = InitStateToml( + **toml.load(resolve_init_state_path(simulator_init_state)) + ) self._network = init_state.network self.dotbots = [ DotBotSimulator( @@ -828,7 +882,7 @@ def _packet_delivered(self, pdr: int) -> bool: def handle_dotbot_frame(self, frame): """Send bytes to the fake serial, similar to the real gateway.""" - addr = hex(frame.header.source)[2:] + addr = f"{frame.header.source:016x}" index = self._address_to_index.get(addr, 0) if self._dotbot_modes[index] == SimulatedNetworkMode.MARI: self._mari.schedule_uplink(frame, index) diff --git a/dotbot/events.py b/dotbot/events.py new file mode 100644 index 00000000..b5614ac7 --- /dev/null +++ b/dotbot/events.py @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Public re-export of the SDK event types, so users write +`from dotbot.events import BotJoined`. The definitions live in +`dotbot.swarm.events`. +""" + +from dotbot.swarm.events import ( # noqa: F401 + BatteryUpdate, + BotJoined, + BotLeft, + Event, + ModeChanged, + PositionUpdate, +) + +__all__ = [ + "Event", + "BotJoined", + "BotLeft", + "PositionUpdate", + "BatteryUpdate", + "ModeChanged", +] diff --git a/dotbot/examples/charging_station/charging_station_sdk.py b/dotbot/examples/charging_station/charging_station_sdk.py new file mode 100644 index 00000000..31ea68b0 --- /dev/null +++ b/dotbot/examples/charging_station/charging_station_sdk.py @@ -0,0 +1,229 @@ +"""charging_station_sdk.py - the full charging-station scenario on the Swarm SDK. + +The fleet forms a single-file queue, then services the charger one bot at a +time: the queue head drives in, dwells while it "charges" (red -> amber -> +green), then peels off to the next free parking slot while the rest of the +queue shifts forward by one. Repeat until every bot has charged and parked. + +This is the SDK rewrite of charging_station.py. The ORCA collision-avoidance +math (dotbot.examples.common.orca) is unchanged domain code; everything the +old version hand-rolled - REST polling, the ws client, the waypoint pydantic +towers, the manual reverse-out-of-the-charger maneuver - collapses into the +Swarm: every motion phase is one `converge(...)` that streams a fresh ORCA step +per `swarm.tick()`, and a charged bot reaches its parking slot as just another +goal in the next convergence (so it threads out past the incoming bot under the +same collision avoidance, no scripted disengage needed). + +Run a controller in simulator mode, then run this script: + + dotbot run controller --conn simulator --headless \\ + --simulator-init-state \\ + dotbot/examples/charging_station/charging_station_init_state.toml + + python -m dotbot.examples.charging_station.charging_station_sdk +""" + +import asyncio +import math + +from dotbot.examples.common.orca import ( + Agent, + OrcaParams, + compute_orca_velocity_for_agent, +) +from dotbot.examples.common.vec2 import Vec2 +from dotbot.swarm import Bot, Position, Swarm + +THRESHOLD = 100 # mm, proximity to consider a goal reached +DT = 0.2 # control-loop period (s) -> 5 Hz +BOT_RADIUS = 60 # mm, used for collision avoidance +MAX_SPEED = 300 # mm/s +CONVERGE_TIMEOUT = 90.0 # s, give up on a phase that never settles + +# World-frame layout (mm). The queue is a horizontal line; the head peels off +# downward to the charger, then out to the parking column on the right. +CHARGER = Position(500, 500) +QUEUE_HEAD = Position(500, 1500) +QUEUE_SPACING = 300 # between consecutive bots in the queue (along +x) +PARK_ORIGIN = Position(1700, 500) +PARK_SPACING = 300 # between parked bots (along +y) + +CHARGE_SECONDS = 2.0 # dwell at the charger while "charging" +CHARGING_COLOR = (255, 128, 0) # amber: plugged in and charging +CHARGED_COLOR = "green" # done charging + + +def _online(swarm: Swarm) -> list[Bot]: + """Bots that are active and have a position fix - the only ones we can plan + for.""" + return [b for b in swarm if b.is_online and b.position is not None] + + +async def _await_fleet(swarm: Swarm, *, timeout: float = 10.0) -> list[Bot]: + """Wait until the fleet has reported position fixes, then return it. Bounded + so the script exits cleanly if no bots ever show up.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + bots = _online(swarm) + while not bots and loop.time() < deadline: + await asyncio.sleep(0.2) + bots = _online(swarm) + return bots + + +def _direction_to_rad(direction) -> float: + rad = ((direction or 0) + 90) * math.pi / 180.0 + return math.atan2(math.sin(rad), math.cos(rad)) + + +def _preferred_vel(bot: Bot, goal: Position | None) -> Vec2: + """Velocity the bot would take toward its goal absent neighbours; zero once + within THRESHOLD (the loop's stop condition).""" + if goal is None or bot.position is None: + return Vec2(x=0, y=0) + dx = goal.x - bot.position.x + dy = goal.y - bot.position.y + if math.hypot(dx, dy) < THRESHOLD: + return Vec2(x=0, y=0) + direction = _direction_to_rad(bot.direction) + angle_to_goal = math.atan2(dy, dx) + delta = math.atan2( + math.sin(angle_to_goal - direction), math.cos(angle_to_goal - direction) + ) + final = direction + delta + return Vec2(x=math.cos(final) * MAX_SPEED, y=math.sin(final) * MAX_SPEED) + + +def _order_by_distance(bots: list[Bot], ref: Position) -> list[Bot]: + """Order bots by distance to a reference point (nearest first), address as a + stable tiebreak.""" + return sorted(bots, key=lambda b: (b.position.distance_to(ref), b.address)) + + +def _queue_goals(ordered: list[Bot]) -> dict[str, Position]: + """Assign the i-th bot to the i-th slot of the queue line.""" + return { + b.address: QUEUE_HEAD + (i * QUEUE_SPACING, 0) for i, b in enumerate(ordered) + } + + +async def converge( + swarm: Swarm, goals: dict[str, Position], params: OrcaParams +) -> bool: + """Stream ORCA steps until every goal-holding bot is online and within + THRESHOLD of its goal. Returns True when the formation settles, or False if + CONVERGE_TIMEOUT elapses first (a bot got stuck, never arrived, or dropped + out). Bots without a goal hold station and are still avoided as neighbours.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + CONVERGE_TIMEOUT + async for _ in swarm.tick(rate_hz=1 / DT): + bots = _online(swarm) + by_address = {b.address: b for b in bots} + # Settled only when EVERY goal-holder is present and has reached its goal + # by position - not off a snapshot of whoever is online this tick. A + # goal-holder that drops out (or an empty fleet) must never look "done". + settled = bool(goals) and all( + addr in by_address + and by_address[addr].position is not None + and by_address[addr].position.distance_to(goal) <= THRESHOLD + for addr, goal in goals.items() + ) + if settled: + return True + if loop.time() > deadline: + return False + agents = [ + Agent( + id=b.address, + position=Vec2(x=b.position.x, y=b.position.y), + velocity=Vec2(x=0, y=0), + radius=BOT_RADIUS, + max_speed=MAX_SPEED, + preferred_velocity=_preferred_vel(b, goals.get(b.address)), + ) + for b in bots + ] + for agent in agents: + neighbors = [n for n in agents if n.id != agent.id] + velocity = compute_orca_velocity_for_agent(agent, neighbors, params) + step = Vec2(x=velocity.x, y=velocity.y) + goal = goals.get(agent.id) + if goal is not None: # clamp the step so it never overshoots the goal + dist = math.hypot(goal.x - agent.position.x, goal.y - agent.position.y) + length = math.hypot(step.x, step.y) + if length > dist and length > 0: + step = Vec2(x=step.x * dist / length, y=step.y * dist / length) + by_address[agent.id].goto( + agent.position.x + step.x, + agent.position.y + step.y, + threshold=int(THRESHOLD * 0.9), + ) + return False # swarm.tick() never ends; here only to satisfy the type + + +async def _charge(bot: Bot) -> None: + """Dwell at the charger: amber while charging, green when full.""" + bot.set_color(CHARGING_COLOR) + await asyncio.sleep(CHARGE_SECONDS) + bot.set_color(CHARGED_COLOR) + await asyncio.sleep(CHARGE_SECONDS / 2) + + +async def charging_station(swarm: Swarm) -> None: + bots = await _await_fleet(swarm) + if not bots: + print("no active bots") + return + total = len(bots) + print(f"{total} bots online; forming the charging queue with ORCA ...") + params = OrcaParams(time_horizon=5 * DT, time_step=DT) + swarm.all.set_color("red") + + # Phase 1: bring the whole fleet into the queue. + await converge(swarm, _queue_goals(_order_by_distance(bots, QUEUE_HEAD)), params) + print("queue formed; servicing the charger one bot at a time ...") + + # Phase 2: charge + park the queue head, shift the rest forward, repeat. + # `park_goals` are bots that have charged and are heading to / sitting in + # their parking slot; they stay in every convergence so they hold position + # and the queue flows around them. + park_goals: dict[str, Position] = {} + while True: + remaining = _order_by_distance( + [b for b in _online(swarm) if b.address not in park_goals], QUEUE_HEAD + ) + if not remaining: + break + head, rest = remaining[0], remaining[1:] + + goals = {head.address: CHARGER, **_queue_goals(rest), **park_goals} + await converge(swarm, goals, params) # head->charger, rest shift, parkers hold + + # Only treat the head as charged once it has actually reached the + # charger: converge() can time out, or the head can drop offline, and a + # bot nowhere near the charger must not be recorded as charged + parked. + head = next((b for b in _online(swarm) if b.address == head.address), None) + if ( + head is None + or head.position is None + or head.position.distance_to(CHARGER) > THRESHOLD + ): + print(" warning: queue head never reached the charger; stopping") + break + + print(f" charging {head.address} ({len(park_goals) + 1}/{total})") + await _charge(head) + + # Hand the charged bot a parking slot; it drives there during the next + # convergence, threading past the incoming bot under ORCA. + park_goals[head.address] = PARK_ORIGIN + (0, len(park_goals) * PARK_SPACING) + + # Final convergence so the last-charged bot reaches its slot. + await converge(swarm, park_goals, params) + swarm.all.set_color(CHARGED_COLOR) + await asyncio.sleep(1.0) # let the colour commands flush before we exit + print(f"done - all {len(park_goals)} bots charged and parked") + + +if __name__ == "__main__": + Swarm.run(charging_station) diff --git a/dotbot/examples/labyrinth/labyrinth_sdk.py b/dotbot/examples/labyrinth/labyrinth_sdk.py new file mode 100644 index 00000000..642cfbc3 --- /dev/null +++ b/dotbot/examples/labyrinth/labyrinth_sdk.py @@ -0,0 +1,58 @@ +"""labyrinth_sdk.py - the labyrinth example rewritten on the Swarm SDK. + +Same behaviour as labyrinth.py (two robots navigate the maze), but the REST +polling, the ws client, the <=12 waypoint chunking, the resend-until-AUTO and +poll-until-arrival loops, and the pydantic message towers are all absorbed by +`Bot.follow`. Run a controller/simulator on :8000, then: + + python labyrinth_sdk.py [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.swarm import Swarm + +ROBOT1_WAYPOINTS = [ + (200, 680), + (200, 1600), + (1000, 1600), + (1300, 1400), + (1000, 1100), + (1300, 750), + (1300, 500), + (600, 500), +] +ROBOT2_WAYPOINTS = [ + (1800, 1700), + (1300, 1700), + (300, 1700), + (300, 900), + (600, 900), + (600, 1200), + (200, 1200), + (200, 1650), + (1300, 1650), + (1300, 400), + (900, 200), + (600, 200), +] + + +async def labyrinth(swarm: Swarm) -> None: + bots = sorted(swarm, key=lambda b: b.address)[:2] + if len(bots) < 2: + print(f"need 2 active DotBots, found {len(bots)}") + return + blue, red = bots + print(f"blue={blue.address} red={red.address}") + blue.set_color("blue") + red.set_color("red") + await asyncio.gather( + blue.follow(ROBOT1_WAYPOINTS), + red.follow(ROBOT2_WAYPOINTS), + ) + print("both robots reached their targets") + + +if __name__ == "__main__": + Swarm.run(labyrinth) diff --git a/dotbot/examples/minimum_naming_game/minimum_naming_game_sdk.py b/dotbot/examples/minimum_naming_game/minimum_naming_game_sdk.py new file mode 100644 index 00000000..32bf4f31 --- /dev/null +++ b/dotbot/examples/minimum_naming_game/minimum_naming_game_sdk.py @@ -0,0 +1,126 @@ +"""minimum_naming_game_sdk.py - the minimum naming game on the Swarm SDK. + +A field of stationary bots play the minimum naming game: each step a bot hears +one random neighbour within COMM_RANGE, folds that word into its inventory, and +shows its word as an LED colour (off until it holds a single word). Through +purely local exchange the swarm converges on one word - every bot the same +colour. The per-bot agent (controller.py) and the SCT runtime +(dotbot.examples.common.sct) are unchanged domain code. + +This is the SDK rewrite of minimum_naming_game.py. The REST polling, the ws +client, the scipy KD-tree and the rgb pydantic tower collapse into the Swarm: +the loop is `async for _ in swarm.tick(...)`, neighbours are a plain range +query, and a colour goes out only when a bot's word actually changes. + +Run a controller in simulator mode, then run this script: + + dotbot run controller --conn simulator --headless \\ + --simulator-init-state \\ + dotbot/examples/minimum_naming_game/init_state.toml + python -m dotbot.examples.minimum_naming_game.minimum_naming_game_sdk +""" + +import asyncio +import random +from pathlib import Path + +from dotbot.examples.minimum_naming_game.controller import Controller +from dotbot.swarm import Bot, Swarm + +COMM_RANGE = 250 # mm, a bot can hear neighbours within this radius +RATE_HZ = 20 # naming-game steps per second +MAX_STEPS = 2000 # safety bound if the swarm never fully converges + +# The naming-game supervisor FSM, resolved next to this example (not the cwd). +SCT_PATH = str(Path(__file__).resolve().parent / "models" / "supervisor.yaml") + + +def _online(swarm: Swarm) -> list[Bot]: + return [b for b in swarm if b.is_online and b.position is not None] + + +async def _await_fleet(swarm: Swarm, *, timeout: float = 10.0) -> list[Bot]: + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + bots = _online(swarm) + while not bots and loop.time() < deadline: + await asyncio.sleep(0.2) + bots = _online(swarm) + return bots + + +def _neighbours(bot: Bot, bots: list[Bot]) -> list[Bot]: + return [ + b + for b in bots + if b.address != bot.address + and bot.position.distance_to(b.position) <= COMM_RANGE + ] + + +def _consensus_word(controllers: dict[str, Controller]) -> int | None: + """The single word the whole swarm agrees on, or None if not yet converged + (some bot holds zero or several words, or two bots disagree).""" + words = set() + for c in controllers.values(): + if len(c.inventory) != 1: + return None + words.add(next(iter(c.inventory))) + return next(iter(words)) if len(words) == 1 else None + + +async def minimum_naming_game(swarm: Swarm) -> None: + bots = await _await_fleet(swarm) + if not bots: + print("no active bots") + return + print(f"{len(bots)} bots; playing the minimum naming game ...") + controllers = {b.address: Controller(b.address, SCT_PATH) for b in bots} + last_color: dict[str, tuple[int, int, int]] = {} + + step = 0 + async for _ in swarm.tick(rate_hz=RATE_HZ): + step += 1 + active = [b for b in _online(swarm) if b.address in controllers] + for bot in active: + controller = controllers[bot.address] + controller.position = bot.position + controller.direction = bot.direction + + neighbours = _neighbours(bot, active) + if neighbours: # hear one random neighbour's current word + heard = controllers[random.choice(neighbours).address] + if heard.w_index != 0: + controller.received_word = heard.w_index + controller.new_word_received = True + controller.received_word_checked = False + controller.neighbors = neighbours + controller.control_step() + + color = controller.led + if last_color.get(bot.address) != color: # only send on a change + bot.set_color(color) + last_color[bot.address] = color + + word = _consensus_word(controllers) + if word is not None: + print(f"consensus reached after {step} steps: word {word}") + return + if step % 50 == 0: + settled = sum(len(c.inventory) == 1 for c in controllers.values()) + distinct = { + next(iter(c.inventory)) + for c in controllers.values() + if len(c.inventory) == 1 + } + print( + f" step {step}: {settled}/{len(controllers)} bots hold one word, " + f"{len(distinct)} distinct" + ) + if step >= MAX_STEPS: + print(f"stopped after {step} steps without full consensus") + return + + +if __name__ == "__main__": + Swarm.run(minimum_naming_game) diff --git a/dotbot/examples/minimum_naming_game/minimum_naming_game_with_motion_sdk.py b/dotbot/examples/minimum_naming_game/minimum_naming_game_with_motion_sdk.py new file mode 100644 index 00000000..030db16a --- /dev/null +++ b/dotbot/examples/minimum_naming_game/minimum_naming_game_with_motion_sdk.py @@ -0,0 +1,137 @@ +"""minimum_naming_game_with_motion_sdk.py - naming game + motion on the SDK. + +The dynamic variant of the minimum naming game: the bots wander the arena +(walk straight, avoid each other and the walls) and play the naming game with +whoever is in range. They start out of communication range of each other, so +consensus depends on them mixing as they move. The per-bot agent +(controller_with_motion.py), its walk-and-avoid step (walk_avoid.py), and the +SCT runtime are unchanged domain code. + +This is the SDK rewrite of minimum_naming_game_with_motion.py. The REST/ws +plumbing and scipy KD-tree collapse into the Swarm: each tick a bot advances +its supervisor, gets a walk vector, and streams it with `bot.goto(...)`. The +walk/avoid step still reads `neighbour.lh2_position`, so neighbours are passed +as lightweight wrappers exposing the bot's current Position. + +Run a controller in simulator mode, then run this script: + + dotbot run controller --conn simulator --headless \\ + --simulator-init-state \\ + dotbot/examples/minimum_naming_game/init_state_with_motion.toml + python -m dotbot.examples.minimum_naming_game.minimum_naming_game_with_motion_sdk +""" + +import asyncio +import random +from pathlib import Path +from types import SimpleNamespace + +from dotbot.examples.minimum_naming_game.controller_with_motion import Controller +from dotbot.swarm import Bot, Swarm + +COMM_RANGE = 250 # mm, a bot can hear neighbours within this radius +MAX_SPEED = 300 # mm/s +ARENA = (2000, 2000) # mm, the simulator map the walk-avoid keeps bots inside +RATE_HZ = 10 # control + naming-game steps per second +GOTO_THRESHOLD = 50 # mm, streaming-waypoint tolerance +MAX_STEPS = 4000 # safety bound if the swarm never fully converges + +SCT_PATH = str(Path(__file__).resolve().parent / "models" / "supervisor.yaml") + + +def _online(swarm: Swarm) -> list[Bot]: + return [b for b in swarm if b.is_online and b.position is not None] + + +async def _await_fleet(swarm: Swarm, *, timeout: float = 10.0) -> list[Bot]: + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + bots = _online(swarm) + while not bots and loop.time() < deadline: + await asyncio.sleep(0.2) + bots = _online(swarm) + return bots + + +def _neighbours(bot: Bot, bots: list[Bot]) -> list[SimpleNamespace]: + """Neighbours within COMM_RANGE, wrapped so the domain code can read + `neighbour.address` and `neighbour.lh2_position.x/.y`.""" + return [ + SimpleNamespace(address=b.address, lh2_position=b.position) + for b in bots + if b.address != bot.address + and bot.position.distance_to(b.position) <= COMM_RANGE + ] + + +def _consensus_word(controllers: dict[str, Controller]) -> int | None: + words = set() + for c in controllers.values(): + if len(c.inventory) != 1: + return None + words.add(next(iter(c.inventory))) + return next(iter(words)) if len(words) == 1 else None + + +async def minimum_naming_game(swarm: Swarm) -> None: + bots = await _await_fleet(swarm) + if not bots: + print("no active bots") + return + print(f"{len(bots)} bots; naming game with motion (Ctrl-C to stop) ...") + controllers = { + b.address: Controller(b.address, SCT_PATH, 0.9 * MAX_SPEED, arena_limits=ARENA) + for b in bots + } + last_color: dict[str, tuple[int, int, int]] = {} + + step = 0 + async for _ in swarm.tick(rate_hz=RATE_HZ): + step += 1 + active = [b for b in _online(swarm) if b.address in controllers] + for bot in active: + controller = controllers[bot.address] + controller.update_pose(bot.position) + + neighbours = _neighbours(bot, active) + if neighbours: # hear one random neighbour's current word + heard = controllers[random.choice(neighbours).address] + if heard.w_index != 0: + controller.received_word = heard.w_index + controller.new_word_received = True + controller.received_word_checked = False + controller.neighbors = neighbours + controller.control_step() # naming game + LED + walk vector + + vx, vy = controller.vector + target_x = min(ARENA[0], max(0.0, bot.position.x + vx)) + target_y = min(ARENA[1], max(0.0, bot.position.y + vy)) + bot.goto(target_x, target_y, threshold=GOTO_THRESHOLD) + + color = controller.led + if last_color.get(bot.address) != color: # only send on a change + bot.set_color(color) + last_color[bot.address] = color + + word = _consensus_word(controllers) + if word is not None: + print(f"consensus reached after {step} steps: word {word}") + return + if step % 50 == 0: + settled = sum(len(c.inventory) == 1 for c in controllers.values()) + distinct = { + next(iter(c.inventory)) + for c in controllers.values() + if len(c.inventory) == 1 + } + print( + f" step {step}: {settled}/{len(controllers)} bots hold one word, " + f"{len(distinct)} distinct" + ) + if step >= MAX_STEPS: + print(f"stopped after {step} steps without full consensus") + return + + +if __name__ == "__main__": + Swarm.run(minimum_naming_game) diff --git a/dotbot/examples/sdk/README.md b/dotbot/examples/sdk/README.md new file mode 100644 index 00000000..2ae4846a --- /dev/null +++ b/dotbot/examples/sdk/README.md @@ -0,0 +1,40 @@ +# Swarm SDK examples + +Small, self-contained examples built on the DotBot Swarm SDK +(`from dotbot.swarm import Swarm`). Each is a few lines and produces a clear visual +on the dashboard - good for a first look at the SDK and for demos. + +## Run + +Start a simulator (it opens the web dashboard at ): + +```bash +dotbot run simulator +``` + +Then run any example against it (watch the bots move and change colour on the +dashboard): + +```bash +python dotbot/examples/sdk/square.py +python dotbot/examples/sdk/circle_formation.py +# point at a remote controller instead of localhost: +python dotbot/examples/sdk/square.py --swarm-url http://HOST:8000 +``` + +## The examples + +Single bot: + +- `square.py` - one bot walks a square. +- `shuttle.py` - one bot shuttles between two points, changing colour each leg. + +Whole swarm: + +- `square_formation.py` - the fleet forms a square, one bot per corner. +- `circle_formation.py` - the fleet spreads out evenly onto a circle. +- `rainbow.py` - a rolling colour show across the fleet (no motion). +- `pulse.py` - the fleet "breathes", expanding and contracting on a ring. + +`tour.py` is a short guided tour that combines several SDK features (fleet +colour, live events, concurrent moves). diff --git a/dotbot/examples/sdk/__init__.py b/dotbot/examples/sdk/__init__.py new file mode 100644 index 00000000..308e16db --- /dev/null +++ b/dotbot/examples/sdk/__init__.py @@ -0,0 +1 @@ +"""Simple, visual examples built on the DotBot Swarm SDK.""" diff --git a/dotbot/examples/sdk/circle_formation.py b/dotbot/examples/sdk/circle_formation.py new file mode 100644 index 00000000..47780124 --- /dev/null +++ b/dotbot/examples/sdk/circle_formation.py @@ -0,0 +1,32 @@ +"""The fleet spreads out evenly onto a circle. + +python circle_formation.py [--swarm-url http://localhost:8000] +""" + +import asyncio +import math + +from dotbot.swarm import Swarm + +CENTER_X, CENTER_Y = 1000, 1000 +RADIUS = 600 # mm + + +async def circle_formation(swarm: Swarm) -> None: + bots = sorted(swarm, key=lambda b: b.address) + n = len(bots) + targets = [ + ( + CENTER_X + RADIUS * math.cos(2 * math.pi * i / n), + CENTER_Y + RADIUS * math.sin(2 * math.pi * i / n), + ) + for i in range(n) + ] + swarm.all.set_color("magenta") + print(f"{n} bots forming a circle ...") + await asyncio.gather(*(bot.move_to(x, y) for bot, (x, y) in zip(bots, targets))) + print("circle formed") + + +if __name__ == "__main__": + Swarm.run(circle_formation) diff --git a/dotbot/examples/sdk/pulse.py b/dotbot/examples/sdk/pulse.py new file mode 100644 index 00000000..ea3e1b59 --- /dev/null +++ b/dotbot/examples/sdk/pulse.py @@ -0,0 +1,36 @@ +"""The fleet "breathes" - expanding and contracting on a ring. + +python pulse.py [--swarm-url http://localhost:8000] +""" + +import asyncio +import math + +from dotbot.swarm import Swarm + +CENTER_X, CENTER_Y = 1000, 1000 + + +async def pulse(swarm: Swarm) -> None: + bots = sorted(swarm, key=lambda b: b.address) + n = len(bots) + + def ring(radius: float): + return [ + ( + CENTER_X + radius * math.cos(2 * math.pi * i / n), + CENTER_Y + radius * math.sin(2 * math.pi * i / n), + ) + for i in range(n) + ] + + swarm.all.set_color("cyan") + print(f"{n} bots breathing ...") + for radius in (700, 250, 700, 250): + targets = ring(radius) + await asyncio.gather(*(bot.move_to(x, y) for bot, (x, y) in zip(bots, targets))) + print("done") + + +if __name__ == "__main__": + Swarm.run(pulse) diff --git a/dotbot/examples/sdk/rainbow.py b/dotbot/examples/sdk/rainbow.py new file mode 100644 index 00000000..af3455ae --- /dev/null +++ b/dotbot/examples/sdk/rainbow.py @@ -0,0 +1,25 @@ +"""A rolling colour show across the fleet (no motion). + +python rainbow.py [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.swarm import Swarm + +PALETTE = ["red", "yellow", "green", "cyan", "blue", "magenta"] + + +async def rainbow(swarm: Swarm) -> None: + bots = sorted(swarm, key=lambda b: b.address) + print(f"colour show on {len(bots)} bots ...") + for shift in range(len(PALETTE) * 3): + for i, bot in enumerate(bots): + bot.set_color(PALETTE[(i + shift) % len(PALETTE)]) + await asyncio.sleep(0.4) + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(rainbow) diff --git a/dotbot/examples/sdk/shuttle.py b/dotbot/examples/sdk/shuttle.py new file mode 100644 index 00000000..91b3c9bc --- /dev/null +++ b/dotbot/examples/sdk/shuttle.py @@ -0,0 +1,23 @@ +"""One DotBot shuttles between two points, changing colour each leg. + +python shuttle.py [--swarm-url http://localhost:8000] +""" + +from dotbot.swarm import Swarm + +LEFT = (400, 1000) +RIGHT = (1600, 1000) +COLORS = ["red", "green", "blue", "yellow"] + + +async def shuttle(swarm: Swarm) -> None: + bot = sorted(swarm, key=lambda b: b.address)[0] + print(f"{bot.address[:8]} shuttling back and forth ...") + for i, color in enumerate(COLORS): + bot.set_color(color) + await bot.move_to(*(RIGHT if i % 2 == 0 else LEFT)) + print("done") + + +if __name__ == "__main__": + Swarm.run(shuttle) diff --git a/dotbot/examples/sdk/square.py b/dotbot/examples/sdk/square.py new file mode 100644 index 00000000..d10e4216 --- /dev/null +++ b/dotbot/examples/sdk/square.py @@ -0,0 +1,28 @@ +"""One DotBot walks a square. + +python square.py [--swarm-url http://localhost:8000] +""" + +from dotbot.swarm import Swarm + +CENTER_X, CENTER_Y = 1000, 1000 +HALF = 400 # half the side length, in mm + + +async def walk_square(swarm: Swarm) -> None: + bot = sorted(swarm, key=lambda b: b.address)[0] + bot.set_color("blue") + corners = [ + (CENTER_X - HALF, CENTER_Y - HALF), + (CENTER_X + HALF, CENTER_Y - HALF), + (CENTER_X + HALF, CENTER_Y + HALF), + (CENTER_X - HALF, CENTER_Y + HALF), + (CENTER_X - HALF, CENTER_Y - HALF), # close the loop + ] + print(f"{bot.address[:8]} walking a square ...") + await bot.follow(corners) + print("done") + + +if __name__ == "__main__": + Swarm.run(walk_square) diff --git a/dotbot/examples/sdk/square_formation.py b/dotbot/examples/sdk/square_formation.py new file mode 100644 index 00000000..04bb1838 --- /dev/null +++ b/dotbot/examples/sdk/square_formation.py @@ -0,0 +1,29 @@ +"""The fleet forms a square - one bot per corner. + +python square_formation.py [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.swarm import Swarm + +CENTER_X, CENTER_Y = 1000, 1000 +HALF = 500 # half the side length, in mm + + +async def square_formation(swarm: Swarm) -> None: + bots = sorted(swarm, key=lambda b: b.address) + corners = [ + (CENTER_X - HALF, CENTER_Y - HALF), + (CENTER_X + HALF, CENTER_Y - HALF), + (CENTER_X + HALF, CENTER_Y + HALF), + (CENTER_X - HALF, CENTER_Y + HALF), + ] + swarm.all.set_color("cyan") + print(f"{len(bots)} bots forming a square ...") + await asyncio.gather(*(bot.move_to(x, y) for bot, (x, y) in zip(bots, corners))) + print("square formed") + + +if __name__ == "__main__": + Swarm.run(square_formation) diff --git a/dotbot/examples/sdk/tour.py b/dotbot/examples/sdk/tour.py new file mode 100644 index 00000000..a55f1725 --- /dev/null +++ b/dotbot/examples/sdk/tour.py @@ -0,0 +1,56 @@ +"""sdk_demo.py - a short guided tour of the DotBot Swarm SDK. + +A quick way to see the SDK in action against the simulator. It connects, colours +the whole fleet, prints live events as they happen, then drives two bots to +opposite corners concurrently and waits for both to arrive. + +Run a simulator/controller on :8000 first, then run this. If you started the +simulator with its web UI, open http://localhost:8000 to watch the bots move on +the map while this script drives them. + + python sdk_demo.py [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.swarm import ModeChanged, Swarm + + +async def demo(swarm: Swarm) -> None: + await asyncio.sleep(1.0) # let the first round of state arrive + bots = sorted(swarm, key=lambda b: b.address) + print(f"connected to {len(bots)} bot(s):") + for bot in bots: + print(" ", bot) + if len(bots) < 2: + print("need at least 2 bots for the drive part; colouring only") + + # 1) one call colours the whole fleet (swarm.all is the broadcast/fleet handle) + swarm.all.set_color("cyan") + + # 2) print mode changes as discrete events while we drive + async def watch_events() -> None: + async for event in swarm.events(): + if isinstance(event, ModeChanged): + print(f" event: {event.address[:8]} -> {event.mode.name}") + + watcher = asyncio.create_task(watch_events()) + + # 3) drive two bots to opposite corners, concurrently, and wait for arrival + if len(bots) >= 2: + blue, red = bots[0], bots[1] + blue.set_color("blue") + red.set_color("red") + print( + f"driving {blue.address[:8]} -> (300, 300) and {red.address[:8]} -> (1700, 1700) ..." + ) + await asyncio.gather(blue.move_to(300, 300), red.move_to(1700, 1700)) + print("both arrived:") + print(" ", blue) + print(" ", red) + + watcher.cancel() + + +if __name__ == "__main__": + Swarm.run(demo) diff --git a/dotbot/examples/sdk_demo/README.md b/dotbot/examples/sdk_demo/README.md new file mode 100644 index 00000000..6a5ba1ea --- /dev/null +++ b/dotbot/examples/sdk_demo/README.md @@ -0,0 +1,81 @@ +# sdk_demo - simple swarm demos + +A small set of self-contained demos built on the DotBot Swarm SDK. Each one is +deliberately simple, reads from the *live* fleet (no hardcoded arena), and is +meant to be tested in the simulator first and then run unchanged on the real +testbed. + +## Run + +Start a controller with a simulated fleet in one terminal: + +```bash +dotbot run controller --conn simulator --dotbot --bots 67 --layout random -m 2500x2500 --headless +``` + +Open the dashboard at http://localhost:8000, then run any demo in another +terminal: + +```bash +python -m dotbot.examples.sdk_demo.led_ripple +``` + +Point a demo at a different controller with `--swarm-url`: + +```bash +python -m dotbot.examples.sdk_demo.led_ripple --swarm-url http://192.168.1.50:8000 +``` + +Every demo stops cleanly on Ctrl-C (motors off, LEDs off). + +## The demos + +LED demos move nothing, so they are collision-free and the safest to run on +real hardware: + +| Demo | What it does | +|------|--------------| +| `led_ripple` | a ring of light ripples out from the swarm centre (set `OUTWARD = False` for edge-first) | +| `led_sweep` | a rainbow gradient sweeps across the field, left to right | + +Motion demos - test in the simulator first, then run on hardware with small +amplitudes: + +| Demo | What it does | Collision risk | +|------|--------------|----------------| +| `wiggle` | every bot rocks side to side in place while a rainbow rolls across the fleet (`--loop` to repeat with a pause) | low (turns in place) | +| `spin` | every bot spins in place, then stops | low (turns in place) | +| `tiny_circle` | each bot traces a small circle around its start | low-medium (small swept disc) | +| `ripple_wiggle` | a wiggle wave: the centre ring rocks in place first, then each ring outward, lighting up as it goes | low (turns in place) | +| `march` | the whole fleet translates as a block: right, up, left, back | medium (formation moves; mind the walls) | +| `disperse` | spreads a clustered fleet outward edge-first into an even disc, paths never crossing (`--loop` to re-gather then re-bloom) | low - the bloom is collision-free (the `--loop` re-gather crowds) | +| `distribute` | spreads a clustered fleet into an even lattice across the arena (`--loop` to cluster then re-spread) | medium-high (bots cross the arena; clustering piles them up) | +| `swarm_rotate` | the whole fleet rotates about its centroid | high (outer bots cross inner paths) | + +Each demo has a few constants at the top (speed, radius, step, angle) - tune +those to your arena. `distribute` and `disperse` have a `MAP_SIZE` to match the +controller's `-m x`. For spreading a clustered fleet, prefer `disperse` on +real hardware (collision-free) and `distribute` for the tidier grid in sim. + +## Before running on the real testbed + +1. **Test one bot first.** Drive a single bot's LED and a single bot's motion + before unleashing the fleet, so you confirm the link end to end. +2. **Lead with the LED demos.** They never move a wheel - zero collision risk, + and they read beautifully on a projector. +3. **Start small on motion.** Shrink `STEP` / `RADIUS` / `TOTAL_ANGLE` to match + your real spacing, then grow them. `swarm_rotate` is the riskiest - keep the + angle small or keep it to the simulator. +4. **Know your stop.** Ctrl-C ends a demo and sends a fleet stop. Keep a hand on + it. +5. **Mind the downlink budget.** The LED demos update only the bots that change + each step. `spin` re-sends to the whole fleet on a timer; if it stutters with + 67 real bots, raise `RESEND` or run it on a subset. + +## Notes + +- All geometry (centre, rings, bearings) is computed from live positions, so the + demos work the same in the simulator and on the testbed, whatever the arena + size. +- Motion uses `move_to` / `follow` (the primary waypoint primitives); `spin` + uses `move_raw` because an in-place turn has no target position. diff --git a/dotbot/examples/sdk_demo/__init__.py b/dotbot/examples/sdk_demo/__init__.py new file mode 100644 index 00000000..a5ead6db --- /dev/null +++ b/dotbot/examples/sdk_demo/__init__.py @@ -0,0 +1,3 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/dotbot/examples/sdk_demo/_lib.py b/dotbot/examples/sdk_demo/_lib.py new file mode 100644 index 00000000..8f72181e --- /dev/null +++ b/dotbot/examples/sdk_demo/_lib.py @@ -0,0 +1,242 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared helpers for the sdk_demo set. + +Every geometric quantity (centre, rings, bearings) is computed from the *live* +fleet positions, never hardcoded, so the same demo runs unchanged in the +simulator and on the real testbed regardless of arena size or where the bots +happen to sit. +""" + +from __future__ import annotations + +import asyncio +import colorsys +import math + +from dotbot.swarm import Position, Swarm, avoid + + +async def settle(swarm: Swarm, seconds: float = 1.5) -> list: + """Wait for the first ws/status round to arrive, then return the bots that + have a position fix, sorted by address (stable order across runs).""" + await asyncio.sleep(seconds) + bots = positioned(swarm) + print(f"{len(bots)}/{len(swarm)} bots have a position fix") + return bots + + +def positioned(swarm: Swarm) -> list: + """The bots that currently have an LH2 fix, ordered by address.""" + return sorted((b for b in swarm if b.position is not None), key=lambda b: b.address) + + +def centroid(bots: list) -> Position: + """The mean position of the fleet - its live centre.""" + n = len(bots) + return Position( + sum(b.position.x for b in bots) / n, + sum(b.position.y for b in bots) / n, + ) + + +def max_radius(bots: list, center: Position) -> float: + return max(b.position.distance_to(center) for b in bots) + + +def make_rings(bots: list, center: Position, n_rings: int) -> list: + """Bucket bots into `n_rings` concentric rings by distance from `center` + (ring 0 = innermost, ring n-1 = outermost edge).""" + r_max = max_radius(bots, center) or 1.0 + rings: list = [[] for _ in range(n_rings)] + for b in bots: + frac = b.position.distance_to(center) / r_max + rings[min(int(frac * n_rings), n_rings - 1)].append(b) + return rings + + +def angle_deg(bot, center: Position) -> float: + """Bearing of a bot from `center`, in degrees.""" + return math.degrees( + math.atan2(bot.position.y - center.y, bot.position.x - center.x) + ) + + +def rotate(p: Position, center: Position, deg: float) -> Position: + """Rotate point `p` about `center` by `deg` degrees (counter-clockwise).""" + rad = math.radians(deg) + dx, dy = p.x - center.x, p.y - center.y + return Position( + center.x + dx * math.cos(rad) - dy * math.sin(rad), + center.y + dx * math.sin(rad) + dy * math.cos(rad), + ) + + +def hsv(h: float, s: float = 1.0, v: float = 1.0) -> tuple: + """HSV (h wrapped into [0, 1)) -> (r, g, b) ints 0..255 for set_color().""" + r, g, b = colorsys.hsv_to_rgb(h % 1.0, s, v) + return int(r * 255), int(g * 255), int(b * 255) + + +# ---- collision-aware driving (buffered Voronoi cells) ----------------------- +# +# Targets alone don't avoid collisions: a bot drives a straight-ish arc to its +# waypoint regardless of who is in the way. The geometry that fixes this lives +# in `dotbot.swarm.avoid` (the SDK's composable low-level rung; the built-in +# counterpart is `Swarm.connect(..., collision_avoidance=True)`). These demos +# drive it by hand through the drive() loop below, doubling as reference code +# for anyone writing their own control loop. + +SAFE_RADIUS = avoid.DEFAULT_SAFE_RADIUS # mm: half the minimum separation +WALL_MARGIN = avoid.DEFAULT_WALL_MARGIN # mm: waypoints stay off the walls +MAX_STEP = avoid.DEFAULT_MAX_STEP # mm: longest hop commanded per tick +ARRIVE = 120.0 # mm: a bot this close to its goal is done +DRIVE_TICK = 1.0 # s between waypoint updates (~1 cmd/s/bot link budget) +SIDESTEP = 200.0 # mm: detour length when stuck (right-hand rule) +PLAN_BUDGET_HZ = 60.0 # cmd/s a drive loop may consume (Mari gateway does ~80) + + +def pace_tick(n_bots: int, base: float = DRIVE_TICK) -> float: + """The tick that keeps `n_bots` one-command-per-tick loops inside the + gateway downlink budget: at 16 bots the base tick stands; at 100+ bots + the loop slows down instead of flooding the link.""" + return max(base, n_bots / PLAN_BUDGET_HZ) + + +MIN_HOP_THRESHOLD = 60 # mm: don't chase precision below the LH2 noise floor + + +def hop_goto(bot, wp: tuple, px: float, py: float) -> None: + """Send a shepherd hop, working around the firmware arrival rule: a + waypoint within the threshold is "already reached" and moves nothing, so + short hops are sent with a scaled-down threshold - but never below the + LH2 noise floor, where a real bot circles a target it can't resolve.""" + hop = math.hypot(wp[0] - px, wp[1] - py) + if hop < MIN_HOP_THRESHOLD: + return + threshold = 100 if hop >= 250 else max(MIN_HOP_THRESHOLD, int(hop * 0.5)) + bot.goto(*wp, threshold=threshold) + + +def safe_hop( + bot, + positions: dict, + goal: tuple, + arena: tuple, + *, + safe_radius: float = SAFE_RADIUS, + yield_ok: bool = True, +) -> tuple: + """`dotbot.swarm.avoid.safe_hop` with the heading read off the live Bot.""" + return avoid.safe_hop( + bot.address, + positions, + goal, + arena, + heading=getattr(bot, "direction", None), + yield_ok=yield_ok, + safe_radius=safe_radius, + ) + + +async def drive( + bots: list, + goals: dict, + arena: tuple, + *, + arrive: float = ARRIVE, + timeout: float = 45.0, + safe_radius: float = SAFE_RADIUS, + tick: float = DRIVE_TICK, +) -> set: + """Shepherd every bot to its goal collision-free and return the addresses + that arrived. Each tick, every unarrived bot is sent the safe waypoint + toward its goal (BVC projection); a bot that stops making progress while + blocked detours to its right for a tick (the standard BVC deadlock + heuristic). Gives up on stragglers after `timeout` rather than hanging.""" + pending = {b.address for b in bots if b.address in goals} + last_pos: dict = {} + stuck: dict = {} + contact: dict = {} + tick = pace_tick(len(pending), tick) + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + while pending and loop.time() < deadline: + positions = { + b.address: (b.position.x, b.position.y) for b in bots if b.position + } + for b in bots: + a = b.address + if a not in pending or a not in positions: + continue + if not b.is_online: + continue # crashed/lost: stop sending, keep it as an obstacle + px, py = positions[a] + gx, gy = goals[a] + if math.hypot(gx - px, gy - py) <= arrive: + pending.discard(a) + continue + # Contact guard: pinned against a neighbour -> stop, don't grind. + nearest = min( + ( + math.hypot(qx - px, qy - py) + for o, (qx, qy) in positions.items() + if o != a + ), + default=float("inf"), + ) + if nearest < 130: + contact[a] = contact.get(a, 0) + 1 + if contact[a] >= 3: + print(f" contact stop {a[-4:]} (neighbour at {nearest:.0f} mm)") + pending.discard(a) + b.stop() + continue + else: + contact.pop(a, None) + if ( + a in last_pos + and math.hypot(px - last_pos[a][0], py - last_pos[a][1]) < 25 + ): + stuck[a] = stuck.get(a, 0) + 1 + else: + stuck[a] = 0 + goal = (gx, gy) + patience = stuck.get(a, 0) + if patience >= 3: # blocked: detour to the right of the goal line + d = math.hypot(gx - px, gy - py) or 1.0 + ux, uy = (gx - px) / d, (gy - py) / d + goal = (px + uy * SIDESTEP, py - ux * SIDESTEP) + stuck[a] = 0 + wp = safe_hop( + b, + positions, + goal, + arena, + safe_radius=safe_radius, + yield_ok=patience < 2, + ) + hop_goto(b, wp, px, py) + last_pos[a] = (px, py) + await asyncio.sleep(tick) + if pending: + positions = { + b.address: (b.position.x, b.position.y) for b in bots if b.position + } + by_addr = {b.address: b for b in bots} + for a in sorted(pending): + if a in positions and a in goals: + px, py = positions[a] + d = math.hypot(goals[a][0] - px, goals[a][1] - py) + wp = safe_hop( + by_addr[a], positions, goals[a], arena, safe_radius=safe_radius + ) + hop = math.hypot(wp[0] - px, wp[1] - py) + direction = getattr(by_addr[a], "direction", None) + print( + f" straggler {a[-4:]}: {d:.0f} mm from goal " + f"(hop {hop:.0f} mm, heading {direction})" + ) + return {b.address for b in bots if b.address in goals} - pending diff --git a/dotbot/examples/sdk_demo/disperse.py b/dotbot/examples/sdk_demo/disperse.py new file mode 100644 index 00000000..d11c0322 --- /dev/null +++ b/dotbot/examples/sdk_demo/disperse.py @@ -0,0 +1,107 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Spread the swarm out by mutual repulsion - bots push apart, not through each +other. + +Each round, every bot wants a small step directly *away* from whoever is +crowding it (near bots weigh most), and the step shrinks each round so the +fleet settles into an even spread. Every step is issued through the buffered +Voronoi cell projection from _lib, so even bots that start overlapping (or get +boxed in mid-bloom) separate cleanly instead of arcing through a neighbour - +the repulsion field picks the direction, the BVC guarantees the safety. + +It works from any start - a tight clump blooms outward, an already-spread fleet +just evens out its spacing - and reads the arena size from the controller, so +it matches whatever `-m x` you launched with. With --loop it re-gathers +the fleet into a tight (but touch-free) block and disperses again. + + python -m dotbot.examples.sdk_demo.disperse [--loop] [--swarm-url http://localhost:8000] +""" + +import argparse +import asyncio +import math + +from dotbot.examples.sdk_demo._lib import drive, hop_goto, pace_tick, safe_hop, settle +from dotbot.swarm import Swarm + +ROUNDS = 22 # repulsion iterations +STEP0 = 160 # mm: desired step on the first round (anneals to ~0) +TICK = 1.0 # s per round (paces the ~2 Hz position refresh and the link budget) +GATHER_PITCH = 380 # mm grid pitch of the --loop re-gather block + + +def _repulsion(me: str, positions: dict, step: float) -> tuple: + """Where `me` wants to go this round: `step` mm away from the net 1/d^2 + pull of its neighbours (near bots dominate).""" + px, py = positions[me] + fx = fy = 0.0 + for other, (qx, qy) in positions.items(): + if other == me: + continue + dx, dy = px - qx, py - qy + d2 = max(dx * dx + dy * dy, 1.0) + fx += dx / d2 + fy += dy / d2 + mag = math.hypot(fx, fy) + if mag < 1e-9: + return (px, py) + return (px + fx / mag * step, py + fy / mag * step) + + +async def _disperse(bots: list, arena: tuple) -> None: + for r in range(ROUNDS): + step = STEP0 * (1 - r / ROUNDS) # anneal so the fleet settles + positions = { + b.address: (b.position.x, b.position.y) for b in bots if b.position + } + for b in bots: + if b.address not in positions: + continue + want = _repulsion(b.address, positions, step) + px, py = positions[b.address] + hop_goto(b, safe_hop(b, positions, want, arena), px, py) + await asyncio.sleep(pace_tick(len(bots), TICK)) + + +async def disperse(swarm: Swarm) -> None: + parser = argparse.ArgumentParser(add_help=False) + parser.add_argument( + "--loop", action="store_true", help="re-gather then disperse again" + ) + opts, _ = parser.parse_known_args() + + bots = await settle(swarm) + if not bots: + return + w, h = await swarm.map_size() + print(f"arena {w}x{h} mm; dispersing {len(bots)} bots ...") + + try: + while True: + swarm.all.set_color("cyan") + await _disperse(bots, (w, h)) + await asyncio.sleep(1.5) + if not opts.loop: + break + print("re-gathering to the centre ...") + swarm.all.set_color("red") + cols = max(1, round(math.sqrt(len(bots)))) + goals = { + b.address: ( + w / 2 + (k % cols - (cols - 1) / 2) * GATHER_PITCH, + h / 2 + (k // cols - (cols - 1) / 2) * GATHER_PITCH, + ) + for k, b in enumerate(bots) + } + await drive(bots, goals, (w, h)) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(disperse) diff --git a/dotbot/examples/sdk_demo/distribute.py b/dotbot/examples/sdk_demo/distribute.py new file mode 100644 index 00000000..606108df --- /dev/null +++ b/dotbot/examples/sdk_demo/distribute.py @@ -0,0 +1,144 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Re-arrange the swarm into an even distribution across the arena. + +The algorithm: lay down N evenly spaced target points over the arena, assign +each bot to the nearest still-free target (greedy - keeps total travel short), +then shepherd everyone there collision-free with the shared BVC drive() helper: +each bot only ever steps inside its own buffered Voronoi cell, so paths that +would cross simply flow around each other, like real robots have to. + +With --loop it gathers the fleet into a compact block (spaced so the bots still +fit without touching) and then re-distributes, over and over. The arena size is +read from the controller, so it matches whatever `-m x` you launched +with. + + python -m dotbot.examples.sdk_demo.distribute [--loop] [--swarm-url http://localhost:8000] +""" + +import argparse +import asyncio +import math +import random + +from dotbot.examples.sdk_demo._lib import SAFE_RADIUS, WALL_MARGIN, drive, settle +from dotbot.swarm import Swarm + +JITTER_MM = 40 # randomise targets a touch so the lattice looks organic +HOLD = 2.5 # s to hold each arrangement +GATHER_PITCH = 2.4 * SAFE_RADIUS # grid pitch of the --loop gather block + + +def even_targets(n: int, w: float, h: float) -> list: + """n points on an even, lightly jittered lattice filling the arena.""" + lo_x, hi_x = WALL_MARGIN, w - WALL_MARGIN + lo_y, hi_y = WALL_MARGIN, h - WALL_MARGIN + rows = max(1, round(math.sqrt(n))) + pts = [] + for r in range(rows): + in_row = n // rows + (1 if r < n % rows else 0) + y = lo_y + (r + 0.5) * (hi_y - lo_y) / rows + for c in range(in_row): + x = lo_x + (c + 0.5) * (hi_x - lo_x) / in_row + pts.append( + ( + min(max(x + random.uniform(-JITTER_MM, JITTER_MM), lo_x), hi_x), + min(max(y + random.uniform(-JITTER_MM, JITTER_MM), lo_y), hi_y), + ) + ) + return pts + + +def assign(bots: list, targets: list) -> dict: + """Assign each bot a target so that routes do not cross: greedy nearest + first, then 2-opt swaps minimising total *squared* distance - the squared + metric is what makes straight-line routes provably non-crossing, which is + most of the collision-avoidance battle won before anyone moves.""" + pairs = sorted( + (bots[bi].position.distance_to(targets[ti]), bi, ti) + for bi in range(len(bots)) + for ti in range(len(targets)) + ) + bot_seen: set = set() + tgt_seen: set = set() + out: dict = {} + for _, bi, ti in pairs: + if bi in bot_seen or ti in tgt_seen: + continue + out[bots[bi].address] = targets[ti] + bot_seen.add(bi) + tgt_seen.add(ti) + + pos = {b.address: (b.position.x, b.position.y) for b in bots} + + def d2(a: str, t: tuple) -> float: + return (pos[a][0] - t[0]) ** 2 + (pos[a][1] - t[1]) ** 2 + + addrs = list(out) + improved = True + while improved: + improved = False + for i, a in enumerate(addrs): + for b in addrs[i + 1 :]: + if d2(a, out[b]) + d2(b, out[a]) < d2(a, out[a]) + d2(b, out[b]) - 1e-6: + out[a], out[b] = out[b], out[a] + improved = True + return out + + +def gather_targets(bots: list, w: float, h: float) -> list: + """A compact grid block at a random spot, spaced so the bots fit without + touching (pitch > 2*SAFE_RADIUS).""" + cols = max(1, round(math.sqrt(len(bots)))) + rows_n = math.ceil(len(bots) / cols) + half_w = (cols - 1) / 2 * GATHER_PITCH + half_h = (rows_n - 1) / 2 * GATHER_PITCH + cx = random.uniform(WALL_MARGIN + half_w + 1, w - WALL_MARGIN - half_w - 1) + cy = random.uniform(WALL_MARGIN + half_h + 1, h - WALL_MARGIN - half_h - 1) + return [ + ( + cx + (k % cols) * GATHER_PITCH - half_w, + cy + (k // cols) * GATHER_PITCH - half_h, + ) + for k in range(len(bots)) + ] + + +async def distribute(swarm: Swarm) -> None: + parser = argparse.ArgumentParser(add_help=False) + parser.add_argument( + "--loop", action="store_true", help="gather then re-distribute, repeatedly" + ) + opts, _ = parser.parse_known_args() + + bots = await settle(swarm) + if not bots: + return + w, h = await swarm.map_size() + print(f"arena {w}x{h} mm; distributing {len(bots)} bots ...") + + try: + while True: + print("distributing ...") + swarm.all.set_color("green") + arrived = await drive( + bots, assign(bots, even_targets(len(bots), w, h)), (w, h) + ) + print(f"{len(arrived)}/{len(bots)} arrived") + await asyncio.sleep(HOLD) + if not opts.loop: + break + print("gathering ...") + swarm.all.set_color("red") + await drive(bots, assign(bots, gather_targets(bots, w, h)), (w, h)) + await asyncio.sleep(HOLD) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(distribute) diff --git a/dotbot/examples/sdk_demo/led_ripple.py b/dotbot/examples/sdk_demo/led_ripple.py new file mode 100644 index 00000000..7ede6362 --- /dev/null +++ b/dotbot/examples/sdk_demo/led_ripple.py @@ -0,0 +1,63 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A ring of colour that ripples out from the centre of the swarm. + +Pure LED, no motion - collision-free and the safest demo to run on the real +testbed. Bots are bucketed into concentric rings by distance from the live +swarm centroid, then a single bright ring travels outward over a dark field, +over and over, shifting hue each pass. + +Only the two rings that change each step are re-sent, so the fleet is never +re-coloured all at once (kind to the downlink budget on real hardware). + +Set OUTWARD = False to ripple inward instead (edge first, then inner rings). + + python -m dotbot.examples.sdk_demo.led_ripple [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.examples.sdk_demo._lib import centroid, hsv, make_rings, settle +from dotbot.swarm import Swarm + +N_RINGS = 7 +STEP_DELAY = 0.18 # seconds between successive rings (the wave speed) +CYCLE_PAUSE = 0.5 # seconds of dark between waves +HUE_SHIFT = 0.15 # colour advance per pass +OUTWARD = True # True: centre -> edge; False: edge -> centre + + +async def led_ripple(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + rings = make_rings(bots, centroid(bots), N_RINGS) + if not OUTWARD: + rings = list(reversed(rings)) + + print(f"rippling a ring of light through {N_RINGS} rings ... (Ctrl-C to stop)") + hue = 0.0 + try: + while True: + prev = None + for ring in rings: + color = hsv(hue) + for b in ring: + b.set_color(color) + if prev is not None: + for b in prev: + b.set_color("off") + prev = ring + await asyncio.sleep(STEP_DELAY) + for b in prev: # extinguish the last ring before the next wave + b.set_color("off") + hue = (hue + HUE_SHIFT) % 1.0 + await asyncio.sleep(CYCLE_PAUSE) + finally: + swarm.all.set_color("off") + + +if __name__ == "__main__": + Swarm.run(led_ripple) diff --git a/dotbot/examples/sdk_demo/led_sweep.py b/dotbot/examples/sdk_demo/led_sweep.py new file mode 100644 index 00000000..dc509f2f --- /dev/null +++ b/dotbot/examples/sdk_demo/led_sweep.py @@ -0,0 +1,57 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A rainbow gradient that sweeps across the swarm, left to right. + +Pure LED, collision-free. Each bot's hue is set by its position along the sweep +axis; the phase advances over time so the whole rainbow travels across the +physical field. A bot is only re-coloured when its hue *bucket* changes, so the +fleet is not re-sent every frame - the demo stays inside the downlink budget on +real hardware. + +Set AXIS = "y" to sweep bottom-to-top instead. + + python -m dotbot.examples.sdk_demo.led_sweep [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.examples.sdk_demo._lib import hsv, settle +from dotbot.swarm import Swarm + +AXIS = "x" # "x": sweep left->right, "y": sweep bottom->top +WAVELENGTHS = 1.5 # how many full rainbows span the field +SPEED = 0.03 # hue phase advanced per frame +FRAME = 0.1 # seconds per frame +BUCKETS = 18 # hue quantisation (fewer = coarser, less traffic) + + +async def led_sweep(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + + coord = {b.address: (b.position.x if AXIS == "x" else b.position.y) for b in bots} + lo, hi = min(coord.values()), max(coord.values()) + span = (hi - lo) or 1.0 + base = {a: ((c - lo) / span) * WAVELENGTHS for a, c in coord.items()} + + print(f"sweeping a rainbow across {len(bots)} bots ... (Ctrl-C to stop)") + last_bucket: dict = {} + phase = 0.0 + try: + while True: + for b in bots: + bucket = int(((base[b.address] + phase) % 1.0) * BUCKETS) + if last_bucket.get(b.address) != bucket: + last_bucket[b.address] = bucket + b.set_color(hsv(bucket / BUCKETS)) + phase = (phase + SPEED) % 1.0 + await asyncio.sleep(FRAME) + finally: + swarm.all.set_color("off") + + +if __name__ == "__main__": + Swarm.run(led_sweep) diff --git a/dotbot/examples/sdk_demo/march.py b/dotbot/examples/sdk_demo/march.py new file mode 100644 index 00000000..44f0c88f --- /dev/null +++ b/dotbot/examples/sdk_demo/march.py @@ -0,0 +1,57 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The whole swarm marches together - right, up, left, down - back to start. + +Every bot gets the same offset from its own home, so the formation translates +as a rigid block and relative spacing is preserved. Targets are taken from each +bot's home snapshot (not its live position), so the path closes exactly instead +of drifting. The step is shrunk automatically if the outermost bots would hit a +wall, and each leg is driven through the shared BVC drive() helper so a bot +that lags a leg never gets run over by a neighbour starting the next one. + + python -m dotbot.examples.sdk_demo.march [--swarm-url http://localhost:8000] +""" + +from dotbot.examples.sdk_demo._lib import WALL_MARGIN, drive, settle +from dotbot.swarm import Swarm + +STEP = 250 # mm per leg (shrunk automatically to fit the arena) + +# Offsets from home, in order: right, up, left, back to start. +LEGS = [(1, 0), (1, 1), (0, 1), (0, 0)] + + +async def march(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + w, h = await swarm.map_size() + home = {b.address: b.position for b in bots} + + # The block shifts right then up by STEP: shrink STEP if the bots nearest + # the right/top walls would be pushed past the margin. + room_x = min(w - WALL_MARGIN - p.x for p in home.values()) + room_y = min(h - WALL_MARGIN - p.y for p in home.values()) + step = max(0.0, min(STEP, room_x, room_y)) + if step < STEP: + print(f"shrinking step to {step:.0f} mm to stay inside the arena") + if step < 50: + print("no room to march; aborting") + return + + print(f"marching {len(bots)} bots as one block ...") + swarm.all.set_color("yellow") + try: + for ox, oy in LEGS: + goals = {a: (p.x + ox * step, p.y + oy * step) for a, p in home.items()} + await drive(bots, goals, (w, h), timeout=30.0) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(march) diff --git a/dotbot/examples/sdk_demo/ripple_wiggle.py b/dotbot/examples/sdk_demo/ripple_wiggle.py new file mode 100644 index 00000000..c75d898d --- /dev/null +++ b/dotbot/examples/sdk_demo/ripple_wiggle.py @@ -0,0 +1,70 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A wiggle that ripples out from the centre of the swarm, ring by ring. + +Like `wiggle`, every bot only rocks in place - it rotates back and forth and +never leaves its spot, so this is collision-safe on real hardware. But the +motion is localised by position: the centre ring wiggles first, then the next +ring out, then the next, so a wave of wiggling travels from the middle to the +edge. Each ring lights up as it wiggles, building an expanding rainbow; at the +rim the fleet fades and the wave repeats from the centre. + +One sweep takes about N_RINGS * BEATS_PER_RING * BEAT seconds (~10 s by default). + + python -m dotbot.examples.sdk_demo.ripple_wiggle [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.examples.sdk_demo._lib import centroid, hsv, make_rings, settle +from dotbot.swarm import Swarm + +N_RINGS = 7 +SPEED = 70 # wheel PWM magnitude of each twist, 0..100 +BEATS_PER_RING = 4 # twists a ring does while the wave dwells on it +BEAT = 0.35 # seconds per twist +RING_HUE_STEP = 0.13 # colour shift from one ring to the next +CYCLE_PAUSE = 0.8 # seconds of stillness between sweeps + + +async def _wiggle_ring(ring: list, color: tuple) -> None: + """Light a ring and rock it in place for BEATS_PER_RING twists, then stop - + the bots rotate back and forth without leaving their spot.""" + for bot in ring: + bot.set_color(color) + direction = 1 + for _ in range(BEATS_PER_RING): + for bot in ring: + bot.move_raw(left=(0, direction * SPEED), right=(0, -direction * SPEED)) + direction = -direction + await asyncio.sleep(BEAT) + for bot in ring: + bot.stop() + + +async def ripple_wiggle(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + rings = [r for r in make_rings(bots, centroid(bots), N_RINGS) if r] + + print(f"wiggling outward through {len(rings)} rings ... (Ctrl-C to stop)") + base = 0.0 + try: + while True: + hue = base + for ring in rings: # centre -> edge: one ring wiggles at a time + await _wiggle_ring(ring, hsv(hue)) + hue += RING_HUE_STEP + await asyncio.sleep(CYCLE_PAUSE) + swarm.all.set_color("off") + base = (base + 0.07) % 1.0 + finally: + swarm.all.stop() + swarm.all.set_color("off") + + +if __name__ == "__main__": + Swarm.run(ripple_wiggle) diff --git a/dotbot/examples/sdk_demo/spin.py b/dotbot/examples/sdk_demo/spin.py new file mode 100644 index 00000000..cbc7141b --- /dev/null +++ b/dotbot/examples/sdk_demo/spin.py @@ -0,0 +1,42 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Every bot spins in place for a few seconds, then stops. + +A motion demo, but each bot turns about its own axis so its *position* barely +moves - low collision risk and a good first motion test on real hardware. The +two wheels are driven in opposite directions via move_raw, and the command is +re-sent periodically so a real bot's command timeout never stalls the spin. + + python -m dotbot.examples.sdk_demo.spin [--swarm-url http://localhost:8000] +""" + +import asyncio + +from dotbot.examples.sdk_demo._lib import settle +from dotbot.swarm import Swarm + +SPEED = 70 # wheel PWM magnitude, 0..100 +DURATION = 6.0 # seconds of spinning +RESEND = 0.5 # seconds between move_raw refreshes (command-timeout safe) + + +async def spin(swarm: Swarm) -> None: + await settle(swarm) + print(f"spinning {len(swarm)} bots in place for {DURATION:.0f}s ...") + swarm.all.set_color("magenta") + loop = asyncio.get_running_loop() + deadline = loop.time() + DURATION + try: + while loop.time() < deadline: + swarm.all.move_raw(left=(0, SPEED), right=(0, -SPEED)) + await asyncio.sleep(RESEND) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(spin) diff --git a/dotbot/examples/sdk_demo/swarm_rotate.py b/dotbot/examples/sdk_demo/swarm_rotate.py new file mode 100644 index 00000000..6769153b --- /dev/null +++ b/dotbot/examples/sdk_demo/swarm_rotate.py @@ -0,0 +1,138 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The whole swarm rotates as one formation about its centroid. + +The showpiece. Three things make it behave like real robots instead of ghosts: + + 1. if the live formation cannot rotate safely - some pair tighter than the + BVC safety floor, or the sweep would leave the arena - the fleet first + re-forms into a disc that keeps every bot's bearing from the centre + (so it still "looks like" the fleet) but spaces everyone out and fits + the walls; + 2. the rotation is a *servo*, not a sequence of jumps: every tick each bot + is steered (through the BVC safety projection) at the formation rotated + by the current angle, and the angle only advances while the slowest bot + is keeping up - the formation turns as fast as its laggard allows; + 3. every waypoint goes through safe_hop, so even a bot that has to U-turn + arcs inside a widened buffer instead of into a neighbour. + +Each bot is coloured by its bearing so the rotating "spokes" are easy to see. + + python -m dotbot.examples.sdk_demo.swarm_rotate [--swarm-url http://localhost:8000] +""" + +import asyncio +import math + +from dotbot.examples.sdk_demo._lib import ( + ARRIVE, + SAFE_RADIUS, + WALL_MARGIN, + angle_deg, + centroid, + drive, + hop_goto, + hsv, + max_radius, + pace_tick, + rotate, + safe_hop, + settle, +) +from dotbot.swarm import Position, Swarm + +TOTAL_ANGLE = 90 # degrees to rotate the whole formation +DEG_TICK = 6 # degrees the formation advances per tick when nobody lags +LAG_HOLD = 280 # mm: a bot this far behind its slot pauses the rotation +SETTLE_SECS = 15.0 # s budget for the final convergence on the end pose +FLOOR_GAP = 2 * SAFE_RADIUS + 60 # spacing every pair needs, plus slack + + +def _disc(bots: list, center, radius: float) -> dict: + """A bearing-preserving disc: each bot keeps its angle from the centre and + is placed at a radius set by its rank, spacing the fleet evenly.""" + ranked = sorted(bots, key=lambda b: b.position.distance_to(center)) + n = len(bots) + out = {} + for k, b in enumerate(ranked): + ang = math.atan2(b.position.y - center.y, b.position.x - center.x) + r = radius * math.sqrt((k + 0.5) / n) + out[b.address] = Position( + center.x + r * math.cos(ang), center.y + r * math.sin(ang) + ) + return out + + +async def swarm_rotate(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + w, h = await swarm.map_size() + center = centroid(bots) + home = {b.address: b.position for b in bots} + + r_fit = min(center.x, w - center.x, center.y, h - center.y) - WALL_MARGIN + r_max = max_radius(bots, center) or 1.0 + min_gap = min( + a.position.distance_to(b.position) + for i, a in enumerate(bots) + for b in bots[i + 1 :] + ) + if min_gap < FLOOR_GAP or r_max > r_fit: + print("re-forming into a safe disc before rotating ...") + # Size the disc from the spacing requirement (mean nearest-neighbour + # distance in a disc of n bots is ~R*sqrt(pi/n)), capped by the walls. + r_disc = min(r_fit, FLOOR_GAP * math.sqrt(len(bots) / math.pi) * 1.2) + home = _disc(bots, center, r_disc) + await drive(bots, {a: (p.x, p.y) for a, p in home.items()}, (w, h)) + + for b in bots: # colour by bearing so the rotation is legible + b.set_color(hsv((angle_deg(b, center) % 360) / 360)) + + print(f"rotating {len(bots)} bots {TOTAL_ANGLE} deg about the centroid ...") + tick = pace_tick(len(bots)) + max_ticks = int(4 * TOTAL_ANGLE / DEG_TICK) + 30 # stalls must not hang us + angle = 0.0 + held = 0 + try: + for _ in range(max_ticks): + if angle >= TOTAL_ANGLE: + break + positions = { + b.address: (b.position.x, b.position.y) for b in bots if b.position + } + targets = {a: rotate(p, center, angle) for a, p in home.items()} + lag = max( + math.hypot(t.x - positions[a][0], t.y - positions[a][1]) + for a, t in targets.items() + if a in positions + ) + # Advance when the fleet keeps up - or after a few held ticks, so a + # single boxed-in laggard slows the show instead of freezing it. + if lag < LAG_HOLD or held >= 4: + angle = min(angle + DEG_TICK, TOTAL_ANGLE) + targets = {a: rotate(p, center, angle) for a, p in home.items()} + held = 0 + else: + held += 1 + for b in bots: + if b.address in positions: + t = targets[b.address] + px, py = positions[b.address] + hop_goto(b, safe_hop(b, positions, (t.x, t.y), (w, h)), px, py) + await asyncio.sleep(tick) + goals = { + a: (rotate(p, center, TOTAL_ANGLE).x, rotate(p, center, TOTAL_ANGLE).y) + for a, p in home.items() + } + arrived = await drive(bots, goals, (w, h), timeout=SETTLE_SECS, arrive=ARRIVE) + print(f"{len(arrived)}/{len(bots)} on the final pose") + finally: + swarm.all.stop() + print("done") + + +if __name__ == "__main__": + Swarm.run(swarm_rotate) diff --git a/dotbot/examples/sdk_demo/tiny_circle.py b/dotbot/examples/sdk_demo/tiny_circle.py new file mode 100644 index 00000000..434b76b0 --- /dev/null +++ b/dotbot/examples/sdk_demo/tiny_circle.py @@ -0,0 +1,78 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Each bot traces a small circle around its own starting point. + +A motion demo built on the primary waypoint primitive (follow). Net +displacement is ~zero - every bot ends where it began - but each one sweeps a +small disc, so each bot's radius is derived from its *live* spacing to its +nearest neighbour: small enough that even two neighbours at the worst phase +of their circles keep a body's width between them. Crowded bots get smaller +circles or sit the round out; the rest of the fleet rides at full size. + + python -m dotbot.examples.sdk_demo.tiny_circle [--swarm-url http://localhost:8000] +""" + +import asyncio +import math + +from dotbot.examples.sdk_demo._lib import SAFE_RADIUS, settle +from dotbot.swarm import Swarm + +MAX_RADIUS = 120 # mm - never bigger than this, however sparse the fleet +MIN_RADIUS = 40 # mm - below this a circle is not worth driving +POINTS = 8 # waypoints per circle + + +def safe_radii(bots: list) -> dict: + """Per-bot circle radius such that two neighbours circling in opposite + phase (each eating 2r of their gap) still keep 2*SAFE_RADIUS clearance. + A crowded bot gets a small circle (or sits the round out); the rest of + the fleet is not punished for it.""" + out = {} + for a in bots: + gap = min( + (a.position.distance_to(b.position) for b in bots if b is not a), + default=float("inf"), + ) + out[a.address] = min(MAX_RADIUS, (gap - 2 * SAFE_RADIUS) / 4) + return out + + +def circle(bot, radius: float) -> list: + # Centre the circle below the start so the path begins (and ends) at home. + cx, cy = bot.position.x, bot.position.y - radius + return [ + ( + cx + radius * math.sin(2 * math.pi * k / POINTS), + cy + radius * math.cos(2 * math.pi * k / POINTS), + ) + for k in range(1, POINTS + 1) + ] + + +async def tiny_circle(swarm: Swarm) -> None: + bots = await settle(swarm) + if not bots: + return + radii = safe_radii(bots) if len(bots) > 1 else {b.address: MAX_RADIUS for b in bots} + riders = [b for b in bots if radii[b.address] >= MIN_RADIUS] + if not riders: + print("fleet too crowded for circles everywhere; aborting") + return + sitting = len(bots) - len(riders) + if sitting: + print(f"{sitting} bots too crowded for a circle; they sit this one out") + print(f"{len(riders)} bots tracing circles (radii up to {MAX_RADIUS} mm) ...") + swarm.all.set_color("cyan") + try: + await asyncio.gather(*(b.follow(circle(b, radii[b.address])) for b in riders)) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(tiny_circle) diff --git a/dotbot/examples/sdk_demo/wiggle.py b/dotbot/examples/sdk_demo/wiggle.py new file mode 100644 index 00000000..ffad6202 --- /dev/null +++ b/dotbot/examples/sdk_demo/wiggle.py @@ -0,0 +1,81 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""A cute in-place wiggle: every bot rocks gently side to side while a rainbow +rolls across the fleet. + +Each beat flips the twist direction with move_raw, so a bot rotates one way, +then back - it never leaves its spot, which makes this collision-safe on the +real testbed. Because the SDK paces the fleet broadcast to the link budget, the +flip ripples across the swarm as a soft wave instead of a rigid snap. Colours +advance as a rolling rainbow; only a rotating slice is recoloured each beat, so +the whole thing stays inside the ~80 cmd/s downlink budget even at ~67 bots. + +Run once (default) or pass --loop to keep going, with a short pause between +rounds, until Ctrl-C: + + python -m dotbot.examples.sdk_demo.wiggle [--loop] [--swarm-url ...] + +Tune the feel with the constants below: SPEED/BEAT set how far and how fast each +twist is (with fewer bots you can shorten BEAT for a snappier wiggle). +""" + +import argparse +import asyncio + +from dotbot.examples.sdk_demo._lib import hsv, settle +from dotbot.swarm import Swarm + +SPEED = 75 # wheel PWM magnitude of each twist, 0..100 +BEAT = 1.0 # seconds per twist (held until the next flip) +BEATS_PER_RUN = 8 # twists in one round before stopping +PAUSE = 2.0 # seconds of stillness between rounds in --loop mode +COLOR_SLICES = 6 # recolour 1/COLOR_SLICES of the fleet each beat (budget-aware) +HUE_STEP = 0.05 # rainbow advance per beat + + +async def _run_once(swarm: Swarm, bots: list, hue: float) -> float: + """One round: BEATS_PER_RUN twists with a rolling rainbow. Returns the hue + to continue from so successive rounds keep drifting through the spectrum.""" + n = len(bots) + direction = 1 + for beat in range(BEATS_PER_RUN): + swarm.all.move_raw(left=(0, direction * SPEED), right=(0, -direction * SPEED)) + direction = -direction + for i, bot in enumerate(bots): + if i % COLOR_SLICES == beat % COLOR_SLICES: + bot.set_color(hsv(hue + i / n)) + hue = (hue + HUE_STEP) % 1.0 + await asyncio.sleep(BEAT) + swarm.all.stop() + return hue + + +async def wiggle(swarm: Swarm) -> None: + parser = argparse.ArgumentParser(add_help=False) + parser.add_argument( + "--loop", action="store_true", help="repeat with a pause until Ctrl-C" + ) + opts, _ = parser.parse_known_args() + + await settle(swarm) + bots = sorted(swarm, key=lambda b: b.address) + print(f"wiggling {len(bots)} bots {'in a loop' if opts.loop else 'once'} ...") + + hue = 0.0 + try: + while True: + hue = await _run_once(swarm, bots, hue) + if not opts.loop: + break + swarm.all.set_color("off") + await asyncio.sleep(PAUSE) + finally: + swarm.all.stop() + swarm.all.set_color("off") + print("done") + + +if __name__ == "__main__": + Swarm.run(wiggle) diff --git a/dotbot/examples/work_and_charge/work_and_charge_sdk.py b/dotbot/examples/work_and_charge/work_and_charge_sdk.py new file mode 100644 index 00000000..c1222477 --- /dev/null +++ b/dotbot/examples/work_and_charge/work_and_charge_sdk.py @@ -0,0 +1,176 @@ +"""work_and_charge_sdk.py - the work-and-charge scenario on the Swarm SDK. + +Each bot runs a per-bot supervisory controller (SCT, synthesized offline into +models/supervisor.yaml) that cycles it between a charge lane and a work lane: +drive to work (green) -> work -> energy low -> drive to charge (red) -> charge +-> energy high -> repeat, forever. The controller (controller.py) and the SCT +runtime (dotbot.examples.common.sct) are unchanged domain code. + +This is the SDK rewrite of work_and_charge.py. Everything the old version +hand-rolled - the REST polling, the ws client, the waypoint/rgb pydantic +towers, the scipy KD-tree, the manual sleep loop - collapses into the Swarm: +the loop body is `async for _ in swarm.tick(...)`, and each bot streams one +fresh collision-avoided step per tick with `bot.goto(...)`. + +Run a controller in simulator mode, then run this script: + + dotbot run controller --conn simulator --headless --bots 3 --layout grid + python -m dotbot.examples.work_and_charge.work_and_charge_sdk +""" + +import asyncio +import math +from pathlib import Path + +from dotbot.examples.common.orca import ( + Agent, + OrcaParams, + compute_orca_velocity_for_agent, +) +from dotbot.examples.common.vec2 import Vec2 +from dotbot.examples.work_and_charge.controller import THRESHOLD, Controller +from dotbot.swarm import Bot, Position, Swarm + +DT = 0.2 # control-loop period (s) -> 5 Hz +BOT_RADIUS = 60 # mm, used for collision avoidance +MAX_SPEED = 200 # mm/s +ORCA_RANGE = 200 # mm, only avoid neighbours within this radius + +# World-frame layout (mm): each bot shuttles along its own lane between the +# charge column (left) and the work column (right). +CHARGE_X = 500 +WORK_X = 1500 +LANE_BASE_Y = 500 +LANE_SPACING = 500 + +# The supervisor FSM, resolved next to this example (not the cwd). +SCT_PATH = str(Path(__file__).resolve().parent / "models" / "supervisor.yaml") + + +def _online(swarm: Swarm) -> list[Bot]: + return [b for b in swarm if b.is_online and b.position is not None] + + +async def _await_fleet(swarm: Swarm, *, timeout: float = 10.0) -> list[Bot]: + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + bots = _online(swarm) + while not bots and loop.time() < deadline: + await asyncio.sleep(0.2) + bots = _online(swarm) + return bots + + +def _direction_to_rad(direction) -> float: + rad = ((direction or 0) + 90) * math.pi / 180.0 + return math.atan2(math.sin(rad), math.cos(rad)) + + +def _preferred_vel(bot: Bot, goal: Position | None) -> Vec2: + """Steer toward the current goal at full speed. Unlike the charging-station + converge, this never zeroes at the goal: the supervisor flips the goal + (work <-> charge) the moment the bot is within threshold, so the fleet is + perpetually in motion.""" + if goal is None or bot.position is None: + return Vec2(x=0, y=0) + dx = goal.x - bot.position.x + dy = goal.y - bot.position.y + if math.hypot(dx, dy) < 1.0: # essentially on the goal; avoid a 0/0 heading + return Vec2(x=0, y=0) + direction = _direction_to_rad(bot.direction) + angle_to_goal = math.atan2(dy, dx) + delta = math.atan2( + math.sin(angle_to_goal - direction), math.cos(angle_to_goal - direction) + ) + final = direction + delta + return Vec2(x=math.cos(final) * MAX_SPEED, y=math.sin(final) * MAX_SPEED) + + +def _setup_controllers(bots: list[Bot]) -> dict[str, Controller]: + """One supervisor per bot; assign each a charge lane and a work lane by + sorted address so the lanes are stable across runs.""" + controllers: dict[str, Controller] = {} + for i, bot in enumerate(sorted(bots, key=lambda b: b.address)): + lane_y = LANE_BASE_Y + i * LANE_SPACING + controller = Controller(bot.address, SCT_PATH) + controller.set_charge_waypoint(Position(CHARGE_X, lane_y)) + controller.set_work_waypoint(Position(WORK_X, lane_y)) + controllers[bot.address] = controller + return controllers + + +async def work_and_charge(swarm: Swarm) -> None: + bots = await _await_fleet(swarm) + if not bots: + print("no active bots") + return + print(f"{len(bots)} bots online; running work-and-charge (Ctrl-C to stop) ...") + controllers = _setup_controllers(bots) + params = OrcaParams(time_horizon=5 * DT, time_step=DT) + swarm.all.set_color("red") + last_color: dict[str, tuple[int, int, int]] = {} + + async for _ in swarm.tick(rate_hz=1 / DT): + active = [b for b in _online(swarm) if b.address in controllers] + + # Advance each bot's supervisor from its current position; the callbacks + # set that bot's current goal (work or charge) and LED colour. + goals: dict[str, Position] = {} + for bot in active: + controller = controllers[bot.address] + controller.set_current_position(bot.position) + controller.control_step() + wp = controller.waypoint_current + if wp is not None: + goals[bot.address] = Position(wp.x, wp.y) + + agents = [ + Agent( + id=b.address, + position=Vec2(x=b.position.x, y=b.position.y), + velocity=Vec2(x=0, y=0), + radius=BOT_RADIUS, + max_speed=MAX_SPEED, + preferred_velocity=_preferred_vel(b, goals.get(b.address)), + ) + for b in active + ] + by_address = {b.address: b for b in active} + + for agent in agents: + neighbors = [ + n + for n in agents + if n.id != agent.id + and math.hypot( + n.position.x - agent.position.x, n.position.y - agent.position.y + ) + <= ORCA_RANGE + ] + if neighbors: + velocity = compute_orca_velocity_for_agent(agent, neighbors, params) + else: + velocity = agent.preferred_velocity + step = Vec2(x=velocity.x, y=velocity.y) + + goal = goals.get(agent.id) + if goal is not None: # clamp the step so it never overshoots the goal + dist = math.hypot(goal.x - agent.position.x, goal.y - agent.position.y) + length = math.hypot(step.x, step.y) + if length > dist and length > 0: + step = Vec2(x=step.x * dist / length, y=step.y * dist / length) + + # Keep the target on the map (the simulator rejects negative coords). + target_x = max(0.0, agent.position.x + step.x) + target_y = max(0.0, agent.position.y + step.y) + + bot = by_address[agent.id] + bot.goto(target_x, target_y, threshold=THRESHOLD) + color = controllers[agent.id].led + if last_color.get(agent.id) != color: # only send the LED on a change + bot.set_color(color) + last_color[agent.id] = color + + +if __name__ == "__main__": + Swarm.run(work_and_charge) diff --git a/dotbot/patterns.py b/dotbot/patterns.py new file mode 100644 index 00000000..63d1a685 --- /dev/null +++ b/dotbot/patterns.py @@ -0,0 +1,200 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Geometric placement patterns for a fleet of simulated bots. + +Pure geometry: each function returns a list of ``(x, y)`` positions in mm inside +a rectangular arena (``width`` x ``height``, both defaulting to the square +``arena``). The simulator uses these to place ``--bots N --layout `` +without hand-writing an init-state file, sized to the active ``--map-size`` so +the fleet fills the whole map. ``grid`` and ``random`` are the two universal +layouts (ARGoS / Gazebo); ``circle`` and ``line`` are cheap extras. ``random`` +keeps every bot at least ``min_dist`` (centre-to-centre) from its neighbours so +bots are never placed on top of each other. +""" + +from __future__ import annotations + +import math +import random as _random + +ARENA = 2000 # default square arena side, in mm (matches the default map size) +MARGIN = 150 # keep bots this far off the walls, in mm + +# A DotBot is ~10-13 cm across; with ~5 cm of clearance, place no two bots +# closer than this (centre-to-centre) in the random layout. Tune here (or pass +# ``min_dist=`` to ``scatter``/``layout``) for tighter or looser packing. +MIN_SEPARATION = 180 # mm + + +def _dims(width: int | None, height: int | None, arena: int) -> tuple[float, float]: + return float(width if width is not None else arena), float( + height if height is not None else arena + ) + + +def grid( + n: int, + *, + arena: int = ARENA, + width: int | None = None, + height: int | None = None, + margin: int = MARGIN, +) -> list[tuple[float, float]]: + """N bots on a grid that fills the arena (column count matched to the + arena's aspect ratio).""" + if n <= 0: + return [] + w, h = _dims(width, height, arena) + span_x = max(1.0, w - 2 * margin) + span_y = max(1.0, h - 2 * margin) + cols = max(1, round(math.sqrt(n * span_x / span_y))) + rows = math.ceil(n / cols) + xs = [margin + span_x * (c + 0.5) / cols for c in range(cols)] + ys = [margin + span_y * (r + 0.5) / rows for r in range(rows)] + return [(x, y) for y in ys for x in xs][:n] + + +def circle( + n: int, + *, + arena: int = ARENA, + width: int | None = None, + height: int | None = None, + margin: int = MARGIN, +) -> list[tuple[float, float]]: + """N bots spaced evenly on a circle centred in the arena.""" + w, h = _dims(width, height, arena) + center_x, center_y = w / 2, h / 2 + radius = min(w, h) / 2 - margin + return [ + ( + center_x + radius * math.cos(2 * math.pi * i / n), + center_y + radius * math.sin(2 * math.pi * i / n), + ) + for i in range(n) + ] + + +def line( + n: int, + *, + arena: int = ARENA, + width: int | None = None, + height: int | None = None, + margin: int = MARGIN, +) -> list[tuple[float, float]]: + """N bots in a horizontal row across the middle of the arena.""" + w, h = _dims(width, height, arena) + center_y = h / 2 + if n == 1: + return [(w / 2, center_y)] + span = w - 2 * margin + return [(margin + span * i / (n - 1), center_y) for i in range(n)] + + +def scatter( + n: int, + *, + arena: int = ARENA, + width: int | None = None, + height: int | None = None, + margin: int = MARGIN, + seed: int = 0, + min_dist: float = MIN_SEPARATION, +) -> list[tuple[float, float]]: + """N bots at random positions, no two closer than ``min_dist`` + (centre-to-centre), seeded for reproducibility. + + Grid-accelerated rejection sampling (so it scales to ~1000 bots). If the + arena is too crowded to honour ``min_dist`` for all ``n``, the spacing is + progressively relaxed (with a warning) rather than looping forever, so a + fleet is always placed. + """ + if n <= 0: + return [] + w, h = _dims(width, height, arena) + lo_x, hi_x = float(margin), max(float(margin), w - margin) + lo_y, hi_y = float(margin), max(float(margin), h - margin) + + def _attempt(d: float) -> list[tuple[float, float]]: + cell = d / math.sqrt(2) if d > 0 else max(hi_x - lo_x, hi_y - lo_y, 1.0) + grid_cells: dict[tuple[int, int], list[tuple[float, float]]] = {} + rng = _random.Random(seed) + + def _far_enough(x: float, y: float) -> bool: + if d <= 0: + return True + ci, cj = int((x - lo_x) / cell), int((y - lo_y) / cell) + d2 = d * d + for i in range(ci - 2, ci + 3): + for j in range(cj - 2, cj + 3): + for px, py in grid_cells.get((i, j), ()): + if (x - px) ** 2 + (y - py) ** 2 < d2: + return False + return True + + placed: list[tuple[float, float]] = [] + # Bounded attempts so an over-constrained arena can never hang. + for _ in range(n * 60): + if len(placed) >= n: + break + x = rng.uniform(lo_x, hi_x) + y = rng.uniform(lo_y, hi_y) + if _far_enough(x, y): + placed.append((x, y)) + grid_cells.setdefault( + (int((x - lo_x) / cell), int((y - lo_y) / cell)), [] + ).append((x, y)) + return placed + + dist = float(min_dist) + placed = _attempt(dist) + while len(placed) < n and dist > 1.0: + dist *= 0.85 # relax and retry (still reproducible: same seed) + placed = _attempt(dist) + if dist < min_dist and placed: + try: + from dotbot.logger import LOGGER + + LOGGER.bind(context=__name__).warning( + "random layout relaxed bot spacing to fit the arena", + requested_mm=round(min_dist), + used_mm=round(dist), + bots=n, + ) + except Exception: # noqa: BLE001 - logging must never break placement + pass + return placed[:n] + + +LAYOUTS = {"grid": grid, "circle": circle, "line": line, "random": scatter} + + +def layout( + n: int, + kind: str = "grid", + *, + seed: int = 0, + arena: int = ARENA, + width: int | None = None, + height: int | None = None, + margin: int = MARGIN, + min_dist: float = MIN_SEPARATION, +) -> list[tuple[float, float]]: + """Positions for ``n`` bots in the named ``kind`` layout, sized to + ``width`` x ``height`` (default: the square ``arena``).""" + if kind not in LAYOUTS: + raise ValueError(f"unknown layout {kind!r}; choose from {sorted(LAYOUTS)}") + if kind == "random": + return scatter( + n, + arena=arena, + width=width, + height=height, + margin=margin, + seed=seed, + min_dist=min_dist, + ) + return LAYOUTS[kind](n, arena=arena, width=width, height=height, margin=margin) diff --git a/dotbot/rest.py b/dotbot/rest.py index b3e67eb3..37945191 100644 --- a/dotbot/rest.py +++ b/dotbot/rest.py @@ -70,13 +70,15 @@ async def fetch_map_size(self) -> DotBotMapSizeModel: }, ) except httpx.ConnectError as exc: + # Propagate: falling through would hit `response` unbound, and a + # silently invented arena would be worse than a clear error. self._logger.warning(f"Failed to fetch map size: {exc}") - else: - if response.status_code != 200: - self._logger.warning( - f"Failed to fetch map size: {response} {response.text}" - ) - raise RuntimeError("Failed to fetch map size") + raise + if response.status_code != 200: + self._logger.warning( + f"Failed to fetch map size: {response} {response.text}" + ) + raise RuntimeError("Failed to fetch map size") return DotBotMapSizeModel(**response.json()) async def _send_command(self, address, application, resource, command): @@ -96,7 +98,10 @@ async def _send_command(self, address, application, resource, command): }, content=command.model_dump_json(), ) - except httpx.ConnectError as exc: + except httpx.HTTPError as exc: + # Fire-and-forget: a transient transport failure (connection refused, + # server disconnected mid-burst, read timeout) must not crash the + # caller. Log and move on; waypoint/control loops re-send anyway. self._logger.warning(f"Failed to send command: {exc}") return if response.status_code != 200: @@ -111,9 +116,11 @@ async def send_move_raw_command(self, address, application, command): """Send a move raw command to a DotBot.""" await self._send_command(address, application, "move_raw", command) - async def send_rgb_led_command(self, address, command): + async def send_rgb_led_command( + self, address, command, application=ApplicationType.DotBot + ): """Send an RGB LED command to a DotBot.""" - await self._send_command(address, ApplicationType.SailBot, "rgb_led", command) + await self._send_command(address, application, "rgb_led", command) async def send_waypoint_command(self, address, application, command): """Send an waypoint command to a DotBot.""" diff --git a/dotbot/server.py b/dotbot/server.py index 0e64a3bd..4b37c8f2 100644 --- a/dotbot/server.py +++ b/dotbot/server.py @@ -317,6 +317,7 @@ async def websocket_endpoint(websocket: WebSocket): except WebSocketDisconnect: if websocket in api.controller.websockets: api.controller.websockets.remove(websocket) + api.controller._ws_send_locks.pop(id(websocket), None) @api.websocket("/controller/ws/dotbots") diff --git a/dotbot/swarm/__init__.py b/dotbot/swarm/__init__.py new file mode 100644 index 00000000..a12be543 --- /dev/null +++ b/dotbot/swarm/__init__.py @@ -0,0 +1,55 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The DotBot Swarm SDK. + +`from dotbot.swarm import Swarm` (and, once wired, `from dotbot import Swarm`). +v1 implements the `http(s)://` connection to a running `dotbot run controller`; +the direct links and simulator backend land behind the same surface. +""" + +from __future__ import annotations + +from dotbot.protocol import ApplicationType, ControlModeType +from dotbot.swarm.action import Action +from dotbot.swarm.avoid import bvc_waypoint, safe_hop +from dotbot.swarm.bot import Bot +from dotbot.swarm.events import ( + BatteryUpdate, + BotJoined, + BotLeft, + Event, + ModeChanged, + PositionUpdate, +) +from dotbot.swarm.fleet import Fleet +from dotbot.swarm.link import GatewayBudget, LinkProfile +from dotbot.swarm.position import Position +from dotbot.swarm.swarm import Swarm + +__all__ = [ + # active objects + "Swarm", + "Bot", + "Fleet", + "Action", + # re-exported enums (the SDK's vocabulary for application + control mode) + "ApplicationType", + "ControlModeType", + # value types + "Position", + "LinkProfile", + "GatewayBudget", + # events + "Event", + "BotJoined", + "BotLeft", + "PositionUpdate", + "BatteryUpdate", + "ModeChanged", + # collision avoidance (the composable low-level rung; the high-level rung + # is Swarm.connect(..., collision_avoidance=True)) + "bvc_waypoint", + "safe_hop", +] diff --git a/dotbot/swarm/_backend.py b/dotbot/swarm/_backend.py new file mode 100644 index 00000000..46560ee6 --- /dev/null +++ b/dotbot/swarm/_backend.py @@ -0,0 +1,190 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The HTTP connection backend for `Swarm.connect("http://...")`. + +Talks to a running `dotbot run controller` two ways: the REST API (initial +fleet fetch + the write path, reusing `dotbot.rest.RestClient`) and the +`ws/status` websocket (live state push). It is a thin composition layer - it +holds no swarm state itself; it just forwards controller notifications to a +callback and exposes typed send methods. +""" + +from __future__ import annotations + +import asyncio +import json +from typing import Callable + +import websockets + +from dotbot.models import ( + DotBotLH2Position, + DotBotModel, + DotBotMoveRawCommandModel, + DotBotRgbLedCommandModel, + DotBotWaypoints, +) +from dotbot.protocol import ApplicationType +from dotbot.rest import RestClient + +# dotbot.models.DotBotNotificationCommand values carried on ws/status. +_NOTIF_RELOAD = 1 +_NOTIF_UPDATE = 2 +_NOTIF_NEW_DOTBOT = 4 + +# A single Mari gateway sustains roughly this many downlink command frames per +# second. The SDK paces every outgoing command to it (see HttpBackend._pace) so +# a swarm-wide broadcast - one command per bot - can't outrun the link or +# overwhelm the controller as one burst. +DEFAULT_DOWNLINK_HZ = 80.0 + + +class HttpBackend: + """Connects to a controller over REST + the `ws/status` websocket.""" + + kind = "http" + + def __init__(self, host: str, port: int, https: bool = False): + self.host = host + self.port = port + self.https = https + self._rest = RestClient(host, port, https) + self._ws_task: asyncio.Task | None = None + self._on_update: Callable[[dict], None] | None = None + self._on_reload: Callable[[], None] | None = None + self._closed = False + # Downlink pacing: a single reservation clock shared by every send, so + # concurrent commands (e.g. one per bot from swarm.all) drain at the + # gateway budget instead of bursting. GET /controller/link is still a + # TODO, so assume one gateway until it reports real ones. + self._downlink_hz = DEFAULT_DOWNLINK_HZ + self._send_lock = asyncio.Lock() + self._next_send = 0.0 + + @property + def _ws_url(self) -> str: + scheme = "wss" if self.https else "ws" + return f"{scheme}://{self.host}:{self.port}/controller/ws/status" + + async def connect( + self, + on_update: Callable[[dict], None], + on_reload: Callable[[], None], + ) -> None: + """Start the `ws/status` reader. `on_update(model_dict)` fires for each + per-bot UPDATE (carrying a full DotBotModel); `on_reload()` fires when + the fleet membership may have changed and a re-fetch is warranted.""" + self._on_update = on_update + self._on_reload = on_reload + self._ws_task = asyncio.create_task(self._read_loop()) + + async def _read_loop(self) -> None: + while not self._closed: + try: + async with websockets.connect(self._ws_url) as ws: + async for raw in ws: + self._dispatch(raw) + except asyncio.CancelledError: + raise + except Exception: # noqa: BLE001 - reconnect on any drop + if self._closed: + return + await asyncio.sleep(0.5) + + def _dispatch(self, raw) -> None: + try: + msg = json.loads(raw) + except (ValueError, TypeError): + return + cmd = msg.get("cmd") + if cmd == _NOTIF_UPDATE and isinstance(msg.get("data"), dict): + if self._on_update is not None: + self._on_update(msg["data"]) + elif cmd in (_NOTIF_RELOAD, _NOTIF_NEW_DOTBOT): + if self._on_reload is not None: + self._on_reload() + + async def fetch_fleet(self) -> list[DotBotModel]: + return await self._rest.fetch_dotbots() + + async def fetch_map_size(self) -> tuple[int, int]: + """The controller's arena as (width, height) in millimetres.""" + size = await self._rest.fetch_map_size() + return (size.width, size.height) + + async def _pace(self) -> None: + """Reserve the next downlink slot and wait for it, so concurrent sends + drain at `self._downlink_hz` instead of all at once. Each caller takes a + unique slot under the lock, then sleeps until it comes up - the actual + HTTP requests still overlap, but they start no faster than the budget.""" + if self._downlink_hz <= 0: + return + interval = 1.0 / self._downlink_hz + async with self._send_lock: + loop = asyncio.get_running_loop() + now = loop.time() + start = self._next_send if self._next_send > now else now + self._next_send = start + interval + delay = start - now + if delay > 0: + await asyncio.sleep(delay) + + async def send_rgb_led( + self, + address: str, + application: ApplicationType, + red: int, + green: int, + blue: int, + ) -> None: + await self._pace() + await self._rest.send_rgb_led_command( + address, + DotBotRgbLedCommandModel(red=red, green=green, blue=blue), + application, + ) + + async def send_move_raw( + self, + address: str, + application: ApplicationType, + left: tuple[int, int], + right: tuple[int, int], + ) -> None: + await self._pace() + await self._rest.send_move_raw_command( + address, + application, + DotBotMoveRawCommandModel( + left_x=left[0], left_y=left[1], right_x=right[0], right_y=right[1] + ), + ) + + async def send_waypoints( + self, + address: str, + application: ApplicationType, + points: list[tuple[float, float]], + threshold: int, + ) -> None: + await self._pace() + await self._rest.send_waypoint_command( + address, + application, + DotBotWaypoints( + threshold=threshold, + waypoints=[DotBotLH2Position(x=x, y=y) for x, y in points], + ), + ) + + async def close(self) -> None: + self._closed = True + if self._ws_task is not None: + self._ws_task.cancel() + try: + await self._ws_task + except (asyncio.CancelledError, Exception): # noqa: BLE001 + pass + await self._rest.close() diff --git a/dotbot/swarm/_shepherd.py b/dotbot/swarm/_shepherd.py new file mode 100644 index 00000000..0f4294f0 --- /dev/null +++ b/dotbot/swarm/_shepherd.py @@ -0,0 +1,207 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The collision-avoidance shepherd behind `collision_avoidance=True`. + +When a Swarm is connected with collision avoidance on, `goto` / `move_to` / +`follow` no longer send the user's waypoint to the bot directly. They register +the waypoint as a *goal* here, and this loop streams safe intermediate hops +toward it - each hop the goal projected into the bot's buffered Voronoi cell +(see `dotbot.swarm.avoid`) - until the bot is within the goal's threshold. The +placement mirrors what Crazyswarm does on board and the Robotarium does on its +server: a setpoint filter *under* the user's commands, so user code stays a +plain "go there". + +Liveness is the shepherd's job too: a bot that stops progressing detours to +its right (the standard BVC deadlock heuristic) and stops yielding to crowded +neighbours, so a blocked fleet unjams itself instead of freezing. + +The loop is paced to the gateway downlink budget: with more active goals than +the link can refresh once per second, the tick stretches instead of flooding +the link (and the per-send pacing in the backend still applies). +""" + +from __future__ import annotations + +import asyncio +import math + +from dotbot.logger import LOGGER +from dotbot.swarm._backend import DEFAULT_DOWNLINK_HZ +from dotbot.swarm.avoid import safe_hop + +_TICK = 1.0 # s between hop refreshes per bot, when the link allows it +_PLAN_BUDGET = 0.75 # fraction of the downlink budget a shepherd may consume +_STUCK_MM = 25.0 # progress below this per tick counts as stalled +_SIDESTEP = 350.0 # mm: detour length when stalled (right-hand rule; > the floor) +_DETOUR_TICKS = 4 # commit to a detour this long, or it just oscillates +_CONTACT_MM = 130.0 # closer than this to a neighbour counts as contact +_CONTACT_TICKS = 3 # in contact this long while commanded -> stop, don't grind +_MIN_HOP_THRESHOLD = 60 # mm: don't chase precision below the LH2 noise floor +# Shepherded goals cannot be resolved tighter than the noise floor either: +# thresholds below this are clamped (in set_goal and in Bot._drive's arrival +# watch) so a precision script arrives at the floor's accuracy instead of +# timing out 60 mm short of its goal. +MIN_GOAL_THRESHOLD = _MIN_HOP_THRESHOLD + + +class Shepherd: + """Streams BVC-safe hops for every registered (bot, goal) pair.""" + + def __init__(self, swarm, min_separation: float): + self._swarm = swarm + self._safe_radius = min_separation / 2 + self._goals: dict[str, tuple[float, float, int]] = {} + self._stuck: dict[str, int] = {} + self._detour: dict[str, tuple[float, float, int]] = {} # (x, y, ticks left) + self._contact: dict[str, int] = {} + self._last_pos: dict[str, tuple[float, float]] = {} + self._task: asyncio.Task | None = None + self._arena: tuple[float, float] | None = None + + def set_goal(self, address: str, x: float, y: float, threshold: int) -> None: + """Register (or replace) a bot's goal and make sure the loop runs.""" + self._goals[address] = (float(x), float(y), max(threshold, MIN_GOAL_THRESHOLD)) + self._stuck.pop(address, None) + self._detour.pop(address, None) + if self._task is None or self._task.done(): + self._task = asyncio.ensure_future(self._loop()) + + def clear(self, address: str) -> None: + """Drop a bot's goal (an explicit stop/move_raw takes back control).""" + self._goals.pop(address, None) + self._detour.pop(address, None) + + async def close(self) -> None: + self._goals.clear() + if self._task is not None: + self._task.cancel() + try: + await self._task + except (asyncio.CancelledError, Exception): # noqa: BLE001 + pass + + # ---- the loop -------------------------------------------------------- + + def _tick(self) -> float: + budget = DEFAULT_DOWNLINK_HZ * _PLAN_BUDGET + return max(_TICK, len(self._goals) / budget) if budget > 0 else _TICK + + async def _loop(self) -> None: + while self._goals: + try: + await self._step() + except Exception: # noqa: BLE001 - a failed send must not kill the loop + LOGGER.exception("collision-avoidance shepherd step failed") + await asyncio.sleep(self._tick()) + + async def _step(self) -> None: + if self._arena is None: + self._arena = await self._swarm._backend.fetch_map_size() + bots = {b.address: b for b in self._swarm} + positions = { + a: (b.position.x, b.position.y) + for a, b in bots.items() + if b.position is not None + } + # Plan synchronously over the snapshot, then fire all sends together: + # awaiting per bot would make the tick cost sum(RTT) and let goal + # mutations (a concurrent stop()/goto()) interleave with the loop. + sends: list = [] + for address in list(self._goals): + bot = bots.get(address) + if bot is None or address not in positions: + continue # no fix yet; keep the goal pending + if not bot.is_online: + continue # crashed/lost: stop sending, keep it as an obstacle + entry = self._goals.get(address) + if entry is None: + continue # goal cleared since the snapshot + gx, gy, threshold = entry + px, py = positions[address] + if math.hypot(gx - px, gy - py) <= threshold: + self._goals.pop(address, None) # arrived; the bot stops itself + continue + # Contact guard: a commanded bot pinned against a neighbour must + # stop pushing, not grind motors until someone notices. + nearest = min( + ( + math.hypot(qx - px, qy - py) + for a, (qx, qy) in positions.items() + if a != address + ), + default=float("inf"), + ) + if nearest < _CONTACT_MM: + self._contact[address] = self._contact.get(address, 0) + 1 + if self._contact[address] >= _CONTACT_TICKS: + LOGGER.warning( + "collision-avoidance contact stop", + address=address, + nearest_mm=int(nearest), + ) + self._goals.pop(address, None) + self._contact.pop(address, None) + sends.append( + self._swarm._backend.send_move_raw( + address, bot.application, (0, 0), (0, 0) + ) + ) + continue + else: + self._contact.pop(address, None) + goal = (gx, gy) + patience = self._stuck.get(address, 0) + detour = self._detour.get(address) + if detour is not None: + dx_, dy_, left = detour + if left <= 0: + self._detour.pop(address, None) + else: + goal = (dx_, dy_) + self._detour[address] = (dx_, dy_, left - 1) + elif patience >= 2: # blocked: commit to a right-hand detour + d = math.hypot(gx - px, gy - py) or 1.0 + ux, uy = (gx - px) / d, (gy - py) / d + goal = (px + uy * _SIDESTEP, py - ux * _SIDESTEP) + self._detour[address] = (goal[0], goal[1], _DETOUR_TICKS) + self._stuck[address] = 0 + patience = 0 + wp = safe_hop( + address, + positions, + goal, + self._arena, + heading=bot.direction, + # A bot on a committed detour must execute it, not yield. + yield_ok=patience < 2 and address not in self._detour, + safe_radius=self._safe_radius, + ) + last = self._last_pos.get(address) + if last is not None and math.hypot(px - last[0], py - last[1]) < _STUCK_MM: + self._stuck[address] = self._stuck.get(address, 0) + 1 + else: + self._stuck[address] = 0 + self._last_pos[address] = (px, py) + hop = math.hypot(wp[0] - px, wp[1] - py) + if hop < _MIN_HOP_THRESHOLD: + continue # below the LH2 noise floor; chasing it means spinning + # A waypoint within the firmware threshold is "already reached" + # and moves nothing - scale the threshold down for short hops, but + # never below the noise floor. + hop_threshold = ( + 100 if hop >= 250 else max(_MIN_HOP_THRESHOLD, int(hop * 0.5)) + ) + sends.append( + self._swarm._backend.send_waypoints( + address, bot.application, [wp], hop_threshold + ) + ) + if sends: + # The backend's _pace staggers these at the downlink budget; a + # failed send must not abort the others. + results = await asyncio.gather(*sends, return_exceptions=True) + for result in results: + if isinstance(result, Exception): + LOGGER.warning("shepherd send failed", error=str(result)) diff --git a/dotbot/swarm/action.py b/dotbot/swarm/action.py new file mode 100644 index 00000000..2504ea46 --- /dev/null +++ b/dotbot/swarm/action.py @@ -0,0 +1,38 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The `Action` handle returned by `Bot.move_to` / `Bot.follow`. + +A motion command returns its handle immediately and starts running in the +background (SDK plan, principle 10): `a = bot.move_to(x, y)` fires it, `await a` +waits for arrival, `a.cancel()` stops waiting (it does not stop the bot). This +lets a student write `await bot.move_to(...)` and a researcher compose +non-blocking actions across a fleet with `asyncio.gather(...)`. +""" + +from __future__ import annotations + +import asyncio + + +class Action: + """A running motion command. Awaitable; resolves when the bot arrives.""" + + def __init__(self, task: asyncio.Task): + # The task is created and tracked by the Swarm (via `_schedule`) so it + # fires immediately, runs concurrently under gather(), and is flushed on + # `Swarm.close()` rather than orphaned at shutdown. The Action just wraps + # it so callers can await arrival, cancel the wait, or poll done(). + self._task = task + + def __await__(self): + return self._task.__await__() + + def cancel(self) -> None: + """Stop waiting for arrival. Does not stop the bot - call `bot.stop()` + for that.""" + self._task.cancel() + + def done(self) -> bool: + return self._task.done() diff --git a/dotbot/swarm/avoid.py b/dotbot/swarm/avoid.py new file mode 100644 index 00000000..7a36b08f --- /dev/null +++ b/dotbot/swarm/avoid.py @@ -0,0 +1,214 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Collision avoidance - buffered Voronoi cells over position-only feedback. + +The algorithm is the buffered Voronoi cell (BVC) method (Zhou, Wang, +Bandyopadhyay, Schwager, RA-L 2017): each bot may only head to points closer +to itself than to any neighbour, with the boundary pulled in by a safety +radius. Cells are disjoint by construction, so as long as every bot only ever +steps inside its own current cell, no two bots can meet. It needs exactly what +a DotBot fleet provides - neighbour *positions* at ~2 Hz, no velocity +estimates - and it produces exactly what a DotBot consumes: one waypoint. + +Two layers, by design: + +- These pure functions (`bvc_waypoint`, `safe_hop`) are the low-level rung: + call them from your own control loop, swap in your own planner, or study + them as reference. Nothing here touches the network. +- `Swarm.connect(..., collision_avoidance=True)` is the high-level rung: the + SDK then routes every `goto` / `move_to` / `follow` through a shepherd that + streams these safe hops for you (see `dotbot.swarm._shepherd`). Its + `min_separation` is the centre-to-centre distance between two bots, i.e. + `min_separation = 2 * safe_radius` in the functions here. + +Real-robot grounding baked into the defaults: a DotBot is ~120 mm across and +arcs while it turns (the firmware steers differentially, it never pivots in +place), positions arrive at ~2 Hz and commands take effect ~200-300 ms after +they are sent. The safety radius must absorb all of that between two hops. +""" + +from __future__ import annotations + +import math + +# Defaults, in mm. min separation between two bots is 2 * DEFAULT_SAFE_RADIUS; +# the DotBot body envelope is ~120 mm, the rest is margin for the 2 Hz +# position staleness and the turning arc. +DEFAULT_SAFE_RADIUS = 150.0 +DEFAULT_WALL_MARGIN = 150.0 +DEFAULT_MAX_STEP = 180.0 # longest hop per tick; bounds overshoot past stale data +YIELD_GAP = 60.0 # mm above the floor within which a misaligned bot yields +TURN_BITE = 80.0 # mm hop cap while turning near a crowd (the arc stays small) + + +def _clip_polygon(poly: list, ax: float, ay: float, c: float) -> list: + """Clip a convex polygon (CCW vertex list) to the half-plane + ax*x + ay*y <= c.""" + out: list = [] + for i, cur in enumerate(poly): + nxt = poly[(i + 1) % len(poly)] + cur_in = ax * cur[0] + ay * cur[1] <= c + nxt_in = ax * nxt[0] + ay * nxt[1] <= c + if cur_in: + out.append(cur) + if cur_in != nxt_in: + denom = ax * (nxt[0] - cur[0]) + ay * (nxt[1] - cur[1]) + if abs(denom) < 1e-12: + # Degenerate edge (duplicate vertices or parallel to the clip + # line, e.g. from noisy duplicate positions): nothing to cut. + continue + t = (c - ax * cur[0] - ay * cur[1]) / denom + out.append((cur[0] + t * (nxt[0] - cur[0]), cur[1] + t * (nxt[1] - cur[1]))) + return out + + +def _closest_in_polygon(poly: list, gx: float, gy: float) -> tuple: + """The point of a convex polygon (CCW) closest to (gx, gy).""" + inside = len(poly) >= 3 + for i, cur in enumerate(poly): + if not inside: + break + nxt = poly[(i + 1) % len(poly)] + ex, ey = nxt[0] - cur[0], nxt[1] - cur[1] + # CCW polygon: interior is left of every edge; right of one = outside. + if ex * (gy - cur[1]) - ey * (gx - cur[0]) < 0: + inside = False + if inside: + return (gx, gy) + best, best_d2 = poly[0], float("inf") + for i, cur in enumerate(poly): + nxt = poly[(i + 1) % len(poly)] + ex, ey = nxt[0] - cur[0], nxt[1] - cur[1] + e2 = ex * ex + ey * ey + t = ( + 0.0 + if e2 < 1e-12 + else max(0.0, min(1.0, ((gx - cur[0]) * ex + (gy - cur[1]) * ey) / e2)) + ) + px, py = cur[0] + t * ex, cur[1] + t * ey + d2 = (gx - px) ** 2 + (gy - py) ** 2 + if d2 < best_d2: + best, best_d2 = (px, py), d2 + return best + + +def bvc_waypoint( + me: str, + positions: dict, + goal: tuple, + arena: tuple, + *, + safe_radius: float = DEFAULT_SAFE_RADIUS, + wall_margin: float = DEFAULT_WALL_MARGIN, + max_step: float = DEFAULT_MAX_STEP, +) -> tuple: + """The next safe waypoint for bot `me`: its goal projected into its + buffered Voronoi cell (and inside the walls), at most `max_step` away. + + `positions` is {address: (x, y)} for every positioned bot (including + `me`), `arena` is (width, height) in mm. Neighbours already inside the + 2*safe_radius floor get a weaker but always-feasible constraint - never + step *toward* them - and the goal becomes "straight away from their net + push"; a fully boxed-in bot is told to stand still until they clear. + """ + px, py = positions[me] + w, h = arena + lo_x, hi_x = wall_margin, w - wall_margin + lo_y, hi_y = wall_margin, h - wall_margin + poly: list = [(lo_x, lo_y), (hi_x, lo_y), (hi_x, hi_y), (lo_x, hi_y)] + + flee_x = flee_y = 0.0 + intruders = 0 + for other, (qx, qy) in positions.items(): + if other == me: + continue + dx, dy = qx - px, qy - py + d = math.hypot(dx, dy) + if d < 1e-9: + continue # exactly stacked: no direction; neighbours will pull apart + nx, ny = dx / d, dy / d + if d < 2 * safe_radius: + intruders += 1 + flee_x -= dx / (d * d) + flee_y -= dy / (d * d) + poly = _clip_polygon(poly, nx, ny, nx * px + ny * py) # no approach + else: + c = nx * (px + qx) / 2 + ny * (py + qy) / 2 - safe_radius + poly = _clip_polygon(poly, nx, ny, c) + if not poly: + break + + if not poly: + # Fully boxed in: stand still until the neighbours clear. + return (min(max(px, lo_x), hi_x), min(max(py, lo_y), hi_y)) + + if intruders: + mag = math.hypot(flee_x, flee_y) + if mag < 1e-9: + return (min(max(px, lo_x), hi_x), min(max(py, lo_y), hi_y)) + goal = (px + flee_x / mag * max_step, py + flee_y / mag * max_step) + + zx, zy = _closest_in_polygon(poly, *goal) + dx, dy = zx - px, zy - py + d = math.hypot(dx, dy) + if d > max_step: + zx, zy = px + dx / d * max_step, py + dy / d * max_step + return (zx, zy) + + +def safe_hop( + me: str, + positions: dict, + goal: tuple, + arena: tuple, + *, + heading: float | None = None, + yield_ok: bool = True, + safe_radius: float = DEFAULT_SAFE_RADIUS, + wall_margin: float = DEFAULT_WALL_MARGIN, + max_step: float = DEFAULT_MAX_STEP, +) -> tuple: + """`bvc_waypoint` with the bot's heading taken into account. + + A DotBot commanded to a point behind it arcs forward while it turns (the + firmware never pivots in place), and that arc is what the straight-segment + safety argument cannot see. So when the hop points far off the current + `heading` (firmware frame: degrees, 0 = +y) and a neighbour sits at or + barely above the safety floor, the bot yields - the returned waypoint is + its own position, i.e. a stop - unless it is the lowest address of that + group; the designated mover takes the turn in short bites instead. A + yielded bot never turns (it is not moving), so callers that track progress + pass `yield_ok=False` after a few stalled ticks to let it creep out. + """ + px, py = positions[me] + wp = bvc_waypoint( + me, + positions, + goal, + arena, + safe_radius=safe_radius, + wall_margin=wall_margin, + max_step=max_step, + ) + if heading is None: + return wp + vx, vy = wp[0] - px, wp[1] - py + hop = math.hypot(vx, vy) + if hop < 1.0: + return wp + bearing = -math.degrees(math.atan2(vx, vy)) # firmware frame: 0 = +y + err = (bearing - heading + 180) % 360 - 180 + if abs(err) <= 60: + return wp + crowd = [ + a + for a, (qx, qy) in positions.items() + if a != me and math.hypot(qx - px, qy - py) < 2 * safe_radius + YIELD_GAP + ] + if yield_ok and crowd and min(crowd) < me: + return (px, py) # yield this tick; a goto to here is a stop + if crowd and hop > TURN_BITE: + wp = (px + vx / hop * TURN_BITE, py + vy / hop * TURN_BITE) + return wp diff --git a/dotbot/swarm/bot.py b/dotbot/swarm/bot.py new file mode 100644 index 00000000..34589efb --- /dev/null +++ b/dotbot/swarm/bot.py @@ -0,0 +1,291 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The `Bot` - one DotBot's live state plus its domain verbs. + +State is read-only and kept fresh by the `Swarm` from the `ws/status` stream; +the verbs (`set_color`, `move_raw`, `move_to`, `follow`, ...) issue commands. +`move_to`/`follow` are the primary motion primitives: they hand the bot a goal +it pursues autonomously off its own local fix (SDK plan section 3), so one +low-rate waypoint per bot is enough. `follow` absorbs the boilerplate every +example hand-rolls today - the <=12 chunking, resend-until-AUTO, and +poll-until-arrival. +""" + +from __future__ import annotations + +import asyncio +import time +from typing import TYPE_CHECKING + +from dotbot.models import DotBotModel, DotBotStatus +from dotbot.protocol import ApplicationType, ControlModeType +from dotbot.swarm.action import Action +from dotbot.swarm.position import Position + +if TYPE_CHECKING: + from dotbot.swarm.swarm import Swarm + +# Hardware limit: a single waypoint command carries at most this many points. +MAX_WAYPOINTS = 12 + +# Motion-loop pacing. A bot reports its mode/position roughly once per position +# report period (~0.5 s on Mari / the simulator advertisement). Resend no faster +# than that, so a re-send can't rewind a bot that has already engaged its goal. +_RESEND_PERIOD = 0.6 # s, between waypoint re-sends while not yet engaged +_ARRIVAL_POLL = 0.2 # s, between arrival checks +# A move that never arrives must not hang the script forever (a bot can be stuck, +# lost, or chasing an unreachable goal). Past this budget the awaited Action +# raises TimeoutError instead of blocking or falsely reporting success. +DEFAULT_MOVE_TIMEOUT = 60.0 # s + +_COLORS: dict[str, tuple[int, int, int]] = { + "red": (255, 0, 0), + "green": (0, 255, 0), + "blue": (0, 0, 255), + "yellow": (255, 255, 0), + "cyan": (0, 255, 255), + "magenta": (255, 0, 255), + "white": (255, 255, 255), + "off": (0, 0, 0), + "black": (0, 0, 0), +} + + +class Bot: + """One DotBot. State is updated by the Swarm; verbs send commands.""" + + def __init__(self, swarm: Swarm, model: DotBotModel): + self._swarm = swarm + self.address: str = model.address + self.application: ApplicationType = model.application + self._lh2 = None + self._lh2_ts = 0.0 + self._lh2_candidate = None + self._battery_emitted: float | None = None + self._moves: set[asyncio.Task] = set() + self._apply(model) + + def _apply(self, model: DotBotModel) -> None: + """Refresh state from a controller DotBotModel (initial fetch or a + ws/status UPDATE).""" + self.application = model.application + self._status: DotBotStatus = model.status + self.mode: ControlModeType = model.mode + # -1000 is the firmware's "no heading yet" sentinel. + direction = model.direction + self.direction: int | None = ( + None if direction is None or direction <= -1000 else direction + ) + self.battery: float | None = model.battery + self._gate_position(model.lh2_position) + self.waypoints = list(model.waypoints or []) + self.waypoints_threshold: int = model.waypoints_threshold + self.last_seen: float = model.last_seen + + # Real LH2 feeds glitch: a fix can jump metres between two 2 Hz reports. + # Gate updates to a plausible speed; a jump is held as a candidate and + # accepted only when the next report lands near it (a real relocation). + _MAX_PLAUSIBLE_MM_S = 500.0 + _CANDIDATE_CONFIRM_MM = 200.0 + + def _gate_position(self, new) -> None: + if new is not None and new.x == 0 and new.y == 0: + return # the firmware's "no fix" sentinel; keep the last real fix + if new is None: + return + now = time.monotonic() + accepted = self._lh2 + if accepted is None: + self._lh2, self._lh2_ts, self._lh2_candidate = new, now, None + return + dt = max(now - self._lh2_ts, 0.05) + jump = ((new.x - accepted.x) ** 2 + (new.y - accepted.y) ** 2) ** 0.5 + candidate = getattr(self, "_lh2_candidate", None) + if jump / dt <= self._MAX_PLAUSIBLE_MM_S: + self._lh2, self._lh2_ts, self._lh2_candidate = new, now, None + elif ( + candidate is not None + and ((new.x - candidate.x) ** 2 + (new.y - candidate.y) ** 2) ** 0.5 + < self._CANDIDATE_CONFIRM_MM + ): + # Second consistent report from the new place: the bot really moved. + self._lh2, self._lh2_ts, self._lh2_candidate = new, now, None + else: + self._lh2_candidate = new # hold the outlier; keep the last good fix + + # ---- read-only state ------------------------------------------------ + + @property + def position(self) -> Position | None: + """The latest LH2 position, or None if the bot has no fix yet.""" + if self._lh2 is None: + return None + return Position(self._lh2.x, self._lh2.y) + + @property + def is_online(self) -> bool: + return self._status == DotBotStatus.ACTIVE + + def __repr__(self) -> str: + p = self.position + where = f"({p.x:.0f},{p.y:.0f})" if p else "no-fix" + return f"" + + # ---- commands ------------------------------------------------------- + + def set_color( + self, color=None, *, red: int = 0, green: int = 0, blue: int = 0 + ) -> None: + """Set the RGB LED. Accepts a name ("blue"), an (r, g, b) tuple, or + red=/green=/blue= keywords. Fire-and-forget.""" + if color is not None: + if isinstance(color, str): + try: + red, green, blue = _COLORS[color.lower()] + except KeyError as exc: + raise ValueError(f"unknown color name: {color!r}") from exc + else: + red, green, blue = color + self._swarm._schedule( + self._swarm._backend.send_rgb_led( + self.address, self.application, red, green, blue + ) + ) + + def move_raw( + self, *, left: tuple[int, int] = (0, 0), right: tuple[int, int] = (0, 0) + ) -> None: + """Direct per-wheel teleop (single-bot, high-rate). Fire-and-forget. + Always takes over: in-flight move_to/follow Actions are cancelled + (awaiting one raises CancelledError) and any shepherded goal is + cleared - otherwise the move's resend loop would re-engage the bot + moments after an operator stop.""" + for task in list(self._moves): + task.cancel() + if self._swarm._shepherd is not None: + self._swarm._shepherd.clear(self.address) + self._swarm._schedule( + self._swarm._backend.send_move_raw( + self.address, self.application, left, right + ) + ) + + def stop(self) -> None: + self.move_raw(left=(0, 0), right=(0, 0)) + + def goto(self, x: float, y: float, *, threshold: int = 100) -> None: + """Fire-and-forget: set a single waypoint and return immediately, + without waiting for arrival. The streaming counterpart to move_to/follow + - for control loops (e.g. ORCA) that send a fresh target every tick. Use + move_to/follow when you want to await arrival. With collision avoidance + on, the waypoint becomes the bot's shepherded goal instead of a direct + send.""" + if self._swarm._shepherd is not None: + self._swarm._shepherd.set_goal(self.address, x, y, threshold) + return + self._swarm._schedule( + self._swarm._backend.send_waypoints( + self.address, self.application, [(float(x), float(y))], threshold + ) + ) + + def move_to( + self, + x: float, + y: float, + *, + threshold: int = 100, + timeout: float = DEFAULT_MOVE_TIMEOUT, + ) -> Action: + """Drive to a single point. Returns an Action; await it to wait for + arrival (it raises TimeoutError if the bot does not arrive in + `timeout` seconds).""" + return self.follow([(x, y)], threshold=threshold, timeout=timeout) + + def follow( + self, waypoints, *, threshold: int = 100, timeout: float = DEFAULT_MOVE_TIMEOUT + ) -> Action: + """Drive through a list of (x, y) waypoints. Returns an Action handle + immediately; await it to wait until the bot reaches the last point. + Absorbs the <=12 chunking and resend-until-engaged (with collision + avoidance on, the points instead become the bot's shepherded goals, + one at a time). Arrival is detected by position, and a bot that never + arrives raises TimeoutError rather than hanging or reporting a false + 'done'.""" + points = [(float(x), float(y)) for x, y in waypoints] + task = self._swarm._schedule(self._drive(points, threshold, timeout)) + self._moves.add(task) + task.add_done_callback(self._moves.discard) + return Action(task) + + async def _drive( + self, points: list[tuple[float, float]], threshold: int, timeout: float + ) -> None: + deadline = asyncio.get_running_loop().time() + timeout + if self._swarm._shepherd is not None: + from dotbot.swarm._shepherd import MIN_GOAL_THRESHOLD + + # Shepherded: each point becomes the bot's goal in turn; the + # shepherd streams the safe hops, we only watch for arrival. + # Avoidance cannot place a bot more precisely than the LH2 noise + # floor, so sub-floor thresholds are clamped (here AND in the + # shepherd) instead of stalling forever short of the goal. + threshold = max(threshold, MIN_GOAL_THRESHOLD) + for x, y in points: + self._swarm._shepherd.set_goal(self.address, x, y, threshold) + await self._await_arrival(Position(x, y), threshold, deadline) + return + for i in range(0, len(points), MAX_WAYPOINTS): + await self._follow_chunk(points[i : i + MAX_WAYPOINTS], threshold, deadline) + + async def _await_arrival( + self, target: Position, threshold: int, deadline: float + ) -> None: + loop = asyncio.get_running_loop() + while True: + pos = self.position + if pos is not None and pos.distance_to(target) <= threshold: + return + if loop.time() > deadline: + # Stop chasing an unreachable goal before surfacing the error. + self._swarm._shepherd.clear(self.address) + raise TimeoutError( + f"{self.address} did not reach " + f"({target.x:.0f}, {target.y:.0f}) within the move timeout" + ) + await asyncio.sleep(_ARRIVAL_POLL) + + async def _follow_chunk( + self, chunk: list[tuple[float, float]], threshold: int, deadline: float + ) -> None: + """Send a waypoint batch and wait until the bot is within `threshold` of + the final point. Sends once, then re-sends only while the bot has not + engaged (AUTO) and no faster than the report period, so a dropped command + can't stall the run but an engaged bot is never rewound. Raises + TimeoutError past `deadline` so the awaited Action surfaces a + stuck/unreachable bot instead of hanging or claiming false success.""" + loop = asyncio.get_running_loop() + target = Position(*chunk[-1]) + await self._swarm._backend.send_waypoints( + self.address, self.application, chunk, threshold + ) + last_send = loop.time() + while True: + pos = self.position + if pos is not None and pos.distance_to(target) <= threshold: + return + if loop.time() > deadline: + raise TimeoutError( + f"{self.address} did not reach " + f"({target.x:.0f}, {target.y:.0f}) within the move timeout" + ) + if self.mode != ControlModeType.AUTO and ( + loop.time() - last_send >= _RESEND_PERIOD + ): + await self._swarm._backend.send_waypoints( + self.address, self.application, chunk, threshold + ) + last_send = loop.time() + await asyncio.sleep(_ARRIVAL_POLL) diff --git a/dotbot/swarm/events.py b/dotbot/swarm/events.py new file mode 100644 index 00000000..ef3ff048 --- /dev/null +++ b/dotbot/swarm/events.py @@ -0,0 +1,72 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Discrete swarm events. + +The SDK keeps two telemetry idioms separate (SDK plan, principle 4): discrete +change-events (this module) versus declared-rate streams +(`swarm.positions(rate_hz=...)`). `swarm.on(BotJoined, cb)` and `swarm.events()` +are keyed by these Event classes, never by stringly-typed names, so there is a +single event vocabulary that cannot typo or drift. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +from dotbot.protocol import ControlModeType +from dotbot.swarm.position import Position + + +@dataclass(frozen=True, slots=True) +class Event: + """Base class for every swarm event. + + `address` is the bot the event concerns; `timestamp` is the host time + (`time.monotonic()`-style) at which the backend emitted it. + """ + + address: str + timestamp: float + + +@dataclass(frozen=True, slots=True) +class BotJoined(Event): + """A bot was seen for the first time, or re-appeared after being lost.""" + + +@dataclass(frozen=True, slots=True) +class BotLeft(Event): + """A bot stopped being heard (went inactive / lost).""" + + +@dataclass(frozen=True, slots=True) +class PositionUpdate(Event): + """A bot reported a fresh LH2 position.""" + + position: Position + + +@dataclass(frozen=True, slots=True) +class BatteryUpdate(Event): + """A bot reported a fresh battery voltage (in volts).""" + + battery: float + + +@dataclass(frozen=True, slots=True) +class ModeChanged(Event): + """A bot's control mode changed (MANUAL <-> AUTO).""" + + mode: ControlModeType + + +__all__ = [ + "Event", + "BotJoined", + "BotLeft", + "PositionUpdate", + "BatteryUpdate", + "ModeChanged", +] diff --git a/dotbot/swarm/fleet.py b/dotbot/swarm/fleet.py new file mode 100644 index 00000000..b5dc1ef2 --- /dev/null +++ b/dotbot/swarm/fleet.py @@ -0,0 +1,47 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The `Fleet` - a handle over many bots that shares the single-bot verbs. + +`swarm.all` and `swarm.select(pred)` return a `Fleet`; `swarm.all.set_color(...)` +reads like `bot.set_color(...)`. Over the HTTP connection a fleet verb fans out +one command per bot; the true one-frame broadcast (SDK plan, principle 1) is a +backend optimization that lands with the direct/mqtt connection, behind this +same surface. +""" + +from __future__ import annotations + +from typing import Iterable, Iterator + +from dotbot.swarm.bot import Bot + + +class Fleet: + """A group of bots addressed together. Verbs fan out to each member.""" + + def __init__(self, bots: Iterable[Bot]): + self._bots: list[Bot] = list(bots) + + def __iter__(self) -> Iterator[Bot]: + return iter(self._bots) + + def __len__(self) -> int: + return len(self._bots) + + def set_color( + self, color=None, *, red: int = 0, green: int = 0, blue: int = 0 + ) -> None: + for bot in self._bots: + bot.set_color(color, red=red, green=green, blue=blue) + + def move_raw( + self, *, left: tuple[int, int] = (0, 0), right: tuple[int, int] = (0, 0) + ) -> None: + for bot in self._bots: + bot.move_raw(left=left, right=right) + + def stop(self) -> None: + for bot in self._bots: + bot.stop() diff --git a/dotbot/swarm/link.py b/dotbot/swarm/link.py new file mode 100644 index 00000000..b2d5e5d5 --- /dev/null +++ b/dotbot/swarm/link.py @@ -0,0 +1,97 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Link-budget value types. + +`swarm.link` returns a `LinkProfile`: a snapshot of the active link and its +per-gateway packet budget (SDK plan sections 3 and 5). The shape is generic on +purpose - the Mari link fills it from marilib `GatewayInfo`, but a wifi or +direct-BLE link would fill the same fields. `LinkProfile.bottleneck` is the +"common link denominator": the most-saturated gateway, so a swarm-wide control +loop can pace itself to the slowest one. + +NOTE (SDK plan section 5, flagged-undecided): `GatewayBudget` / `.gateways` lean +on Mari's "gateway" word inside an otherwise link-agnostic profile. A neutral +term (`segment`, with Mari mapping gateway -> segment) is still under +consideration; kept as `gateways` here to match the plan's primary text. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True, slots=True) +class GatewayBudget: + """The packet budget of a single gateway (one Mari network / schedule). + + All rates are per second. `per_bot_command_rate_hz` is the downlink budget + divided by the bots currently on this gateway, so it shrinks as the gateway + fills (this is the number the SDK paces unicast commands against). + `queue_depth` is the live backpressure signal (slotframes of backlog), set + only when the backend can read it directly (a `mqtts://` / serial + connection); it stays None over an HTTP connection unless the controller + chooses to expose it. + """ + + max_bots: int + downlink_rate_hz: float + uplink_rate_hz: float + per_bot_command_rate_hz: float + address: str | None = None + bots: int | None = None + queue_depth: float | None = None + + @classmethod + def from_dict(cls, d: dict) -> GatewayBudget: + """Build from the JSON the `GET /controller/link` endpoint returns.""" + return cls( + max_bots=int(d["max_bots"]), + downlink_rate_hz=float(d["downlink_rate_hz"]), + uplink_rate_hz=float(d["uplink_rate_hz"]), + per_bot_command_rate_hz=float(d["per_bot_command_rate_hz"]), + address=d.get("address"), + bots=d.get("bots"), + queue_depth=d.get("queue_depth"), + ) + + +@dataclass(frozen=True, slots=True) +class LinkProfile: + """A snapshot of the active link and its per-gateway budget. + + `kind` is "mari" | "wifi" | "ble-direct" | ...; `position_rate_hz` is the + host-side position report rate (~2 Hz on Mari, via the DotBot + advertisement). Mari scales horizontally, so the budget is per gateway and + `gateways` may hold several entries; a single-bot BLE link reports one. + """ + + kind: str + position_rate_hz: float + gateways: tuple[GatewayBudget, ...] = () + + @property + def bottleneck(self) -> GatewayBudget | None: + """The most-saturated gateway - the common denominator a swarm-wide loop + should pace to. Prefers live queue depth when any gateway reports it, + otherwise the gateway with the lowest per-bot command rate. None when + there are no gateways. + """ + if not self.gateways: + return None + if any(g.queue_depth is not None for g in self.gateways): + return max( + self.gateways, + key=lambda g: g.queue_depth if g.queue_depth is not None else -1.0, + ) + return min(self.gateways, key=lambda g: g.per_bot_command_rate_hz) + + @classmethod + def from_dict(cls, d: dict) -> LinkProfile: + """Build from the JSON the `GET /controller/link` endpoint returns.""" + return cls( + kind=str(d.get("kind", "unknown")), + position_rate_hz=float(d.get("position_rate_hz", 0.0)), + gateways=tuple(GatewayBudget.from_dict(g) for g in d.get("gateways", ())), + ) diff --git a/dotbot/swarm/position.py b/dotbot/swarm/position.py new file mode 100644 index 00000000..11859664 --- /dev/null +++ b/dotbot/swarm/position.py @@ -0,0 +1,66 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The Swarm SDK's 2D position value type. + +`Position` is intentionally distinct from the wire/controller +`DotBotLH2Position` (a pydantic model): the SDK exposes a lightweight, immutable +value that supports vector arithmetic, so a planner can write +`bot.position + step` directly (see the charging_station rewrite in the SDK +plan). The backend maps a `DotBotLH2Position` onto a `Position`; the SDK never +re-uses the pydantic model on its hot path. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from math import hypot +from typing import Iterator + + +def _xy(other: Position | tuple[float, float]) -> tuple[float, float]: + """Coerce a Position or any (x, y) sequence (tuple, list, numpy array) to a + plain float pair.""" + if isinstance(other, Position): + return other.x, other.y + x, y = other + return float(x), float(y) + + +@dataclass(frozen=True, slots=True) +class Position: + """An immutable 2D position in millimetres, in the LH2 map frame. + + Treated as both a point and a vector: adding/subtracting another position + (or any `(x, y)` pair) yields a new `Position`, so `bot.position + step` + reads directly when `step` is an offset from a planner. + """ + + x: float + y: float + + def __add__(self, other: Position | tuple[float, float]) -> Position: + ox, oy = _xy(other) + return Position(self.x + ox, self.y + oy) + + def __sub__(self, other: Position | tuple[float, float]) -> Position: + ox, oy = _xy(other) + return Position(self.x - ox, self.y - oy) + + def __mul__(self, scalar: float) -> Position: + return Position(self.x * scalar, self.y * scalar) + + __rmul__ = __mul__ + + def __iter__(self) -> Iterator[float]: + yield self.x + yield self.y + + def distance_to(self, other: Position | tuple[float, float]) -> float: + """Euclidean distance in millimetres to another position.""" + ox, oy = _xy(other) + return hypot(self.x - ox, self.y - oy) + + def as_tuple(self) -> tuple[float, float]: + return (self.x, self.y) diff --git a/dotbot/swarm/swarm.py b/dotbot/swarm/swarm.py new file mode 100644 index 00000000..fe4595cb --- /dev/null +++ b/dotbot/swarm/swarm.py @@ -0,0 +1,379 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""The `Swarm` - the SDK's entry point and live fleet. + +`Swarm.connect(conn)` discriminates on the connection string (SDK plan section +10). v1 implements the `http(s)://` connection (a running `dotbot run +controller`); the direct links (`mqtts://`, serial) and `simulator` come later +behind this same surface. The Swarm holds the live `Bot` objects, keeps them +fresh from the controller's `ws/status` stream, and exposes iteration, the +`all` / `select(...)` fleet handles, and the `link` budget. +""" + +from __future__ import annotations + +import argparse +import asyncio +import time +from typing import AsyncIterator, Callable, Iterator +from urllib.parse import urlparse + +from dotbot.logger import LOGGER +from dotbot.models import DotBotModel, DotBotStatus +from dotbot.swarm._backend import HttpBackend +from dotbot.swarm.bot import Bot +from dotbot.swarm.events import ( + BatteryUpdate, + BotJoined, + BotLeft, + Event, + ModeChanged, + PositionUpdate, +) +from dotbot.swarm.fleet import Fleet +from dotbot.swarm.link import LinkProfile +from dotbot.swarm.position import Position + + +def _backend_for(conn: str): + """Pick a backend from the connection string. v1: http(s) only.""" + if conn.startswith(("http://", "https://")): + parsed = urlparse(conn) + https = parsed.scheme == "https" + host = parsed.hostname or "localhost" + port = parsed.port or (443 if https else 8000) + return HttpBackend(host, port, https) + raise ValueError( + f"unsupported connection {conn!r} - v1 supports http(s):// " + "(a running `dotbot run controller`); mqtts://, serial and simulator come later" + ) + + +class Swarm: + """The live swarm. Use `async with Swarm.connect(url) as swarm:`.""" + + def __init__( + self, + backend, + *, + collision_avoidance: bool = False, + min_separation: float | None = None, + ): + self._backend = backend + self._bots: dict[str, Bot] = {} + self._tasks: set[asyncio.Task] = set() + self._handlers: dict[type, list[Callable]] = {} + self._event_queues: set[asyncio.Queue] = set() + self._positions_clamped = False + self._tick_warned = False + self._shepherd = None + if collision_avoidance: + from dotbot.swarm._shepherd import Shepherd + from dotbot.swarm.avoid import DEFAULT_SAFE_RADIUS + + self._shepherd = Shepherd(self, min_separation or 2 * DEFAULT_SAFE_RADIUS) + + @classmethod + def connect( + cls, + conn: str, + *, + collision_avoidance: bool = False, + min_separation: float | None = None, + ) -> Swarm: + """Return a Swarm for `conn`. Enter it as an async context manager to + actually open the connection. + + With `collision_avoidance=True` every `goto` / `move_to` / `follow` is + shepherded through buffered-Voronoi-cell waypoints (positions only, no + extra hardware), so bots flow around each other and stay off the walls + instead of driving straight through occupied space. `min_separation` + is the enforced centre-to-centre distance in mm (default 300). + Separation is the guarantee; arrival stays best-effort - a goal that + is blocked or unreachable surfaces as the usual move timeout. `stop()` + and `move_raw()` always bypass the shepherd.""" + return cls( + _backend_for(conn), + collision_avoidance=collision_avoidance, + min_separation=min_separation, + ) + + @property + def collision_avoidance(self) -> bool: + """Whether motion commands are shepherded around other bots.""" + return self._shepherd is not None + + async def __aenter__(self) -> Swarm: + await self._open() + return self + + async def __aexit__(self, *exc) -> None: + await self.close() + + async def _open(self) -> None: + for model in await self._backend.fetch_fleet(): + self._bots[model.address] = Bot(self, model) + await self._backend.connect(self._on_update, self._on_reload) + + # ---- state plumbing ------------------------------------------------- + + def _on_update(self, data: dict) -> None: + try: + model = DotBotModel(**data) + except Exception: # noqa: BLE001 - ignore malformed pushes + return + bot = self._bots.get(model.address) + if bot is None: + bot = Bot(self, model) + self._bots[model.address] = bot + self._emit(BotJoined(bot.address, time.monotonic())) + return + before = (bot.position, bot.battery, bot.mode, bot._status) + bot._apply(model) + self._emit_changes(bot, *before) + + def _emit_changes(self, bot, old_pos, old_battery, old_mode, old_status) -> None: + ts = time.monotonic() + if bot.position is not None and bot.position != old_pos: + self._emit(PositionUpdate(bot.address, ts, bot.position)) + # Compare against the last *emitted* value, not the previous report: a + # battery drains a few mV per report, so per-report deltas never cross + # the threshold and a slow drain would otherwise never emit at all. + if bot.battery is not None and ( + bot._battery_emitted is None + or abs(bot.battery - bot._battery_emitted) >= 0.05 + ): + bot._battery_emitted = bot.battery + self._emit(BatteryUpdate(bot.address, ts, bot.battery)) + if bot.mode != old_mode: + self._emit(ModeChanged(bot.address, ts, bot.mode)) + if old_status == DotBotStatus.ACTIVE and bot._status != DotBotStatus.ACTIVE: + self._emit(BotLeft(bot.address, ts)) + + def _on_reload(self) -> None: + self._schedule(self._refetch()) + + async def _refetch(self) -> None: + # A reload (e.g. a NEW_DOTBOT notification) is the real path by which a + # bot joins after connect; the per-bot UPDATE stream never carries the + # first sight of it. Emit BotJoined here so `swarm.on(BotJoined, ...)` + # actually fires for a mid-run join, not only for the initial fleet. + for model in await self._backend.fetch_fleet(): + bot = self._bots.get(model.address) + if bot is None: + bot = Bot(self, model) + self._bots[model.address] = bot + self._emit(BotJoined(bot.address, time.monotonic())) + else: + bot._apply(model) + + def _schedule(self, coro) -> asyncio.Task: + """Run a fire-and-forget command coroutine, keeping a reference so it is + not garbage-collected mid-flight. Failures are logged - a dropped + fire-and-forget command should be visible, not a silent 'task exception + was never retrieved' at exit.""" + task = asyncio.ensure_future(coro) + self._tasks.add(task) + task.add_done_callback(self._task_done) + return task + + def _task_done(self, task: asyncio.Task) -> None: + self._tasks.discard(task) + if task.cancelled(): + return + exc = task.exception() + if exc is not None and not isinstance(exc, TimeoutError): + # TimeoutError is the awaited-Action contract, surfaced to the + # caller; everything else on a fire-and-forget task is logged here. + LOGGER.warning("background command failed", error=str(exc)) + + # ---- collection protocol ------------------------------------------- + + def __iter__(self) -> Iterator[Bot]: + return iter(list(self._bots.values())) + + def __len__(self) -> int: + return len(self._bots) + + def __getitem__(self, address: str) -> Bot: + return self._bots[address] + + @property + def bots(self) -> list[Bot]: + return list(self._bots.values()) + + # ---- fleet + link --------------------------------------------------- + + @property + def all(self) -> Fleet: + return Fleet(self._bots.values()) + + def select(self, predicate: Callable[[Bot], bool]) -> Fleet: + return Fleet(bot for bot in self._bots.values() if predicate(bot)) + + async def map_size(self) -> tuple[int, int]: + """The controller's arena size as (width, height) in millimetres.""" + return await self._backend.fetch_map_size() + + @property + def link(self) -> LinkProfile | None: + # TODO: read GET /controller/link once the endpoint exists; until then + # report a minimal Mari profile (host position rate, no gateway budget). + return LinkProfile(kind=self._backend.kind, position_rate_hz=2.0, gateways=()) + + # ---- events + telemetry -------------------------------------------- + + def on(self, event_type: type[Event], callback: Callable[[Event], object]) -> None: + """Register `callback(event)` for an Event class (e.g. + `swarm.on(PositionUpdate, cb)`). The callback may be sync or async + (async is scheduled). Register on `Event` to receive every event.""" + self._handlers.setdefault(event_type, []).append(callback) + + def _emit(self, event: Event) -> None: + for event_type in (type(event), Event): + for callback in self._handlers.get(event_type, ()): + # A raising user handler must not propagate into the backend's + # ws read loop (it would silently drop and reconnect the + # websocket) nor starve the other subscribers. + try: + result = callback(event) + except Exception: # noqa: BLE001 + LOGGER.exception( + "event handler failed", event_type=type(event).__name__ + ) + continue + if asyncio.iscoroutine(result): + self._schedule(result) + for queue in self._event_queues: + queue.put_nowait(event) + + async def events(self) -> AsyncIterator[Event]: + """Async-iterate discrete events: `async for event in swarm.events():`.""" + queue: asyncio.Queue = asyncio.Queue() + self._event_queues.add(queue) + try: + while True: + yield await queue.get() + finally: + self._event_queues.discard(queue) + + async def positions( + self, *, rate_hz: float | None = None + ) -> AsyncIterator[dict[str, Position]]: + """Yield `{address: Position}` snapshots at a declared rate. `rate_hz` + None clamps to the link's host position rate; a higher request is + clamped with a one-time notice - you cannot sample faster than the link + reports.""" + max_rate = self.link.position_rate_hz if self.link else None + if rate_hz is None: + rate_hz = max_rate or 2.0 + elif max_rate and rate_hz > max_rate: + if not self._positions_clamped: + self._positions_clamped = True + LOGGER.warning( + "positions rate clamped to link budget", + requested_hz=rate_hz, + link_hz=max_rate, + ) + rate_hz = max_rate + period = 1.0 / rate_hz + while True: + yield { + address: bot.position + for address, bot in self._bots.items() + if bot.position is not None + } + await asyncio.sleep(period) + + async def tick(self, rate_hz: float = 10) -> AsyncIterator[None]: + """Yield once per control cycle at `rate_hz`, paced (drift-corrected) for + budget-aware control loops. For swarm-wide per-bot commands, keep + `rate_hz` at or below the link bottleneck's per-bot command rate; a + higher rate is flagged once - you can issue commands faster than the link + drains them, but they queue.""" + bottleneck = self.link.bottleneck if self.link else None + if ( + bottleneck is not None + and rate_hz > bottleneck.per_bot_command_rate_hz + and not self._tick_warned + ): + self._tick_warned = True + LOGGER.warning( + "tick rate exceeds per-bot command budget", + requested_hz=rate_hz, + budget_hz=bottleneck.per_bot_command_rate_hz, + ) + period = 1.0 / rate_hz + loop = asyncio.get_running_loop() + next_tick = loop.time() + while True: + yield + next_tick += period + delay = next_tick - loop.time() + if delay > 0: + await asyncio.sleep(delay) + else: + next_tick = loop.time() # body overran the period; resync + + async def close(self) -> None: + if self._shepherd is not None: + await self._shepherd.close() + # Flush pending fire-and-forget commands (e.g. a final stop()) before + # tearing down, so they are not lost on shutdown - cancelling them would + # strand a bot mid-move. Bounded so a stuck async callback can't hang us. + pending = [t for t in self._tasks if not t.done()] + if pending: + try: + await asyncio.wait_for( + asyncio.gather(*pending, return_exceptions=True), timeout=2.0 + ) + except asyncio.TimeoutError: + for task in pending: + task.cancel() + await self._backend.close() + + # ---- launcher ------------------------------------------------------- + + @classmethod + def run( + cls, + fn: Callable, + *, + conn: str | None = None, + collision_avoidance: bool = False, + min_separation: float | None = None, + ) -> None: + """Parse argv (--swarm-url, or --host/--port), connect, run `fn(swarm)`, + and tear down on Ctrl-C. The zero-ceremony entry point for scripts. + + `collision_avoidance=True` (or the `--collision-avoidance` flag, so an + operator can force it on any script without editing it) shepherds all + motion commands around other bots and the arena walls - see + `Swarm.connect`.""" + parser = argparse.ArgumentParser() + parser.add_argument("--swarm-url", default=conn or "http://localhost:8000") + parser.add_argument("--host", default=None) + parser.add_argument("--port", type=int, default=None) + parser.add_argument("--collision-avoidance", action="store_true") + args, _ = parser.parse_known_args() + # Honor --host and/or --port whenever either is given (so `--port 9000` + # alone works); otherwise fall back to --swarm-url. + if args.host is not None or args.port is not None: + url = f"http://{args.host or 'localhost'}:{args.port or 8000}" + else: + url = args.swarm_url + + async def _main() -> None: + async with cls.connect( + url, + collision_avoidance=collision_avoidance or args.collision_avoidance, + min_separation=min_separation, + ) as swarm: + await fn(swarm) + + try: + asyncio.run(_main()) + except KeyboardInterrupt: + pass diff --git a/dotbot/tests/test_swarm_avoid.py b/dotbot/tests/test_swarm_avoid.py new file mode 100644 index 00000000..ff81b235 --- /dev/null +++ b/dotbot/tests/test_swarm_avoid.py @@ -0,0 +1,202 @@ +# SPDX-FileCopyrightText: 2026-present Inria +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Unit tests for the buffered-Voronoi-cell collision avoidance geometry.""" + +import math + +from dotbot.swarm.avoid import bvc_waypoint, safe_hop + +ARENA = (3000, 3000) + + +def dist(a, b): + return math.hypot(a[0] - b[0], a[1] - b[1]) + + +def test_goal_through_neighbor_stops_at_buffered_bisector(): + pos = {"A": (1000.0, 1500.0), "B": (2000.0, 1500.0)} + wp = bvc_waypoint("A", pos, (2500.0, 1500.0), ARENA, max_step=10000) + # midpoint is x=1500, buffer 150 -> the waypoint may not pass x=1350 + assert abs(wp[0] - 1350) < 1 + assert abs(wp[1] - 1500) < 1 + + +def test_goal_inside_own_cell_is_returned_unchanged(): + pos = {"A": (1000.0, 1500.0), "B": (2000.0, 1500.0)} + assert bvc_waypoint("A", pos, (1200.0, 1200.0), ARENA, max_step=10000) == ( + 1200.0, + 1200.0, + ) + + +def test_max_step_caps_the_hop(): + pos = {"A": (1000.0, 1500.0), "B": (2000.0, 1500.0)} + wp = bvc_waypoint("A", pos, (2500.0, 1500.0), ARENA, max_step=100) + assert dist(wp, pos["A"]) <= 100.001 + + +def test_goal_outside_arena_is_clamped_to_wall_margin(): + pos = {"A": (2800.0, 1500.0), "B": (1000.0, 1000.0)} + wp = bvc_waypoint("A", pos, (3500.0, 1500.0), ARENA, max_step=10000) + assert wp[0] <= 2850.0 # 3000 - wall margin + + +def test_intruders_flee_apart_never_toward(): + pos = {"A": (1500.0, 1500.0), "B": (1620.0, 1500.0)} + wa = bvc_waypoint("A", pos, (2500.0, 1500.0), ARENA) + wb = bvc_waypoint("B", pos, (500.0, 1500.0), ARENA) + assert wa[0] < 1500.0 # A moves away from B even though its goal is past B + assert wb[0] > 1620.0 + + +def test_recovery_still_respects_other_neighbors(): + # A has an intruder B to the east and a well-spaced C to the west: + # fleeing B must not cross C's buffered bisector. + pos = {"A": (1500.0, 1500.0), "B": (1600.0, 1500.0), "C": (1100.0, 1500.0)} + wp = bvc_waypoint("A", pos, (2500.0, 1500.0), ARENA) + assert wp[0] >= 1449.0 # bisector at 1300 + 150 buffer + + +def test_waypoints_of_two_bots_never_violate_the_floor(): + # For any pair both heading at each other, commanded waypoints stay + # >= 2*safe_radius apart - the BVC invariant the demos rely on. + pos = {"A": (1200.0, 1500.0), "B": (1800.0, 1500.0)} + wa = bvc_waypoint("A", pos, pos["B"], ARENA) + wb = bvc_waypoint("B", pos, pos["A"], ARENA) + assert dist(wa, wb) >= 300.0 - 1e-6 + + +def test_safe_hop_aligned_returns_plain_waypoint(): + pos = {"A": (1000.0, 1500.0), "B": (2600.0, 1500.0)} + goal = (1000.0, 2000.0) # straight +y, heading 0 = +y -> aligned + assert safe_hop("A", pos, goal, ARENA, heading=0.0) == bvc_waypoint( + "A", pos, goal, ARENA + ) + + +def test_safe_hop_misaligned_in_crowd_yields_to_lower_address(): + # B is 280 mm from A (inside floor+yield gap); A points away from its hop. + pos = {"B": (1500.0, 1500.0), "C": (1780.0, 1500.0)} + # C's hop points -x ... heading +x (=-90 in firmware frame is +x; 90 is -x) + wp = safe_hop("C", pos, (2500.0, 1500.0), ARENA, heading=90.0) + assert wp == pos["C"] # yields: stop in place + + +def test_safe_hop_misaligned_mover_takes_short_bites(): + pos = {"B": (1500.0, 1500.0), "C": (1780.0, 1500.0)} + # B is the lowest address, so it moves - but it faces +x (heading -90) + # while its hop points -x, so the hop is capped to a short bite. + wp = safe_hop("B", pos, (500.0, 1500.0), ARENA, heading=-90.0, yield_ok=True) + hop = dist(wp, pos["B"]) + assert 0 < hop <= 80.001 + + +def test_safe_hop_yield_override_lets_the_stuck_bot_move(): + pos = {"B": (1500.0, 1500.0), "C": (1780.0, 1500.0)} + wp = safe_hop("C", pos, (2500.0, 1500.0), ARENA, heading=90.0, yield_ok=False) + assert wp != pos["C"] + + +def test_boxed_in_bot_stands_still(): + pos = { + "A": (1500.0, 1500.0), + "N": (1500.0, 1720.0), + "S": (1500.0, 1280.0), + "E": (1720.0, 1500.0), + "W": (1280.0, 1500.0), + } + wp = bvc_waypoint("A", pos, (2500.0, 1500.0), ARENA) + assert dist(wp, pos["A"]) < 1e-6 + + +def test_duplicate_positions_do_not_crash(): + # Real LH2 feeds can report two bots at the same coordinates; the clip + # must tolerate the degenerate geometry instead of dividing by zero. + pos = { + "A": (1500.0, 1500.0), + "B": (1700.0, 1500.0), + "C": (1700.0, 1500.0), # duplicate of B + "D": (1500.0, 1700.0), + } + wp = bvc_waypoint("A", pos, (2500.0, 2500.0), ARENA) + assert math.isfinite(wp[0]) and math.isfinite(wp[1]) + + +# ---- Bot position gating (real-LH2 tolerance) ------------------------------- + +from dotbot.models import DotBotLH2Position, DotBotModel # noqa: E402 +from dotbot.swarm.bot import Bot # noqa: E402 + + +def _model(x=None, y=None, direction=0): + lh2 = None if x is None else DotBotLH2Position(x=x, y=y, z=0) + return DotBotModel( + address="aaaa", last_seen=0, lh2_position=lh2, direction=direction + ) + + +def test_zero_zero_fix_is_not_a_position(): + bot = Bot(None, _model(0, 0, direction=-1000)) + assert bot.position is None + assert bot.direction is None + + +def test_glitch_jump_is_held_until_confirmed(): + bot = Bot(None, _model(1000, 1000)) + assert (bot.position.x, bot.position.y) == (1000, 1000) + bot._apply(_model(2500, 1000)) # implies an impossible speed + assert (bot.position.x, bot.position.y) == (1000, 1000) # held + bot._apply(_model(2510, 1000)) # second consistent report: accepted + assert (bot.position.x, bot.position.y) == (2510, 1000) + + +def test_lost_fix_keeps_last_known_position(): + bot = Bot(None, _model(1000, 1000)) + bot._apply(_model(0, 0, direction=-1000)) # fix lost mid-run + assert (bot.position.x, bot.position.y) == (1000, 1000) + + +# ---- Swarm event semantics --------------------------------------------------- + +from dotbot.swarm.events import BatteryUpdate # noqa: E402 +from dotbot.swarm.swarm import Swarm # noqa: E402 + + +def _bat_model(battery): + return DotBotModel(address="aaaa", last_seen=0, battery=battery) + + +def test_battery_update_compares_against_last_emitted(): + swarm = Swarm(object()) + bot = Bot(swarm, _bat_model(3.0)) + swarm._bots[bot.address] = bot + got = [] + swarm.on(BatteryUpdate, lambda e: got.append(e.battery)) + # First report emits; then a slow drain in 0.01 V steps must emit again + # once the cumulative drop from the last *emitted* value reaches 0.05. + for v in (3.0, 2.99, 2.98, 2.97, 2.96, 2.95, 2.94): + before = (bot.position, bot.battery, bot.mode, bot._status) + bot._apply(_bat_model(v)) + swarm._emit_changes(bot, *before) + assert got[0] == 3.0 + assert len(got) >= 2 # the drain crossed the threshold exactly once + assert got[1] <= 2.95 + + +def test_raising_handler_does_not_break_other_handlers(): + swarm = Swarm(object()) + bot = Bot(swarm, _bat_model(3.0)) + swarm._bots[bot.address] = bot + seen = [] + + def bad(_event): + raise RuntimeError("user bug") + + swarm.on(BatteryUpdate, bad) + swarm.on(BatteryUpdate, lambda e: seen.append(e)) + before = (bot.position, bot.battery, bot.mode, bot._status) + bot._apply(_bat_model(2.0)) + swarm._emit_changes(bot, *before) # must not raise + assert len(seen) == 1