add file test

CMakeLists.txt

@@ -92,7 +92,7 @@ else()
 
   catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_utils
+    LIBRARIES ${PROJECT_NAME}
     CATKIN_DEPENDS
       robot_costmap_2d
       robot_nav_core
@@ -105,7 +105,7 @@ else()
       robot_visualization_msgs
       robot_nav_2d_utils
       data_convert
-    DEPENDS PCL Boost Eigen
+    DEPENDS PCL Boost
   )
 
   include_directories(
@@ -218,11 +218,11 @@ endif()
 # ========================================================
 if(BUILDING_WITH_CATKIN)
 
-  # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
+  install(TARGETS ${PROJECT_NAME}
-  #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  #   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-  # )
+  )
 
   install(DIRECTORY include/${PROJECT_NAME}/
     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
@@ -237,12 +237,12 @@ if(BUILDING_WITH_CATKIN)
 
 else()
 
-  # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
+  install(TARGETS ${PROJECT_NAME}
-  #   EXPORT ${PROJECT_NAME}-targets
+    EXPORT ${PROJECT_NAME}-targets
-  #   ARCHIVE DESTINATION lib
+    ARCHIVE DESTINATION lib
-  #   LIBRARY DESTINATION lib
+    LIBRARY DESTINATION lib
-  #   RUNTIME DESTINATION bin
+    RUNTIME DESTINATION bin
-  # )
+  )
 
   install(EXPORT ${PROJECT_NAME}-targets
     FILE ${PROJECT_NAME}-targets.cmake
@@ -290,24 +290,15 @@ if(CATKIN_ENABLE_TESTING)
     message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
   endif()
 
-  find_package(GTest REQUIRED)
+  catkin_add_gtest(test_mission_adapters
-
+    test/mission_adapters_test.cpp
-  add_executable(test_mission_adapters
-    src/test_mission_adapters.cpp
-  )
-
-  target_link_libraries(test_mission_adapters
-    mission_adapters
-    GTest::GTest
-    GTest::Main
-    pthread
-    ${catkin_LIBRARIES}
   )
 
   if(TARGET test_mission_adapters)
     target_link_libraries(test_mission_adapters
       mission_adapters
       ${catkin_LIBRARIES}
+      pthread
     )
 
     target_include_directories(test_mission_adapters PRIVATE

include/mission_adapters/mission_adapters.h

@@ -7,8 +7,10 @@
 #include <memory>
 #include <thread>
 #include <mutex>
+#include <condition_variable>
 #include <functional>
 #include <atomic>
+#include <cstdint>
 
 #include <robot/robot.h>
 #include <robot_protocol_msgs/Order.h>
@@ -68,17 +70,17 @@ namespace mission_adapters
     class Action
     {
     public:
-        int sequenceId;
+        int sequenceId = 0;
-        ActionType type;
+        ActionType type = ActionType::NODE_ACTION;
         robot_protocol_msgs::Action action;
     };
 
     class Mission
     {
     public:
-        int sequenceId;
+        int sequenceId = 0;
-        MissionType type;
+        MissionType type = MissionType::SIMPLE_GOAL;
-        int priority;
+        int priority = 0;
         robot_geometry_msgs::PoseStamped start;
         robot_geometry_msgs::PoseStamped goal;
         std::vector<robot_protocol_msgs::Node> nodes;
@@ -88,8 +90,9 @@ namespace mission_adapters
 
     struct Event
     {
-        EventType type;
+        EventType type = EventType::SUBMIT_MISSIONS;
-        int priority;
+        int priority = PRIORITY_ORDER;
+        uint64_t sequence = 0;
         std::vector<std::shared_ptr<Mission>> missions;
     };
 
@@ -97,7 +100,10 @@ namespace mission_adapters
     {
         bool operator()(const Event& lhs, const Event& rhs) const
         {
+            if (lhs.priority != rhs.priority)
                 return lhs.priority > rhs.priority;
+
+            return lhs.sequence > rhs.sequence;
         }
     };
 
@@ -108,6 +114,7 @@ namespace mission_adapters
         std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
         std::condition_variable cv_;
         bool stop_ = false;
+        uint64_t next_sequence_ = 0;
 
     public:
         void push(const Event& event);

src/mission_adapters.cpp

@@ -11,13 +11,16 @@ namespace mission_adapters
     void EventBus::push(const Event& event)
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        queue_.push(event);
+        Event queued_event = event;
+        queued_event.sequence = next_sequence_++;
+        queue_.push(std::move(queued_event));
         cv_.notify_one();
     }
 
     void EventBus::push(Event&& event)
     {
         std::lock_guard<std::mutex> lock(mutex_);
+        event.sequence = next_sequence_++;
         queue_.push(std::move(event));
         cv_.notify_one();
     }
@@ -47,6 +50,8 @@ namespace mission_adapters
     {
         std::lock_guard<std::mutex> lock(mutex_);
         stop_ = false;
+        next_sequence_ = 0;
+        while (!queue_.empty()) queue_.pop();
     }
 
     // ─────────────────────────────────────────────────────────────────────────
@@ -76,6 +81,12 @@ namespace mission_adapters
 
         if (order.nodes.empty()) return missions;
 
+        if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
+        {
+            robot::log_error("Invalid VDA5050 order: edge count is smaller than node_count - 1");
+            return missions;
+        }
+
         for (size_t i = 0; i < order.nodes.size(); ++i)
         {
             if (!order.nodes[i].actions.empty())
@@ -169,6 +180,10 @@ namespace mission_adapters
     void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
     {
         std::lock_guard<std::mutex> lock(mutex_);
+
+        if (state_ == MissionState::EMERGENCY)
+            return;
+
         if (!mission_queue_.empty())
         {
             mission_queue_ = {};
@@ -188,7 +203,6 @@ namespace mission_adapters
                 case MissionState::COMPLETED:
                 case MissionState::FAILED:
                 case MissionState::CANCELLED:
-                case MissionState::EMERGENCY:
                 case MissionState::CLEAR_EMERGENCY:
                     state_ = MissionState::QUEUED;
                     break;
@@ -230,6 +244,10 @@ namespace mission_adapters
     void MissionManager::onNavigationDone()
     {
         std::lock_guard<std::mutex> lock(mutex_);
+
+        if (state_ != MissionState::RUNNING)
+            return;
+
         current_mission_.reset();
         state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED;
     }
@@ -237,6 +255,10 @@ namespace mission_adapters
     void MissionManager::onNavigationFailed()
     {
         std::lock_guard<std::mutex> lock(mutex_);
+
+        if (state_ != MissionState::RUNNING)
+            return;
+
         current_mission_.reset();
         while (!mission_queue_.empty()) mission_queue_.pop();
         state_ = MissionState::FAILED;
@@ -374,38 +396,59 @@ namespace mission_adapters
 
     void EventProcessor::pauseEvent()
     {
-        event_bus_.push({EventType::PAUSE, PRIORITY_PAUSE, {}});
+        Event event;
+        event.type = EventType::PAUSE;
+        event.priority = PRIORITY_PAUSE;
+        event_bus_.push(std::move(event));
     }
 
     void EventProcessor::resumeEvent()
     {
-        event_bus_.push({EventType::RESUME, PRIORITY_RESUME, {}});
+        Event event;
+        event.type = EventType::RESUME;
+        event.priority = PRIORITY_RESUME;
+        event_bus_.push(std::move(event));
     }
 
     void EventProcessor::cancelEvent()
     {
-        event_bus_.push({EventType::CANCEL, PRIORITY_CANCEL, {}});
+        Event event;
+        event.type = EventType::CANCEL;
+        event.priority = PRIORITY_CANCEL;
+        event_bus_.push(std::move(event));
     }
 
     void EventProcessor::navDoneEvent()
     {
-        event_bus_.push({EventType::NAV_DONE, PRIORITY_NAV_DONE, {}});
+        Event event;
+        event.type = EventType::NAV_DONE;
+        event.priority = PRIORITY_NAV_DONE;
+        event_bus_.push(std::move(event));
     }
 
     // FIX #4: Use PRIORITY_NAV_FAILED (correct constant name).
     void EventProcessor::navFailedEvent()
     {
-        event_bus_.push({EventType::NAV_FAILED, PRIORITY_NAV_FAILED, {}});
+        Event event;
+        event.type = EventType::NAV_FAILED;
+        event.priority = PRIORITY_NAV_FAILED;
+        event_bus_.push(std::move(event));
     }
 
     void EventProcessor::emergencyEvent()
     {
-        event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}});
+        Event event;
+        event.type = EventType::EMERGENCY;
+        event.priority = PRIORITY_EMERGENCY;
+        event_bus_.push(std::move(event));
     }
 
     void EventProcessor::clearEmergencyEvent()
     {
-        event_bus_.push({EventType::CLEAR_EMERGENCY, PRIORITY_EMERGENCY, {}});
+        Event event;
+        event.type = EventType::CLEAR_EMERGENCY;
+        event.priority = PRIORITY_EMERGENCY;
+        event_bus_.push(std::move(event));
     }
 
     // ─────────────────────────────────────────────────────────────────────────

src/robot_control_test.cpp

@@ -1,6 +1,8 @@
 #include <mission_adapters/mission_adapters.h>
 #include <move_base_core/navigation.h>
 
+#include <utility>
+
 using namespace mission_adapters;
 
 class RobotControlTest
@@ -23,7 +25,7 @@ class RobotControlTest
     }
 
 public:
-    RobotControlTest();
+    explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base);
     ~RobotControlTest();
 
     void run();
@@ -32,7 +34,8 @@ private:
     void executeMission(const Mission& mission);
 };
 
-RobotControlTest::RobotControlTest()
+RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base)
+    : move_base_ptr_(std::move(move_base))
 {}
 
 RobotControlTest::~RobotControlTest()
@@ -43,27 +46,39 @@ RobotControlTest::~RobotControlTest()
 
 void RobotControlTest::run()
 {
+    if (!move_base_ptr_)
+    {
+        robot::log_error("RobotControlTest requires a valid BaseNavigation pointer");
+        return;
+    }
+
     robot::Rate rate(50);
 
-    // FIX #1: Set the callback ONCE before starting the executor, not inside the loop.
     mission_executor_.setMissionCallback(
         [this](const std::shared_ptr<Mission>& mission)
         {
             executeMission(*mission);
         });
 
+    event_processor_.start();
+    mission_executor_.start();
 
-
+    while (robot::ok())
-    while (true)
     {
         auto feedback = move_base_ptr_->getFeedback();
+        if (!feedback)
+        {
+            rate.sleep();
+            continue;
+        }
+
         auto nav_state = feedback->navigation_state;
 
-        if (nav_state != prev_nav_done_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
+        if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
         {
             event_processor_.navDoneEvent();
         }
-        else if (nav_state == robot::move_base_core::State::ABORTED)
+        else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED)
         {
             event_processor_.navFailedEvent();
         }
@@ -86,10 +101,3 @@ void RobotControlTest::executeMission(const Mission& mission)
     // TODO: send mission goal to move_base_ptr_
     (void)mission;
 }
-
-int main()
-{
-    RobotControlTest test;
-    test.run();
-    return 0;
-}

src/test_mission_adapters.cpp

@@ -1,578 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <atomic>
-#include <chrono>
-#include <thread>
-
-#include "mission_adapters/mission_adapters.h"
-
-using namespace mission_adapters;
-
-namespace
-{
-
-robot_geometry_msgs::PoseStamped MakeGoal(double x, double y)
-{
-    robot_geometry_msgs::PoseStamped g;
-    g.pose.position.x = x;
-    g.pose.position.y = y;
-    return g;
-}
-
-robot_protocol_msgs::Action MakeAction(
-    const std::string &id)
-{
-    robot_protocol_msgs::Action a;
-    a.actionId = id;
-    a.actionType = "TEST";
-    return a;
-}
-
-robot_protocol_msgs::Node MakeNode(
-    int seq,
-    bool add_action = false)
-{
-    robot_protocol_msgs::Node n;
-    n.sequenceId = seq;
-    n.nodeId = "node_" + std::to_string(seq);
-
-    n.nodePosition.x = seq;
-    n.nodePosition.y = seq;
-
-    if (add_action)
-        n.actions.push_back(
-            MakeAction("node_action_" + std::to_string(seq)));
-
-    return n;
-}
-
-robot_protocol_msgs::Edge MakeEdge(
-    int seq,
-    bool add_action = false)
-{
-    robot_protocol_msgs::Edge e;
-
-    e.sequenceId = seq;
-    e.edgeId = "edge_" + std::to_string(seq);
-
-    if (add_action)
-        e.actions.push_back(
-            MakeAction("edge_action_" + std::to_string(seq)));
-
-    return e;
-}
-
-robot_protocol_msgs::Order MakeOrder(
-    int node_count)
-{
-    robot_protocol_msgs::Order order;
-
-    for (int i = 0; i < node_count; ++i)
-        order.nodes.push_back(
-            MakeNode(i));
-
-    for (int i = 0; i < node_count - 1; ++i)
-        order.edges.push_back(
-            MakeEdge(i));
-
-    return order;
-}
-
-} // namespace
-
-//--------------------------------------------------------------
-// EventBus
-//--------------------------------------------------------------
-
-TEST(EventBus, PriorityOrdering)
-{
-    EventBus bus;
-
-    bus.push({EventType::PAUSE,
-              PRIORITY_PAUSE,
-              {}});
-
-    bus.push({EventType::CANCEL,
-              PRIORITY_CANCEL,
-              {}});
-
-    bus.push({EventType::EMERGENCY,
-              PRIORITY_EMERGENCY,
-              {}});
-
-    Event e;
-
-    ASSERT_TRUE(bus.pop(e));
-    EXPECT_EQ(e.priority,
-              PRIORITY_EMERGENCY);
-
-    ASSERT_TRUE(bus.pop(e));
-    EXPECT_EQ(e.priority,
-              PRIORITY_CANCEL);
-
-    ASSERT_TRUE(bus.pop(e));
-    EXPECT_EQ(e.priority,
-              PRIORITY_PAUSE);
-}
-
-TEST(EventBus, StopReturnsFalse)
-{
-    EventBus bus;
-
-    std::thread t([&]()
-                  { bus.stop(); });
-
-    Event e;
-
-    EXPECT_FALSE(bus.pop(e));
-
-    t.join();
-}
-
-TEST(EventBus, ConcurrentPush)
-{
-    EventBus bus;
-
-    constexpr int N = 100;
-
-    std::thread producer([&]()
-                         {
-                             for (int i = 0; i < N; i++)
-                             {
-                                 bus.push(
-                                     {EventType::NAV_DONE,
-                                      PRIORITY_NAV_DONE,
-                                      {}});
-                             }
-                         });
-
-    int count = 0;
-
-    while (count < N)
-    {
-        Event e;
-
-        if (bus.pop(e))
-            ++count;
-    }
-
-    producer.join();
-
-    EXPECT_EQ(count, N);
-}
-
-//--------------------------------------------------------------
-// GoalAdapter
-//--------------------------------------------------------------
-
-TEST(GoalAdapter, ConvertSingleGoal)
-{
-    GoalAdapter adapter;
-
-    auto missions =
-        adapter.convert(
-            MakeGoal(5.5, 9.1));
-
-    ASSERT_EQ(missions.size(), 1u);
-
-    EXPECT_EQ(
-        missions[0]->type,
-        MissionType::SIMPLE_GOAL);
-
-    EXPECT_DOUBLE_EQ(
-        missions[0]->goal.pose.position.x,
-        5.5);
-
-    EXPECT_DOUBLE_EQ(
-        missions[0]->goal.pose.position.y,
-        9.1);
-}
-
-//--------------------------------------------------------------
-// VDA5050Adapter
-//--------------------------------------------------------------
-
-TEST(VDA5050Adapter, EmptyOrder)
-{
-    VDA5050Adapter adapter;
-
-    robot_protocol_msgs::Order order;
-
-    auto missions =
-        adapter.convert(order);
-
-    EXPECT_TRUE(
-        missions.empty());
-}
-
-TEST(VDA5050Adapter, OrderWithoutActionsCreatesTailMission)
-{
-    VDA5050Adapter adapter;
-
-    auto order =
-        MakeOrder(4);
-
-    auto missions =
-        adapter.convert(order);
-
-    ASSERT_EQ(
-        missions.size(),
-        1u);
-
-    EXPECT_EQ(
-        missions[0]->nodes.size(),
-        4u);
-
-    EXPECT_EQ(
-        missions[0]->edges.size(),
-        3u);
-}
-
-TEST(VDA5050Adapter, SplitAtNodeAction)
-{
-    VDA5050Adapter adapter;
-
-    auto order =
-        MakeOrder(5);
-
-    order.nodes[2].actions.push_back(
-        MakeAction("dock"));
-
-    auto missions =
-        adapter.convert(order);
-
-    ASSERT_EQ(
-        missions.size(),
-        2u);
-
-    EXPECT_EQ(
-        missions[0]->nodes.size(),
-        3u);
-
-    EXPECT_EQ(
-        missions[1]->nodes.size(),
-        3u);
-}
-
-TEST(VDA5050Adapter, CollectEdgeAction)
-{
-    VDA5050Adapter adapter;
-
-    auto order =
-        MakeOrder(2);
-
-    order.nodes[1].actions.push_back(
-        MakeAction("node"));
-
-    order.edges[0].actions.push_back(
-        MakeAction("edge"));
-
-    auto missions =
-        adapter.convert(order);
-
-    ASSERT_EQ(
-        missions.size(),
-        1u);
-
-    ASSERT_EQ(
-        missions[0]->actions.size(),
-        2u);
-
-    EXPECT_LT(
-        missions[0]->actions[0].sequenceId,
-        missions[0]->actions[1].sequenceId);
-}
-
-//--------------------------------------------------------------
-// MissionManager
-//--------------------------------------------------------------
-
-TEST(MissionManager, SubmitChangesState)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(1, 2)));
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::QUEUED);
-
-    EXPECT_TRUE(
-        mgr.hasMission());
-}
-
-TEST(MissionManager, NextMission)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(1, 2)));
-
-    auto m =
-        mgr.nextMission();
-
-    ASSERT_NE(
-        m,
-        nullptr);
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::RUNNING);
-}
-
-TEST(MissionManager, NavigationDone)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(0, 0)));
-
-    mgr.nextMission();
-
-    mgr.onNavigationDone();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::IDLE);
-
-    EXPECT_FALSE(
-        mgr.hasMission());
-}
-
-TEST(MissionManager, NavigationFailed)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(0, 0)));
-
-    mgr.nextMission();
-
-    mgr.onNavigationFailed();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::FAILED);
-}
-
-TEST(MissionManager, PauseResume)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(1, 1)));
-
-    mgr.pause();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::PAUSED);
-
-    mgr.resume();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::QUEUED);
-}
-
-TEST(MissionManager, Cancel)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(1, 1)));
-
-    mgr.cancel();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::CANCELLED);
-
-    EXPECT_FALSE(
-        mgr.hasMission());
-}
-
-TEST(MissionManager, Emergency)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(2, 2)));
-
-    mgr.emergency();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::EMERGENCY);
-}
-
-TEST(MissionManager, ClearEmergency)
-{
-    MissionManager mgr;
-
-    mgr.emergency();
-
-    mgr.clearEmergency();
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::CLEAR_EMERGENCY);
-}
-
-//--------------------------------------------------------------
-// EventProcessor
-//--------------------------------------------------------------
-
-TEST(EventProcessor, GoalEvent)
-{
-    MissionManager mgr;
-
-    EventProcessor proc(mgr);
-
-    proc.start();
-
-    proc.goalEvent(
-        MakeGoal(5, 6));
-
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(100));
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::QUEUED);
-
-    proc.stop();
-}
-
-TEST(EventProcessor, EmergencyHasHighestPriority)
-{
-    MissionManager mgr;
-
-    EventProcessor proc(mgr);
-
-    proc.start();
-
-    proc.goalEvent(
-        MakeGoal(1, 1));
-
-    proc.emergencyEvent();
-
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(100));
-
-    EXPECT_EQ(
-        mgr.state(),
-        MissionState::EMERGENCY);
-
-    proc.stop();
-}
-
-//--------------------------------------------------------------
-// MissionExecutor
-//--------------------------------------------------------------
-
-TEST(MissionExecutor, CallbackInvokedOnce)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(10, 10)));
-
-    MissionExecutor exec(mgr);
-
-    std::atomic<int> count{0};
-
-    exec.setMissionCallback(
-        [&](const std::shared_ptr<Mission> &)
-        {
-            count++;
-        });
-
-    exec.start();
-
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(200));
-
-    exec.stop();
-
-    EXPECT_EQ(
-        count.load(),
-        1);
-}
-
-TEST(MissionExecutor, NewMissionAfterDone)
-{
-    MissionManager mgr;
-
-    GoalAdapter adapter;
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(1, 1)));
-
-    MissionExecutor exec(mgr);
-
-    std::atomic<int> count{0};
-
-    exec.setMissionCallback(
-        [&](const std::shared_ptr<Mission> &)
-        {
-            count++;
-        });
-
-    exec.start();
-
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(100));
-
-    mgr.onNavigationDone();
-
-    mgr.submit(
-        adapter.convert(
-            MakeGoal(2, 2)));
-
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(150));
-
-    exec.stop();
-
-    EXPECT_EQ(
-        count.load(),
-        2);
-}
-
-//--------------------------------------------------------------
-
-int main(int argc, char **argv)
-{
-    ::testing::InitGoogleTest(
-        &argc,
-        argv);
-
-    return RUN_ALL_TESTS();
-}

test/mission_adapters_test.cpp

@@ -0,0 +1,360 @@
+#include <gtest/gtest.h>
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+
+#include <mission_adapters/mission_adapters.h>
+
+namespace
+{
+using namespace mission_adapters;
+
+robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
+{
+    robot_geometry_msgs::PoseStamped goal;
+    goal.pose.position.x = x;
+    goal.pose.position.y = y;
+    return goal;
+}
+
+robot_protocol_msgs::Action makeAction(const std::string& id)
+{
+    robot_protocol_msgs::Action action;
+    action.actionId = id;
+    action.actionType = "TEST";
+    return action;
+}
+
+robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Node node;
+    node.sequenceId = sequence_id;
+    node.nodeId = "node_" + std::to_string(sequence_id);
+    node.nodePosition.x = sequence_id;
+    node.nodePosition.y = sequence_id;
+
+    if (add_action)
+        node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));
+
+    return node;
+}
+
+robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Edge edge;
+    edge.sequenceId = sequence_id;
+    edge.edgeId = "edge_" + std::to_string(sequence_id);
+
+    if (add_action)
+        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
+
+    return edge;
+}
+
+robot_protocol_msgs::Order makeOrder(int node_count)
+{
+    robot_protocol_msgs::Order order;
+
+    for (int i = 0; i < node_count; ++i)
+        order.nodes.push_back(makeNode(i));
+
+    for (int i = 0; i < node_count - 1; ++i)
+        order.edges.push_back(makeEdge(i));
+
+    return order;
+}
+
+bool waitForState(MissionManager& manager, MissionState expected, std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+        if (manager.state() == expected)
+            return true;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+
+    return manager.state() == expected;
+}
+
+class MissionAdaptersTest : public ::testing::Test
+{
+protected:
+    GoalAdapter goal_adapter;
+    VDA5050Adapter order_adapter;
+};
+
+TEST(EventBusTest, PopsHighestPriorityFirst)
+{
+    EventBus bus;
+
+    Event pause;
+    pause.type = EventType::PAUSE;
+    pause.priority = PRIORITY_PAUSE;
+    bus.push(pause);
+
+    Event cancel;
+    cancel.type = EventType::CANCEL;
+    cancel.priority = PRIORITY_CANCEL;
+    bus.push(cancel);
+
+    Event emergency;
+    emergency.type = EventType::EMERGENCY;
+    emergency.priority = PRIORITY_EMERGENCY;
+    bus.push(emergency);
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::EMERGENCY);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::CANCEL);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::PAUSE);
+}
+
+TEST(EventBusTest, PreservesFifoOrderForSamePriority)
+{
+    EventBus bus;
+
+    Event emergency;
+    emergency.type = EventType::EMERGENCY;
+    emergency.priority = PRIORITY_EMERGENCY;
+    bus.push(emergency);
+
+    Event clear_emergency;
+    clear_emergency.type = EventType::CLEAR_EMERGENCY;
+    clear_emergency.priority = PRIORITY_EMERGENCY;
+    bus.push(clear_emergency);
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::EMERGENCY);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::CLEAR_EMERGENCY);
+}
+
+TEST(EventBusTest, StopUnblocksPop)
+{
+    EventBus bus;
+    Event event;
+
+    std::thread stopper([&bus] { bus.stop(); });
+    EXPECT_FALSE(bus.pop(event));
+    stopper.join();
+}
+
+TEST(EventBusTest, ResetDropsPendingEvents)
+{
+    EventBus bus;
+
+    Event pause;
+    pause.type = EventType::PAUSE;
+    pause.priority = PRIORITY_PAUSE;
+    bus.push(pause);
+
+    bus.reset();
+    EXPECT_TRUE(bus.empty());
+}
+
+TEST_F(MissionAdaptersTest, GoalAdapterCreatesSingleMission)
+{
+    const auto missions = goal_adapter.convert(makeGoal(5.5, 9.1));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
+}
+
+TEST_F(MissionAdaptersTest, EmptyOrderCreatesNoMissions)
+{
+    robot_protocol_msgs::Order order;
+    EXPECT_TRUE(order_adapter.convert(order).empty());
+}
+
+TEST_F(MissionAdaptersTest, OrderWithoutActionsCreatesOneTailMission)
+{
+    const auto missions = order_adapter.convert(makeOrder(4));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 4u);
+    EXPECT_EQ(missions.front()->edges.size(), 3u);
+}
+
+TEST_F(MissionAdaptersTest, InvalidOrderWithMissingEdgesCreatesNoMissions)
+{
+    auto order = makeOrder(4);
+    order.edges.pop_back();
+
+    EXPECT_TRUE(order_adapter.convert(order).empty());
+}
+
+TEST_F(MissionAdaptersTest, OrderSplitsAtNodeAction)
+{
+    auto order = makeOrder(5);
+    order.nodes[2].actions.push_back(makeAction("dock"));
+
+    const auto missions = order_adapter.convert(order);
+
+    ASSERT_EQ(missions.size(), 2u);
+    EXPECT_EQ(missions[0]->nodes.size(), 3u);
+    EXPECT_EQ(missions[1]->nodes.size(), 3u);
+}
+
+TEST_F(MissionAdaptersTest, OrderCollectsAndSortsActions)
+{
+    auto order = makeOrder(2);
+    order.edges[0].actions.push_back(makeAction("edge"));
+    order.nodes[1].actions.push_back(makeAction("node"));
+
+    const auto missions = order_adapter.convert(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    ASSERT_EQ(missions.front()->actions.size(), 2u);
+    EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION);
+    EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION);
+}
+
+TEST_F(MissionAdaptersTest, ManagerRunsMissionLifecycle)
+{
+    MissionManager manager;
+
+    manager.submit(goal_adapter.convert(makeGoal(1.0, 2.0)));
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    auto mission = manager.nextMission();
+    ASSERT_NE(mission, nullptr);
+    EXPECT_EQ(manager.state(), MissionState::RUNNING);
+
+    manager.onNavigationDone();
+    EXPECT_EQ(manager.state(), MissionState::IDLE);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST_F(MissionAdaptersTest, ManagerHandlesPauseResumeCancelAndFailure)
+{
+    MissionManager manager;
+
+    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
+    manager.pause();
+    EXPECT_EQ(manager.state(), MissionState::PAUSED);
+
+    manager.resume();
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+
+    manager.cancel();
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
+    manager.nextMission();
+    manager.onNavigationFailed();
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST_F(MissionAdaptersTest, NavigationResultIsIgnoredOutsideRunningState)
+{
+    MissionManager manager;
+
+    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
+    manager.emergency();
+    manager.onNavigationDone();
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    manager.onNavigationFailed();
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    manager.clearEmergency();
+    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
+    manager.cancel();
+    manager.onNavigationDone();
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+}
+
+TEST_F(MissionAdaptersTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
+{
+    MissionManager manager;
+
+    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
+    manager.emergency();
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.clearEmergency();
+    manager.submit(goal_adapter.convert(makeGoal(3.0, 3.0)));
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+}
+
+TEST_F(MissionAdaptersTest, EventProcessorProcessesGoalAndEmergency)
+{
+    MissionManager manager;
+    EventProcessor processor(manager);
+
+    processor.start();
+    processor.goalEvent(makeGoal(5.0, 6.0));
+    EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, std::chrono::milliseconds(250)));
+
+    processor.emergencyEvent();
+    EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, std::chrono::milliseconds(250)));
+
+    processor.goalEvent(makeGoal(7.0, 8.0));
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    processor.clearEmergencyEvent();
+    EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, std::chrono::milliseconds(250)));
+
+    processor.stop();
+}
+
+TEST_F(MissionAdaptersTest, MissionExecutorDispatchesEachMissionOnce)
+{
+    MissionManager manager;
+    MissionExecutor executor(manager);
+    std::atomic<int> callback_count{0};
+
+    executor.setMissionCallback(
+        [&callback_count](const std::shared_ptr<Mission>& mission)
+        {
+            ASSERT_NE(mission, nullptr);
+            ++callback_count;
+        });
+
+    manager.submit(goal_adapter.convert(makeGoal(10.0, 10.0)));
+    executor.start();
+
+    const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
+    while (callback_count.load() < 1 && std::chrono::steady_clock::now() < deadline)
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+
+    manager.onNavigationDone();
+    manager.submit(goal_adapter.convert(makeGoal(11.0, 11.0)));
+
+    const auto second_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
+    while (callback_count.load() < 2 && std::chrono::steady_clock::now() < second_deadline)
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+
+    executor.stop();
+
+    EXPECT_EQ(callback_count.load(), 2);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}