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/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();
+}