change file readme

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
+Framework cung cấp:
-- ⚙️ **C++ Standalone** - Thư viện C++ thuần cho các ứng dụng không dùng ROS
 
-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:
+* Chuyển đổi Goal hoặc VDA5050 Order thành Mission.
-- **Heading Error** - Lỗi góc định hướng
+* Quản lý hàng đợi Mission.
-- **Cross-track Error** - Sai lệch ngang so với đường dẫn
+* 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  
+```text
-✅ **Bicycle Kinematic Model** - Mô hình chính xác cho xe, xe tải, xe điện  
+Order / Goal
-✅ **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  
+|  EventProcessor  |
-✅ **Chuyển động mượt mà** - Điều khiển lái góc mức** (smooth steering)  
++------------------+
-
+      │
----
+      ▼
-
++------------------+
-## 📚 Nguyên lý Stanley Method
+|  MissionManager  |
-
++------------------+
-### Bicycle Model
+      │
-
+      ▼
-Mô hình bicycle kinematic mô tả động học của robot:
++------------------+
-
+| MissionExecutor  |
-```
++------------------+
-    θ (yaw angle)
+      │
-    ↑
+      ▼
-    |     Front Axle (steering angle δ)
+ Navigation Stack
-    |    /
+ (MoveBase, MPPI,
-    |___/______ Rear Axle (reference point)
+  Pure Pursuit...)
-         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
 ```
 
 ---
 
-## 🎯 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
+```cpp
-# Robot Parameters
+class Mission
-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²)
+public:
-robot_wheelbase: 0.5           # Khoảng cách trục bánh (m)
+    int sequenceId;
-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)
 
-# Stanley Controller Parameters
+    MissionType type;
-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)
 
-# Trajectory Parameters
+    int priority;
-lookahead_distance: 1.0         # Tầm nhìn trước (m)
-path_resolution: 0.1            # Độ phân giải đường dẫn (m)
 
-# Goal Tolerance
+    PoseStamped start;
-xy_goal_tolerance: 0.1          # Sai số vị trí (m)
+
-yaw_goal_tolerance: 0.1         # Sai số định hướng (rad)
+    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 |
+* Goal đơn giản
-|---------|------|------|
+* VDA5050 Order
-| `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`
 
 ---
 
-## 📊 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ú |
+```cpp
-|--------|-------|--------|
+class Action
-| Tracking Error | < 0.08m | Path deviation |
+{
-| Heading Error | < 0.05 rad | Angular accuracy |
+public:
-| Response Time | < 50ms | Control loop |
+    int sequenceId;
-| CPU Usage | ~2-3% | Single core |
+
-| Memory | ~5MB | Runtime |
+    ActionType type;
-| Max Speed | 1.5 m/s | Tested |
+
+    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.
 
-```
+```cpp
-Cho đường dẫn từ điểm P_i đến P_{i+1}
+GoalAdapter adapter;
-Vị trí robot: R = (x, y)
 
-1. Tìm hình chiếu vuông góc từ R lên đường P_i → P_{i+1}
+auto missions =
-2. e_y = khoảng cách từ R đến điểm chiếu
+    adapter.convert(goal);
-3. Dấu: dương nếu robot ở bên trái, âm nếu ở bên phải
 ```
 
-### Heading Error Calculation
+Kết quả:
 
-```
+```text
-ψ_desired = atan2(P_{i+1}.y - P_i.y, P_{i+1}.x - P_i.x)
+Goal
-ψ_current = θ (yaw angle của robot)
+ └── Mission
-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)
 ```
 
 ---
 
-## 📚 Tài liệu tham khảo
+## VDA5050Adapter
 
-- **Stanley Method Paper**: "Journal of Field Robotics" - Thrun et al. (2006)
+Chuyển đổi Order thành danh sách Mission.
-- **Bicycle Kinematic Model**: Classic control theory
+
-- **ROS Navigation**: [http://wiki.ros.org/navigation](http://wiki.ros.org/navigation)
+Ví dụ:
-- **Eigen Documentation**: [https://eigen.tuxfamily.org/](https://eigen.tuxfamily.org/)
+
+```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.

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<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 
+        inline bool empty() const
-        { 
+        {
             std::lock_guard<std::mutex> lock(mutex_);
-
+            return queue_.empty();
-            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 =
+        using MissionCallback = std::function<void(const std::shared_ptr<Mission>&)>;
-            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

src/mission_adapters.cpp

@@ -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,
+        if (stop_ || queue_.empty()) return false;
-        [this]
-        {
-            return stop_ || !queue_.empty();
-        });
-
-        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,172 +41,145 @@ 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->type = MissionType::SIMPLE_GOAL;
-
         mission->priority = 0;
+        mission->goal     = goal;
 
-        /////////////////////////// Để tạm thời  ///////////////////////////
+        return {mission};
-        mission->goal = goal;
-        ////////////////////////////////////////////////////////////////////
-
-        missions.push_back(mission);
-
-        return missions;
     }
 
+    // ─────────────────────────────────────────────────────────────────────────
+    // 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)
-        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>();
-
+            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
+        // Remaining segment after the last action node
-        if(start_node_idx < order.nodes.size() - 1)
+        if (start_node_idx < order.nodes.size() - 1)
         {
             auto mission = std::make_shared<Mission>();
-
+            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
+        // Collect and sort actions
-        for(auto& mission : missions)
+        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;
+                    Action ma;
-                    mission_action.sequenceId = edge.sequenceId; // Giữ nguyên sequenceId từ action gốc
+                    // FIX #6: Use action.sequenceId, not edge.sequenceId,
-                    mission_action.type = ActionType::EDGE_ACTION;
+                    //         so the sort below produces the correct execution order.
-                    mission_action.action = action;
+                    ma.sequenceId = action.sequenceId;
-                    mission->actions.push_back(std::move(mission_action));
+                    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;
+                Action ma;
-                mission_action.sequenceId = node.sequenceId; // Giữ nguyên sequenceId từ action gốc
+                // FIX #6: Same fix for node actions.
-                mission_action.type = ActionType::NODE_ACTION;
+                ma.sequenceId = action.sequenceId;
-                mission_action.action = action;
+                ma.type       = ActionType::NODE_ACTION;
-                mission->actions.push_back(std::move(mission_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);
-        }
 
-        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 ||
+            switch (state_)
-            state_ == MissionState::FAILED ||
-            state_ == MissionState::CANCELLED ||
-            state_ == MissionState::EMERGENCY)
             {
-                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<Mission> MissionManager::nextMission()
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        if(state_ == MissionState::PAUSED || 
+
-        state_ == MissionState::FAILED || 
+        if (state_ == MissionState::PAUSED    ||
-        state_ == MissionState::CANCELLED || 
+            state_ == MissionState::FAILED    ||
-        state_ == MissionState::EMERGENCY)
+            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();
-            current_mission_ = mission_queue_.front();
+        state_ = MissionState::RUNNING;
-
-            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();
-
+        state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED;
-        if(mission_queue_.empty())
-        {
-            state_ = MissionState::IDLE;
-        }
-        else
-        {
-            state_ = 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())
+        state_ = MissionState::FAILED;
-        {
-            mission_queue_.pop();
-        }
     }
 
     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,350 +238,217 @@ 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;
     }
 
     void MissionManager::pause()
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        if(state_ == MissionState::RUNNING)
+        if (state_ == MissionState::RUNNING)
-        {
             state_ = MissionState::PAUSED;
-        }
     }
 
     void MissionManager::resume()
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        if(state_ != MissionState::PAUSED)
+        if (state_ != MissionState::PAUSED) return;
-        {
-            return;
-        }
 
-        if(current_mission_)
+        if (current_mission_)
-        {
             state_ = MissionState::RUNNING;
-        }
+        else if (!mission_queue_.empty())
-        else if(!mission_queue_.empty())//Nếu đang thực hiện mission cuối cần xem lại logic
-        {
             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 current_mission_ != nullptr || !mission_queue_.empty();
-        return !mission_queue_.empty();
     }
 
+    // ─────────────────────────────────────────────────────────────────────────
+    // EventProcessor
+    // ─────────────────────────────────────────────────────────────────────────
+
     EventProcessor::EventProcessor(MissionManager& mission_manager)
-    : mission_manager_(mission_manager)
+        : mission_manager_(mission_manager)
     {}
 
-    EventProcessor::~EventProcessor()
+    EventProcessor::~EventProcessor() { stop(); }
-    {
-        stop();
-    }
 
     void EventProcessor::start()
     {
-        if(running_)
+        if (running_) return;
-        {
-            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;
-
+        while (running_)
-        // robot::Rate rate(50);
-
-        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:
+            case EventType::SUBMIT_MISSIONS: mission_manager_.submit(event.missions);      break;
-            {
+            case EventType::NAV_DONE:        mission_manager_.onNavigationDone();          break;
-                mission_manager_.submit(
+            case EventType::NAV_FAILED:      mission_manager_.onNavigationFailed();        break;
-                    event.missions);
+            case EventType::PAUSE:           mission_manager_.pause();                     break;
-                break;
+            case EventType::RESUME:          mission_manager_.resume();                    break;
-            }
+            case EventType::CANCEL:          mission_manager_.cancel();                    break;
-
+            case EventType::EMERGENCY:       mission_manager_.emergency();                 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 = vda5050_adapter_.convert(order);
-        event.missions = std::move(missions);
-
         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 = goal_adapter_.convert(goal);
-        event.missions = std::move(missions);
-
         event_bus_.push(event);
     }
 
     void EventProcessor::pauseEvent()
     {
-        Event event;
+        event_bus_.push({EventType::PAUSE, PRIORITY_PAUSE, {}});
-
-        event.type = EventType::PAUSE;
-
-        event.priority = PRIORITY_PAUSE;
-
-        event_bus_.push(event);
     }
 
     void EventProcessor::resumeEvent()
     {
-        Event event;
+        event_bus_.push({EventType::RESUME, PRIORITY_RESUME, {}});
-
-        event.type = EventType::RESUME;
-
-        event.priority = PRIORITY_RESUME;
-
-        event_bus_.push(event);
     }
 
     void EventProcessor::cancelEvent()
     {
-        Event event;
+        event_bus_.push({EventType::CANCEL, PRIORITY_CANCEL, {}});
-
-        event.type =
-            EventType::CANCEL;
-
-        event.priority =
-            PRIORITY_CANCEL;
-
-        event_bus_.push(event);
     }
 
     void EventProcessor::navDoneEvent()
     {
-        Event event;
+        event_bus_.push({EventType::NAV_DONE, PRIORITY_NAV_DONE, {}});
-
-        event.type =
-            EventType::NAV_DONE;
-
-        event.priority =
-            PRIORITY_NAV_DONE;
-
-        event_bus_.push(event);
     }
 
+    // FIX #4: Use PRIORITY_NAV_FAILED (correct constant name).
     void EventProcessor::navFailedEvent()
     {
-        Event event;
+        event_bus_.push({EventType::NAV_FAILED, PRIORITY_NAV_FAILED, {}});
-
-        event.type = EventType::NAV_FAILED;
-
-        event.priority = PRIORITY_NAV_DONE;
-
-        event_bus_.push(event);
     }
 
     void EventProcessor::emergencyEvent()
     {
-        Event event;
+        event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}});
-
-        event.type = EventType::EMERGENCY;
-
-        event.priority = PRIORITY_EMERGENCY;
-
-        event_bus_.push(event);
     }
 
+    // ─────────────────────────────────────────────────────────────────────────
+    // MissionExecutor
+    // ─────────────────────────────────────────────────────────────────────────
+
     MissionExecutor::MissionExecutor(MissionManager& manager)
         : mission_manager_(manager)
     {}
 
-    MissionExecutor::~MissionExecutor()
+    MissionExecutor::~MissionExecutor() { stop(); }
-    {
-        stop();
-    }
 
     void MissionExecutor::start()
     {
-        if(running_)
+        if (running_) return;
-        {
-            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 =
+            auto mission = mission_manager_.nextMission();
-                mission_manager_.nextMission();
 
-            if(mission)
+            if (mission)
             {
                 bool is_new_mission = false;
 
                 {
                     std::lock_guard<std::mutex> lock(mutex_);
-
+                    if (mission != last_dispatched_mission_)
-                    if(mission != last_dispatched_mission_)
                     {
                         last_dispatched_mission_ = mission;
-                        mission_excute_ = mission;
+                        mission_execute_         = mission;
-                        is_new_mission = true;
+                        is_new_mission           = true;
                     }
                 }
 
-                if(is_new_mission)
+                if (is_new_mission)
                 {
                     MissionCallback callback;
-
                     {
                         std::lock_guard<std::mutex> lock(mutex_);
                         callback = mission_callback_;
                     }
-
+                    if (callback) callback(mission);
-                    if(callback)
-                    {
-                        callback(mission);
-                    }
                 }
             }
-            if(!mission)
+            else
             {
-                std::lock_guard<std::mutex> lock(mutex_);
+                // FIX #5: Only reset the dedup token when the manager is truly
-                last_dispatched_mission_.reset();
+                //         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

src/robot_control_test.cpp

@@ -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::MissionManager  mission_manager_;
-    mission_adapters::EventProcessor event_processor_{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()  = 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;
+    // FIX #1: Set the callback ONCE before starting the executor, not inside the loop.
-        //kiểm tra acction done chưa
+    mission_executor_.setMissionCallback(
-
+        [this](const std::shared_ptr<Mission>& mission)
-        //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);
-        }
-
-
-        mission_executor_.setMissionCallback(
-        [&](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;
-}
+}
-