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

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.