|
| 1 | +# Reactive row executor — design note |
| 2 | + |
| 3 | +*Status: design / scoping. No code yet — this is the shape to decide on.* |
| 4 | + |
| 5 | +## Why |
| 6 | + |
| 7 | +The coverage **planner** decides *what* to clean and in *what order* (cells → |
| 8 | +serpentine passes → gap-fill). Today it also **executes** motion by firing one |
| 9 | +Nav2 `NavigateToPose` goal per waypoint. Measured on the living_room sim, that |
| 10 | +execution path is the wrong tool for the job: |
| 11 | + |
| 12 | +- **~3 s of overhead per waypoint** — drive ~0.4 m, reach the goal tolerance, |
| 13 | + wait for `bt_navigator` to declare success, send the next goal. At ~190 |
| 14 | + waypoints that is **~10 minutes for one clean even when nothing goes wrong**, |
| 15 | + most of it goal-handshake, not cleaning. |
| 16 | +- **Costmap phantom** — RPP's forward collision check stopped the robot short of |
| 17 | + obstacles (fixed by disabling it, but the machinery is still fighting us). |
| 18 | +- **Corner-cutting** — pure pursuit arcs through row-end U-turns. |
| 19 | +- **Collision-averse recoveries** — Nav2 `spin`/`backup` refuse to move in a |
| 20 | + wedge (footprint in lethal cells), so they stall exactly when needed. |
| 21 | + |
| 22 | +None of this is a bug to patch; it is Nav2 being a *point-to-point, costmap-aware* |
| 23 | +navigator asked to do *coverage*, whose motion is really **drive-straight → |
| 24 | +turn → drive-straight → (bump) edge-follow**. That motion wants a small reactive |
| 25 | +controller, not a global planning stack. |
| 26 | + |
| 27 | +## Principle (subsumption / hybrid) |
| 28 | + |
| 29 | +Split *planning* from *executing*, and use each tool where it is strong: |
| 30 | + |
| 31 | +| Concern | Owner | |
| 32 | +|---|---| |
| 33 | +| What to clean, pass order, gap-fill | **coverage planner** (done) | |
| 34 | +| Driving a sweep pass (straight line + row-end turn) | **reactive row executor** (new) | |
| 35 | +| Contact reflex (peel off / edge-follow) | **bumper escape** (built) — overrides the row drive while in contact | |
| 36 | +| Longer inter-cell transit, routing around furniture, go-to-dock | **Nav2** `NavigateToPose` (kept) | |
| 37 | +| Localization | **Nav2 AMCL** (kept) | |
| 38 | + |
| 39 | +Coverage plans the open floor; the executor drives it; the bumper reflex handles |
| 40 | +the last few centimetres; Nav2 is demoted to the few jobs it is actually good at. |
| 41 | +This is the reactive-control layer the recovery-safety RFC anticipated. |
| 42 | + |
| 43 | +## How it slots in — as a mode of the coverage planner (at first) |
| 44 | + |
| 45 | +The `coverage_planner` node **already has everything the executor needs**: a |
| 46 | +`cmd_vel` publisher (used by the escape), `bumper_left|right/contact` |
| 47 | +subscriptions, the `amcl_pose` subscription, the 10 Hz `_tick` loop, and the |
| 48 | +contact-aware peel-off. So the executor starts life as a **RowDriver mode inside |
| 49 | +the existing node**, not a new process — minimal new infrastructure, and it |
| 50 | +reuses the escape reflex directly. If it grows, it splits into its own |
| 51 | +`reactive-control` node later (same plan the recovery-safety RFC sketches). |
| 52 | + |
| 53 | +The plan the planner already produces is a list of `(x, y)` waypoints. We tag |
| 54 | +each segment as one of: |
| 55 | + |
| 56 | +- **`row`** — a straight run of collinear waypoints (a sweep pass, and the short |
| 57 | + intra-cell connector to the next pass). Driven reactively. |
| 58 | +- **`transit`** — the hop between two cells (or robot → first cell), which may |
| 59 | + need to route around furniture. Dispatched to Nav2 as one `NavigateToPose`. |
| 60 | + |
| 61 | +`_tick` looks at the current segment's tag and calls the row driver or the Nav2 |
| 62 | +dispatcher. Rows collapse ~190 discrete goals into ~a few dozen continuous |
| 63 | +drives; transits stay ~one Nav2 goal per cell boundary (≈10), where Nav2's |
| 64 | +overhead is negligible and its furniture-avoidance is worth having. |
| 65 | + |
| 66 | +## The row-drive control law (the one genuinely new piece) |
| 67 | + |
| 68 | +Given a segment `(p_start → p_end)` and the robot pose from localization: |
| 69 | + |
| 70 | +1. **Follow the line.** `heading_err = wrap(atan2(p_end − pos) − yaw)`; |
| 71 | + `cross_track = signed perpendicular distance from the p_start→p_end line`. |
| 72 | + `cmd.angular.z = −k_h·heading_err − k_ct·cross_track`; |
| 73 | + `cmd.linear.x = v_cruise`, tapered down as `|heading_err|` grows or near p_end. |
| 74 | +2. **Turn at the row end.** Within `row_end_tol` of `p_end`, stop linear and |
| 75 | + rotate in place to the next segment's heading (`angular.z = k·heading_err`) |
| 76 | + until aligned, then start the next segment. (Crisp 90° corners — the thing |
| 77 | + pure pursuit couldn't give us — for the price of a short in-place rotate, |
| 78 | + which is cheap because it happens per *pass*, not per waypoint.) |
| 79 | +3. **Feedback.** Use `odom` for the high-rate, smooth heading/velocity loop and |
| 80 | + `amcl_pose` to correct odometry drift periodically (a slow vacuum tolerates |
| 81 | + 10 Hz control fine). No costmap in the loop — the robot is *meant* to touch |
| 82 | + things; safety is the bumper. |
| 83 | + |
| 84 | +A pass ends on: **reached `p_end`** (→ next segment), **bumper contact** (→ |
| 85 | +escape reflex, below), **no-progress** (→ skip the pass, same watchdog we just |
| 86 | +added), or **off-map / lost localization** (→ stop, hand to safety). |
| 87 | + |
| 88 | +## How it shares the bumper escape |
| 89 | + |
| 90 | +The contact-aware peel-off already built (held-bumper → rotate away from the |
| 91 | +pressed side, record a no-go pocket) **becomes the executor's contact reflex**, |
| 92 | +unchanged in spirit: |
| 93 | + |
| 94 | +- While driving a row, the executor watches `bumper_*/contact`. Sustained |
| 95 | + contact **overrides** the row drive (subsumption): run the peel-off open-loop |
| 96 | + on `cmd_vel`, then resume the row (or skip it if the pocket is now no-go). |
| 97 | +- **Edge-follow (floor-care)** is the natural next behavior on the *same* signal: |
| 98 | + instead of only peeling off, hug the contour (back a hair → turn out → arc in → |
| 99 | + re-contact) to clean right up to the object. Shared contact plumbing means the |
| 100 | + executor, the wedge escape, and edge cleaning are one reflex layer, not three. |
| 101 | + |
| 102 | +## Interfaces |
| 103 | + |
| 104 | +- **Consumes:** the internal plan (already in-node); `amcl_pose` + `odom` + TF |
| 105 | + `map→base_link`; `bumper_left|right/contact`; `coverage_meter/ratio` (skip |
| 106 | + already-clean passes). |
| 107 | +- **Produces:** `cmd_vel` (rows + escape); `~/plan` Path (done, for RViz); |
| 108 | + optionally `~/status` (current segment, mode: row/transit/escape) for |
| 109 | + observability. Keep the topic/behaviour contract aligned with |
| 110 | + `SOFTWARE_INTERFACES.md`. |
| 111 | +- **New params:** `v_cruise`, `k_heading`, `k_crosstrack`, `row_end_tol`, |
| 112 | + `rotate_speed`, `min_transit_len` (how long a hop must be to go via Nav2 vs. |
| 113 | + drive reactively). All tunable, all with sane defaults. |
| 114 | + |
| 115 | +## Phasing (no big-bang rewrite) |
| 116 | + |
| 117 | +1. **Row driver behind a flag.** Add the RowDriver + segment tagging; `executor:=reactive` |
| 118 | + drives rows reactively and transits via Nav2, `executor:=nav2` keeps today's |
| 119 | + all-Nav2 path. A/B them: **time-to-coverage** should drop sharply (the ~3 s/wp |
| 120 | + overhead disappears), coverage and turns hold or improve. |
| 121 | +2. **Fold in the reflex.** Route the bumper escape through the executor; confirm |
| 122 | + peel-off still frees wedges. Add edge-follow as an opt-in. |
| 123 | +3. **Retire per-row Nav2.** Once the reactive path wins the A/B, rows never touch |
| 124 | + Nav2; Nav2 is transits + dock + localization only. |
| 125 | + |
| 126 | +The `executor:=nav2` fallback stays as the regression baseline and a safety net. |
| 127 | + |
| 128 | +## Risks / open questions |
| 129 | + |
| 130 | +- **Open-loop straightness.** How well can it hold a line on `odom`+`amcl` alone, |
| 131 | + with no costmap? (Mitigation: the cross-track term + amcl drift correction; a |
| 132 | + bump just triggers the reflex — contact is acceptable.) |
| 133 | +- **Dynamic obstacles mid-row** (a foot). No costmap means we rely on the bumper |
| 134 | + and, optionally, **Nav2 Collision Monitor** (`nav2_collision_monitor`) as a |
| 135 | + reactive slow/stop zone from the LiDAR — cheap and composes with this. (Ties |
| 136 | + into Deepak's dynamic-obstacle-yielding work — coordinate.) |
| 137 | +- **Transit boundary.** Deciding which connectors are "rows" vs "transits" — a |
| 138 | + length + free-corridor test; err toward Nav2 when a hop crosses unknown space. |
| 139 | +- **Localization dropout.** If `map→odom` goes stale mid-row, stop and re-localize |
| 140 | + rather than dead-reckon into a wall. |
| 141 | + |
| 142 | +## Testing / acceptance |
| 143 | + |
| 144 | +Headless regression, `executor:=reactive` vs `executor:=nav2` on living_room |
| 145 | +(and a tall room, and a simple rectangle): |
| 146 | + |
| 147 | +- **Time-to-coverage** (the headline win) — expect a large drop. |
| 148 | +- **Coverage %** and **turns** — hold or improve. |
| 149 | +- **Stuck-freedom** — no wedge lasts more than a couple of seconds (reflex + |
| 150 | + no-progress skip). |
| 151 | +- **Regression gate** stays green with the Nav2 fallback so we never lose the |
| 152 | + known-good baseline. |
| 153 | + |
| 154 | +## Related |
| 155 | + |
| 156 | +- `recovery-safety` RFC — this *is* the reactive-control layer it anticipated; |
| 157 | + the wedge escape and edge-follow live here. |
| 158 | +- `floor-care` — edge cleaning rides the same bumper reflex. |
| 159 | +- `clean-and-map` (Deepak) — coordinate the executor boundary with his SLAM + |
| 160 | + coverage work and the dynamic-obstacle yielding. |
| 161 | +- `health-monitor` — stack-liveness / MCU watchdog is orthogonal but shares the |
| 162 | + "reactive, contact-tolerant vacuum" worldview. |
0 commit comments