first commit

docs/ARCHITECTURE.md

@@ -0,0 +1,122 @@
+# Kiến trúc
+
+## Ba lớp
+
+```
+┌──────────────────────────────────────────────────────────────────────┐
+│  Biên (adapter)                                                      │
+│                                                                      │
+│  NavigationServer  ──implements──▶  move_base_core::BaseNavigation   │
+│    · quy 6 entry point di chuyển về 1 NavigationRequest              │
+│    · nhận dữ liệu sensor từ host                                     │
+│    · kết xuất trạng thái ra kiểu mà host mong đợi                    │
+└──────────────────────────────────┬───────────────────────────────────┘
+                                   │
+┌──────────────────────────────────▼───────────────────────────────────┐
+│  Điều phối                                                           │
+│                                                                      │
+│  ControlLoop — một control cycle: gom dữ liệu, chạy state machine,   │
+│                thi hành output, phát lệnh vận tốc                    │
+└──────────────────────────────────┬───────────────────────────────────┘
+                                   │
+┌──────────────────────────────────▼───────────────────────────────────┐
+│  Lõi quyết định (logic thuần, không I/O)                             │
+│                                                                      │
+│  StateMachine      — bảng chuyển state                               │
+│  VelocityArbiter   — ai được phát lệnh, và lệnh đó có an toàn không  │
+│  NavigationRequest — kiểu dữ liệu duy nhất đi vào lõi                │
+└──────────────────────────────────────────────────────────────────────┘
+                                   │
+                                   ▼  (qua port, không qua kiểu cụ thể)
+     ClockPort   PosePort   PlannerPort   ControllerPort
+                 RecoveryPort   MissionPort   ActionPort
+```
+
+Lõi không đụng costmap, không đụng TF, không log, không cấp phát trong vòng lặp. Vì vậy nó kiểm được
+bằng bảng thay vì phải dựng runtime hay chạy robot.
+
+## Chiều phụ thuộc
+
+```
+move_base2 ──▶ mission framework
+           ──▶ recovery framework
+```
+
+**Một chiều, không có đường ngược.** Hai framework kia không biết `move_base2` tồn tại và không biết
+lẫn nhau. Hệ quả có thật, không phải hình thức:
+
+- Ba gói test được độc lập.
+- Không có vòng phụ thuộc trong build.
+- `move_base2` không bị khoá cứng vào một hiện thực mission hay recovery cụ thể — đổi framework chỉ
+  cần viết lại lớp nối, không phải sửa lõi.
+
+Ở Phase 1, chiều này còn được giữ ở mức mạnh hơn: **không file nào trong `move_base2` include hai
+framework kia**. Lớp nối (`MissionAdapterBridge`, `RecoveryRunner`) được thêm ở bước sau, và chúng
+mới là chỗ duy nhất được phép include.
+
+Kiểm bằng:
+
+```bash
+grep -rn "mission_adapters\|recovery_core" src/AMR_T800/Test/move_base2/include \
+                                            src/AMR_T800/Test/move_base2/src
+```
+
+## Vì sao là port chứ không phải gọi thẳng
+
+Bảy port đều nhỏ và đều tồn tại vì một lý do vận hành cụ thể:
+
+| Port | Lý do tồn tại |
+|---|---|
+| `ClockPort` | Không có nguồn thời gian tiêm được thì không cách nào kiểm hành vi khi control loop chạy chậm hơn chu kỳ cấu hình — đúng lớp lỗi mà dead-reckoning theo chu kỳ danh nghĩa mắc phải |
+| `PosePort` | Contract "trả false = dừng an toàn" phải kiểm được mà không cần dựng TF thật |
+| `PlannerPort` | Gộp hai overload `makePlan` của interface gốc thành một; "có Order hay không" chỉ là một nhánh nhỏ bên trong |
+| `ControllerPort` | Giữ nguyên bộ hàm và **thứ tự gọi** của interface được bọc (hỏi đã tới đích trước, chỉ khi chưa mới tính lệnh) |
+| `RecoveryPort` | Chuẩn hoá kết quả tick về ngôn ngữ của `move_base2`, để lõi không phải include recovery framework |
+| `MissionPort` | Luồng một chiều có callback, thay cho việc phía mission phải poll trạng thái navigation |
+| `ActionPort` | (D8) Runtime điều phối trọn một mission — nav xong chạy nốt action rồi mới báo kết quả. Tick-based cùng nhịp control loop như recovery, và tick của nó **không có** vận tốc: action cần chuyển động phải là motion profile, không phải action |
+
+## Quyết định thiết kế đáng ghi lại
+
+**Sáu entry point gộp thành một.** `moveTo` ×2, `dockTo` ×2, `moveStraightTo`, `rotateTo` của
+contract host chỉ khác nhau ở kiểu chuyển động và sai số mặc định. Bảng `ProfileBinding` mô tả đúng
+phần khác nhau đó; sáu hàm còn lại chỉ dựng struct rồi gọi một đường vào duy nhất.
+
+**Một đường vào duy nhất.** `ControlLoop::submit()` là chỗ duy nhất một goal lọt được vào lõi, và
+`IDLE → PLANNING` là transition duy nhất bắt đầu một chặng. Nhờ đó việc chống hai nguồn goal tranh
+nhau là tính chất cấu trúc, không phải thứ phải nhớ khoá bằng tay.
+
+**Từ chối tại cửa.** Goal có quaternion hỏng, toạ độ không hữu hạn, hoặc profile không nạp được
+planner đều bị từ chối ngay trong `submit()` kèm lý do — không để state machine bắt đầu một chặng rồi
+mới phát hiện không có planner nào chạy được.
+
+**Phản hồi trễ một cycle.** State machine chạy với phản hồi thu được từ cycle trước, rồi mới gọi
+cổng. Điều này là tường minh và có chủ đích: nó cắt vòng "gọi để biết nên gọi gì", nhờ vậy output của
+state machine là một danh sách hành động thuần tuý.
+
+**Lệnh 0 là tức thì.** Nguồn `kNone` phát đúng 0, không giảm tốc dần. Lệnh vận tốc bị chốt lại ở tầng
+dưới, nên nếu control loop dừng giữa lúc đang giảm tốc thì lệnh khác 0 cuối cùng vẫn còn hiệu lực.
+Việc giảm tốc theo động học thuộc về bộ điều khiển bánh xe, nơi biết tải và ma sát thật.
+
+## Trạng thái hiện tại và phần còn thiếu
+
+Đã có và chạy được:
+
+- Toàn bộ lõi quyết định, có test phủ đủ bảng chuyển state.
+- `ControlLoop` chạy end-to-end với thành phần giả.
+- `NavigationServer` hiện thực đủ contract host.
+- `SensorGateway`: dữ liệu cảm biến đi từ contract host tới đúng layer của hai costmap, có phát lại
+  static map nhận trước khi costmap tồn tại. Đây là file duy nhất trong gói include `robot_costmap_2d`.
+- `getTwist()` trả **lệnh** vận tốc từ `VelocityArbiter`, đóng dấu theo đồng hồ của control loop.
+- Plugin `libmove_base2.so` export alias `MoveBase2`.
+
+Chưa có, thuộc bước nối dây runtime:
+
+- Hiện thực thật của `PlannerPort` / `ControllerPort` / `PosePort`
+  (bọc costmap, boost::dll, TF).
+- Dựng hai `Costmap2DROBOT` thật. Hiện `NavigationServer::attachCostmaps()` nhận `LayeredCostmap*`
+  từ bên ngoài bơm vào — cố ý, để đường cảm biến kiểm được mà không cần TF và cây config thật.
+- Thread planner riêng và bộ đệm plan ba lớp. Hiện `ControlLoop` lập plan đồng bộ ngay trong cycle;
+  tách như vậy để phần quyết định kiểm được mà không cần thread.
+- Lớp nối tới mission framework.
+- `setTwistLinear` / `setTwistAngular` (hiện trả `false` để host biết lệnh không có hiệu lực, thay vì
+  âm thầm bỏ qua).

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.

docs/THREADING.md

@@ -0,0 +1,72 @@
+# Mô hình thread và quyền sở hữu
+
+## Bảng thread
+
+| Thread | Sở hữu | Chu kỳ | Ghi chú |
+|---|---|---|---|
+| `control` | state machine, bộ trọng tài vận tốc, controller, recovery, **cmd_vel** | `controller_frequency` | **Thread duy nhất được phát cmd_vel.** Toàn bộ `ControlLoop::step()` chạy ở đây |
+| `planner` | global planner, bộ đệm plan | `planner_frequency` hoặc theo biến điều kiện | *Chưa có ở trạng thái hiện tại* — xem mục dưới |
+| `mission_event` | hàng đợi sự kiện mission | theo sự kiện | Thuộc mission framework, không thuộc gói này |
+| `mission_exec` | điều phối chặng mission | theo sự kiện | Thuộc mission framework. Chỉ gọi callback, **không được block** |
+| host (ROS / C#) | nhận dữ liệu sensor, gọi API `BaseNavigation` | tuỳ host | Ghi vào vùng dữ liệu sensor qua mutex |
+
+## Bất biến quan trọng nhất
+
+> Chỉ thread `control` được phát cmd_vel.
+
+Đây là lý do recovery được **tick từ control thread** thay vì chạy trong thread riêng. Recovery thế
+hệ 2 có thể phát vận tốc; nếu nó chạy ở thread riêng thì sẽ có hai bộ điều khiển cùng ghi vào một
+đường lệnh, và không có cách nào xác định được lệnh nào tới trước. Đó là hai bộ điều khiển tranh
+nhau, không phải hai tác vụ song song.
+
+Bất biến này được củng cố ở hai chỗ, độc lập nhau:
+
+1. **Cấu trúc:** state machine cho ra đúng một `velocity_source` mỗi cycle, và `run_controller` với
+   `tick_recovery` không bao giờ cùng bật.
+2. **Cổng ra:** mọi lệnh đều đi qua `VelocityArbiter`, và bộ trọng tài chỉ được gọi một lần trong
+   `ControlLoop::step()`.
+
+## Mutex
+
+| Mutex | Bảo vệ | Ai giữ |
+|---|---|---|
+| `NavigationServer::data_mutex_` | bản đồ tĩnh, laser scan, point cloud, depth camera, odometry, footprint | host khi ghi, control thread khi đọc |
+| mutex của costmap | dữ liệu costmap | phần nối dây; **lấy lại mỗi lần dùng, không cache** |
+
+`ControlLoop` và `StateMachine` **không** có mutex nào và cố ý như vậy: chúng chỉ chạy trên control
+thread. Thêm mutex vào đó sẽ che mất việc có ai đó gọi sai thread.
+
+## Con trỏ costmap
+
+Con trỏ lấy từ `getCostmap()` là **non-owning và có thể bị thay giữa hai cycle**. Cache lại chính là
+nguyên nhân lỗi double-free đã ghi nhận trong workspace. Quy tắc áp cho mọi hiện thực port:
+
+- Lấy lại con trỏ ở đầu mỗi lần dùng.
+- Không giữ tham chiếu qua nhiều cycle.
+- Không giữ tham chiếu qua ranh giới thread.
+
+## Trạng thái hiện tại: một thread
+
+`ControlLoop` hiện chạy đồng bộ, một thread, và lập plan ngay trong `step()`. Đây là lựa chọn có chủ
+đích cho bước dựng khung: phần quyết định kiểm được đầy đủ mà không cần thread nào, nên test chạy tất
+định và không có race.
+
+Khi thêm thread planner ở bước sau:
+
+- Giữ nguyên mô hình bộ đệm plan ba lớp của bản cũ. Nó đang hoạt động tốt và là đoạn code tinh tế —
+  bê nguyên si trước, không "cải tiến" cùng lúc với việc chuyển sang kiến trúc mới.
+- Điểm nối là `ControlLoop::runPlanner()`: thay lời gọi đồng bộ bằng việc đánh thức thread planner và
+  đọc kết quả từ bộ đệm.
+- `PlannerFeedback` không đổi. Đó chính là mục đích của việc tách nó thành một enum: state machine
+  không cần biết plan được tính đồng bộ hay bất đồng bộ.
+
+## Vòng đời
+
+| Đối tượng | Ai sở hữu |
+|---|---|
+| `NavigationServer` | host, qua `shared_ptr` do factory plugin trả về |
+| `ControlLoop`, `StateMachine`, `VelocityArbiter` | `NavigationServer`, theo giá trị |
+| Mọi cổng (`ControlLoopDeps`) | **không sở hữu**; phải sống lâu hơn `ControlLoop` |
+
+`ControlLoopDeps` chỉ chứa con trỏ trần và điều này là cố ý: nó nói rõ rằng control loop không sở hữu
+gì cả. Bên nối dây chịu trách nhiệm giữ các cổng sống đủ lâu.