171 lines
13 KiB
Markdown
171 lines
13 KiB
Markdown
# 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`; 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`, 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` |
|
|
| `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** |
|
|
| 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
|
|
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.
|
|
|
|
## 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ể:
|
|
|
|
- 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.
|