Gói không biết navigation runtime nào đang chạy phía sau. Mission đi xuống qua cổng NavigationClient, kết quả quay về theo MissionId. Nhờ vậy cùng một lớp mission dùng lại được cho runtime khác, và test được mà không cần robot.

Nguồn mission là plugin nạp bằng Boost.DLL theo khoá library_path trong YAML — xem docs/PLUGIN_GUIDE.md.

Luồng runtime

   host (MQTT / OPC-UA / REST / UI)
        │  goalEvent / orderEvent / submitRequest(MissionRequest)
        ▼
  ┌──────────────────┐   tra schema    ┌────────────────┐
  │  EventProcessor  │ ──────────────► │ PluginRegistry │  (các .so nguồn mission)
  │  (thread event)  │ ◄────────────── └────────────────┘
  └──────────────────┘  ConversionResult
        │  submit(missions) / append(missions)
        ▼
  ┌──────────────────┐
  │  MissionManager  │   hàng đợi + MissionState, cấp MissionId
  └──────────────────┘
        │  nextMission() / takePendingCancel()
        ▼
  ┌──────────────────┐
  │ MissionExecutor  │   thread DUY NHẤT phát lệnh ra ngoài
  │  (thread exec)   │
  └──────────────────┘
        │  NavigationClient::dispatch(mission) / cancelActive(id)
        ▼
   navigation runtime
        │  navDoneEvent(id) / navFailedEvent(id)
        └──────────────────────────► quay lại EventProcessor

Bất biến quan trọng

Đây là những tính chất mà bộ test khoá lại; đọc phần này trước khi sửa code.

Mọi outcome mang MissionId. navDoneEvent(id) / navFailedEvent(id) chỉ được chấp nhận khi id khớp mission đang chạy. Outcome đến trễ của một mission đã bị thay thế bị loại bỏ kèm log — nếu không, kết quả của chặng cũ sẽ "hoàn thành" chặng mới mà robot chưa hề chạy.
submit() với danh sách rỗng là no-op tuyệt đối. Một order lỗi không phải lệnh huỷ việc đang chạy; nó không được đụng tới hàng đợi.
Mọi đường thoát của một mission đều bảo navigation dừng. Bị thay thế (preempt), bị huỷ, emergency, quá hạn — tất cả đều đi qua NavigationClient::cancelActive(). Mission layer quên mission mà không bảo navigation dừng thì robot vẫn chạy tiếp tới goal cũ.
Thứ tự sự kiện = thứ tự phát sinh (FIFO). Phát order rồi huỷ ngay thì huỷ phải được xử lý sau order, nếu không robot chạy đúng cái người dùng vừa huỷ. Ngoại lệ duy nhất là emergency: nó bật cờ atomic ngay tại chỗ gọi nên độ trễ phản ứng không phụ thuộc độ dài hàng đợi.
nextMission() trả mỗi mission đúng một lần, tại đúng bước chuyển QUEUED → RUNNING. Bên gọi không phải tự khử trùng lặp.
Mission bất biến sau khi submit. MissionManager gán id rồi chia sẻ dưới dạng shared_ptr<const Mission> — ba thread đọc chung mà không cần khoá.
MissionManager không bao giờ gọi ra ngoài khi đang giữ khoá. Nó chỉ ghi "mission này cần được dừng" vào ô pending-cancel; MissionExecutor mới là thread thực hiện lời gọi đó.

State machine

IDLE ──submit──► QUEUED ──dequeue──► RUNNING ──nav_done──► còn mission? ──có──► QUEUED
                                        │      (nav + action đều xong)  │
                                        │ nav_failed / mission_timeout  │ không
                                        ▼                               ▼
                                     FAILED ──submit mới──► QUEUED   COMPLETED ──submit mới──► QUEUED

PAUSED ◄── pause / resume ──► (state trước đó)
cancel (mọi state active) ──► CANCELLED ──submit mới──► QUEUED
EMERGENCY: từ MỌI state; clear_emergency ──► CLEAR_EMERGENCY ──submit mới──► QUEUED

nav_done(id) nghĩa là cả chặng hoàn tất — navigation lẫn action. Mission layer không có state chờ action riêng: action do navigation runtime thực thi, và mission layer giữ nguyên RUNNING trọn chặng.

Mọi lần đổi trạng thái được log một dòng from -> to (lý do, mission id) ở mức info, và chỉ khi state đổi thật.

Kiểu dữ liệu

using MissionId = std::uint64_t;   // 0 = kInvalidMissionId

class Mission
{
public:
    MissionId id;          // MissionManager cấp khi submit, đơn điệu tăng
    MissionType type;      // SIMPLE_GOAL | VDA5050_ORDER
    bool has_goal;         // false = mission chỉ-có-action, navigation bỏ qua phần di chuyển
    robot_geometry_msgs::PoseStamped start;   // chỉ hợp lệ khi has_goal
    robot_geometry_msgs::PoseStamped goal;    // chỉ hợp lệ khi has_goal
    std::vector<robot_protocol_msgs::Node> nodes;
    std::vector<robot_protocol_msgs::Edge> edges;
    std::vector<Action> actions;               // đã sắp theo sequenceId, đi qua nguyên vẹn
};

Mission là self-contained: consumer không phải suy goal ra từ nodes.back(). Adapter VDA5050 set sẵn start/goal từ nodePosition (theta [rad] → quaternion quanh trục z).

has_goal == false bắt buộc đi kèm ít nhất một action; core từ chối cả lô nếu adapter vi phạm.

VDA5050 conformance

VDA5050SourceAdapter chịu trách nhiệm ba điểm:

Điểm	Hành vi
`released` (base/horizon)	Chỉ phần base được thực thi. Horizon là dự định của fleet manager, chưa được phép chạy. Order không điền `released` ở đâu cả thì cả order được coi là base, kèm log cảnh báo.
`orderId` / `orderUpdateId`	`orderId` mới → thay hàng đợi (`kReplace`). Cùng `orderId` + `orderUpdateId` lớn hơn → chỉ sinh phần vừa release thêm và nối tiếp (`kAppend`). `orderUpdateId` không mới hơn → từ chối.
`goal` / `start`	Set từ node cuối / node đầu của chặng, trong frame `global_frame` (mặc định `map`). VDA5050 `mapId` là danh tính bản đồ, không phải frame TF, nên không dùng làm `frame_id`.

Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action. Action ngay tại node xuất phát sinh ra một chặng has_goal == false.

Config

Bản runtime: pnkx_nav_core/config/mission_adapters_params.yaml. Bản test: test/config/mission_adapters_params.yaml, chỉ được đọc khi chạy kèm PNKX_NAV_CORE_CONFIG_DIR.

Khoá	Đơn vị / mặc định	Ý nghĩa
`mission_adapters/mission_sources`	—	Danh sách nguồn mission `{name, type}`
`mission_adapters/mission_timeout`	`[s]`, `0.0`	Trần thời gian cho một chặng; `0` = tắt. Quá hạn → chặng thất bại và navigation được bảo dừng
`mission_adapters/clear_queue_on_failure`	`true`	Một chặng hỏng thì xoá sạch hàng đợi. Đặt `false` chỉ khi các mission độc lập với nhau
`<type>/library_path`	—	Tên `.so` của plugin — thiếu khoá này là lỗi runtime phổ biến nhất

Dùng

mission_adapters::PluginRegistry registry;
robot::NodeHandle nh;
registry.loadFromConfig(nh);                     // nạp nguồn mission từ YAML

mission_adapters::MissionConfig config;
config.loadFromParams(nh);

mission_adapters::MissionManager  manager(config);
mission_adapters::EventProcessor  processor(manager, registry);
mission_adapters::MissionExecutor executor(manager);

executor.setNavigationClient(&my_navigation_client);   // non-owning, phải sống lâu hơn executor
processor.start();
executor.start();

processor.goalEvent(goal);                       // hoặc orderEvent(order) / submitRequest(request)
// ... navigation runtime gọi processor.navDoneEvent(mission->id) khi chặng xong ...

Ví dụ đầy đủ: examples/robot_control_example.cpp — có build, nên nó không thể trôi khỏi API.

Build và test

catkin_make --pkg mission_adapters
source devel/setup.bash

./devel/lib/mission_adapters/event_bus_test
./devel/lib/mission_adapters/adapter_test
./devel/lib/mission_adapters/mission_manager_test
./devel/lib/mission_adapters/mission_lifecycle_test

PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
  ./devel/lib/mission_adapters/plugin_registry_test

Test cũng đăng ký với ctest: cd build && ctest -R mission_adapters --output-on-failure.

Thread

Thread	Sở hữu	Đánh thức bởi
`EventProcessor`	hàng đợi sự kiện, gọi adapter	`EventBus::pop()` chờ sự kiện
`MissionExecutor`	mọi lời gọi xuống `NavigationClient`	`MissionManager::waitForWork()` chờ condition variable
host	phát sự kiện, nhận outcome	—

Chuyển đổi payload → mission chạy trên thread của EventProcessor, không phải thread host: adapter được phép có state, và chuyển đổi trên nhiều thread host sẽ tranh chấp state đó.

MissionExecutor không poll. Trong một vòng nó luôn cancel trước, dispatch sau — chặng cũ phải được bảo dừng trước khi chặng mới bắt đầu.

Chưa có

Mission persistence (mất điện giữa order thì hàng đợi mất).
VDA5050 instant actions.
Mission priority queue — hàng đợi hiện là FIFO thuần.
Multi-robot fleet.