optimal & fix file cmake
This commit is contained in:
356
test/mission_lifecycle_test.cpp
Normal file
356
test/mission_lifecycle_test.cpp
Normal file
@@ -0,0 +1,356 @@
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include <mission_adapters/mission_adapters.h>
|
||||
#include <mission_adapters/plugin_registry.h>
|
||||
|
||||
#include "goal_source_adapter.h"
|
||||
#include "vda5050_source_adapter.h"
|
||||
#include "mission_test_utils.h"
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace mission_adapters;
|
||||
using mission_test::FakeNavigationClient;
|
||||
using mission_test::makeGoal;
|
||||
using mission_test::makeMission;
|
||||
using mission_test::waitForState;
|
||||
|
||||
/// Chờ tới khi predicate đúng hoặc hết hạn. Trả giá trị cuối cùng của predicate.
|
||||
template <typename Predicate>
|
||||
bool waitFor(Predicate predicate, std::chrono::milliseconds timeout)
|
||||
{
|
||||
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
while (std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
if (predicate())
|
||||
return true;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
|
||||
return predicate();
|
||||
}
|
||||
|
||||
constexpr std::chrono::milliseconds kWaitTimeout{500};
|
||||
|
||||
class MissionLifecycleTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
// Đăng ký thẳng thay vì nạp .so: test này kiểm luồng mission, không kiểm đường Boost.DLL
|
||||
// (việc đó thuộc plugin_registry_test).
|
||||
ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::GoalSourceAdapter>()));
|
||||
ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::VDA5050SourceAdapter>()));
|
||||
}
|
||||
|
||||
/// Một mission dựng thẳng, đóng gói trong vector để submit().
|
||||
std::vector<std::shared_ptr<Mission>> oneMission(double x, double y)
|
||||
{
|
||||
return {makeMission(x, y)};
|
||||
}
|
||||
|
||||
PluginRegistry registry;
|
||||
};
|
||||
|
||||
TEST_F(MissionLifecycleTest, EventProcessorProcessesGoalAndEmergency)
|
||||
{
|
||||
MissionManager manager;
|
||||
EventProcessor processor(manager, registry);
|
||||
|
||||
processor.start();
|
||||
processor.goalEvent(makeGoal(5.0, 6.0));
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));
|
||||
|
||||
processor.emergencyEvent();
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, kWaitTimeout));
|
||||
|
||||
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, kWaitTimeout));
|
||||
|
||||
processor.stop();
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, MissionExecutorDispatchesEachMissionOnce)
|
||||
{
|
||||
MissionManager manager;
|
||||
MissionExecutor executor(manager);
|
||||
FakeNavigationClient client;
|
||||
|
||||
executor.setNavigationClient(&client);
|
||||
manager.submit(oneMission(10.0, 10.0));
|
||||
executor.start();
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
|
||||
const auto first = client.dispatched().front();
|
||||
ASSERT_TRUE(manager.onNavigationDone(first->id));
|
||||
manager.submit(oneMission(11.0, 11.0));
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));
|
||||
|
||||
// Chặng thứ hai chưa xong -> không được giao thêm lần nào nữa.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(120));
|
||||
executor.stop();
|
||||
|
||||
EXPECT_EQ(client.dispatchCount(), 2u);
|
||||
EXPECT_TRUE(client.cancelled().empty());
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// A3 — cancel/emergency ở tầng mission phải tới được navigation.
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
TEST_F(MissionLifecycleTest, CancelStopsNavigationExactlyOnce)
|
||||
{
|
||||
MissionManager manager;
|
||||
MissionExecutor executor(manager);
|
||||
FakeNavigationClient client;
|
||||
|
||||
executor.setNavigationClient(&client);
|
||||
manager.submit(oneMission(1.0, 1.0));
|
||||
executor.start();
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
const MissionId running_id = client.dispatched().front()->id;
|
||||
|
||||
manager.cancel();
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return !client.cancelled().empty(); }, kWaitTimeout));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(120));
|
||||
executor.stop();
|
||||
|
||||
ASSERT_EQ(client.cancelled().size(), 1u) << "cancelActive must be called exactly once";
|
||||
EXPECT_EQ(client.cancelled().front(), running_id);
|
||||
EXPECT_EQ(manager.state(), MissionState::CANCELLED);
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, PreemptCancelsOldMissionBeforeDispatchingNewOne)
|
||||
{
|
||||
MissionManager manager;
|
||||
MissionExecutor executor(manager);
|
||||
FakeNavigationClient client;
|
||||
|
||||
executor.setNavigationClient(&client);
|
||||
manager.submit(oneMission(1.0, 1.0));
|
||||
executor.start();
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
const MissionId old_id = client.dispatched().front()->id;
|
||||
|
||||
// Order mới thay order cũ trong lúc chặng đầu đang chạy.
|
||||
manager.submit(oneMission(2.0, 2.0));
|
||||
|
||||
ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));
|
||||
executor.stop();
|
||||
|
||||
ASSERT_EQ(client.cancelled().size(), 1u) << "the replaced leg was not told to stop";
|
||||
EXPECT_EQ(client.cancelled().front(), old_id);
|
||||
EXPECT_NE(client.dispatched().back()->id, old_id);
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, RejectedDispatchFailsMissionInsteadOfHanging)
|
||||
{
|
||||
MissionManager manager;
|
||||
MissionExecutor executor(manager);
|
||||
FakeNavigationClient client;
|
||||
|
||||
client.setAcceptDispatch(false);
|
||||
executor.setNavigationClient(&client);
|
||||
manager.submit(oneMission(3.0, 3.0));
|
||||
executor.start();
|
||||
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::FAILED, kWaitTimeout))
|
||||
<< "navigation rejected it yet the mission is still stuck in RUNNING";
|
||||
|
||||
executor.stop();
|
||||
EXPECT_EQ(client.dispatchCount(), 1u) << "a rejected mission was handed over again on the next "
|
||||
"round";
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, OrderProducingNoMissionLeavesQueueUntouched)
|
||||
{
|
||||
MissionManager manager;
|
||||
EventProcessor processor(manager, registry);
|
||||
|
||||
processor.start();
|
||||
processor.goalEvent(makeGoal(4.0, 4.0));
|
||||
ASSERT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));
|
||||
|
||||
// Order thiếu edge -> convert() rỗng -> không được đụng vào hàng đợi hiện tại (A1).
|
||||
auto invalid_order = mission_test::makeOrder(4);
|
||||
invalid_order.edges.pop_back();
|
||||
processor.orderEvent(invalid_order);
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(80));
|
||||
processor.stop();
|
||||
|
||||
EXPECT_EQ(manager.state(), MissionState::QUEUED);
|
||||
EXPECT_TRUE(manager.hasMission()) << "a failed order cleared the queue";
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
// End-to-end: một Order đi hết đường qua cả ba thành phần.
|
||||
// ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
/// Dựng bộ ba thành phần đã nối dây sẵn, dùng chung cho các test end-to-end bên dưới.
|
||||
struct MissionStack
|
||||
{
|
||||
explicit MissionStack(PluginRegistry& registry)
|
||||
: processor(manager, registry)
|
||||
, executor(manager)
|
||||
{
|
||||
executor.setNavigationClient(&client);
|
||||
processor.start();
|
||||
executor.start();
|
||||
}
|
||||
|
||||
~MissionStack()
|
||||
{
|
||||
executor.stop();
|
||||
processor.stop();
|
||||
}
|
||||
|
||||
MissionManager manager;
|
||||
EventProcessor processor;
|
||||
MissionExecutor executor;
|
||||
FakeNavigationClient client;
|
||||
};
|
||||
|
||||
TEST_F(MissionLifecycleTest, OrderRunsThreeLegsThenCompletes)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
// Order 4 node, action tại node 1 và node 2 -> cắt thành 3 chặng.
|
||||
auto order = mission_test::makeOrder(4);
|
||||
order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
|
||||
order.nodes[2].actions.push_back(mission_test::makeAction("drop"));
|
||||
|
||||
stack.processor.orderEvent(order);
|
||||
|
||||
for (int leg = 0; leg < 3; ++leg)
|
||||
{
|
||||
const size_t expected = static_cast<size_t>(leg) + 1;
|
||||
ASSERT_TRUE(waitFor([&stack, expected] { return stack.client.dispatchCount() == expected; },
|
||||
kWaitTimeout))
|
||||
<< "leg number " << leg << " was not handed over";
|
||||
|
||||
const auto mission = stack.client.dispatched().back();
|
||||
stack.processor.navDoneEvent(mission->id);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(waitForState(stack.manager, MissionState::COMPLETED, kWaitTimeout));
|
||||
EXPECT_EQ(stack.client.dispatchCount(), 3u);
|
||||
EXPECT_TRUE(stack.client.cancelled().empty());
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, CancelMidOrderThenAcceptNewOrder)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
auto order = mission_test::makeOrder(4);
|
||||
order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
|
||||
order.nodes[2].actions.push_back(mission_test::makeAction("drop"));
|
||||
|
||||
stack.processor.orderEvent(order);
|
||||
ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
|
||||
const MissionId running_id = stack.client.dispatched().front()->id;
|
||||
stack.processor.cancelEvent();
|
||||
|
||||
ASSERT_TRUE(waitFor([&stack] { return !stack.client.cancelled().empty(); }, kWaitTimeout));
|
||||
EXPECT_EQ(stack.client.cancelled().front(), running_id);
|
||||
EXPECT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));
|
||||
|
||||
// Sau khi huỷ vẫn nhận được order mới.
|
||||
stack.processor.orderEvent(mission_test::makeOrder(2, "order_moi"));
|
||||
|
||||
ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 2u; }, kWaitTimeout));
|
||||
EXPECT_EQ(stack.manager.state(), MissionState::RUNNING);
|
||||
EXPECT_NE(stack.client.dispatched().back()->id, running_id);
|
||||
}
|
||||
|
||||
// A5 end-to-end: huỷ ngay sau khi phát order thì không mission nào được chạy.
|
||||
TEST_F(MissionLifecycleTest, OrderThenImmediateCancelDispatchesNothing)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
stack.processor.orderEvent(mission_test::makeOrder(3));
|
||||
stack.processor.cancelEvent();
|
||||
|
||||
// Cho cả hai sự kiện chạy xong rồi mới kết luận.
|
||||
ASSERT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(120));
|
||||
|
||||
EXPECT_EQ(stack.client.dispatchCount(), 0u)
|
||||
<< "the cancel was processed before the order so the order still runs — the robot drives "
|
||||
"what the user just cancelled";
|
||||
EXPECT_FALSE(stack.manager.hasMission());
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, EmergencyOvertakesLongEventQueue)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
// Nhồi hàng đợi bằng nhiều goal rồi mới bấm emergency.
|
||||
for (int i = 0; i < 50; ++i)
|
||||
stack.processor.goalEvent(makeGoal(i, i));
|
||||
|
||||
stack.processor.emergencyEvent();
|
||||
|
||||
EXPECT_TRUE(waitForState(stack.manager, MissionState::EMERGENCY, kWaitTimeout))
|
||||
<< "emergency must queue-jump, not wait behind 50 events";
|
||||
EXPECT_FALSE(stack.manager.hasMission());
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, ActionsReachNavigationIntactAndInOrder)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
auto order = mission_test::makeOrder(3);
|
||||
order.edges[0].sequenceId = 1;
|
||||
order.edges[0].actions.push_back(mission_test::makeAction("beep"));
|
||||
order.nodes[1].sequenceId = 2;
|
||||
order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
|
||||
|
||||
stack.processor.orderEvent(order);
|
||||
ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
|
||||
const auto mission = stack.client.dispatched().front();
|
||||
ASSERT_EQ(mission->actions.size(), 2u) << "the action was lost on the way down to navigation";
|
||||
EXPECT_EQ(mission->actions[0].action.actionId, "beep");
|
||||
EXPECT_EQ(mission->actions[1].action.actionId, "lift");
|
||||
EXPECT_TRUE(mission->has_goal);
|
||||
}
|
||||
|
||||
TEST_F(MissionLifecycleTest, GoallessMissionReachesNavigationWithItsActions)
|
||||
{
|
||||
MissionStack stack(registry);
|
||||
|
||||
// Action ngay tại node xuất phát -> chặng đầu không có quãng đường nào để đi.
|
||||
auto order = mission_test::makeOrder(3);
|
||||
order.nodes[0].actions.push_back(mission_test::makeAction("pick_at_start"));
|
||||
|
||||
stack.processor.orderEvent(order);
|
||||
ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
|
||||
|
||||
const auto mission = stack.client.dispatched().front();
|
||||
EXPECT_FALSE(mission->has_goal);
|
||||
ASSERT_EQ(mission->actions.size(), 1u);
|
||||
EXPECT_EQ(mission->actions.front().action.actionId, "pick_at_start");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user