optimal & fix file cmake

2026-08-03 22:40:26 +07:00
parent 33ee9b7f31
commit 2223453639
120 changed files with 12204 additions and 1886 deletions
--- a/test/mission_manager_test.cpp
+++ b/test/mission_manager_test.cpp
@@ -0,0 +1,405 @@
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_manager.h>
+
+#include "mission_test_utils.h"
+
+namespace
+{
+using namespace mission_adapters;
+using mission_test::makeMission;
+using mission_test::makeMissions;
+
+TEST(MissionManagerTest, ManagerRunsMissionLifecycle)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 2.0)});
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    auto mission = manager.nextMission();
+    ASSERT_NE(mission, nullptr);
+    EXPECT_NE(mission->id, kInvalidMissionId) << "submit must return a MissionId";
+    EXPECT_EQ(manager.state(), MissionState::RUNNING);
+
+    EXPECT_TRUE(manager.onNavigationDone(mission->id));
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, ManagerHandlesPauseResumeCancelAndFailure)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(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(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    const auto mission = manager.nextMission();
+    ASSERT_NE(mission, nullptr);
+    EXPECT_TRUE(manager.onNavigationFailed(mission->id));
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, NavigationResultIsIgnoredOutsideRunningState)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
+    const MissionId queued_id = manager.nextMission()->id;
+
+    manager.emergency();
+    EXPECT_FALSE(manager.onNavigationDone(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    EXPECT_FALSE(manager.onNavigationFailed(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    manager.clearEmergency();
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    manager.cancel();
+    EXPECT_FALSE(manager.onNavigationDone(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+}
+
+TEST(MissionManagerTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
+    manager.emergency();
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.clearEmergency();
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(3.0, 3.0)});
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A1 — submit rỗng không được đụng vào hàng đợi đang chạy.
+//
+// Một order lỗi từ fleet manager (convert() trả rỗng) không phải là lệnh "huỷ mọi thứ". Nếu nó xoá
+// queue thì chặng đang chạy mất chủ: robot vẫn đi tới goal cũ vì không ai bảo navigation dừng, còn
+// mission layer thì kẹt ở RUNNING mà không còn mission nào để hoàn tất.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, EmptySubmitLeavesRunningMissionUntouched)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+    ASSERT_EQ(manager.state(), MissionState::RUNNING);
+
+    // Order lỗi -> convert() rỗng -> submit rỗng.
+    manager.submit({});
+
+    EXPECT_EQ(manager.state(), MissionState::RUNNING);
+    EXPECT_TRUE(manager.hasMission()) << "an empty submit wiped the running mission and the queue";
+    EXPECT_EQ(manager.currentMissionId(), running->id) << "the running mission was replaced";
+    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId)
+        << "an empty submit must not ask navigation to stop";
+
+    // Chặng thứ hai vẫn phải còn trong hàng đợi.
+    EXPECT_TRUE(manager.onNavigationDone(running->id));
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A2 — outcome phải mang MissionId.
+//
+// Không có ID thì kết quả của chặng cũ (đến trễ, sau khi order đã bị thay) được ghi nhận cho chặng
+// mới: mission mới "hoàn thành" trong khi robot chưa hề chạy nó.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, StaleOutcomeOfPreemptedMissionIsRejected)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // Order mới thay order cũ trong lúc chặng đầu đang chạy.
+    manager.submit(makeMissions(1));
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    ASSERT_NE(first->id, second->id);
+
+    // Outcome của chặng ĐẦU về muộn, sau khi chặng hai đã bắt đầu.
+    EXPECT_FALSE(manager.onNavigationDone(first->id));
+    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "a late outcome completed the wrong "
+                                                         "mission";
+    EXPECT_EQ(manager.currentMissionId(), second->id);
+
+    // Outcome đúng của chặng hai vẫn được nhận bình thường.
+    EXPECT_TRUE(manager.onNavigationDone(second->id));
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+}
+
+TEST(MissionManagerTest, MissionIdsAreUniqueAndMonotonic)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(3));
+
+    MissionId previous = kInvalidMissionId;
+    for (int i = 0; i < 3; ++i)
+    {
+        const auto mission = manager.nextMission();
+        ASSERT_NE(mission, nullptr) << "leg number " << i;
+        EXPECT_GT(mission->id, previous);
+        previous = mission->id;
+
+        ASSERT_TRUE(manager.onNavigationDone(mission->id));
+    }
+
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+}
+
+TEST(MissionManagerTest, RunningMissionIsDispatchedOnlyOnce)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // M6: trong lúc RUNNING không có mission mới nào để giao.
+    EXPECT_EQ(manager.nextMission(), nullptr);
+    EXPECT_EQ(manager.nextMission(), nullptr);
+
+    ASSERT_TRUE(manager.onNavigationDone(first->id));
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    EXPECT_NE(second->id, first->id);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A3 — mission layer phải có đường bảo navigation dừng.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, CancelRequestsNavigationStopOfRunningMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    manager.cancel();
+
+    EXPECT_EQ(manager.takePendingCancel(), running->id);
+    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId) << "the stop request was taken twice";
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, EmergencyRequestsNavigationStopOfRunningMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    manager.emergency();
+
+    EXPECT_EQ(manager.takePendingCancel(), running->id);
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+}
+
+TEST(MissionManagerTest, PreemptRequestsNavigationStopOfReplacedMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    manager.submit(makeMissions(1));
+
+    EXPECT_EQ(manager.takePendingCancel(), first->id)
+        << "a new order replaced the old one without telling navigation to stop the old leg";
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+}
+
+TEST(MissionManagerTest, SubmitWhilePausedKeepsRobotStopped)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    manager.pause();
+
+    manager.submit(makeMissions(1));
+
+    EXPECT_EQ(manager.state(), MissionState::PAUSED) << "a new order started running on its own "
+                                                        "while PAUSED";
+    EXPECT_EQ(manager.nextMission(), nullptr);
+
+    manager.resume();
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_NE(manager.nextMission(), nullptr);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// M8 — một chặng hỏng thì phần còn lại của hàng đợi đi đâu.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, FailureClearsRemainingQueueByDefault)
+{
+    MissionManager manager;   // clear_queue_on_failure mặc định = true
+
+    manager.submit(makeMissions(3));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(first->id));
+
+    // Tuyến tuần tự: không tới được chặng 1 thì chặng 2 nằm sau nó cũng không còn ý nghĩa.
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, FailureKeepsQueueWhenConfigured)
+{
+    MissionConfig config;
+    config.clear_queue_on_failure = false;   // các mission trong hàng đợi độc lập với nhau
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(3));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(first->id));
+
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    EXPECT_NE(second->id, first->id);
+}
+
+TEST(MissionManagerTest, LastMissionFailureEndsInFailedEvenWhenQueueKept)
+{
+    MissionConfig config;
+    config.clear_queue_on_failure = false;
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(1));
+    const auto only = manager.nextMission();
+    ASSERT_NE(only, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(only->id));
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// M7 — mission_timeout là lưới cuối khi navigation không bao giờ báo kết quả về.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, TimeoutDisabledByDefaultLetsMissionRunOn)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    ASSERT_NE(manager.nextMission(), nullptr);
+
+    // waitForWork phải chặn (không có việc); wakeUp là đường ra duy nhất.
+    std::thread waker([&manager] {
+        std::this_thread::sleep_for(std::chrono::milliseconds(80));
+        manager.wakeUp();
+    });
+
+    EXPECT_FALSE(manager.waitForWork());
+    waker.join();
+
+    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "the mission was cancelled although the "
+                                                         "timeout is off";
+}
+
+TEST(MissionManagerTest, TimeoutFailsMissionAndRequestsNavigationStop)
+{
+    MissionConfig config;
+    config.mission_timeout = 0.15;   // [s]
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(1));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    const auto started = std::chrono::steady_clock::now();
+
+    // waitForWork tự thức dậy đúng lúc hết hạn và biến quá hạn thành việc cần làm.
+    ASSERT_TRUE(manager.waitForWork());
+
+    const double elapsed =
+        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();
+    EXPECT_GE(elapsed, 0.15) << "the leg was cancelled before it expired";
+
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_EQ(manager.takePendingCancel(), running->id)
+        << "it expired but navigation was not told to stop — the robot keeps driving to the old "
+           "goal";
+}
+
+TEST(MissionManagerTest, TimeoutIsMeasuredPerMissionNotPerQueue)
+{
+    MissionConfig config;
+    config.mission_timeout = 0.2;   // [s]
+    config.clear_queue_on_failure = false;
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(2));
+
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // Chặng đầu xong sớm; mốc thời gian phải được gieo lại cho chặng hai.
+    std::this_thread::sleep_for(std::chrono::milliseconds(120));
+    ASSERT_TRUE(manager.onNavigationDone(first->id));
+
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+
+    // Nếu mốc không được gieo lại, chặng hai sẽ hết hạn gần như tức thì.
+    const auto started = std::chrono::steady_clock::now();
+    ASSERT_TRUE(manager.waitForWork());
+    const double elapsed =
+        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();
+
+    EXPECT_GE(elapsed, 0.19) << "the second leg inherited the clock of the first one";
+    EXPECT_EQ(manager.takePendingCancel(), second->id);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}