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:

cancel_requested
pause_requested
plan mới sẵn sàng
phản hồi của controller / recovery / action
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

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

Mỗi cycle có đúng một nguồn vận tốc.
run_controller, tick_recovery và tick_action đôi một không bao giờ cùng bật.
Ở 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.
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.

9.5 KiB Raw Blame History