update to windows

CMakeLists.txt

@@ -262,3 +262,58 @@ else()
   endif()
 
 endif()
+
+# ========================================================
+# Unit Test
+# ========================================================
+if(CATKIN_ENABLE_TESTING)
+
+  find_package(catkin REQUIRED COMPONENTS
+    robot_costmap_2d
+    robot_nav_core
+    robot_nav_core2
+    robot_nav_msgs
+    robot_std_msgs
+    robot_geometry_msgs
+    robot_cpp
+    robot_tf3_geometry_msgs
+    robot_visualization_msgs
+    robot_nav_2d_utils
+    data_convert
+  )
+
+  message(STATUS "CATKIN_ENABLE_TESTING=${CATKIN_ENABLE_TESTING}")
+
+  if(COMMAND catkin_add_gtest)
+    message(STATUS "catkin_add_gtest exists")
+  else()
+    message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
+  endif()
+
+  find_package(GTest REQUIRED)
+
+  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}
+    )
+
+    target_include_directories(test_mission_adapters PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${catkin_INCLUDE_DIRS}
+    )
+  endif()
+
+endif()

include/mission_adapters/mission_adapters.h

@@ -33,12 +33,12 @@ namespace mission_adapters
         QUEUED,
         RUNNING,
         PAUSED,
-        WAITING_ACTION,
         RECOVERY,
         COMPLETED,
         FAILED,
         CANCELLED,
-        EMERGENCY
+        EMERGENCY,
+        CLEAR_EMERGENCY
     };
 
     enum class EventType
@@ -49,7 +49,8 @@ namespace mission_adapters
         PAUSE,
         RESUME,
         CANCEL,
-        EMERGENCY
+        EMERGENCY,
+        CLEAR_EMERGENCY
     };
 
     enum class MissionType
@@ -110,6 +111,7 @@ namespace mission_adapters
 
     public:
         void push(const Event& event);
+        void push(Event&& event);
         bool pop(Event& event);
         void stop();
         void reset();
@@ -152,6 +154,7 @@ namespace mission_adapters
         void pause();
         void resume();
         void emergency();
+        void clearEmergency();
 
         MissionState state() const;
 
@@ -182,6 +185,7 @@ namespace mission_adapters
         void navDoneEvent();
         void navFailedEvent();
         void emergencyEvent();
+        void clearEmergencyEvent();
 
     private:
         void spin();

package.xml

@@ -47,4 +47,5 @@
   <run_depend>robot_visualization_msgs</run_depend>
   <run_depend>robot_nav_2d_utils</run_depend>
   <run_depend>data_convert</run_depend>
+  <test_depend>gtest</test_depend>
 </package>

src/mission_adapters.cpp

@@ -1,3 +1,4 @@
+//file mission_adapters.cpp
 #include <algorithm>
 #include <mission_adapters/mission_adapters.h>
 
@@ -14,6 +15,13 @@ namespace mission_adapters
         cv_.notify_one();
     }
 
+    void EventBus::push(Event&& event)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        queue_.push(std::move(event));
+        cv_.notify_one();
+    }
+
     bool EventBus::pop(Event& event)
     {
         std::unique_lock<std::mutex> lock(mutex_);
@@ -66,6 +74,8 @@ namespace mission_adapters
         std::vector<std::shared_ptr<Mission>> missions;
         std::vector<size_t> action_node_indices;
 
+        if(order.nodes.empty()) return missions;
+
         for (size_t i = 0; i < order.nodes.size(); ++i)
         {
             if (!order.nodes[i].actions.empty())
@@ -126,9 +136,7 @@ namespace mission_adapters
                 for (const auto& action : edge.actions)
                 {
                     Action ma;
-                    // FIX #6: Use action.sequenceId, not edge.sequenceId,
+                    ma.sequenceId = 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));
@@ -139,8 +147,7 @@ namespace mission_adapters
             for (const auto& action : node.actions)
             {
                 Action ma;
-                // FIX #6: Same fix for node actions.
+                ma.sequenceId = node.sequenceId;
-                ma.sequenceId = action.sequenceId;
                 ma.type       = ActionType::NODE_ACTION;
                 ma.action     = action;
                 mission->actions.push_back(std::move(ma));
@@ -162,6 +169,12 @@ namespace mission_adapters
     void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
     {
         std::lock_guard<std::mutex> lock(mutex_);
+        if (!mission_queue_.empty())
+        {
+            mission_queue_ = {};
+            current_mission_.reset();
+        }
+
         for (const auto& mission : missions)
             mission_queue_.push(mission);
 
@@ -176,6 +189,7 @@ namespace mission_adapters
                 case MissionState::FAILED:
                 case MissionState::CANCELLED:
                 case MissionState::EMERGENCY:
+                case MissionState::CLEAR_EMERGENCY:
                     state_ = MissionState::QUEUED;
                     break;
                 default:
@@ -188,10 +202,12 @@ namespace mission_adapters
     {
         std::lock_guard<std::mutex> lock(mutex_);
 
-        if (state_ == MissionState::PAUSED    ||
+        if (state_ == MissionState::IDLE      ||
+            state_ == MissionState::PAUSED    ||
             state_ == MissionState::FAILED    ||
             state_ == MissionState::CANCELLED ||
-            state_ == MissionState::EMERGENCY)
+            state_ == MissionState::EMERGENCY ||
+            state_ == MissionState::CLEAR_EMERGENCY) 
         {
             return nullptr;
         }
@@ -231,6 +247,7 @@ namespace mission_adapters
         std::lock_guard<std::mutex> lock(mutex_);
         current_mission_.reset();
         while (!mission_queue_.empty()) mission_queue_.pop();
+        if(state_ == MissionState::EMERGENCY) return;
         state_ = MissionState::CANCELLED;
     }
 
@@ -242,10 +259,19 @@ namespace mission_adapters
         state_ = MissionState::EMERGENCY;
     }
 
+    void MissionManager::clearEmergency()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if(state_ == MissionState::EMERGENCY)
+            state_ = MissionState::CLEAR_EMERGENCY;
+    }
+
     void MissionManager::pause()
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        if (state_ == MissionState::RUNNING)
+        if (state_ == MissionState::IDLE    ||
+            state_ == MissionState::RUNNING ||
+            state_ == MissionState::QUEUED)
             state_ = MissionState::PAUSED;
     }
 
@@ -256,7 +282,7 @@ namespace mission_adapters
 
         if (current_mission_)
             state_ = MissionState::RUNNING;
-        else if (!mission_queue_.empty())
+        else if (!current_mission_ && !mission_queue_.empty())
             state_ = MissionState::QUEUED;
         else
             state_ = MissionState::IDLE;
@@ -321,6 +347,7 @@ namespace mission_adapters
             case EventType::RESUME:          mission_manager_.resume();                    break;
             case EventType::CANCEL:          mission_manager_.cancel();                    break;
             case EventType::EMERGENCY:       mission_manager_.emergency();                 break;
+            case EventType::CLEAR_EMERGENCY: mission_manager_.clearEmergency();            break;
             default:
                 robot::log_error("Unknown event type");
                 break;
@@ -376,6 +403,11 @@ namespace mission_adapters
         event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}});
     }
 
+    void EventProcessor::clearEmergencyEvent()
+    {
+        event_bus_.push({EventType::CLEAR_EMERGENCY, PRIORITY_EMERGENCY, {}});
+    }
+
     // ─────────────────────────────────────────────────────────────────────────
     // MissionExecutor
     // ─────────────────────────────────────────────────────────────────────────

src/robot_control_test.cpp

@@ -23,7 +23,7 @@ class RobotControlTest
     }
 
 public:
-    RobotControlTest()  = default;
+    RobotControlTest();
     ~RobotControlTest();
 
     void run();
@@ -32,6 +32,9 @@ private:
     void executeMission(const Mission& mission);
 };
 
+RobotControlTest::RobotControlTest()
+{}
+
 RobotControlTest::~RobotControlTest()
 {
     event_processor_.stop();
@@ -49,18 +52,14 @@ void RobotControlTest::run()
             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_done_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
-        if (nav_state != prev_nav_state_)
-        {
-            if (nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
         {
             event_processor_.navDoneEvent();
         }
@@ -69,7 +68,6 @@ void RobotControlTest::run()
             event_processor_.navFailedEvent();
         }
             prev_nav_state_ = nav_state;
-        }
 
         // Example: receive an order (replace condition with your real source)
         if (/* new order available */ false)

src/test_mission_adapters.cpp

@@ -0,0 +1,578 @@
+#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();
+}