optimal & fix file cmake

test/adapter_test.cpp

@@ -0,0 +1,527 @@
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+
+#include <cmath>
+#include <string>
+
+#include <mission_adapters/mission_request.h>
+
+#include "goal_source_adapter.h"
+#include "mission_test_utils.h"
+#include "vda5050_source_adapter.h"
+
+namespace
+{
+using namespace mission_adapters;
+using mission_plugins::GoalSourceAdapter;
+using mission_plugins::VDA5050SourceAdapter;
+using mission_test::makeAction;
+using mission_test::makeGoal;
+using mission_test::makeOrder;
+
+class AdapterTest : public ::testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        robot::NodeHandle nh;
+        ASSERT_TRUE(goal_adapter.configure("goal_src", nh));
+        ASSERT_TRUE(order_adapter.configure("vda5050_src", nh));
+    }
+
+    /// Chuyển một order, trả về danh sách mission (bỏ phần mode).
+    std::vector<std::shared_ptr<Mission>> convertOrder(const robot_protocol_msgs::Order& order)
+    {
+        return order_adapter.convert(MissionRequest::fromOrder(order)).missions;
+    }
+
+    /// Chuyển một order và giữ nguyên cả ConversionResult để kiểm mode.
+    ConversionResult convertOrderFull(const robot_protocol_msgs::Order& order)
+    {
+        return order_adapter.convert(MissionRequest::fromOrder(order));
+    }
+
+    GoalSourceAdapter goal_adapter;
+    VDA5050SourceAdapter order_adapter;
+};
+
+// ── Schema ──────────────────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, AdaptersDeclareDistinctSchemas)
+{
+    EXPECT_EQ(goal_adapter.schema(), schema::kPoseStamped);
+    EXPECT_EQ(order_adapter.schema(), schema::kVda5050Order);
+    EXPECT_NE(goal_adapter.schema(), order_adapter.schema());
+}
+
+// ── GoalSourceAdapter ───────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, GoalAdapterCreatesSingleMission)
+{
+    const auto missions = goal_adapter.convert(MissionRequest::fromPose(makeGoal(5.5, 9.1))).missions;
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
+    EXPECT_EQ(missions.front()->motion_hint, "position");
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsMissingPayload)
+{
+    MissionRequest request;
+    request.schema = schema::kPoseStamped;
+
+    std::string reason;
+    EXPECT_FALSE(goal_adapter.validate(request, reason));
+    EXPECT_FALSE(reason.empty());
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsNonFiniteGoal)
+{
+    auto goal = makeGoal(1.0, 1.0);
+    goal.pose.position.x = std::numeric_limits<double>::quiet_NaN();
+    goal.pose.orientation.w = 1.0;
+
+    std::string reason;
+    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason))
+        << "a goal containing NaN slipped through the boundary into navigation";
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsZeroQuaternion)
+{
+    auto goal = makeGoal(1.0, 1.0);
+
+    std::string reason;
+    ASSERT_TRUE(goal_adapter.validate(MissionRequest::fromPose(goal), reason)) << reason;
+
+    // Host quên set orientation: quaternion toàn 0 không phải "hướng bất kỳ", nó là dữ liệu hỏng.
+    goal.pose.orientation.w = 0.0;
+    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason));
+}
+
+// ── VDA5050SourceAdapter ────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, EmptyOrderIsRejectedByValidate)
+{
+    robot_protocol_msgs::Order order;
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST_F(AdapterTest, OrderWithoutActionsCreatesOneTailMission)
+{
+    const auto missions = convertOrder(makeOrder(4));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 4u);
+    EXPECT_EQ(missions.front()->edges.size(), 3u);
+}
+
+TEST_F(AdapterTest, InvalidOrderWithMissingEdgesIsRejected)
+{
+    auto order = makeOrder(4);
+    order.edges.pop_back();
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST_F(AdapterTest, OrderSplitsAtNodeAction)
+{
+    auto order = makeOrder(5);
+    order.nodes[2].actions.push_back(makeAction("dock"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 2u);
+    EXPECT_EQ(missions[0]->nodes.size(), 3u);
+    EXPECT_EQ(missions[1]->nodes.size(), 3u);
+}
+
+TEST_F(AdapterTest, OrderCollectsAndSortsActions)
+{
+    auto order = makeOrder(2);
+    order.edges[0].actions.push_back(makeAction("edge"));
+    order.nodes[1].actions.push_back(makeAction("node"));
+
+    const auto missions = convertOrder(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);
+}
+
+// ── A4: conformance VDA5050 ─────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, MissionCarriesGoalAndStartFromNodes)
+{
+    auto order = makeOrder(3);
+    order.nodes[2].nodePosition.x = 7.0;
+    order.nodes[2].nodePosition.y = 8.0;
+    order.nodes[2].nodePosition.theta = 0.0;
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    // Mission phải self-contained: consumer không phải tự đoán goal từ nodes.back().
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 7.0);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 8.0);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.orientation.w, 1.0);
+    EXPECT_EQ(missions.front()->goal.header.frame_id, "map");
+
+    EXPECT_DOUBLE_EQ(missions.front()->start.pose.position.x, 0.0);
+}
+
+TEST_F(AdapterTest, NodeThetaBecomesQuaternion)
+{
+    auto order = makeOrder(2);
+    order.nodes[1].nodePosition.theta = M_PI;   // [rad]
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    // theta = pi -> quay 180 độ quanh z: (z, w) = (1, 0).
+    EXPECT_NEAR(missions.front()->goal.pose.orientation.z, 1.0, 1e-9);
+    EXPECT_NEAR(missions.front()->goal.pose.orientation.w, 0.0, 1e-9);
+}
+
+TEST_F(AdapterTest, HorizonNodesAreNeverDispatched)
+{
+    auto order = makeOrder(5);
+    // Chỉ 3 node đầu là base; hai node cuối là horizon (fleet manager chưa cho phép đi).
+    order.nodes[3].released = false;
+    order.nodes[4].released = false;
+    order.edges[3].released = false;
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 3u) << "the robot was handed the horizon part that "
+                                                     "is not released yet";
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 2.0);
+}
+
+TEST_F(AdapterTest, OrderWithoutReleasedFlagIsTreatedAsFullBase)
+{
+    auto order = makeOrder(3);
+    for (auto& node : order.nodes) node.released = false;
+    for (auto& edge : order.edges) edge.released = false;
+
+    // Host không điền `released`: chạy cả order còn hơn đứng im không dấu hiệu.
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 3u);
+}
+
+TEST_F(AdapterTest, StaleOrderUpdateIdIsRejected)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 2)).empty());
+
+    std::string reason;
+
+    // Cùng orderId, orderUpdateId cũ hơn -> phát lại/đến trễ, không được chạy lại tuyến đường.
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 1)),
+                                        reason));
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 2)),
+                                        reason));
+
+    // Cùng orderId, update mới hơn -> chấp nhận.
+    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(4, "order_A", 3)),
+                                       reason)) << reason;
+
+    // orderId khác -> order mới, không so orderUpdateId.
+    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_B", 0)),
+                                       reason)) << reason;
+}
+
+TEST_F(AdapterTest, OrderUpdateAppendsOnlyNewlyReleasedSegment)
+{
+    // Base ban đầu: 3 node released, 2 node horizon.
+    auto order = makeOrder(5, "order_A", 0);
+    order.nodes[3].released = false;
+    order.nodes[4].released = false;
+    order.edges[3].released = false;
+
+    const auto first = convertOrderFull(order);
+    ASSERT_EQ(first.missions.size(), 1u);
+    EXPECT_EQ(first.mode, SubmitMode::kReplace);
+    EXPECT_EQ(first.missions.front()->nodes.size(), 3u);
+
+    // Fleet manager release nốt horizon.
+    auto update = makeOrder(5, "order_A", 1);
+    const auto second = convertOrderFull(update);
+
+    ASSERT_EQ(second.missions.size(), 1u);
+    EXPECT_EQ(second.mode, SubmitMode::kAppend)
+        << "an order update replaced the whole queue -> the robot cancels and redoes the leg it is "
+           "on";
+
+    // Chặng mới bắt đầu từ node cuối của phần đã chạy, không lặp lại phần cũ.
+    EXPECT_EQ(second.missions.front()->nodes.size(), 3u);
+    EXPECT_DOUBLE_EQ(second.missions.front()->start.pose.position.x, 2.0);
+    EXPECT_DOUBLE_EQ(second.missions.front()->goal.pose.position.x, 4.0);
+}
+
+TEST_F(AdapterTest, OrderUpdateWithoutNewNodesProducesNoWork)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());
+
+    // Update không release thêm node nào: không có việc mới, và cũng không được xoá gì.
+    const auto result = convertOrderFull(makeOrder(3, "order_A", 1));
+    EXPECT_TRUE(result.empty());
+}
+
+TEST_F(AdapterTest, NewOrderIdReplacesQueue)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());
+
+    const auto result = convertOrderFull(makeOrder(2, "order_B", 0));
+
+    ASSERT_FALSE(result.empty());
+    EXPECT_EQ(result.mode, SubmitMode::kReplace);
+}
+
+// ── D8: goal optional, action đi nguyên vẹn ─────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, ActionAtFirstNodeBecomesGoallessMission)
+{
+    auto order = makeOrder(3);
+    order.nodes[0].actions.push_back(makeAction("pick_at_start"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 2u);
+
+    // Chặng đầu: action ngay tại chỗ đứng, không có quãng đường nào để đi.
+    EXPECT_FALSE(missions[0]->has_goal);
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions.front().action.actionId, "pick_at_start");
+
+    // Chặng sau: di chuyển bình thường.
+    EXPECT_TRUE(missions[1]->has_goal);
+    EXPECT_DOUBLE_EQ(missions[1]->goal.pose.position.x, 2.0);
+}
+
+TEST_F(AdapterTest, MovingMissionsAlwaysHaveGoal)
+{
+    const auto missions = convertOrder(makeOrder(3));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_TRUE(missions.front()->has_goal);
+}
+
+TEST_F(AdapterTest, ActionsPassThroughUnchangedAndInOrder)
+{
+    auto order = makeOrder(3);
+    order.edges[0].sequenceId = 1;
+    order.edges[0].actions.push_back(makeAction("beep"));
+    order.edges[1].sequenceId = 3;
+    order.edges[1].actions.push_back(makeAction("horn"));
+    order.nodes[2].sequenceId = 4;
+    order.nodes[2].actions.push_back(makeAction("lift"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    const auto& actions = missions.front()->actions;
+
+    ASSERT_EQ(actions.size(), 3u);
+    EXPECT_EQ(actions[0].action.actionId, "beep");
+    EXPECT_EQ(actions[1].action.actionId, "horn");
+    EXPECT_EQ(actions[2].action.actionId, "lift");
+
+    // Mission layer không diễn giải actionType — nó chỉ chuyển tiếp.
+    EXPECT_EQ(actions[2].action.actionType, "TEST");
+    EXPECT_EQ(actions[2].type, ActionType::NODE_ACTION);
+}
+
+TEST_F(AdapterTest, NonFiniteNodePositionIsRejected)
+{
+    auto order = makeOrder(3);
+    order.nodes[1].nodePosition.x = std::numeric_limits<double>::infinity();
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+}
+
+}  // namespace
+
+// ── Compound action: mở rộng thành chuỗi chặng ──────────────────────────────────────────────────
+
+namespace
+{
+/// @brief Gắn một action có tham số vào node.
+void addAction(robot_protocol_msgs::Node& node, const std::string& type,
+               const std::string& param_key = "", const std::string& param_value = "")
+{
+    robot_protocol_msgs::Action action;
+    action.actionType = type;
+    action.actionId = type + "_id";
+
+    if (!param_key.empty())
+    {
+        robot_protocol_msgs::ActionParameter p;
+        p.key = param_key;
+        p.value = param_value;
+        action.actionParameters.push_back(p);
+    }
+    node.actions.push_back(action);
+}
+}  // namespace
+
+TEST_F(AdapterTest, CompoundActionExpandsIntoASequenceOfLegs)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");
+
+    const auto missions = convertOrder(order);
+
+    // nav -> n1 | DetectCharger | docking | startCharging
+    ASSERT_EQ(missions.size(), 4u);
+
+    EXPECT_TRUE(missions[0]->has_goal);
+    EXPECT_TRUE(missions[0]->actions.empty()) << "action compound phải được GỠ khỏi chặng nav";
+    EXPECT_EQ(missions[0]->motion_hint, "position");
+
+    ASSERT_EQ(missions[1]->actions.size(), 1u);
+    EXPECT_EQ(missions[1]->actions[0].action.actionType, "DetectCharger");
+    EXPECT_FALSE(missions[1]->has_goal);
+
+    EXPECT_TRUE(missions[2]->has_goal);
+    EXPECT_EQ(missions[2]->motion_hint, "docking");
+    EXPECT_EQ(missions[2]->goal_frame, "charger_02_goal") << "frame phải lấy từ actionParameters";
+    EXPECT_EQ(missions[2]->marker, "charger") << "marker phải chọn override planner độc lập TF";
+
+    ASSERT_EQ(missions[3]->actions.size(), 1u);
+    EXPECT_EQ(missions[3]->actions[0].action.actionType, "startCharging");
+    EXPECT_FALSE(missions[3]->has_goal);
+}
+
+TEST_F(AdapterTest, GeneratedActionsCarryTheOriginalParameters)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 4u);
+
+    // Handler dò cần biết dò trạm nào; nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
+    const auto& detect = missions[1]->actions[0].action;
+    ASSERT_EQ(detect.actionParameters.size(), 1u);
+    EXPECT_EQ(detect.actionParameters[0].key, "goal_frame");
+    EXPECT_EQ(detect.actionParameters[0].value, "charger_02_goal");
+
+    // actionId suy từ id gốc để truy vết được trong log.
+    EXPECT_NE(detect.actionId.find("charge_id"), std::string::npos);
+}
+
+TEST_F(AdapterTest, PlainActionsKeepTheirJsonOrderAroundACompound)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "MutedOn");
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_goal");
+    addAction(order.nodes[1], "MutedOff");
+
+    const auto missions = convertOrder(order);
+
+    // nav+MutedOn | Detect | docking | startCharging | MutedOff
+    ASSERT_EQ(missions.size(), 5u);
+
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");
+
+    // MutedOff phải nằm SAU chuỗi charge. Gom hết action thường vào chặng nav sẽ bật lại cảm biến
+    // an toàn trước khi robot lùi vào trạm — đúng thứ MutedOn sinh ra để tránh.
+    ASSERT_EQ(missions[4]->actions.size(), 1u);
+    EXPECT_EQ(missions[4]->actions[0].action.actionType, "MutedOff");
+    EXPECT_FALSE(missions[4]->has_goal);
+}
+
+TEST_F(AdapterTest, ActionOutsideTheTableRunsAsAPlainAction)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "MutedOn");
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u) << "action thường không được mở rộng";
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");
+}
+
+TEST_F(AdapterTest, RelativeMoveStepBecomesALegWithADistance)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "UnDockFromStation");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 2u);
+
+    EXPECT_TRUE(missions[1]->has_goal);
+    EXPECT_EQ(missions[1]->motion_hint, "go_straight");
+    EXPECT_DOUBLE_EQ(missions[1]->relative_distance, -1.0);
+    EXPECT_TRUE(missions[1]->goal_frame.empty());
+}
+
+TEST_F(AdapterTest, FixedFrameStepDoesNotNeedAnyParameter)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "FixedDock");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 2u);
+    EXPECT_EQ(missions[1]->goal_frame, "dock_target");
+    EXPECT_EQ(missions[1]->marker, "trolley");
+}
+
+TEST_F(AdapterTest, MissingStructuralParameterDropsTheWholeOrder)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge");   // thiếu goal_frame
+
+    // Bỏ CẢ order: một order chạy nửa vời — robot tới trạm sạc rồi không sạc — nguy hiểm hơn là
+    // không chạy. Và hàng đợi đang chạy không bị đụng tới (A1).
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST(CompoundConfig, RejectsAStepWithTwoKeywords)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_bad_step", nh))
+        << "không đúng một từ khoá thì không có cách diễn giải nào hiển nhiên đúng";
+}
+
+TEST(CompoundConfig, RejectsARecursiveCompound)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_recursive", nh))
+        << "expander sẽ mở rộng chính output của mình";
+}
+
+TEST(CompoundConfig, RejectsInvalidOrActionOnlyProfiles)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_bad_profile", nh));
+}
+
+int main(int argc, char** argv)
+{
+    // Bảng `compound_actions` nằm trong cây config test. `overwrite = 0` để shell vẫn override được
+    // khi cần chạy tay với cây khác — cùng cách `plugin_registry_test` làm.
+#ifdef MISSION_ADAPTERS_TEST_CONFIG_DIR
+    setenv("PNKX_NAV_CORE_CONFIG_DIR", MISSION_ADAPTERS_TEST_CONFIG_DIR, 0);
+#endif
+
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/config/mission_adapters_params.yaml

@@ -0,0 +1,107 @@
+# Config CHỈ dùng cho test của gói. Bản runtime nằm ở
+# `pnkx_nav_core/config/mission_adapters_params.yaml` (C2) — sửa tham số vận hành thì sửa ở đó.
+#
+# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config
+
+mission_adapters:
+  # Nguồn mission. Thêm loại mới chỉ cần thêm một entry ở đây + một plugin .so.
+  mission_sources:
+    - {name: goal_src,    type: GoalSourceAdapter}
+    - {name: vda5050_src, type: VDA5050SourceAdapter}
+
+  mission_timeout: 0.0           # [s] 0 = tắt
+  clear_queue_on_failure: false  # false = chỉ bỏ chặng lỗi, giữ phần còn lại của order
+
+# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
+# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
+GoalSourceAdapter:
+  library_path: libmission_adapters_goal_source
+
+VDA5050SourceAdapter:
+  library_path: libmission_adapters_vda5050_source
+
+# ── Các case lỗi mà plugin_registry_test cố ý dựng ra ────────────────────────────────────────────
+#
+# Mỗi case là một namespace riêng để test gọi loadFromConfig(nh, "<namespace>") mà không phải sinh
+# file YAML lúc chạy.
+
+registry_test_missing_library_path:
+  mission_sources:
+    - {name: bad_src, type: MissingLibraryPathAdapter}
+
+registry_test_missing_library_file:
+  mission_sources:
+    - {name: bad_src, type: MissingLibraryFileAdapter}
+
+registry_test_wrong_symbol:
+  mission_sources:
+    - {name: bad_src, type: WrongSymbolAdapter}
+
+registry_test_duplicate_schema:
+  mission_sources:
+    - {name: goal_a, type: GoalSourceAdapter}
+    - {name: goal_b, type: GoalSourceAdapter}
+
+registry_test_entry_without_type:
+  mission_sources:
+    - {name: nameless_src}
+
+registry_test_empty:
+  mission_timeout: 0.0
+
+
+# Khai trong mission_sources nhưng KHÔNG có khoá library_path.
+MissingLibraryPathAdapter:
+  description: "cố ý thiếu library_path"
+
+# library_path trỏ tới file không tồn tại.
+MissingLibraryFileAdapter:
+  library_path: libmission_adapters_does_not_exist
+
+# Thư viện có thật nhưng symbol không tồn tại trong đó.
+WrongSymbolAdapter:
+  library_path: libmission_adapters_goal_source
+
+# ── Compound action: action "phải dò rồi mới biết đích" ─────────────────────────────────────────
+#
+# Bảng chỉ giữ CẤU TRÚC; frame cụ thể tới từ actionParameters của chính order.
+vda5050_src:
+  global_frame: map
+  compound_actions:
+    charge:
+      steps:
+        - {action: DetectCharger}
+        - {move_to_param: goal_frame, profile: docking, marker: charger}
+        - {action: startCharging}
+
+    UnDockFromStation:
+      steps:
+        - {move: -1.0, profile: go_straight}
+
+    FixedDock:
+      steps:
+        - {move_to: dock_target, profile: docking, marker: trolley}
+
+# Step có hai từ khoá -> configure() phải từ chối lúc boot.
+vda5050_bad_step:
+  compound_actions:
+    charge:
+      steps:
+        - {action: DetectCharger, move: -1.0}
+
+# Compound sinh ra một actionType cũng là compound -> đệ quy.
+vda5050_recursive:
+  compound_actions:
+    charge:
+      steps:
+        - {action: PickUp}
+    PickUp:
+      steps:
+        - {move: 0.5}
+
+# Profile chỉ có nghĩa trên step navigation và phải thuộc bốn profile runtime.
+vda5050_bad_profile:
+  compound_actions:
+    invalid_name:
+      steps:
+        - {move: 0.5, profile: dock}

test/event_bus_test.cpp

@@ -0,0 +1,133 @@
+#include <gtest/gtest.h>
+
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/event.h>
+
+namespace
+{
+using namespace mission_adapters;
+
+Event makeEvent(EventType type)
+{
+    Event event;
+    event.type = type;
+    return event;
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A5 — thứ tự xử lý phải bằng thứ tự phát sinh.
+//
+// Bản trước sắp theo priority toàn phần: CANCEL (1) luôn đứng trước SUBMIT (5). Phát order rồi huỷ
+// ngay thì huỷ được xử lý trước, order vào hàng đợi sau, và robot chạy đúng cái người dùng vừa huỷ.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(EventBusTest, SubmitThenCancelKeepsCausalOrder)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::SUBMIT_REQUEST));
+    bus.push(makeEvent(EventType::CANCEL));
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::SUBMIT_REQUEST);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::CANCEL) << "the cancel was processed before the order it "
+                                                "cancels";
+}
+
+TEST(EventBusTest, PreservesFifoOrderAcrossEventTypes)
+{
+    EventBus bus;
+
+    const std::vector<EventType> pushed = {
+        EventType::PAUSE,
+        EventType::SUBMIT_REQUEST,
+        EventType::RESUME,
+        EventType::NAV_DONE,
+        EventType::CANCEL,
+    };
+
+    for (const auto type : pushed)
+        bus.push(makeEvent(type));
+
+    for (size_t i = 0; i < pushed.size(); ++i)
+    {
+        Event event;
+        ASSERT_TRUE(bus.pop(event)) << "event number " << i;
+        EXPECT_EQ(event.type, pushed[i]) << "wrong order at position " << i;
+        EXPECT_EQ(event.sequence, i);
+    }
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// Emergency đi ngoài hàng đợi: độ trễ phản ứng không được phụ thuộc độ dài hàng đợi.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(EventBusTest, EmergencyOvertakesQueue)
+{
+    EventBus bus;
+
+    for (int i = 0; i < 50; ++i)
+        bus.push(makeEvent(EventType::SUBMIT_REQUEST));
+
+    EXPECT_FALSE(bus.emergencyPending());
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    // Thấy được ngay, không phải rút hết 50 sự kiện kia ra trước.
+    EXPECT_TRUE(bus.emergencyPending());
+    EXPECT_EQ(bus.size(), 51u) << "an EMERGENCY event must still sit in the queue to keep the log "
+                                  "sequence";
+
+    EXPECT_TRUE(bus.takeEmergency());
+    EXPECT_FALSE(bus.takeEmergency()) << "the emergency flag was consumed twice";
+}
+
+TEST(EventBusTest, EmergencyEventStillArrivesInOrder)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::PAUSE));
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::PAUSE);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::EMERGENCY);
+}
+
+TEST(EventBusTest, StopUnblocksPop)
+{
+    EventBus bus;
+    Event event;
+
+    std::thread stopper([&bus] { bus.stop(); });
+    EXPECT_FALSE(bus.pop(event));
+    stopper.join();
+}
+
+TEST(EventBusTest, ResetDropsPendingEventsAndEmergencyFlag)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::PAUSE));
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    bus.reset();
+
+    EXPECT_TRUE(bus.empty());
+    EXPECT_FALSE(bus.emergencyPending());
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/mission_adapters_test.cpp

@@ -1,360 +0,0 @@
-#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();
-}

test/mission_lifecycle_test.cpp

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

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

test/mission_test_utils.h

@@ -0,0 +1,177 @@
+/*********************************************************************
+ *
+ * Helper dùng chung cho test của gói: dựng goal/order giả và chờ trạng thái.
+ *
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+#define MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+
+#include <chrono>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_adapters.h>
+
+namespace mission_test
+{
+
+/**
+ * @brief NavigationClient giả: ghi lại mọi lệnh xuống navigation để test kiểm thứ tự và số lần.
+ *
+ * Không mô phỏng chuyển động — test tự quyết định khi nào outcome quay về.
+ */
+class FakeNavigationClient : public mission_adapters::NavigationClient
+{
+public:
+    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        dispatched_.push_back(mission);
+        return accept_dispatch_;
+    }
+
+    void cancelActive(mission_adapters::MissionId id) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        cancelled_.push_back(id);
+    }
+
+    /// Ép navigation từ chối mọi mission tiếp theo.
+    void setAcceptDispatch(bool accept)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        accept_dispatch_ = accept;
+    }
+
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_;
+    }
+
+    std::vector<mission_adapters::MissionId> cancelled() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return cancelled_;
+    }
+
+    size_t dispatchCount() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_.size();
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched_;
+    std::vector<mission_adapters::MissionId> cancelled_;
+    bool accept_dispatch_ = true;
+};
+
+
+/// @brief Goal hợp lệ: quaternion đã chuẩn hoá (w=1) để qua được validate() của adapter.
+inline robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
+{
+    robot_geometry_msgs::PoseStamped goal;
+    goal.header.frame_id = "map";
+    goal.pose.position.x = x;
+    goal.pose.position.y = y;
+    goal.pose.orientation.w = 1.0;
+    return goal;
+}
+
+/// @brief Mission dựng thẳng, không qua adapter — dùng cho test của MissionManager/Executor.
+inline std::shared_ptr<mission_adapters::Mission> makeMission(double x, double y)
+{
+    auto mission = std::make_shared<mission_adapters::Mission>();
+    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
+    mission->goal = makeGoal(x, y);
+    return mission;
+}
+
+/// @brief n mission độc lập, goal khác nhau.
+inline std::vector<std::shared_ptr<mission_adapters::Mission>> makeMissions(int count)
+{
+    std::vector<std::shared_ptr<mission_adapters::Mission>> missions;
+    missions.reserve(static_cast<size_t>(count));
+    for (int i = 0; i < count; ++i)
+        missions.push_back(makeMission(i, i));
+
+    return missions;
+}
+
+inline robot_protocol_msgs::Action makeAction(const std::string& id)
+{
+    robot_protocol_msgs::Action action;
+    action.actionId = id;
+    action.actionType = "TEST";
+    return action;
+}
+
+/// @brief Node đã released (base). Order thật của fleet manager luôn có ít nhất phần base.
+inline 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.released = true;
+    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;
+}
+
+inline 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);
+    edge.released = true;
+
+    if (add_action)
+        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
+
+    return edge;
+}
+
+inline robot_protocol_msgs::Order makeOrder(int node_count, const std::string& order_id = "order_1",
+                                           std::uint32_t order_update_id = 0)
+{
+    robot_protocol_msgs::Order order;
+    order.orderId = order_id;
+    order.orderUpdateId = order_update_id;
+
+    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;
+}
+
+inline bool waitForState(mission_adapters::MissionManager& manager,
+                         mission_adapters::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;
+}
+
+}  // namespace mission_test
+
+#endif  // MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_

test/plugin_registry_test.cpp

@@ -0,0 +1,287 @@
+/*********************************************************************
+ *
+ * Kiểm đường nạp plugin thật: YAML -> library_path -> Boost.DLL -> schema.
+ *
+ * Test này cần PNKX_NAV_CORE_CONFIG_DIR trỏ vào test/config của gói:
+ *
+ *   PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
+ *     ./devel/lib/mission_adapters/plugin_registry_test
+ *
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/robot.h>
+
+#include <mission_adapters/mission_adapters.h>
+#include <mission_adapters/plugin_registry.h>
+
+#include "mission_test_utils.h"
+
+namespace
+{
+using namespace mission_adapters;
+
+/**
+ * @brief Nguồn mission thứ ba, viết hoàn toàn trong test.
+ *
+ * Sự tồn tại của nó là bài kiểm tra thật cho tính plugin: thêm một loại nguồn mới mà không sửa file
+ * nào trong `src/` của gói.
+ */
+class DummySourceAdapter : public MissionSourceAdapter
+{
+public:
+    static constexpr const char* kSchema = "test.dummy";
+
+    bool configure(const std::string& name, robot::NodeHandle& nh) override
+    {
+        (void)nh;
+        name_ = name;
+        return true;
+    }
+
+    std::string schema() const override { return kSchema; }
+
+    bool validate(const MissionRequest& request, std::string& reason) const override
+    {
+        if (request.raw_payload.empty())
+        {
+            reason = "raw_payload is empty";
+            return false;
+        }
+        return true;
+    }
+
+    ConversionResult convert(const MissionRequest& request) override
+    {
+        (void)request;
+        auto mission = std::make_shared<Mission>();
+        mission->type = MissionType::SIMPLE_GOAL;
+
+        ConversionResult result;
+        result.missions.push_back(mission);
+        return result;
+    }
+
+private:
+    std::string name_;
+};
+
+class PluginRegistryTest : public ::testing::Test
+{
+protected:
+    robot::NodeHandle nh;
+};
+
+// ── Đường nạp bình thường ────────────────────────────────────────────────────────────────────────
+
+TEST_F(PluginRegistryTest, LoadsBothSourcesFromConfig)
+{
+    PluginRegistry registry;
+
+    ASSERT_TRUE(registry.loadFromConfig(nh))
+        << "could not load the plugin — check PNKX_NAV_CORE_CONFIG_DIR and devel/lib";
+
+    EXPECT_EQ(registry.size(), 2u);
+    EXPECT_NE(registry.find(schema::kPoseStamped), nullptr);
+    EXPECT_NE(registry.find(schema::kVda5050Order), nullptr);
+    EXPECT_EQ(registry.find("does.not.exist"), nullptr);
+}
+
+TEST_F(PluginRegistryTest, LoadedAdapterConvertsRealPayload)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.loadFromConfig(nh));
+
+    auto* adapter = registry.find(schema::kPoseStamped);
+    ASSERT_NE(adapter, nullptr);
+
+    const auto request = MissionRequest::fromPose(mission_test::makeGoal(3.0, 4.0));
+
+    std::string reason;
+    ASSERT_TRUE(adapter->validate(request, reason)) << reason;
+
+    const auto result = adapter->convert(request);
+    ASSERT_EQ(result.missions.size(), 1u);
+    EXPECT_DOUBLE_EQ(result.missions.front()->goal.pose.position.x, 3.0);
+}
+
+// ── Các cách hỏng, đều phải báo lỗi rõ ràng chứ không crash ──────────────────────────────────────
+
+TEST_F(PluginRegistryTest, MissingLibraryPathKeyFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_missing_library_path"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, MissingLibraryFileFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_missing_library_file"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, WrongSymbolNameFailsCleanly)
+{
+    PluginRegistry registry;
+
+    // Thư viện có thật, symbol thì không: import_alias ném system_error, registry phải nuốt và báo.
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_wrong_symbol"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, DuplicateSchemaIsRejected)
+{
+    PluginRegistry registry;
+
+    // Hai instance cùng khai schema "geometry.pose_stamped": cái thứ hai bị từ chối thay vì ghi đè
+    // im lặng, vì định tuyến khi đó sẽ phụ thuộc thứ tự nạp.
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_duplicate_schema"));
+    EXPECT_EQ(registry.size(), 1u);
+}
+
+TEST_F(PluginRegistryTest, EntryWithoutTypeFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_entry_without_type"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, MissingSourceListFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_empty"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+// ── Đăng ký trực tiếp (nguồn biên dịch thẳng vào host, hoặc test) ───────────────────────────────
+
+TEST_F(PluginRegistryTest, RejectsNullAndDuplicateManualRegistration)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.registerAdapter(nullptr));
+
+    auto first = std::make_shared<DummySourceAdapter>();
+    EXPECT_TRUE(registry.registerAdapter(first));
+
+    auto second = std::make_shared<DummySourceAdapter>();
+    EXPECT_FALSE(registry.registerAdapter(second)) << "two adapters with the same schema were both "
+                                                      "accepted";
+
+    EXPECT_EQ(registry.size(), 1u);
+}
+
+/**
+ * Nguồn thứ ba đi hết đường: đăng ký -> EventProcessor định tuyến theo schema -> mission vào hàng
+ * đợi. Không có dòng nào trong `src/` của gói biết tới DummySourceAdapter.
+ */
+TEST_F(PluginRegistryTest, ThirdPartyAdapterFlowsThroughCoreUnchanged)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.registerAdapter(std::make_shared<DummySourceAdapter>()));
+
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+    processor.start();
+
+    processor.submitRequest(
+        MissionRequest::fromRaw(DummySourceAdapter::kSchema, "{\"job\":\"pick\"}"));
+
+    EXPECT_TRUE(mission_test::waitForState(manager, MissionState::QUEUED,
+                                           std::chrono::milliseconds(500)));
+
+    // Payload không hợp lệ -> validate() chặn -> hàng đợi không đổi (A1).
+    processor.submitRequest(MissionRequest::fromRaw(DummySourceAdapter::kSchema, ""));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    processor.stop();
+}
+
+/**
+ * @brief Plugin hỏng: sinh mission không goal và cũng không action.
+ *
+ * Core phải chặn, vì chặng như vậy không có việc gì để làm và sẽ không bao giờ báo kết quả về —
+ * mission layer kẹt RUNNING vĩnh viễn.
+ */
+class EmptyMissionAdapter : public MissionSourceAdapter
+{
+public:
+    static constexpr const char* kSchema = "test.empty_mission";
+
+    bool configure(const std::string&, robot::NodeHandle&) override { return true; }
+    std::string schema() const override { return kSchema; }
+    bool validate(const MissionRequest&, std::string&) const override { return true; }
+
+    ConversionResult convert(const MissionRequest&) override
+    {
+        auto mission = std::make_shared<Mission>();
+        mission->has_goal = false;   // và actions rỗng
+
+        ConversionResult result;
+        result.missions.push_back(mission);
+        return result;
+    }
+};
+
+TEST_F(PluginRegistryTest, MissionWithoutGoalAndWithoutActionIsRejected)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.registerAdapter(std::make_shared<EmptyMissionAdapter>()));
+
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.submitRequest(MissionRequest::fromRaw(EmptyMissionAdapter::kSchema, "payload"));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    processor.stop();
+
+    EXPECT_EQ(manager.state(), MissionState::IDLE);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST_F(PluginRegistryTest, UnknownSchemaIsIgnoredNotCrashing)
+{
+    PluginRegistry registry;
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.submitRequest(MissionRequest::fromRaw("nobody.handles.this", "payload"));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    processor.stop();
+
+    EXPECT_EQ(manager.state(), MissionState::IDLE);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    // Test tự trỏ vào cây config và thư mục .so của chính nó, để chạy được cả qua ctest lẫn khi gọi
+    // thẳng binary (đúng cách test_costmap đang làm). overwrite = 0 nên biến môi trường do người
+    // chạy đặt vẫn thắng.
+#ifdef MISSION_ADAPTERS_TEST_CONFIG_DIR
+    setenv("PNKX_NAV_CORE_CONFIG_DIR", MISSION_ADAPTERS_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MISSION_ADAPTERS_TEST_LIBRARY_DIR
+    setenv("PNKX_NAV_CORE_LIBRARY_PATH", MISSION_ADAPTERS_TEST_LIBRARY_DIR, 0);
+#endif
+
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}