# 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

Bộ khung đã chạy được end-to-end với thành phần giả. Phần chưa có — hiện thực port thật, thread
planner, đẩy sensor vào costmap, lớp nối tới mission và recovery framework — được liệt kê ở cuối
`docs/ARCHITECTURE.md`.