first commit

docs/STATE_MACHINE.md

@@ -0,0 +1,119 @@
+# State machine
+
+Đây là **nguồn chuẩn** cho hành vi chuyển trạng thái của navigation runtime. Code trong
+`src/state_machine.cpp` phải khớp với bảng dưới đây, và `test/state_machine_test.cpp` là thứ chứng
+minh điều đó — mỗi transition trong tài liệu này có ít nhất một test tương ứng.
+
+Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi mới sửa code.
+
+## Bảng state
+
+| 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)` |
+| `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` |
+| `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` |
+| `SUCCEEDED` / `ABORTED` / `CANCELLED` | không ai (0) | như trên | terminal; báo kết quả rồi về `IDLE` ở cycle kế tiếp |
+
+## Thứ tự ưu tiên trong một cycle
+
+Ở mọi state đang chạy, sự kiện được xét theo đúng thứ tự sau. Thứ tự này là một phần của contract:
+
+1. `cancel_requested`
+2. `pause_requested`
+3. plan mới sẵn sàng
+4. phản hồi của controller / recovery / action
+5. các ngưỡng kiên nhẫn và chống quẩn
+
+## Ba khác biệt so với runtime thế hệ 1
+
+1. **`RECOVERING` thay cho `CLEARING`, và là state có thời lượng.** Nhiều control cycle, không phải
+   một lời gọi blocking. Bắt buộc như vậy vì recovery thế hệ 2 trả kết quả từng tick và có thể phát
+   vận tốc — nghĩa là quyền phát cmd_vel phải chuyển tay từ local planner sang recovery và ngược lại.
+
+2. **`PAUSED` và `CANCELLING` là state thật**, không phải cờ đọc rải rác trong vòng lặp. Đường huỷ ở
+   bản cũ được viết hai lần, ở nhánh `try` và nhánh `catch`, gần 80 dòng giống hệt nhau.
+
+3. **`SUCCEEDED`/`ABORTED`/`CANCELLED` là state**, không phải `return` giữa hàm. Đây là cơ chế giữ
+   bất biến báo-kết-quả-một-lần: cờ `report_outcome` chỉ bật tại cycle bước vào state terminal, và
+   state terminal chỉ sống đúng một cycle.
+
+4. **`EXECUTING_ACTIONS` (D8): runtime điều phối trọn một mission.** Tới goal chưa phải là xong —
+   còn action của mission (nâng/hạ, sạc, chờ…) phải chạy nốt, và kết quả chỉ được báo **sau action
+   cuối**, để mission layer thấy trọn một chặng nav + action. Yêu cầu `has_goal == false` bỏ qua
+   `PLANNING`/`CONTROLLING` và vào thẳng state này.
+
+## Action của mission (D8) — các quyết định robotics
+
+- **Không vận tốc trong khi chạy action.** `EXECUTING_ACTIONS` thuộc nhóm `mustBeStopped`; action
+  cần chuyển động phải được mô hình hoá thành motion profile của navigation. Đây là hàng rào chống
+  hai nguồn điều khiển.
+- **`pause()` đóng băng, không huỷ.** Khác `RECOVERING` (bị huỷ khi tạm dừng vì dead-reckon theo
+  thời gian), action thiết bị không idempotent — chạy lại một lần nâng kệ từ đầu không chắc an
+  toàn. Tạm dừng chỉ ngừng tick; `resume()` tick tiếp đúng action dở dang, không `start` lại.
+- **Action hỏng → `ABORTED` thẳng, không qua recovery.** Recovery behavior là công cụ phục hồi
+  navigation (dọn costmap, lùi, xoay), không giúp gì được một thiết bị đang hỏng. Mission layer là
+  nơi quyết định làm gì với phần còn lại của order.
+- **Mất pose không chặn action.** Robot đứng yên, thao tác thiết bị không cần định vị; vận tốc vẫn
+  bị ép về 0 như mọi state phải dừng. Đồng hồ kiên nhẫn của planner/controller không chạy trong
+  state này — action dài (sạc pin) không được phép bị tính là "controller hỏng".
+- **Timeout 3 tầng, tầng 1 là chính.** (1) Mỗi ActionHandler tự timeout theo hiểu biết thiết bị
+  của nó và trả `failed` — chỉ handler biết "nâng kệ quá 20 s là bất thường" còn "sạc 30 phút là
+  bình thường"; (2) `action_patience` là lưới cuối ở tầng navigation cho handler treo, **mặc định
+  tắt**, tính cho từng action, quá hạn thì cancel action rồi `ABORTED`; (3) `mission_timeout` của
+  mission layer đo cả chặng. Trạng thái host trong lúc chạy action là `ACTIVE` (không phải
+  `CONTROLLING` — host VDA5050 suy `driving = true` từ `CONTROLLING`, mà robot đang đứng yên).
+
+## Đồng hồ và bộ đếm
+
+| Biến | Đặt lại khi | Không đặt lại khi |
+|---|---|---|
+| `last_valid_plan_` (đo `planner_patience`) | nhận yêu cầu mới; bắt đầu một chu kỳ lập plan mới; tiếp tục sau tạm dừng | — |
+| `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 | — |
+| `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
+mới hai đồng hồ đó mỗi lần có plan, vòng lặp `CONTROLLING → PLANNING → CONTROLLING` sẽ liên tục gia
+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.
+
+## 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ể:
+
+- Nguồn vận tốc bị ép về `kNone` ở **mọi** state, kể cả `RECOVERING`.
+- Controller không được gọi.
+- Recovery **vẫn** được tick, để behavior tự báo lỗi theo contract của nó; lệnh nó sinh ra bị chặn.
+- Ở `CONTROLLING`, mất pose được tính là "không sinh được lệnh" nhưng **không** chuyển sang
+  `PLANNING`: lập lại plan không giúp gì khi vấn đề là định vị, và nhảy sang `PLANNING` sẽ khiến lý
+  do vào recovery bị ghi nhận sai thành "lập plan hỏng".
+- Các đồng hồ kiên nhẫn vẫn chạy, nên mất TF kéo dài cuối cùng vẫn dẫn tới `RECOVERING` rồi
+  `ABORTED` — không treo im lặng.
+
+## Bất biến
+
+Bốn tính chất sau được chốt lại ở cuối mỗi lần chuyển state và được test kiểm ở từng cycle:
+
+1. Mỗi cycle có **đúng một** nguồn vận tốc.
+2. `run_controller`, `tick_recovery` và `tick_action` đôi một không bao giờ cùng bật.
+3. Ở state phải dừng (mọi state trừ `CONTROLLING` và `RECOVERING` — gồm cả `EXECUTING_ACTIONS`),
+   nguồn vận tốc luôn là `kNone`.
+4. `report_outcome` bật đúng một lần cho mỗi yêu cầu — kể cả yêu cầu có action: chỉ báo sau action
+   cuối (D8).
+
+## Vì sao `CANCELLING` luôn kết thúc
+
+`CANCELLING` chờ `robot_stopped`. Vì nguồn vận tốc ở state này đã là `kNone` và bộ trọng tài phát
+lệnh 0 tức thì, `robot_stopped` thành true ngay ở cycle kế tiếp. Do đó state này kết thúc sau hữu hạn
+cycle mà không cần thêm tham số timeout nào.
+
+Ở bước nối dây sau, `robot_stopped` nên được lấy từ vận tốc **đo được** (odometry) thay vì từ lệnh
+đã phát. Đó là lý do nó là một trường dữ liệu vào của state machine chứ không phải thứ state machine
+tự tính.