add file test

2026-06-29 13:45:50 +07:00
parent 829ce20fbc
commit 33ee9b7f31
7 changed files with 469 additions and 638 deletions
--- a/test/mission_adapters_test.cpp
+++ b/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();
+}