move_base2/docs/STATE_MACHINE.md

# 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.