DuongTD/mission_adapters
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

change file readme

Browse Source
This commit is contained in:
duongtd
2026-06-06 10:34:18 +07:00
parent 5cdacdeebf
commit 1bd09d5e8e
4 changed files with 632 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files

576
README.md
View File

@@ -1,217 +1,461 @@
# 🎯 Stanley Local Planner
# Mission Adapters
**Stanley Local Planner** là một bộ điều khiển cấp địa phương (Local Planner) sử dụng **Stanley Method** cho robot di động. Bộ điều khiển này được thiết kế để hoạt động trên hai nền tảng:
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.
- 🤖 **ROS Noetic** - Tích hợp vào ROS Navigation Stack
- ⚙️ **C++ Standalone** - Thư viện C++ thuần cho các ứng dụng không dùng ROS
Framework cung cấp:
Bộ điều khiển này sử dụng **Bicycle Kinematic Model** và kết hợp hai thành phần điều khiển:
- **Heading Error** - Lỗi góc định hướng
- **Cross-track Error** - Sai lệch ngang so với đường dẫn
* 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.
---
## 🚀 Đặc điểm chính
# Kiến trúc tổng thể
✅ **Stanley Method kinh điển** - Thuật toán được chứng minh hiệu quả trong tự lái
✅ **Bicycle Kinematic Model** - Mô hình chính xác cho xe, xe tải, xe điện
✅ **Dual Platform Support** - ROS Noetic + C++ Standalone
✅ **Hiệu suất cao** - CPU usage < 5%
✅ **Cấu hình đơn giản** - Chỉ cần 3-4 tham số chính
✅ **Theo dõi đường chính xác** - Tracking error < 0.1m
✅ **Chuyển động mượt mà** - Điều khiển lái góc mức** (smooth steering)
---
## 📚 Nguyên lý Stanley Method
### Bicycle Model
Mô hình bicycle kinematic mô tả động học của robot:
```
θ (yaw angle)
↑
| Front Axle (steering angle δ)
| /
|___/______ Rear Axle (reference point)
L (wheelbase)
```
Phương trình:
```
ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = v * tan(δ) / L
```
### Stanley Control Law
Stanley controller kết hợp hai thành phần lỗi:
```
δ = atan2(k_e * e_y, v) + e_θ
Trong đó:
δ - Góc lái (steering angle)
k_e - Cross-track error gain
e_y - Sai lệch ngang (cross-track error)
v - Vận tốc tiến (forward velocity)
e_θ - Sai lệch góc (heading error)
```
**Thành phần 1: Cross-track Error (e_y)**
- Khoảng cách ngang từ robot đến đường dẫn
- Được cân nhân bằng vận tốc để ổn định (adaptive gain)
- Ưu điểm: Quay về đường nhanh chóng
**Thành phần 2: Heading Error (e_θ)**
- Sai lệch góc so với hướng mong muốn của đường dẫn
- Đảm bảo robot song song với đường dẫn
- Ưu điểm: Tránh dao động, điều khiển ổn định
---
## 🛠️ Cài đặt
### Yêu cầu hệ thống
**Chung:**
- C++17 trở lên
- Eigen 3.3+
- CMake 3.10+
### Build từ source
```bash
# Clone repository
cd ~/ros_workspace/src
git clone link_to_repo
cd ..
# Build với ROS
catkin_make
# Hoặc build standalone C++ library
cd src/stanley_local_planner
mkdir build && cd build
cmake -DBUILD_ROS=OFF ..
make
sudo make install
```text
Order / Goal
│
▼
+------------------+
| EventProcessor |
+------------------+
│
▼
+------------------+
| MissionManager |
+------------------+
│
▼
+------------------+
| MissionExecutor |
+------------------+
│
▼
Navigation Stack
(MoveBase, MPPI,
Pure Pursuit...)
```
---
## 🎯 Cấu hình
# Thành phần chính
### Tham số chính
## Mission
Tất cả tham số được cấu hình trong `config/stanley_local_planner_params.yaml`:
Đơn vị thực thi cơ bản.
```yaml
# Robot Parameters
robot_max_v: 1.0 # Vận tốc tối đa (m/s)
robot_max_a: 1.0 # Gia tốc tối đa (m/s²)
robot_wheelbase: 0.5 # Khoảng cách trục bánh (m)
robot_max_steering_angle: 0.5 # Góc lái tối đa (rad ≈ 28°)
robot_min_radius: 0.3 # Bán kính quay tối thiểu (m)
```cpp
class Mission
{
public:
int sequenceId;
# Stanley Controller Parameters
k_e: 2.0 # Cross-track error gain
k_theta: 0.5 # Heading error gain
time_step: 0.05 # Chu kỳ điều khiển (s)
MissionType type;
# Trajectory Parameters
lookahead_distance: 1.0 # Tầm nhìn trước (m)
path_resolution: 0.1 # Độ phân giải đường dẫn (m)
int priority;
# Goal Tolerance
xy_goal_tolerance: 0.1 # Sai số vị trí (m)
yaw_goal_tolerance: 0.1 # Sai số định hướng (rad)
PoseStamped start;
PoseStamped goal;
std::vector<Node> nodes;
std::vector<Edge> edges;
std::vector<Action> actions;
};
```
### Điều chỉnh hiệu suất
Mission có thể được tạo từ:
| Tham số | Tăng | Giảm |
|---------|------|------|
| `k_e` | Quay nhanh, có thể dao động | Quay chậm, trễ |
| `k_theta` | Định hướng ổn định hơn | Có thể dao động |
| `lookahead_distance` | Chuyển động mượt, mục tiêu xa | Phản ứng chậm, sai lệch lớn |
**Khuyến nghị:**
- Start với `k_e=2.0, k_theta=0.5`
- Nếu quá khúc: giảm `k_e` hoặc tăng `lookahead_distance`
- Nếu quá mận: tăng `k_e` hoặc giảm `lookahead_distance`
* Goal đơn giản
* VDA5050 Order
---
## 📊 Hiệu suất
## Action
### Benchmark Results
Đại diện cho một hành động tại Node hoặc Edge.
| Metric | Value | Ghi chú |
|--------|-------|--------|
| Tracking Error | < 0.08m | Path deviation |
| Heading Error | < 0.05 rad | Angular accuracy |
| Response Time | < 50ms | Control loop |
| CPU Usage | ~2-3% | Single core |
| Memory | ~5MB | Runtime |
| Max Speed | 1.5 m/s | Tested |
```cpp
class Action
{
public:
int sequenceId;
ActionType type;
robot_protocol_msgs::Action action;
};
```
Các Action sẽ được sắp xếp theo `sequenceId`.
---
## 📐 Mô hình toán học
## GoalAdapter
### Cross-track Error Calculation
Chuyển đổi Goal thành Mission.
```
Cho đường dẫn từ điểm P_i đến P_{i+1}
Vị trí robot: R = (x, y)
```cpp
GoalAdapter adapter;
1. Tìm hình chiếu vuông góc từ R lên đường P_i → P_{i+1}
2. e_y = khoảng cách từ R đến điểm chiếu
3. Dấu: dương nếu robot ở bên trái, âm nếu ở bên phải
auto missions =
adapter.convert(goal);
```
### Heading Error Calculation
Kết quả:
```
ψ_desired = atan2(P_{i+1}.y - P_i.y, P_{i+1}.x - P_i.x)
ψ_current = θ (yaw angle của robot)
e_θ = normalize(ψ_desired - ψ_current) [-π, π]
```
### Steering Angle Computation
```
δ_ff = atan(tan(path_curvature) * wheelbase) // Feedforward
δ_fb = atan2(k_e * e_y, v) + k_theta * e_θ // Feedback
δ = δ_ff + δ_fb
δ = clamp(δ, -max_angle, max_angle)
```text
Goal
└── Mission
```
---
## 📚 Tài liệu tham khảo
## VDA5050Adapter
- **Stanley Method Paper**: "Journal of Field Robotics" - Thrun et al. (2006)
- **Bicycle Kinematic Model**: Classic control theory
- **ROS Navigation**: [http://wiki.ros.org/navigation](http://wiki.ros.org/navigation)
- **Eigen Documentation**: [https://eigen.tuxfamily.org/](https://eigen.tuxfamily.org/)
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.
---
## 📄 Giấy phép
## EventBus
**MIT License** - Tự do sử dụng, sửa đổi, phân phối
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
```
---
## ✉️ Liên hệ & Support
## EventProcessor
- **Email**: duongtd@phenikaa-x.com
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.
---
**Cảm ơn bạn đã sử dụng Stanley Local Planner!** 🚗
## 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.

69
include/mission_adapters/mission_adapters.h
View File

@@ -19,11 +19,12 @@ namespace mission_adapters
// ── Priority constants ────────────────────────────────────────────────────
// Số càng nhỏ = ưu tiên càng cao (min-heap)
static constexpr int PRIORITY_EMERGENCY = 0;
static constexpr int PRIORITY_CANCEL = 1; // resume > pause
static constexpr int PRIORITY_CANCEL = 1;
static constexpr int PRIORITY_RESUME = 2;
static constexpr int PRIORITY_PAUSE = 3;
static constexpr int PRIORITY_NAV_DONE = 4;
static constexpr int PRIORITY_ACT_DONE = 4;
// FIX #4: Dedicated constant — same value as NAV_DONE but semantically correct name.
static constexpr int PRIORITY_NAV_FAILED = 4;
static constexpr int PRIORITY_ORDER = 5;
enum class MissionState
@@ -66,7 +67,7 @@ namespace mission_adapters
class Action
{
public:
int sequenceId; //id này theo id của action trong node hoặc edge, để đảm bảo thứ tự thực hiện
int sequenceId;
ActionType type;
robot_protocol_msgs::Action action;
};
@@ -75,28 +76,19 @@ namespace mission_adapters
{
public:
int sequenceId;
MissionType type;
int priority;
robot_geometry_msgs::PoseStamped start;
robot_geometry_msgs::PoseStamped goal;
std::vector<robot_protocol_msgs::Node> nodes;
std::vector<robot_protocol_msgs::Edge> edges;
std::vector<Action> actions;
};
struct Event
{
EventType type;
int priority;
std::vector<std::shared_ptr<Mission>> missions;
};
@@ -112,36 +104,26 @@ namespace mission_adapters
{
private:
mutable std::mutex mutex_;
std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
std::condition_variable cv_;
bool stop_ = false;
public:
void push(const Event& event);
bool pop(Event& event);
void stop();
void reset();
inline bool empty() const
{
std::lock_guard<std::mutex> lock(mutex_);
return queue_.empty();
}
inline void clear()
{
std::lock_guard<std::mutex> lock(mutex_);
while(!queue_.empty())
{
queue_.pop();
}
while (!queue_.empty()) queue_.pop();
}
};
@@ -160,35 +142,26 @@ namespace mission_adapters
class MissionManager
{
public:
void submit(const std::vector<std::shared_ptr<Mission>>& missions);
std::shared_ptr<Mission> nextMission();
void onNavigationDone();
void onNavigationFailed();
void cancel();
void pause();
void resume();
void emergency();
MissionState state() const;
// FIX #3: Returns true if there is any pending work — queue OR active mission.
bool hasMission() const;
private:
mutable std::mutex mutex_;
MissionState state_ = MissionState::IDLE;
std::queue<std::shared_ptr<Mission>> mission_queue_;
std::shared_ptr<Mission> current_mission_;
};
@@ -196,53 +169,37 @@ namespace mission_adapters
{
public:
explicit EventProcessor(MissionManager& mission_manager);
~EventProcessor();
void start();
void stop();
void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
void orderEvent(const robot_protocol_msgs::Order& order);
void cancelEvent();
void pauseEvent();
void resumeEvent();
void navDoneEvent();
void navFailedEvent();
void emergencyEvent();
private:
void spin();
void process(const Event& event);
private:
EventBus event_bus_;
GoalAdapter goal_adapter_;
VDA5050Adapter vda5050_adapter_;
MissionManager& mission_manager_;
std::thread worker_;
std::atomic<bool> running_{false};
};
class MissionExecutor
{
public:
using MissionCallback =
std::function<void(const std::shared_ptr<Mission>&)>;
using MissionCallback = std::function<void(const std::shared_ptr<Mission>&)>;
explicit MissionExecutor(MissionManager& manager);
~MissionExecutor();
@@ -250,6 +207,7 @@ namespace mission_adapters
void start();
void stop();
// FIX #1: Must be called BEFORE start(), not inside the run loop.
void setMissionCallback(MissionCallback cb)
{
std::lock_guard<std::mutex> lock(mutex_);
@@ -257,23 +215,20 @@ namespace mission_adapters
}
private:
std::shared_ptr<Mission> mission_excute_;
std::shared_ptr<Mission> mission_execute_;
// Mission cuối cùng đã dispatch
// Last mission that was dispatched to the callback.
std::shared_ptr<Mission> last_dispatched_mission_;
MissionCallback mission_callback_;
MissionManager& mission_manager_;
mutable std::mutex mutex_;
std::thread worker_;
std::atomic<bool> running_{false};
void spin();
};
}
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H

411
src/mission_adapters.cpp
View File

@@ -3,36 +3,26 @@
namespace mission_adapters
{
// ─────────────────────────────────────────────────────────────────────────
// EventBus
// ─────────────────────────────────────────────────────────────────────────
void EventBus::push(const Event& event)
{
std::lock_guard<std::mutex> lock(mutex_);
queue_.push(event);
cv_.notify_one();
}
bool EventBus::pop(Event& event)
{
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait(lock, [this] { return stop_ || !queue_.empty(); });
cv_.wait(lock,
[this]
{
return stop_ || !queue_.empty();
});
if(stop_ || queue_.empty())
{
return false;
}
if (stop_ || queue_.empty()) return false;
event = queue_.top();
queue_.pop();
return true;
}
@@ -42,7 +32,6 @@ namespace mission_adapters
std::lock_guard<std::mutex> lock(mutex_);
stop_ = true;
}
cv_.notify_all();
}
@@ -52,44 +41,36 @@ namespace mission_adapters
stop_ = false;
}
// ─────────────────────────────────────────────────────────────────────────
// GoalAdapter
// ─────────────────────────────────────────────────────────────────────────
std::vector<std::shared_ptr<Mission>>
GoalAdapter::convert(const robot_geometry_msgs::PoseStamped& goal)
{
std::vector<std::shared_ptr<Mission>> missions;
auto mission = std::make_shared<Mission>();
mission->type = MissionType::SIMPLE_GOAL;
mission->priority = 0;
/////////////////////////// Để tạm thời ///////////////////////////
mission->goal = goal;
////////////////////////////////////////////////////////////////////
missions.push_back(mission);
return missions;
return {mission};
}
// ─────────────────────────────────────────────────────────────────────────
// VDA5050Adapter
// ─────────────────────────────────────────────────────────────────────────
std::vector<std::shared_ptr<Mission>>
VDA5050Adapter::convert(const robot_protocol_msgs::Order& order)
{
std::vector<std::shared_ptr<Mission>> missions;
std::vector<size_t> action_node_indices;
// Tìm tất cả node có action
for (size_t i = 0; i < order.nodes.size(); ++i)
{
if (!order.nodes[i].actions.empty())
{
action_node_indices.push_back(i);
}
}
// if(action_node_indices.empty())
// {
// return missions;
// }
size_t start_node_idx = 0;
@@ -98,16 +79,13 @@ namespace mission_adapters
size_t end_node_idx = action_node_indices[i];
auto mission = std::make_shared<Mission>();
mission->type = MissionType::VDA5050_ORDER;
mission->priority = 0;
// Nodes [start_node_idx, end_node_idx]
mission->nodes.assign(
order.nodes.begin() + start_node_idx,
order.nodes.begin() + end_node_idx + 1);
// Edges [start_node_idx, end_node_idx - 1]
if (end_node_idx > start_node_idx)
{
mission->edges.assign(
@@ -115,28 +93,14 @@ namespace mission_adapters
order.edges.begin() + end_node_idx);
}
// /////////////////////////// Để tạm thời ///////////////////////////
// robot_geometry_msgs::PoseStamped start, goal;
// start.header = ;
// start.pose = ;
// goal.header = ;
// goal.pose = ;
// ////////////////////////////////////////////////////////////////////
// mission->start = start;
// mission->goal = goal;
missions.push_back(mission);
// Mission tiếp theo bắt đầu sau node action hiện tại
start_node_idx = end_node_idx;
}
// Phần còn lại sau action cuối cùng
// Remaining segment after the last action node
if (start_node_idx < order.nodes.size() - 1)
{
auto mission = std::make_shared<Mission>();
mission->type = MissionType::VDA5050_ORDER;
mission->priority = 0;
@@ -151,73 +115,71 @@ namespace mission_adapters
order.edges.end());
}
// /////////////////////////// Để tạm thời ///////////////////////////
// robot_geometry_msgs::PoseStamped start, goal;
// start.header.frame_id = "map"
// start.pose = ;
// goal.header = ;
// goal.pose = ;
// ////////////////////////////////////////////////////////////////////
// mission->start = start;
// mission->goal = goal;
missions.push_back(mission);
}
// Collect actions
// Collect and sort actions
for (auto& mission : missions)
{
for (const auto& edge : mission->edges)
{
for (const auto& action : edge.actions)
{
Action mission_action;
mission_action.sequenceId = edge.sequenceId; // Giữ nguyên sequenceId từ action gốc
mission_action.type = ActionType::EDGE_ACTION;
mission_action.action = action;
mission->actions.push_back(std::move(mission_action));
Action ma;
// FIX #6: Use action.sequenceId, not edge.sequenceId,
// so the sort below produces the correct execution order.
ma.sequenceId = action.sequenceId;
ma.type = ActionType::EDGE_ACTION;
ma.action = action;
mission->actions.push_back(std::move(ma));
}
}
const auto& node = mission->nodes.back();
if(node.actions.empty()) continue;
for (const auto& action : node.actions)
{
Action mission_action;
mission_action.sequenceId = node.sequenceId; // Giữ nguyên sequenceId từ action gốc
mission_action.type = ActionType::NODE_ACTION;
mission_action.action = action;
mission->actions.push_back(std::move(mission_action));
Action ma;
// FIX #6: Same fix for node actions.
ma.sequenceId = action.sequenceId;
ma.type = ActionType::NODE_ACTION;
ma.action = action;
mission->actions.push_back(std::move(ma));
}
// xắp xếp actions theo sequenceId để đảm bảo thứ tự thực hiện
std::sort(mission->actions.begin(), mission->actions.end(),
[](const Action& a, const Action& b) {
return a.sequenceId < b.sequenceId;
});
}
return missions;
}
// ─────────────────────────────────────────────────────────────────────────
// MissionManager
// ─────────────────────────────────────────────────────────────────────────
void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
{
std::lock_guard<std::mutex> lock(mutex_);
for(auto& mission : missions)
{
for (const auto& mission : missions)
mission_queue_.push(mission);
}
// FIX #9: Transition to QUEUED from any "inactive" state, including
// COMPLETED and QUEUED itself, so the executor always re-arms.
if (!missions.empty())
{
if(state_ == MissionState::IDLE ||
state_ == MissionState::FAILED ||
state_ == MissionState::CANCELLED ||
state_ == MissionState::EMERGENCY)
switch (state_)
{
case MissionState::IDLE:
case MissionState::COMPLETED:
case MissionState::FAILED:
case MissionState::CANCELLED:
case MissionState::EMERGENCY:
state_ = MissionState::QUEUED;
break;
default:
break; // RUNNING / PAUSED / QUEUED — already active, leave as-is
}
}
}
@@ -225,6 +187,7 @@ namespace mission_adapters
std::shared_ptr<Mission> MissionManager::nextMission()
{
std::lock_guard<std::mutex> lock(mutex_);
if (state_ == MissionState::PAUSED ||
state_ == MissionState::FAILED ||
state_ == MissionState::CANCELLED ||
@@ -233,24 +196,17 @@ namespace mission_adapters
return nullptr;
}
// FIX #2: Removed the redundant `if(current_mission_ == nullptr)` check.
// The two guards are now a clean early-return then fall-through.
if (current_mission_)
{
return current_mission_;
}
if (mission_queue_.empty())
{
return nullptr;
}
if(current_mission_ == nullptr)
{
current_mission_ = mission_queue_.front();
mission_queue_.pop();
state_ = MissionState::RUNNING;
}
return current_mission_;
}
@@ -259,40 +215,22 @@ namespace mission_adapters
{
std::lock_guard<std::mutex> lock(mutex_);
current_mission_.reset();
if(mission_queue_.empty())
{
state_ = MissionState::IDLE;
}
else
{
state_ = MissionState::QUEUED;
}
state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED;
}
void MissionManager::onNavigationFailed()
{
std::lock_guard<std::mutex> lock(mutex_);
state_ = MissionState::FAILED;
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
while (!mission_queue_.empty()) mission_queue_.pop();
state_ = MissionState::FAILED;
}
void MissionManager::cancel()
{
std::lock_guard<std::mutex> lock(mutex_);
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
while (!mission_queue_.empty()) mission_queue_.pop();
state_ = MissionState::CANCELLED;
}
@@ -300,12 +238,7 @@ namespace mission_adapters
{
std::lock_guard<std::mutex> lock(mutex_);
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
while (!mission_queue_.empty()) mission_queue_.pop();
state_ = MissionState::EMERGENCY;
}
@@ -313,289 +246,157 @@ namespace mission_adapters
{
std::lock_guard<std::mutex> lock(mutex_);
if (state_ == MissionState::RUNNING)
{
state_ = MissionState::PAUSED;
}
}
void MissionManager::resume()
{
std::lock_guard<std::mutex> lock(mutex_);
if(state_ != MissionState::PAUSED)
{
return;
}
if (state_ != MissionState::PAUSED) return;
if (current_mission_)
{
state_ = MissionState::RUNNING;
}
else if(!mission_queue_.empty())//Nếu đang thực hiện mission cuối cần xem lại logic
{
else if (!mission_queue_.empty())
state_ = MissionState::QUEUED;
}
else
{
state_ = MissionState::IDLE;
}
}
MissionState MissionManager::state() const
{
std::lock_guard<std::mutex> lock(mutex_);
return state_;
}
// FIX #3: hasMission now reflects ALL pending work, not just the queue.
bool MissionManager::hasMission() const
{
std::lock_guard<std::mutex> lock(mutex_);
return !mission_queue_.empty();
return current_mission_ != nullptr || !mission_queue_.empty();
}
// ─────────────────────────────────────────────────────────────────────────
// EventProcessor
// ─────────────────────────────────────────────────────────────────────────
EventProcessor::EventProcessor(MissionManager& mission_manager)
: mission_manager_(mission_manager)
{}
EventProcessor::~EventProcessor()
{
stop();
}
EventProcessor::~EventProcessor() { stop(); }
void EventProcessor::start()
{
if(running_)
{
return;
}
if (running_) return;
running_ = true;
event_bus_.reset();
worker_ =
std::thread(
&EventProcessor::spin,
this);
worker_ = std::thread(&EventProcessor::spin, this);
}
void EventProcessor::stop()
{
running_ = false;
event_bus_.stop();
if(worker_.joinable())
{
worker_.join();
}
if (worker_.joinable()) worker_.join();
}
void EventProcessor::spin()
{
Event event;
// robot::Rate rate(50);
while (running_)
{
if (event_bus_.pop(event))
{
process(event);
}
// rate.sleep();
}
}
void EventProcessor::process(const Event& event)
{
switch (event.type)
{
case EventType::SUBMIT_MISSIONS:
{
mission_manager_.submit(
event.missions);
break;
}
case EventType::NAV_DONE:
{
mission_manager_.onNavigationDone();
break;
}
case EventType::NAV_FAILED:
{
mission_manager_.onNavigationFailed();
break;
}
case EventType::PAUSE:
{
mission_manager_.pause();
break;
}
case EventType::RESUME:
{
mission_manager_.resume();
break;
}
case EventType::CANCEL:
{
mission_manager_.cancel();
break;
}
case EventType::EMERGENCY:
{
mission_manager_.emergency();
break;
}
case EventType::SUBMIT_MISSIONS: mission_manager_.submit(event.missions); break;
case EventType::NAV_DONE: mission_manager_.onNavigationDone(); break;
case EventType::NAV_FAILED: mission_manager_.onNavigationFailed(); break;
case EventType::PAUSE: mission_manager_.pause(); break;
case EventType::RESUME: mission_manager_.resume(); break;
case EventType::CANCEL: mission_manager_.cancel(); break;
case EventType::EMERGENCY: mission_manager_.emergency(); break;
default:
{
robot::log_error("Unknown event type");
break;
}
}
}
void EventProcessor::orderEvent(const robot_protocol_msgs::Order& order)
{
auto missions = vda5050_adapter_.convert(order);
Event event;
event.type = EventType::SUBMIT_MISSIONS;
event.priority = PRIORITY_ORDER;
event.missions = std::move(missions);
event.missions = vda5050_adapter_.convert(order);
event_bus_.push(event);
}
void EventProcessor::goalEvent(const robot_geometry_msgs::PoseStamped& goal)
{
auto missions = goal_adapter_.convert(goal);
Event event;
event.type = EventType::SUBMIT_MISSIONS;
event.priority = PRIORITY_ORDER;
event.missions = std::move(missions);
event.missions = goal_adapter_.convert(goal);
event_bus_.push(event);
}
void EventProcessor::pauseEvent()
{
Event event;
event.type = EventType::PAUSE;
event.priority = PRIORITY_PAUSE;
event_bus_.push(event);
event_bus_.push({EventType::PAUSE, PRIORITY_PAUSE, {}});
}
void EventProcessor::resumeEvent()
{
Event event;
event.type = EventType::RESUME;
event.priority = PRIORITY_RESUME;
event_bus_.push(event);
event_bus_.push({EventType::RESUME, PRIORITY_RESUME, {}});
}
void EventProcessor::cancelEvent()
{
Event event;
event.type =
EventType::CANCEL;
event.priority =
PRIORITY_CANCEL;
event_bus_.push(event);
event_bus_.push({EventType::CANCEL, PRIORITY_CANCEL, {}});
}
void EventProcessor::navDoneEvent()
{
Event event;
event.type =
EventType::NAV_DONE;
event.priority =
PRIORITY_NAV_DONE;
event_bus_.push(event);
event_bus_.push({EventType::NAV_DONE, PRIORITY_NAV_DONE, {}});
}
// FIX #4: Use PRIORITY_NAV_FAILED (correct constant name).
void EventProcessor::navFailedEvent()
{
Event event;
event.type = EventType::NAV_FAILED;
event.priority = PRIORITY_NAV_DONE;
event_bus_.push(event);
event_bus_.push({EventType::NAV_FAILED, PRIORITY_NAV_FAILED, {}});
}
void EventProcessor::emergencyEvent()
{
Event event;
event.type = EventType::EMERGENCY;
event.priority = PRIORITY_EMERGENCY;
event_bus_.push(event);
event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}});
}
// ─────────────────────────────────────────────────────────────────────────
// MissionExecutor
// ─────────────────────────────────────────────────────────────────────────
MissionExecutor::MissionExecutor(MissionManager& manager)
: mission_manager_(manager)
{}
MissionExecutor::~MissionExecutor()
{
stop();
}
MissionExecutor::~MissionExecutor() { stop(); }
void MissionExecutor::start()
{
if(running_)
{
return;
}
if (running_) return;
running_ = true;
worker_ =
std::thread(
&MissionExecutor::spin,
this);
worker_ = std::thread(&MissionExecutor::spin, this);
}
void MissionExecutor::stop()
{
running_ = false;
if(worker_.joinable())
{
worker_.join();
}
if (worker_.joinable()) worker_.join();
}
void MissionExecutor::spin()
@@ -604,8 +405,7 @@ namespace mission_adapters
while (running_)
{
auto mission =
mission_manager_.nextMission();
auto mission = mission_manager_.nextMission();
if (mission)
{
@@ -613,11 +413,10 @@ namespace mission_adapters
{
std::lock_guard<std::mutex> lock(mutex_);
if (mission != last_dispatched_mission_)
{
last_dispatched_mission_ = mission;
mission_excute_ = mission;
mission_execute_ = mission;
is_new_mission = true;
}
}
@@ -625,25 +424,31 @@ namespace mission_adapters
if (is_new_mission)
{
MissionCallback callback;
{
std::lock_guard<std::mutex> lock(mutex_);
callback = mission_callback_;
}
if(callback)
if (callback) callback(mission);
}
}
else
{
callback(mission);
}
}
}
if(!mission)
// FIX #5: Only reset the dedup token when the manager is truly
// done (IDLE / CANCELLED / FAILED / EMERGENCY), NOT when
// it is merely PAUSED — the mission hasn't changed there.
auto s = mission_manager_.state();
if (s == MissionState::IDLE ||
s == MissionState::CANCELLED ||
s == MissionState::FAILED ||
s == MissionState::EMERGENCY)
{
std::lock_guard<std::mutex> lock(mutex_);
last_dispatched_mission_.reset();
}
}
rate.sleep();
}
}
}
} // namespace mission_adapters

96
src/robot_control_test.cpp
View File

@@ -2,86 +2,96 @@
#include <move_base_core/navigation.h>
using namespace mission_adapters;
class RobotControlTest
{
// Giả sử chúng ta có một implementation của BaseNavigation, ví dụ MoveBase và đã được load vào move_base_ptr_
robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
mission_adapters::MissionManager mission_manager_;
mission_adapters::EventProcessor event_processor_{mission_manager_};
mission_adapters::MissionExecutor mission_executor_{mission_manager_};
std::shared_ptr<Mission> mission_prev_ = nullptr;
robot::move_base_core::State prev_state_ = PENDING;
// FIX #7: Fully qualified namespace for the initial value.
robot::move_base_core::State prev_nav_state_ = robot::move_base_core::State::PENDING;
// Tracks whether the actions of the current mission are complete.
// FIX #8: Stub — replace with real action-done check from your action executor.
bool areActionsDone() const
{
// TODO: query your action executor for completion status.
return true;
}
public:
RobotControlTest() = default;
~RobotControlTest();
void run();
private:
void executeMission(const Mission& mission);
};
RobotControlTest::~RobotControlTest()
{
event_processor_.stop();
mission_executor_.stop();
}
void RobotControlTest::run()
{
robot::Rate rate(50);
event_processor_.start();
mission_executor_.start();
while(true)
{
// chạy trong một luồng riêng
auto state = move_base_ptr_->getFeedback()->navigation_state;
bool action_done = true;
//kiểm tra acction done chưa
//thêm kiểm tra navDoneEvent state == robot::move_base_core::State::SUCCEEDED và action_done == true
if(state != prev_state_)
{
if(state ==
robot::move_base_core::State::SUCCEEDED)
{
event_processor_.navDoneEvent();
}
if(state ==
robot::move_base_core::State::ABORTED)
{
event_processor_.navFailedEvent();
}
prev_state_ = state;
}
//nhận oder trong một luồng riêng
//ví dụ: robot_protocol_msgs::Order order;
if(/* có order mới */)
{
robot_protocol_msgs::Order order;
// ... nhận order từ đâu đó, ví dụ qua ROS topic hoặc service
event_processor_.orderEvent(order);
}
// FIX #1: Set the callback ONCE before starting the executor, not inside the loop.
mission_executor_.setMissionCallback(
[&](const std::shared_ptr<Mission>& mission)
[this](const std::shared_ptr<Mission>& mission)
{
// ví dụ hàm executeMission đã có, nhận mission và đẩy dữ liệu goal sang move_base_ptr_
executeMission(*mission);
});
event_processor_.start();
mission_executor_.start();
while (true)
{
auto feedback = move_base_ptr_->getFeedback();
auto nav_state = feedback->navigation_state;
// FIX #8: navDoneEvent fires only when navigation AND actions are both done.
if (nav_state != prev_nav_state_)
{
if (nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
{
event_processor_.navDoneEvent();
}
else if (nav_state == robot::move_base_core::State::ABORTED)
{
event_processor_.navFailedEvent();
}
prev_nav_state_ = nav_state;
}
// Example: receive an order (replace condition with your real source)
if (/* new order available */ false)
{
robot_protocol_msgs::Order order;
// ... populate order ...
event_processor_.orderEvent(order);
}
rate.sleep();
}
}
void RobotControlTest::executeMission(const Mission& mission)
{
// TODO: send mission goal to move_base_ptr_
(void)mission;
}
int main()
{
RobotControlTest test;
test.run();
return 0;
}