# move_base2

Navigation runtime thế hệ 2: nhận yêu cầu di chuyển, điều phối global planner, local planner và
recovery behavior qua một state machine tường minh, và phát lệnh vận tốc từ **đúng một nguồn** tại
mỗi thời điểm.

Gói hiện thực contract host `robot::move_base_core::BaseNavigation` và được nạp bằng Boost.DLL
(alias `MoveBase2`) như mọi plugin khác của workspace.

## Điểm khác biệt

Phần lõi quyết định — state machine và bộ trọng tài vận tốc — là **logic thuần, không I/O**. Nó 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 hành vi của nó kiểm
được bằng bảng chuyển trạng thái thay vì phải chạy robot, và mọi thay đổi hành vi đều để lại dấu vết
trong test.

Mọi phụ thuộc ra ngoài đi qua **port**: thời gian, pose, global planner, local planner, recovery,
mission. Lõi không biết mission framework hay recovery framework nào đang chạy phía sau.

## Cấu trúc

```
include/move_base2/
├── ports/            # sáu cổng ra: clock, pose, planner, controller, recovery, mission
├── core/             # navigation_request, navigation_state, state_machine, velocity_arbiter
├── control_loop.h    # một control cycle
└── navigation_server.h   # facade hiện thực BaseNavigation
src/
test/                 # fake_ports.h + ba bộ test
docs/                 # ARCHITECTURE.md, STATE_MACHINE.md, THREADING.md
```

`docs/STATE_MACHINE.md` là **nguồn chuẩn** cho hành vi chuyển state. Đổi hành vi thì sửa tài liệu đó
trước, sửa test, rồi mới sửa code.

## Luồng runtime

```
host / mission layer
        │  moveTo / dockTo / moveStraightTo / rotateTo
        ▼
NavigationServer::submit()          quy về một NavigationRequest
        │                           từ chối ngay tại cửa nếu goal hỏng hoặc không nạp được planner
        ▼
ControlLoop::step()                 mỗi control cycle:
        │  1. đọc thời gian và pose, tính dt thật
        │  2. chạy state machine với phản hồi của cycle TRƯỚC
        │  3. thi hành output: nhận yêu cầu, đẩy plan, start/tick recovery, chạy controller
        │  4. đưa lệnh ứng viên qua bộ trọng tài
        │  5. báo kết quả cho mission layer nếu state machine yêu cầu
        ▼
    cmd_vel
```

Nguồn vận tốc chỉ có hai khả năng khác `kNone`: local planner khi đang ở `CONTROLLING`, recovery
behavior khi đang ở `RECOVERING`. Đổi nguồn luôn chèn đúng một cycle vận tốc 0.

## Build và test

```bash
cd /home/duongtd/T800_ws
catkin_make --pkg nav_test_harness
catkin_make --pkg move_base2

./devel/lib/move_base2/state_machine_test
./devel/lib/move_base2/velocity_arbiter_test
./devel/lib/move_base2/walking_skeleton_test
```

Gói build với `-Wall -Wextra` qua `target_compile_options` và **không** kế thừa cờ tắt warning của
cây nav core.

## Cấu hình

Ở trạng thái hiện tại, cấu hình được truyền vào bằng struct `ControlLoopConfig` (có `validate()` và
`describe()`). Phần đọc từ YAML qua `robot::NodeHandle` thuộc bước nối dây runtime; khi thêm, file
cấu hình đang có hiệu lực sẽ nằm trong cây config của nav core, không nằm trong gói này — bản trong
`test/` nếu có chỉ phục vụ test và phải chạy kèm biến môi trường trỏ vào đúng thư mục đó.

## Trạng thái

Runtime thật đã chạy trọn trên sim: costmap, planner/controller/recovery/action nạp qua boost::dll,
và mission layer (`mission_adapters`) được dựng trong `NavigationRuntime` nên order VDA5050 được cắt
thành từng chặng thay vì dồn thành một goal. Phần còn thiếu được liệt kê ở cuối
`docs/ARCHITECTURE.md`.

### Order VDA5050 đi đường nào

`NavigationServer::moveTo(Order, …)` thử `MissionLayer::submitOrder()` trước. Layer nhận thì order đi
qua `mission_adapters`: cắt chặng tại mỗi node có action, chỉ chạy phần `released`, `orderUpdateId`
nối tiếp thay vì chạy lại, `mission_timeout` làm lưới cuối. Layer từ chối (tắt bằng config, hoặc
không nạp được nguồn nào cho schema `vda5050.order`) thì order rơi xuống đường trực tiếp — một
`NavigationRequest` cho cả order, đúng hành vi gen-1.

Sáu entry point còn lại (`moveTo(goal)`, `dockTo`, `moveStraightTo`, `rotateTo`) **không** đi qua
mission layer: chúng mang theo sai số hình học và motion profile riêng, mà mission layer không có
chỗ chứa hai thứ đó.