mission_adapters/README.md

# Mission Adapters

Mission Adapters là một framework quản lý nhiệm vụ (Mission Management Framework) được xây dựng bằng C++ thuần, hướng tới các hệ thống AGV/AMR và tương thích với mô hình nhiệm vụ của VDA5050.

Framework cung cấp:

* Chuyển đổi Goal hoặc VDA5050 Order thành Mission.
* Quản lý hàng đợi Mission.
* Xử lý Event bất đồng bộ.
* Điều phối Mission theo trạng thái Robot.
* Hỗ trợ Pause / Resume / Cancel / Emergency.
* Tách biệt Mission Scheduling và Mission Execution.
* Không phụ thuộc ROS.

---

# Kiến trúc tổng thể

```text
Order / Goal
      │
      ▼
+------------------+
|  EventProcessor  |
+------------------+
      │
      ▼
+------------------+
|  MissionManager  |
+------------------+
      │
      ▼
+------------------+
| MissionExecutor  |
+------------------+
      │
      ▼
 Navigation Stack
 (MoveBase, MPPI,
  Pure Pursuit...)
```

---

# Thành phần chính

## Mission

Đơn vị thực thi cơ bản.

```cpp
class Mission
{
public:
    int sequenceId;

    MissionType type;

    int priority;

    PoseStamped start;

    PoseStamped goal;

    std::vector<Node> nodes;

    std::vector<Edge> edges;

    std::vector<Action> actions;
};
```

Mission có thể được tạo từ:

* Goal đơn giản
* VDA5050 Order

---

## Action

Đại diện cho một hành động tại Node hoặc Edge.

```cpp
class Action
{
public:
    int sequenceId;

    ActionType type;

    robot_protocol_msgs::Action action;
};
```

Các Action sẽ được sắp xếp theo `sequenceId`.

---

## GoalAdapter

Chuyển đổi Goal thành Mission.

```cpp
GoalAdapter adapter;

auto missions =
    adapter.convert(goal);
```

Kết quả:

```text
Goal
 └── Mission
```

---

## VDA5050Adapter

Chuyển đổi Order thành danh sách Mission.

Ví dụ:

```text
N1 ---- N2 ---- N3(Action)
                 |
                 |
                 V
               Mission A

N3 ---- N4 ---- N5(Action)
                 |
                 |
                 V
               Mission B
```

Order sẽ được chia thành nhiều Mission tại các Node chứa Action.

---

## EventBus

Hàng đợi ưu tiên cho các Event.

```cpp
std::priority_queue<
    Event,
    std::vector<Event>,
    EventCompare>;
```

Priority nhỏ hơn sẽ được xử lý trước.

```cpp
EMERGENCY = 0
CANCEL    = 1
RESUME    = 2
PAUSE     = 3
NAV_DONE  = 4
ORDER     = 5
```

---

## EventProcessor

Tiếp nhận Event từ bên ngoài.

Ví dụ:

```cpp
event_processor.orderEvent(order);

event_processor.pauseEvent();

event_processor.resumeEvent();

event_processor.cancelEvent();

event_processor.emergencyEvent();
```

EventProcessor hoạt động trên một worker thread riêng.

---

## MissionManager

Quản lý trạng thái và hàng đợi Mission.

Các trạng thái hỗ trợ:

```cpp
IDLE
QUEUED
RUNNING
PAUSED
WAITING_ACTION
RECOVERY
COMPLETED
FAILED
CANCELLED
EMERGENCY
```

Chức năng:

* Submit Mission
* Lấy Mission tiếp theo
* Pause
* Resume
* Cancel
* Emergency
* Navigation Done
* Navigation Failed

---

## MissionExecutor

Thread chuyên lấy Mission từ MissionManager.

Khi có Mission mới, callback sẽ được gọi.

```cpp
using MissionCallback =
    std::function<
        void(
            const std::shared_ptr<Mission>&
        )
    >;
```

Đăng ký callback:

```cpp
mission_executor.setMissionCallback(
    [&](const std::shared_ptr<Mission>& mission)
    {
        executeMission(*mission);
    });
```

---

# Luồng hoạt động

## Goal

```text
Goal
  │
  ▼
GoalAdapter
  │
  ▼
Mission
  │
  ▼
MissionManager
  │
  ▼
MissionExecutor
  │
  ▼
Navigation
```

---

## Order

```text
Order
  │
  ▼
VDA5050Adapter
  │
  ▼
Mission A
Mission B
Mission C
  │
  ▼
Mission Queue
  │
  ▼
MissionExecutor
```

---

# Navigation Feedback

Khi Navigation hoàn thành:

```cpp
event_processor.navDoneEvent();
```

Khi Navigation thất bại:

```cpp
event_processor.navFailedEvent();
```

Ví dụ:

```cpp
auto state =
    move_base_ptr_->getFeedback()
        ->navigation_state;

if(state != prev_state_)
{
    if(state ==
        State::SUCCEEDED)
    {
        event_processor.navDoneEvent();
    }

    if(state ==
        State::ABORTED)
    {
        event_processor.navFailedEvent();
    }

    prev_state_ = state;
}
```

---

# Ví dụ sử dụng

## Khởi tạo

```cpp
MissionManager mission_manager;

EventProcessor event_processor(
    mission_manager);

MissionExecutor mission_executor(
    mission_manager);
```

---

## Start

```cpp
event_processor.start();

mission_executor.start();
```

---

## Đăng ký callback

```cpp
mission_executor.setMissionCallback(
    [&](const std::shared_ptr<Mission>& mission)
    {
        executeMission(*mission);
    });
```

---

## Nhận Order

```cpp
robot_protocol_msgs::Order order;

event_processor.orderEvent(order);
```

---

## Pause

```cpp
event_processor.pauseEvent();
```

---

## Resume

```cpp
event_processor.resumeEvent();
```

---

## Cancel

```cpp
event_processor.cancelEvent();
```

---

## Emergency Stop

```cpp
event_processor.emergencyEvent();
```

---

# Thread Model

Framework sử dụng 2 worker thread:

```text
Thread 1
 └─ EventProcessor

Thread 2
 └─ MissionExecutor
```

Navigation Stack hoạt động độc lập.

```text
Main Thread
     │
     ├─ Navigation
     │
     ├─ EventProcessor
     │
     └─ MissionExecutor
```

---

# TODO

Các chức năng dự kiến bổ sung:

* Action Executor
* WAITING_ACTION state
* ACTION_DONE event
* ACTION_FAILED event
* Recovery Framework
* Mission Priority Queue
* Mission Persistence
* VDA5050 Instant Actions
* Multi-Robot Fleet Support

---

# License

Internal Project.