update to windows

2026-06-28 20:08:54 +07:00
parent 1bd09d5e8e
commit 829ce20fbc
7 changed files with 694 additions and 26 deletions
--- a/CMakeLists.txt
+++ b/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()
--- a/include/mission_adapters/mission_adapters.h
+++ b/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();
--- a/package.xml
+++ b/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>
--- a/src/mission_adapters.cpp
+++ b/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,
-                    //         so the sort below produces the correct execution order.
-                    ma.sequenceId = action.sequenceId;
+                    ma.sequenceId = edge.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 = action.sequenceId;
+                ma.sequenceId = node.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
    // ─────────────────────────────────────────────────────────────────────────
--- a/src/robot_control_test.cpp
+++ b/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_state_)
-        {
-            if (nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
+        if (nav_state != prev_nav_done_state_ && 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)
--- a/src/test_mission_adapters
+++ b/src/test_mission_adapters
--- a/src/test_mission_adapters.cpp
+++ b/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();
+}