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ể
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.
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.
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.
GoalAdapter adapter;
auto missions =
adapter.convert(goal);
Kết quả:
Goal
└── Mission
VDA5050Adapter
Chuyển đổi Order thành danh sách Mission.
Ví dụ:
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.
std::priority_queue<
Event,
std::vector<Event>,
EventCompare>;
Priority nhỏ hơn sẽ được xử lý trước.
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ụ:
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ợ:
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.
using MissionCallback =
std::function<
void(
const std::shared_ptr<Mission>&
)
>;
Đăng ký callback:
mission_executor.setMissionCallback(
[&](const std::shared_ptr<Mission>& mission)
{
executeMission(*mission);
});
Luồng hoạt động
Goal
Goal
│
▼
GoalAdapter
│
▼
Mission
│
▼
MissionManager
│
▼
MissionExecutor
│
▼
Navigation
Order
Order
│
▼
VDA5050Adapter
│
▼
Mission A
Mission B
Mission C
│
▼
Mission Queue
│
▼
MissionExecutor
Navigation Feedback
Khi Navigation hoàn thành:
event_processor.navDoneEvent();
Khi Navigation thất bại:
event_processor.navFailedEvent();
Ví dụ:
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
MissionManager mission_manager;
EventProcessor event_processor(
mission_manager);
MissionExecutor mission_executor(
mission_manager);
Start
event_processor.start();
mission_executor.start();
Đăng ký callback
mission_executor.setMissionCallback(
[&](const std::shared_ptr<Mission>& mission)
{
executeMission(*mission);
});
Nhận Order
robot_protocol_msgs::Order order;
event_processor.orderEvent(order);
Pause
event_processor.pauseEvent();
Resume
event_processor.resumeEvent();
Cancel
event_processor.cancelEvent();
Emergency Stop
event_processor.emergencyEvent();
Thread Model
Framework sử dụng 2 worker thread:
Thread 1
└─ EventProcessor
Thread 2
└─ MissionExecutor
Navigation Stack hoạt động độc lập.
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.