first update

include/mission_adapters/mission_adapters.h

@@ -0,0 +1,279 @@
+#ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H
+#define MISSION_ADAPTERS_MISSION_ADAPTERS_H
+
+#include <string>
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <functional>
+#include <atomic>
+
+#include <robot/robot.h>
+#include <robot_protocol_msgs/Order.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+
+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_RESUME    = 2;
+    static constexpr int PRIORITY_PAUSE     = 3;
+    static constexpr int PRIORITY_NAV_DONE  = 4;
+    static constexpr int PRIORITY_ACT_DONE  = 4;
+    static constexpr int PRIORITY_ORDER     = 5;
+
+    enum class MissionState
+    {
+        IDLE,
+        QUEUED,
+        RUNNING,
+        PAUSED,
+        WAITING_ACTION,
+        RECOVERY,
+        COMPLETED,
+        FAILED,
+        CANCELLED,
+        EMERGENCY
+    };
+
+    enum class EventType
+    {
+        SUBMIT_MISSIONS,
+        NAV_DONE,
+        NAV_FAILED,
+        PAUSE,
+        RESUME,
+        CANCEL,
+        EMERGENCY
+    };
+
+    enum class MissionType
+    {
+        SIMPLE_GOAL,
+        VDA5050_ORDER
+    };
+
+    enum class ActionType
+    {
+        NODE_ACTION,
+        EDGE_ACTION
+    };
+
+    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
+        ActionType type;
+        robot_protocol_msgs::Action action;
+    };
+
+    class Mission
+    {
+    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;
+    };
+
+    struct EventCompare
+    {
+        bool operator()(const Event& lhs, const Event& rhs) const
+        {
+            return lhs.priority > rhs.priority;
+        }
+    };
+
+    class EventBus
+    {
+    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();
+            }
+        }
+    };
+
+    class VDA5050Adapter
+    {
+    public:
+        std::vector<std::shared_ptr<Mission>> convert(const robot_protocol_msgs::Order& order);
+    };
+
+    class GoalAdapter
+    {
+    public:
+        std::vector<std::shared_ptr<Mission>> convert(const robot_geometry_msgs::PoseStamped& goal);
+    };
+
+    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;
+
+        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_;
+    };
+
+    class EventProcessor
+    {
+    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>&)>;
+
+        explicit MissionExecutor(MissionManager& manager);
+        ~MissionExecutor();
+
+        void start();
+        void stop();
+
+        void setMissionCallback(MissionCallback cb)
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            mission_callback_ = std::move(cb);
+        }
+
+    private:
+        std::shared_ptr<Mission> mission_excute_;
+
+        // Mission cuối cùng đã dispatch
+        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();
+    };
+}
+
+#endif  // MISSION_ADAPTERS_MISSION_ADAPTERS_H