# mission_adapters

Lớp mission độc lập ROS: nhận yêu cầu từ nguồn ngoài (goal đơn lẻ, VDA5050 Order), chuyển thành hàng
đợi mission, và giữ trạng thái của hàng đợi đó.

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](docs/PLUGIN_GUIDE.md).

## Luồng runtime

```text
   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

```text
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

```cpp
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

```cpp
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`](examples/robot_control_example.cpp) — có build,
nên nó không thể trôi khỏi API.

## Build và test

```bash
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.
