196 lines
10 KiB
Markdown
196 lines
10 KiB
Markdown
# 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.
|