optimal & fix file cmake
This commit is contained in:
574
README.md
574
README.md
@@ -1,461 +1,195 @@
|
||||
# Mission Adapters
|
||||
# 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.
|
||||
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 đó.
|
||||
|
||||
Framework cung cấp:
|
||||
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.
|
||||
|
||||
* 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.
|
||||
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).
|
||||
|
||||
---
|
||||
|
||||
# Kiến trúc tổng thể
|
||||
## Luồng runtime
|
||||
|
||||
```text
|
||||
Order / Goal
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| EventProcessor |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| MissionManager |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
+------------------+
|
||||
| MissionExecutor |
|
||||
+------------------+
|
||||
│
|
||||
▼
|
||||
Navigation Stack
|
||||
(MoveBase, MPPI,
|
||||
Pure Pursuit...)
|
||||
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
|
||||
|
||||
# Thành phần chính
|
||||
Đâ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.
|
||||
|
||||
## Mission
|
||||
- **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 đó.
|
||||
|
||||
Đơn vị thực thi cơ bản.
|
||||
## 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:
|
||||
int sequenceId;
|
||||
|
||||
MissionType type;
|
||||
|
||||
int priority;
|
||||
|
||||
PoseStamped start;
|
||||
|
||||
PoseStamped goal;
|
||||
|
||||
std::vector<Node> nodes;
|
||||
|
||||
std::vector<Edge> edges;
|
||||
|
||||
std::vector<Action> actions;
|
||||
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 có thể được tạo từ:
|
||||
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).
|
||||
|
||||
* Goal đơn giản
|
||||
* VDA5050 Order
|
||||
`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
|
||||
|
||||
## Action
|
||||
`VDA5050SourceAdapter` chịu trách nhiệm ba điểm:
|
||||
|
||||
Đại diện cho một hành động tại Node hoặc Edge.
|
||||
| Đ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
|
||||
class Action
|
||||
{
|
||||
public:
|
||||
int sequenceId;
|
||||
mission_adapters::PluginRegistry registry;
|
||||
robot::NodeHandle nh;
|
||||
registry.loadFromConfig(nh); // nạp nguồn mission từ YAML
|
||||
|
||||
ActionType type;
|
||||
mission_adapters::MissionConfig config;
|
||||
config.loadFromParams(nh);
|
||||
|
||||
robot_protocol_msgs::Action action;
|
||||
};
|
||||
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 ...
|
||||
```
|
||||
|
||||
Các Action sẽ được sắp xếp theo `sequenceId`.
|
||||
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
|
||||
|
||||
## GoalAdapter
|
||||
```bash
|
||||
catkin_make --pkg mission_adapters
|
||||
source devel/setup.bash
|
||||
|
||||
Chuyển đổi Goal thành Mission.
|
||||
./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
|
||||
|
||||
```cpp
|
||||
GoalAdapter adapter;
|
||||
|
||||
auto missions =
|
||||
adapter.convert(goal);
|
||||
PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
|
||||
./devel/lib/mission_adapters/plugin_registry_test
|
||||
```
|
||||
|
||||
Kết quả:
|
||||
Test cũng đăng ký với ctest: `cd build && ctest -R mission_adapters --output-on-failure`.
|
||||
|
||||
```text
|
||||
Goal
|
||||
└── Mission
|
||||
```
|
||||
## 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 | — |
|
||||
|
||||
## VDA5050Adapter
|
||||
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 đó.
|
||||
|
||||
Chuyển đổi Order thành danh sách Mission.
|
||||
`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.
|
||||
|
||||
Ví dụ:
|
||||
## Chưa có
|
||||
|
||||
```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.
|
||||
- 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.
|
||||
|
||||
Reference in New Issue
Block a user