From 1bd09d5e8e825ff3a8cbc6cc30d6267e14ba761a Mon Sep 17 00:00:00 2001 From: duongtd Date: Sat, 6 Jun 2026 10:34:18 +0700 Subject: [PATCH] change file readme --- README.md | 576 ++++++++++++++------ include/mission_adapters/mission_adapters.h | 77 +-- src/mission_adapters.cpp | 489 +++++------------ src/robot_control_test.cpp | 108 ++-- 4 files changed, 632 insertions(+), 618 deletions(-) diff --git a/README.md b/README.md index 5fd0ff2..5728f22 100644 --- a/README.md +++ b/README.md @@ -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 nodes; + + std::vector edges; + + std::vector 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, + 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& + ) + >; +``` + +Đăng ký callback: + +```cpp +mission_executor.setMissionCallback( + [&](const std::shared_ptr& 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) + { + 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. diff --git a/include/mission_adapters/mission_adapters.h b/include/mission_adapters/mission_adapters.h index 244e5fb..0dc71f8 100644 --- a/include/mission_adapters/mission_adapters.h +++ b/include/mission_adapters/mission_adapters.h @@ -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 nodes; - std::vector edges; - std::vector actions; }; struct Event { EventType type; - int priority; - std::vector> missions; }; @@ -112,36 +104,26 @@ namespace mission_adapters { private: mutable std::mutex mutex_; - std::priority_queue, 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 - { + inline bool empty() const + { std::lock_guard lock(mutex_); - - return queue_.empty(); + return queue_.empty(); } inline void clear() { std::lock_guard 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>& missions); std::shared_ptr 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> mission_queue_; - std::shared_ptr 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 running_{false}; }; - + class MissionExecutor { public: - using MissionCallback = - std::function&)>; + using MissionCallback = std::function&)>; 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 lock(mutex_); @@ -257,23 +215,20 @@ namespace mission_adapters } private: - std::shared_ptr mission_excute_; + std::shared_ptr mission_execute_; - // Mission cuối cùng đã dispatch + // Last mission that was dispatched to the callback. std::shared_ptr last_dispatched_mission_; MissionCallback mission_callback_; - MissionManager& mission_manager_; - mutable std::mutex mutex_; - std::thread worker_; - std::atomic running_{false}; void spin(); }; -} + +} // namespace mission_adapters #endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H \ No newline at end of file diff --git a/src/mission_adapters.cpp b/src/mission_adapters.cpp index 99c8fa4..4edfd59 100644 --- a/src/mission_adapters.cpp +++ b/src/mission_adapters.cpp @@ -3,36 +3,26 @@ namespace mission_adapters { + // ───────────────────────────────────────────────────────────────────────── + // EventBus + // ───────────────────────────────────────────────────────────────────────── + void EventBus::push(const Event& event) { - std::lock_guard lock(mutex_); - queue_.push(event); - cv_.notify_one(); } bool EventBus::pop(Event& event) { std::unique_lock 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 lock(mutex_); stop_ = true; } - cv_.notify_all(); } @@ -52,172 +41,145 @@ namespace mission_adapters stop_ = false; } + // ───────────────────────────────────────────────────────────────────────── + // GoalAdapter + // ───────────────────────────────────────────────────────────────────────── + std::vector> GoalAdapter::convert(const robot_geometry_msgs::PoseStamped& goal) { - std::vector> missions; auto mission = std::make_shared(); - - mission->type = MissionType::SIMPLE_GOAL; - + mission->type = MissionType::SIMPLE_GOAL; mission->priority = 0; + mission->goal = goal; - /////////////////////////// Để tạm thời /////////////////////////// - mission->goal = goal; - //////////////////////////////////////////////////////////////////// - - missions.push_back(mission); - - return missions; + return {mission}; } + // ───────────────────────────────────────────────────────────────────────── + // VDA5050Adapter + // ───────────────────────────────────────────────────────────────────────── + std::vector> VDA5050Adapter::convert(const robot_protocol_msgs::Order& order) { std::vector> missions; std::vector action_node_indices; - // Tìm tất cả node có action - for(size_t i = 0; i < order.nodes.size(); ++i) + for (size_t i = 0; i < order.nodes.size(); ++i) { - if(!order.nodes[i].actions.empty()) - { + 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; - for(size_t i = 0; i < action_node_indices.size(); ++i) + for (size_t i = 0; i < action_node_indices.size(); ++i) { size_t end_node_idx = action_node_indices[i]; auto mission = std::make_shared(); - - mission->type = MissionType::VDA5050_ORDER; + 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) + if (end_node_idx > start_node_idx) { mission->edges.assign( order.edges.begin() + start_node_idx, 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 - if(start_node_idx < order.nodes.size() - 1) + // Remaining segment after the last action node + if (start_node_idx < order.nodes.size() - 1) { auto mission = std::make_shared(); - - mission->type = MissionType::VDA5050_ORDER; + mission->type = MissionType::VDA5050_ORDER; mission->priority = 0; mission->nodes.assign( order.nodes.begin() + start_node_idx, order.nodes.end()); - if(start_node_idx < order.edges.size()) + if (start_node_idx < order.edges.size()) { mission->edges.assign( order.edges.begin() + start_node_idx, 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 - for(auto& mission : missions) + // Collect and sort actions + for (auto& mission : missions) { - for(const auto& edge : mission->edges) + for (const auto& edge : mission->edges) { - for(const auto& action : edge.actions) + 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) + 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>& missions) { std::lock_guard lock(mutex_); - for(auto& mission : missions) - { + for (const auto& mission : missions) mission_queue_.push(mission); - } - if(!missions.empty()) + // 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_) { - state_ = MissionState::QUEUED; + 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,32 +187,26 @@ namespace mission_adapters std::shared_ptr MissionManager::nextMission() { std::lock_guard lock(mutex_); - if(state_ == MissionState::PAUSED || - state_ == MissionState::FAILED || - state_ == MissionState::CANCELLED || - state_ == MissionState::EMERGENCY) + + if (state_ == MissionState::PAUSED || + state_ == MissionState::FAILED || + state_ == MissionState::CANCELLED || + state_ == MissionState::EMERGENCY) { return nullptr; } - if(current_mission_) - { + // 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()) - { + if (mission_queue_.empty()) return nullptr; - } - if(current_mission_ == nullptr) - { - current_mission_ = mission_queue_.front(); - - mission_queue_.pop(); - - state_ = MissionState::RUNNING; - } + current_mission_ = mission_queue_.front(); + mission_queue_.pop(); + state_ = MissionState::RUNNING; return current_mission_; } @@ -259,40 +215,22 @@ namespace mission_adapters { std::lock_guard 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 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 lock(mutex_); current_mission_.reset(); - - while(!mission_queue_.empty()) - { - mission_queue_.pop(); - } - + while (!mission_queue_.empty()) mission_queue_.pop(); state_ = MissionState::CANCELLED; } @@ -300,350 +238,217 @@ namespace mission_adapters { std::lock_guard lock(mutex_); current_mission_.reset(); - - while(!mission_queue_.empty()) - { - mission_queue_.pop(); - } - + while (!mission_queue_.empty()) mission_queue_.pop(); state_ = MissionState::EMERGENCY; } void MissionManager::pause() { std::lock_guard lock(mutex_); - if(state_ == MissionState::RUNNING) - { + if (state_ == MissionState::RUNNING) state_ = MissionState::PAUSED; - } } void MissionManager::resume() { std::lock_guard lock(mutex_); - if(state_ != MissionState::PAUSED) - { - return; - } + if (state_ != MissionState::PAUSED) return; - if(current_mission_) - { + 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 lock(mutex_); - return state_; } + // FIX #3: hasMission now reflects ALL pending work, not just the queue. bool MissionManager::hasMission() const { std::lock_guard lock(mutex_); - - return !mission_queue_.empty(); + return current_mission_ != nullptr || !mission_queue_.empty(); } + // ───────────────────────────────────────────────────────────────────────── + // EventProcessor + // ───────────────────────────────────────────────────────────────────────── + EventProcessor::EventProcessor(MissionManager& mission_manager) - : mission_manager_(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_) + while (running_) { - if(event_bus_.pop(event)) - { + if (event_bus_.pop(event)) process(event); - } - - // rate.sleep(); } } void EventProcessor::process(const Event& event) { - switch(event.type) + 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.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.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() { robot::Rate rate(20); - while(running_) + while (running_) { - auto mission = - mission_manager_.nextMission(); + auto mission = mission_manager_.nextMission(); - if(mission) + if (mission) { bool is_new_mission = false; { std::lock_guard lock(mutex_); - - if(mission != last_dispatched_mission_) + if (mission != last_dispatched_mission_) { last_dispatched_mission_ = mission; - mission_excute_ = mission; - is_new_mission = true; + mission_execute_ = mission; + is_new_mission = true; } } - if(is_new_mission) + if (is_new_mission) { MissionCallback callback; - { std::lock_guard lock(mutex_); callback = mission_callback_; } - - if(callback) - { - callback(mission); - } + if (callback) callback(mission); } } - if(!mission) + else { - std::lock_guard lock(mutex_); - last_dispatched_mission_.reset(); + // 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 lock(mutex_); + last_dispatched_mission_.reset(); + } } rate.sleep(); } } -} + +} // namespace mission_adapters \ No newline at end of file diff --git a/src/robot_control_test.cpp b/src/robot_control_test.cpp index 3f957f0..27c9b54 100644 --- a/src/robot_control_test.cpp +++ b/src/robot_control_test.cpp @@ -2,86 +2,96 @@ #include 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::MissionManager mission_manager_; + mission_adapters::EventProcessor event_processor_{mission_manager_}; mission_adapters::MissionExecutor mission_executor_{mission_manager_}; - std::shared_ptr 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() = 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_) + // FIX #1: Set the callback ONCE before starting the executor, not inside the loop. + mission_executor_.setMissionCallback( + [this](const std::shared_ptr& mission) { - 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); - } - - - mission_executor_.setMissionCallback( - [&](const std::shared_ptr& 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; -} - +} \ No newline at end of file