How I built a multi-robot warehouse planner, and why green CI was not enough

I built multi-robot-warehouse-planner to learn how a fleet of warehouse robots can share narrow aisles without colliding. It is a CPU-only simulation demo of 4 to 12 robots on a fixed 24x14 grid. It is not connected to real robots, a fleet manager, or any production warehouse system.

Image: multi-robot-warehouse-planner repo.

This post is about how I checked it. CI was green early. Then I ran the fleet for a long time with fixed seeds, and it stopped delivering. Here is the planner, the harness that caught them, and each fix, using the code at the ea74426 merge.

The warehouse and the tick loop

The map has shelf pockets, aisles, 5 chargers along the top, pick stations along the bottom, and parking cells. Robots are single-cell points: no footprint, kinematics or sensors.

SimulationEngine in app/simulation.py runs one tick loop: apply commands, run recovery checks, allocate orders, replan, validate the next joint step, then move. app/allocator.py offers greedy_assign (nearest robot, first come first served) and optimal_assign (scipy.optimize.linear_sum_assignment), both with a battery reserve check. app/battery.py drains 0.5% per move, charges 5% per tick on a charger, sends robots to charge below 40%, and releases them at 90%.

The planner chain: CBS, then PP, then HOLD

Conflict-Based Search (CBS) plans each robot alone with space-time A*, finds the first collision between plans, and branches: each child adds one constraint to one robot. In app/planner/cbs.py, _find_conflict walks padded paths tick by tick and returns the first vertex (same cell) or edge (swap) conflict:

app/planner/cbs.py:

                if ca == cb:
                    return Conflict("vertex", a, b, ca, cb, k)
                if k > 0:
                    pa_prev = pa[k - 1] if k - 1 < len(pa) else pa[-1]
                    pb_prev = pb[k - 1] if k - 1 < len(pb) else pb[-1]
                    if ca == pb_prev and cb == pa_prev and ca != cb:
                        return Conflict("edge", a, b, pa_prev, pb_prev, k)

In app/planner/astar_st.py, the low-level goal test only accepts arrival after the latest constraint on the goal cell, and only if the robot can stay there. That handles target conflicts, where one robot parks on a cell another must still cross:

app/planner/astar_st.py:

        if cell == goal and k > goal_latest and can_remain_at_goal(k):

A wait also costs 1 unless the robot is already at its goal; free waiting let CBS return valid but costlier plans.

Replanning has a wall-clock budget of min(1.5, 0.3 + 0.1*(n-4)) seconds: 0.3 s at 4 robots, 1.1 s at 12.

app/simulation.py:

    def _replan_time_budget_s(self) -> float:
        extra = max(0, self.robot_count - 4)
        return min(1.5, 0.3 + 0.1 * extra)

app/planner/chain.py gives CBS (400 node cap) 45% of that budget for five or fewer moving agents and 65% for more. If CBS fails, prioritized planning (PP) tries several robot orderings against a reservation table. If that fails, HOLD keeps the previous plan if still valid, or everyone waits. Every executed step is also validated before motion: no shared cell, no swap, no jump. An invalid step makes everyone wait and replan.

Why green CI was not enough

Short tests start clean: full batteries, empty chargers, robots spread out. Real failures need time. So I wrote a seeded long-run harness. tests/test_fleet_long_run.py checks every tick for collisions, for "charging" off a charger cell, and for robots parked on a charger at full charge, then counts real deliveries per 100-tick bucket:

tests/test_fleet_long_run.py:

def test_fleet_completes_every_hundred_ticks_after_warmup() -> None:
    ticks = 800
    for robot_count in (4, 8, 12):
        for seed in (1, 2, 3):
            r = _run_long(seed, robot_count, ticks=ticks)
            assert r["collisions"] == 0
            assert r["off_charger_charging"] == 0
            assert r["max_full_charge_streak"] <= 5
            for count in r["buckets"][1:]:
                assert count >= 1
            assert r["deadlocks"] <= robot_count * 2

Every bucket after warmup needs at least one delivery, so a fleet that freezes partway fails even if the total looks fine. I count per 100 ticks, not per minute, because the tick rate is a UI setting.

Bug 1: false stalls

A robot counts as stalled after 15 ticks without progress. The first version treated any move that did not end at the goal as no progress, so long legs looked stalled: the robot dropped its order and went to park. _made_progress now counts advancing along the committed path, or, as in this fallback, getting closer by BFS distance:

app/simulation.py:

        if robot.goal:
            dist = self.grid.bfs_distances(robot.goal)
            before = dist.get(old_pos, 10**6)
            after = dist.get(robot.position, 10**6)
            if after < before:
                return True
        return False

Bug 2: charging became a terminal state

In long runs every robot ended up parked on a charger at full charge. Charge completion only ran inside a replan, and with everyone charging, nothing queued one. Even then, status stayed "charging", so planner and allocator both ignored the robot. Now _finish_charging_if_ready runs every tick, then gives the robot a parking goal and queues a replan:

app/simulation.py:

    def _finish_charging_if_ready(self, r: Robot) -> None:
        if r.battery_pct < CHARGE_COMPLETE_THRESHOLD:
            return
        if r.leg != "charging" and r.status != "charging":
            return
        if not self._on_charger_cell(r.position):
            return
        blocked = set(self.blocked_cells)
        r.leg = "idle"
        r.status = "idle"

Bug 3: phantom charging

The charge branch never checked the cell. After the stall handler rewrote a goal, a robot could "charge" on an aisle or a pick station and block it. Now both the cell and the goal must be chargers:

app/simulation.py:

                    on_charger = self._on_charger_cell(r.position)
                    goal_is_charger = r.goal is not None and self._on_charger_cell(r.goal)
                    if (
                        on_charger
                        and goal_is_charger
                        and r.leg in ("charging", "to_charger")
                        and r.position == r.goal
                    ):

In every final run there was 0 off-charger charging and 0 robots charging more than 5 ticks at 100%. Deadlocked robots now recover after 25 ticks with counters reset.

Bug 4: one dead battery froze the fleet

In a 1600-tick run with 12 robots, one robot drained to 0% on its way to a busy charger, and no robot moved again. _validate_step added every dead robot's cell to the blocked set, including that robot's own "stay" cell, so even an all-wait step failed and the engine held everyone forever. The bug dated back to the initial commit. The fix collects dead robots into dead_positions, lets each stay put, and makes everyone else avoid those cells:

app/simulation.py:

            if r.robot_id in dead_positions:
                if c != r.position:
                    return False
                continue
            if c in dead_positions.values():
                return False

Depleted robots are also treated as static obstacles in planning. For the dead robot I chose demo behavior: after 120 ticks at 0% off a charger, the simulation teleports it to a free charger at 55%. That is not a real tow. It deviates from my original plan, and the README discloses it.

Bug 5: cancelled orders shown as completed

Orders that wait too long without a robot get cancelled. The old code did that by setting completed_tick, so the UI showed cancelled orders as "completed". Order now has a separate cancelled_tick, the API reports "cancelled", and metrics.orders_cancelled_total counts them. In every final run, 0 cancelled orders were shown as completed.

Bug 6: orders cancelled the moment they were released

Honest cancellations exposed too many of them at 12 robots, with free robots nearby. The stale check measured age from creation, not from when the order lost its robot, and it ran before allocation. So when a stall released an old order, the order was cancelled in the same tick, before the allocator could hand it to a free robot.

Now every release path goes through one helper that stamps unassigned_since_tick, and the cancel counts from that stamp, using 80 ticks:

app/simulation.py:

    def _unassign_order(self, order: Order) -> None:
        order.assigned_robot_id = None
        order.pick_station = None
        order.picked_tick = None
        order.unassigned_since_tick = self.tick

    def _cancel_stale_unassigned_orders(self) -> None:
        for o in self.orders:
            if not o.is_open() or o.assigned_robot_id is not None:
                continue
            if o.unassigned_since_tick is None:
                continue
            if self.tick - o.unassigned_since_tick < 80:
                continue
            o.cancelled_tick = self.tick
            self.metrics.record_cancellation()
            self._replan_queued = True

The allocator clears the stamp on assignment. And step() now runs allocation before the cancel:

app/simulation.py:

        self._release_stale_order_assignments()
        self._fleet_unfreeze_watchdog()

        pending = [
            o for o in self.orders if o.assigned_robot_id is None and o.is_open()
        ]
        if pending and any(
            r.current_order_id is None and r.status not in ("charging", "deadlocked", "blocked")
            for r in self.robots
        ):
            self._run_allocation()

        self._cancel_stale_unassigned_orders()

A new test, tests/test_stale_cancel_reassign.py, releases an order from a stalled robot and checks that the free robot r2 gets it and the cancel leaves it alone.

After the fix, cancels are rare: 0 to 1 per 1600 ticks at 12 robots, and 0 at 4 and 8 robots. The single cancel left, in the 12-robot seed 1 run at tick 1212, was a never-assigned order that sat unassigned for 80 ticks while the only free robot was battery-ineligible. That one is legitimate.

The final numbers

The final check ran at 2fcb4f9, which is the same tree as the main merge ea74426. 54 tests pass including e2e, ruff is clean, and CI run 39896439791 is green on all three jobs (lint-and-unit, e2e, docker). "Delivered" counts only real deliveries on the order's assigned pick station, and equals the throughput_total metric. Every run had 0 collisions and 0 non-tow jumps.

Robots	Ticks	Seed 1 / 2 / 3 delivered	Min bucket	Cancelled	Tows	Deadlocks	HOLD % of replans
4	800	68 / 66 / 66	5 / 6 / 7	0 / 0 / 0	0 / 0 / 0	0 / 0 / 0	0.0 / 0.0 / 0.5
8	800	125 / 130 / 119	13 / 14 / 10	0 / 0 / 0	0 / 0 / 0	0 / 0 / 0	4.0 / 6.8 / 6.6
12	1600	261 / 255 / 253	4 / 10 / 5	1 / 0 / 0	1 / 2 / 1	14 / 13 / 18	21.2 / 25.2 / 20.3

Throughput is about 8 deliveries per 100 ticks with 4 robots, about 15.5 with 8, and about 16 with 12. Going from 8 to 12 robots adds little on this map because of congestion: HOLD rises to 20 to 25% of replans, there are occasional dips (minimum buckets of 4 and 5), deadlocks happen and recover, and robots are sometimes towed.

What these numbers do not say

At 12 robots CBS still handles most replans, but HOLD covers about 20 to 25% of them, and PP is rare, under 1% (5 to 10 replans per long run). The README now says this plainly: HOLD roughly 17-30% at 12 robots, PP rare.

Replanning is wall-clock budgeted, so results vary a little with CPU load; these runs were 9 parallel processes on an 8-core box. The CBS optimality test uses an exact brute-force oracle only on tiny 2-agent grids. The 120-tick tow is demo behavior, and there are no real robots or sensors.

Run it yourself

make install
make run

Open http://localhost:8000. Raise the robot count to 12 and watch HOLD climb.

Green tests prove the pieces work; long seeded runs prove the system keeps working. See also my browser teleop console tutorial, and for single-robot navigation in a full simulator, the ROS 2 Nav2 voice command robot.