optimal & fix file cmake

docs/STATE_MACHINE.md

@@ -11,9 +11,9 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
 | State | Ai phát cmd_vel | Vào state khi | Ra khi |
 |---|---|---|---|
 | `IDLE` | không ai (0) | khởi động; cycle ngay sau một state terminal | có `NavigationRequest` đang chờ → `PLANNING`; yêu cầu chỉ-có-action (`has_goal == false`, D8) → `EXECUTING_ACTIONS`; yêu cầu không có goal lẫn action → `ABORTED` (tự vệ) |
-| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; quá `planner_patience` hoặc quá `max_planning_retries` → `RECOVERING(planning_failed)` |
+| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; global planner chính fail/plan rỗng và có backup chưa dùng → đổi sang backup, lập plan lại; backup fail (hoặc không có backup) / quá `planner_patience` / quá `max_planning_retries` → `RECOVERING(planning_failed)` |
 | `CONTROLLING` | **local planner** | có plan hợp lệ; tiếp tục sau tạm dừng | `isGoalReached` → `SUCCEEDED` (hết action) hoặc `EXECUTING_ACTIONS` (còn action — D8); quá `controller_patience` → `RECOVERING(controlling_failed)`; quá `oscillation_timeout` → `RECOVERING(oscillation)`; không sinh được lệnh (còn kiên nhẫn, còn pose) → `PLANNING` |
-| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, chỉ số behavior tăng 1; hết behavior khi định vào → `ABORTED` |
+| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, cursor của **route thuộc trigger đó** tăng 1; hết route khi định vào → `ABORTED` |
 | `EXECUTING_ACTIONS` | không ai (0) — **D8** | tới goal mà yêu cầu còn action; nhận yêu cầu `has_goal == false` | action xong mà còn action kế → start action kế (ở nguyên state); action cuối `succeeded` → `SUCCEEDED`; một action `failed` → `ABORTED` (không qua recovery); quá `action_patience` (nếu bật) → cancel action + `ABORTED`; `cancel()` → `CANCELLING`; `pause()` → `PAUSED` (không huỷ action) |
 | `PAUSED` | không ai (0) | `pause()` từ `PLANNING`/`CONTROLLING`/`RECOVERING`/`EXECUTING_ACTIONS` | `resume()` → về state trước đó; `cancel()` → `CANCELLING` |
 | `CANCELLING` | không ai (0) | `cancel()` từ mọi state đang chạy | robot đã dừng → `CANCELLED` |
@@ -76,7 +76,7 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
 | `planning_retries_` (đếm `max_planning_retries`) | như trên | — |
 | `last_valid_control_` (đo `controller_patience`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; recovery chạy xong; controller sinh được lệnh hợp lệ | **có plan mới** |
 | `last_oscillation_reset_` (đo `oscillation_timeout`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; robot đi được quá `oscillation_distance` | **có plan mới** |
-| `recovery_index_` | nhận yêu cầu mới | — |
+| cursor các recovery route | nhận yêu cầu mới | route của trigger khác; cursor chỉ tăng sau lượt recovery của chính trigger đó |
 | `action_started_at_` (đo `action_patience`, D8) | start một action (trần tính cho TỪNG action); tiếp tục sau tạm dừng (quãng dừng không tính vào trần) | — |
 
 Hai ô "không đặt lại khi có plan mới" là điểm dễ sai nhất và đã từng sai trong lúc thi công: nếu làm
@@ -84,6 +84,57 @@ mới hai đồng hồ đó mỗi lần có plan, vòng lặp `CONTROLLING → P
 hạn, và một controller hỏng vĩnh viễn sẽ không bao giờ chạm `controller_patience`. Test
 `ControllerPatienceSurvivesReplanLoop` giữ tính chất này.
 
+## Global planner dự phòng
+
+`backup_global_planner` là một alias tùy chọn ở root config. Khi global planner active trả `false`
+hoặc plan rỗng, `ControlLoop` đổi sang alias này **một lần duy nhất cho mỗi request**, giữ nguyên
+local planner và state `PLANNING`. Lượt backup thành công đi bình thường vào `CONTROLLING`; lượt
+backup fail mới được đưa vào `PlannerFeedback::kFailed`, nên state machine đi theo recovery hiện có.
+
+Backup dùng overload `makePlan(start, goal, plan)` (không mang VDA5050 `Order`) để
+`SBPLLatticePlanner` dùng được khi `CustomPlanner` của position fail. Đây là đường lùi hình học:
+không được kỳ vọng giữ trajectory/edge metadata riêng của `CustomPlanner`. Backup không kích hoạt
+khi planner bị treo — worker plugin không có cancel cưỡng bức; `planner_patience` vẫn là hàng rào
+cho trường hợp đó.
+
+## Recovery routes
+
+`recovery/behaviors` là registry toàn bộ plugin có thể dùng; `recovery/routes` chọn **tên instance**
+theo trigger, không phụ thuộc thứ tự nạp plugin:
+
+```yaml
+recovery:
+  behaviors:
+    - {name: wait,        type: WaitRecovery}
+    - {name: clear,       type: ClearCostmapRecovery}
+    - {name: detour_path, type: DetourPathRecovery}
+    - {name: rotate,      type: RotateRecovery}
+    - {name: back_up,     type: BackUpRecovery}
+  routes:
+    planning_failed:    [wait, clear, rotate, back_up]
+    controlling_failed: [wait, clear, detour_path, rotate, back_up]
+    oscillation:        [detour_path, rotate, back_up]
+```
+
+Khi dựng runtime, `RecoveryRunner` nạp registry trước rồi resolve tên route thành index thật; chỉ
+sau đó `NavigationRuntime` mới gán `recovery_behavior_count` và các route này vào
+`StateMachineConfig`. Route phải khai đủ cả ba trigger, không rỗng sau resolve, không lặp tên và
+không có trigger lạ. Schema cũ không có `routes` vẫn tương thích: cả ba trigger dùng toàn bộ registry
+theo thứ tự nạp.
+
+`DetourPathRecovery` hiện chưa có plugin/library. Vì vậy entry `detour_path` có thể được commit trước:
+registry báo plugin thiếu, `RecoveryRunner` cảnh báo và bỏ riêng tên đó khỏi route; không bao giờ
+đưa index giả vào state machine. Với config hiện tại, trước khi plugin được thêm, route hữu hiệu là
+`planning=[wait, clear, rotate, back_up]`, `controlling=[wait, clear, rotate, back_up]`,
+`oscillation=[rotate, back_up]`. Khi plugin SBPL được nạp thành công, hai route sau tự có
+`detour_path`, không cần sửa move_base2.
+
+Mỗi request giữ ba cursor độc lập. Một lượt behavior `succeeded` **hoặc** `failed` luôn quay về
+`PLANNING` để lập đường mới và tiêu thụ một phần tử của route đã kích hoạt; lần lỗi kế tiếp cùng
+trigger thử phần tử sau. Lỗi bởi trigger khác dùng cursor của route khác. Log runner ghi `registry
+index`, không phải vị trí trong route, để không đánh lừa vận hành khi một behavior bị dùng ở nhiều
+route.
+
 ## Mất pose (TF thiếu hoặc quá hạn)
 
 Không biết robot đang ở đâu thì không được cho nó chạy. Cụ thể: