mission_adapters/test/mission_adapters_test.cpp

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