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

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ứ đó.