optimal

test/config/move_base2_params.yaml

@@ -169,3 +169,27 @@ TestControllerRefusesLimits:
   library_path: libmove_base2_test_local_planner
 
 # TestControllerMissing cố ý KHÔNG khai library_path.
+
+# --- Schema gen-1 (move_base_common_params.yaml) cho test đường legacy --------------------------
+legacy_move_base:
+  controller_frequency: 20.0      # [Hz]
+  planner_frequency: 0.0          # [Hz]
+  planner_patience: 0.0           # gen-1: "fail -> recovery NGAY", KHÔNG phải "tắt"
+  controller_patience: 15.0       # [s]
+  oscillation_timeout: 0.0        # [s]
+  oscillation_distance: 0.5       # [m]
+  max_planning_retries: 0
+  recovery_behavior_enabled: true
+
+  xy_goal_tolerance: 0.25         # [m] default chung cho cả bốn profile
+  yaw_goal_tolerance: 0.30        # [rad]
+
+  base_global_planner: SBPLLatticePlanner
+  base_local_planner: LocalPlannerAdapter   # phải bị BỎ QUA có log
+
+  position_planner_name: HybridLocalPlanner
+  docking_planner_name: DockLocalPlanner
+
+  # Global planner riêng của profile nằm trong namespace mang tên chính planner đó.
+  DockLocalPlanner:
+    base_global_planner: DockPlanner

test/config_validation_test.cpp

@@ -235,6 +235,105 @@ TEST(MoveBase2Config, DescribeMentionsEveryGroup)
 
 }  // namespace
 
+// ================================================================================================
+// Schema gen-1 (legacy)
+//
+// Cây config production hiện tại là schema gen-1. Không đọc được nó thì move_base2 không chạy được
+// trên cấu hình thật, và bước 11 (chạy song song bản cũ/mới) không thực hiện được.
+// ================================================================================================
+
+TEST(MoveBase2ConfigLegacy, MapsPlannerNamesToProfiles)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  MoveBase2Config config;
+  config.fromLegacyNodeHandle(nh);
+
+  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
+  EXPECT_EQ(config.docking.local_planner_name, "DockLocalPlanner");
+
+  // Profile không khai global planner riêng thì rơi về khoá ở root.
+  EXPECT_EQ(config.position.global_planner_name, "SBPLLatticePlanner");
+  // Profile có khai riêng thì dùng bản riêng.
+  EXPECT_EQ(config.docking.global_planner_name, "DockPlanner");
+}
+
+TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  MoveBase2Config config;
+  config.fromLegacyNodeHandle(nh);
+
+  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25);
+  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30);
+  EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25);
+}
+
+TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
+{
+  // Khác biệt NGỮ NGHĨA, không phải khác biệt giá trị. Gen-1: 0 = "fail -> recovery ngay". Gen-2:
+  // 0 = "tắt đồng hồ". Dịch nhầm là biến một cấu hình phản ứng-ngay thành cấu hình không-bao-giờ.
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  MoveBase2Config config;
+  config.fromLegacyNodeHandle(nh);
+
+  EXPECT_GT(config.state_machine.planner_patience, 0.0);
+  EXPECT_NEAR(config.state_machine.planner_patience, 1.0 / 20.0, 1e-9);
+}
+
+TEST(MoveBase2ConfigLegacy, KeepsGen1DefaultForRobotBaseFrame)
+{
+  // Gen-1 mặc định "base_footprint", gen-2 "base_link". Chế độ legacy giữ default gen-1 để không
+  // đổi hành vi của một hệ đang chạy chỉ vì đổi runtime.
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  MoveBase2Config config;
+  config.fromLegacyNodeHandle(nh);
+
+  EXPECT_EQ(config.robot_base_frame, "base_footprint");
+}
+
+TEST(MoveBase2ConfigLegacy, LoadedLegacyConfigPassesValidation)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  MoveBase2Config config;
+  config.fromLegacyNodeHandle(nh);
+  config.state_machine.recovery_behavior_count = 2;
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
+}
+
+TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
+{
+  // Không trộn từng khoá giữa hai schema: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu
+  // lỗi "sửa config mãi không ăn" mà hai cây config trùng tên của workspace đã gây ra một lần.
+  robot::NodeHandle root;
+  const MoveBase2Config config = MoveBase2Config::load(root);
+
+  EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới";
+  EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc";
+}
+
+TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, "legacy_move_base");
+
+  const MoveBase2Config config = MoveBase2Config::load(nh);
+
+  EXPECT_EQ(config.robot_base_frame, "base_footprint");
+  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
+}
+
 int main(int argc, char** argv)
 {
 #ifdef MOVE_BASE2_TEST_CONFIG_DIR

test/controller_runner_test.cpp

@@ -20,9 +20,25 @@
 
 #include <move_base2/runners/controller_runner.h>
 
+#include "fake_ports.h"
+
 namespace
 {
 using move_base2::ControllerRunner;
+using move_base2::testing::FakePosePort;
+
+/// Pose cố định — gen-2 nhận pose làm tham số của computeVelocityCommands và isGoalReached.
+FakePosePort& fixedPose()
+{
+  static FakePosePort pose;
+  static bool ready = false;
+  if (!ready)
+  {
+    pose.setPosition(0.0, 0.0);
+    ready = true;
+  }
+  return pose;
+}
 
 /// [m/s] Lệnh nền của plugin test khi chưa đặt trần và vận tốc đo được bằng 0.
 constexpr double kBaseSpeed = 0.25;
@@ -80,7 +96,7 @@ public:
   {
     robot::NodeHandle nh;
     std::string error;
-    ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error);
+    ok_ = runner_.configure(nh, nullptr, dummyCostmap(), &fixedPose(), name, error);
     error_ = error;
   }
 
@@ -113,18 +129,30 @@ TEST(ControllerRunner, RefusesNullCostmap)
   ControllerRunner runner;
   std::string error;
 
-  EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error));
+  EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, &fixedPose(), "TestControllerOk", error));
   EXPECT_FALSE(error.empty());
   EXPECT_FALSE(runner.configured());
 }
 
+TEST(ControllerRunner, RefusesNullPosePort)
+{
+  // Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands và isGoalReached; không có nguồn pose
+  // thì không gọi được hàm nào trong hai hàm đó.
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), nullptr, "TestControllerOk", error));
+  EXPECT_FALSE(runner.configured());
+}
+
 TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
 {
   robot::NodeHandle nh;
   ControllerRunner runner;
   std::string error;
 
-  EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), "TestControllerMissing", error));
+  EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error));
   EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
 }
 
@@ -209,7 +237,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
   robot::NodeHandle nh;
   ControllerRunner runner;
   std::string error;
-  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error;
 
   EXPECT_TRUE(runner.setTwistLinear(vec(0.08)));  // [m/s], chưa có controller nào
   ASSERT_TRUE(runner.swapPlanner("TestControllerOk"));
@@ -309,7 +337,7 @@ TEST(ControllerRunner, SetPlanWithoutAControllerFails)
   robot::NodeHandle nh;
   ControllerRunner runner;
   std::string error;
-  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error;
 
   EXPECT_FALSE(runner.setPlan(makePlan()));
 }
@@ -319,7 +347,7 @@ TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand)
   robot::NodeHandle nh;
   ControllerRunner runner;
   std::string error;
-  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error;
 
   robot_geometry_msgs::Twist cmd;
   EXPECT_FALSE(runner.computeVelocityCommands(cmd));

test/fake_ports.h

@@ -375,6 +375,17 @@ public:
     measured_velocity_ = velocity;
   }
 
+  void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) override
+  {
+    plan = local_plan_;
+  }
+
+  /// @brief Đặt quỹ đạo cục bộ mà fake sẽ trả về.
+  void setLocalPlan(const robot_nav_2d_msgs::Path2D& plan)
+  {
+    local_plan_ = plan;
+  }
+
   bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override
   {
     if (linear.x < 0.0)
@@ -472,6 +483,7 @@ public:
   }
 
 private:
+  robot_nav_2d_msgs::Path2D local_plan_;
   robot_geometry_msgs::Twist measured_velocity_;
   double limit_forward_ = 0.0;   ///< [m/s]
   double limit_backward_ = 0.0;  ///< [m/s], âm

test/mission_adapter_bridge_test.cpp

@@ -0,0 +1,287 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test MissionAdapterBridge.
+ *
+ * Hai thứ đáng khoá lại: phần chuyển đổi Mission -> NavigationRequest, và **biên thread** — mission
+ * layer đẩy chặng xuống từ thread của nó, nhưng chặng đó chỉ được chạm vào control loop trên control
+ * thread.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <mission_adapters/mission_manager.h>
+#include <mission_adapters/types.h>
+
+#include <move_base2/bridges/mission_adapter_bridge.h>
+
+namespace
+{
+using mission_adapters::Mission;
+using mission_adapters::MissionManager;
+using move_base2::MissionAdapterBridge;
+using move_base2::NavigationOutcome;
+using move_base2::NavigationRequest;
+
+std::shared_ptr<Mission> makeMission(mission_adapters::MissionId id, double goal_x = 2.0,
+                                     std::size_t actions = 0, bool has_goal = true)
+{
+  auto mission = std::make_shared<Mission>();
+  mission->id = id;
+  mission->has_goal = has_goal;
+  mission->goal.header.frame_id = "map";
+  mission->goal.pose.position.x = goal_x;  // [m]
+  mission->goal.pose.orientation.w = 1.0;
+
+  for (std::size_t i = 0; i < actions; ++i)
+  {
+    mission_adapters::Action action;
+    action.sequenceId = static_cast<int>(i);
+    action.action.actionType = "act_" + std::to_string(i);
+    mission->actions.push_back(action);
+  }
+  return mission;
+}
+
+/// @brief Bridge đã start, kèm chỗ nhận yêu cầu để test đọc lại.
+class Fixture
+{
+public:
+  Fixture()
+  {
+    bridge_.setRequestCallback([this](const NavigationRequest& request) {
+      received_.push_back(request);
+    });
+    bridge_.setCancelCallback([this]() { ++cancel_calls_; });
+    bridge_.start();
+  }
+
+  MissionAdapterBridge bridge_;
+  std::vector<NavigationRequest> received_;
+  std::size_t cancel_calls_ = 0;
+};
+
+}  // namespace
+
+// ================================================================================================
+// Chuyển đổi Mission -> NavigationRequest
+// ================================================================================================
+
+TEST(MissionAdapterBridgeConversion, CarriesIdGoalAndActions)
+{
+  const auto mission = makeMission(7, 3.5, 2);
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+
+  EXPECT_EQ(request.mission_sequence_id, 7U);
+  EXPECT_TRUE(request.has_goal);
+  EXPECT_DOUBLE_EQ(request.goal.pose.position.x, 3.5);
+  ASSERT_EQ(request.actions.size(), 2U);
+  EXPECT_EQ(request.actions[0].actionType, "act_0");
+  EXPECT_EQ(request.actions[1].actionType, "act_1");
+}
+
+TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse)
+{
+  // D8: mission chỉ-có-action. Navigation bỏ qua phần di chuyển và vào thẳng thực thi action.
+  const auto mission = makeMission(9, 0.0, 3, /*has_goal=*/false);
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+
+  EXPECT_FALSE(request.has_goal);
+  EXPECT_EQ(request.actions.size(), 3U);
+}
+
+TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
+{
+  // Quy ước của NavigationRequest: sai số <= 0 nghĩa "dùng default của profile trong config".
+  // Mission layer không biết gì về sai số hình học nên không được đặt.
+  const auto mission = makeMission(1);
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+
+  EXPECT_FALSE(request.tolerance.hasXy());
+  EXPECT_FALSE(request.tolerance.hasYaw());
+}
+
+// ================================================================================================
+// Biên thread: dispatch chỉ cất lại, control thread mới đẩy xuống
+// ================================================================================================
+
+TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped)
+{
+  // `MissionExecutor` gọi dispatch từ thread của nó, còn ControlLoop không thread-safe. Đẩy thẳng
+  // xuống ở đây nghĩa là hai thread cùng ghi pending_request_ của control loop.
+  Fixture fixture;
+
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3)));
+  EXPECT_TRUE(fixture.received_.empty()) << "dispatch đi thẳng xuống navigation, bỏ qua biên thread";
+
+  EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
+  ASSERT_EQ(fixture.received_.size(), 1U);
+  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 3U);
+}
+
+TEST(MissionAdapterBridge, PumpingWithNothingPendingIsANoOp)
+{
+  Fixture fixture;
+
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
+  EXPECT_TRUE(fixture.received_.empty());
+}
+
+TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce)
+{
+  Fixture fixture;
+
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4)));
+  EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "cùng một mission bị đẩy xuống hai lần";
+  EXPECT_EQ(fixture.received_.size(), 1U);
+}
+
+TEST(MissionAdapterBridge, DispatchBeforeStartIsRefused)
+{
+  // Mission layer phải biết chặng của nó không được nhận, chứ không phải chờ một kết quả sẽ không
+  // bao giờ tới.
+  MissionAdapterBridge bridge;
+  EXPECT_FALSE(bridge.dispatch(makeMission(1)));
+}
+
+TEST(MissionAdapterBridge, DispatchAfterStopIsRefused)
+{
+  Fixture fixture;
+  fixture.bridge_.stop();
+
+  EXPECT_FALSE(fixture.bridge_.dispatch(makeMission(1)));
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
+}
+
+TEST(MissionAdapterBridge, StopDropsAMissionThatWasStillWaiting)
+{
+  // Giữ lại để nó chạy sau một lần start() nào đó về sau là hành vi không ai mong đợi.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(5)));
+
+  fixture.bridge_.stop();
+  fixture.bridge_.start();
+
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
+  EXPECT_TRUE(fixture.received_.empty());
+}
+
+TEST(MissionAdapterBridge, NullMissionIsRefused)
+{
+  Fixture fixture;
+  EXPECT_FALSE(fixture.bridge_.dispatch(nullptr));
+}
+
+TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted)
+{
+  // Không nên xảy ra — MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Nhưng im lặng ở
+  // đây nghĩa là một chặng biến mất trong khi fleet master vẫn chờ nó.
+  Fixture fixture;
+
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(1)));
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2)));
+
+  EXPECT_EQ(fixture.bridge_.droppedRequests(), 1U);
+
+  ASSERT_TRUE(fixture.bridge_.pumpPendingRequest());
+  ASSERT_EQ(fixture.received_.size(), 1U);
+  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "mission cũ thắng mission mới";
+}
+
+// ================================================================================================
+// Huỷ
+// ================================================================================================
+
+TEST(MissionAdapterBridge, CancelReachesNavigationThroughTheCallback)
+{
+  Fixture fixture;
+  fixture.bridge_.cancelActive(1);
+
+  EXPECT_EQ(fixture.cancel_calls_, 1U);
+}
+
+TEST(MissionAdapterBridge, CancelDropsAMissionThatHadNotBeenPushedDownYet)
+{
+  // Đẩy nó xuống rồi mới huỷ là cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(6)));
+
+  fixture.bridge_.cancelActive(6);
+
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
+  EXPECT_TRUE(fixture.received_.empty());
+}
+
+// ================================================================================================
+// Báo kết quả về mission layer
+// ================================================================================================
+
+TEST(MissionAdapterBridge, ReportingWithoutAManagerIsSafe)
+{
+  // Chạy không có mission layer là cấu hình hợp lệ: goal có thể đến thẳng từ contract host.
+  Fixture fixture;
+  EXPECT_NO_THROW(fixture.bridge_.reportOutcome(1, NavigationOutcome::kSucceeded));
+}
+
+TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported)
+{
+  MissionManager manager;
+  Fixture fixture;
+  fixture.bridge_.attach(&manager);
+
+  fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded);
+
+  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "goal trực tiếp bị đem báo lên mission layer";
+}
+
+TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
+{
+  MissionManager manager;
+  Fixture fixture;
+  fixture.bridge_.attach(&manager);
+
+  manager.submit({ makeMission(0) });
+  const auto running = manager.nextMission();
+  ASSERT_TRUE(running) << "manager không giao mission nào để chạy";
+
+  fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);
+
+  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
+  EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
+      << "mission vẫn còn đang chạy sau khi đã báo hoàn tất";
+}
+
+TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted)
+{
+  // Outcome tới trễ sau khi mission đã bị thay. Manager từ chối nó — đúng ý, nhưng phải đếm được,
+  // vì đếm tăng bất thường nghĩa là bất biến "báo đúng một lần" đang bị vi phạm ở đâu đó.
+  MissionManager manager;
+  Fixture fixture;
+  fixture.bridge_.attach(&manager);
+
+  fixture.bridge_.reportOutcome(9999, NavigationOutcome::kSucceeded);
+
+  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 1U);
+}
+
+TEST(MissionAdapterBridge, HasActiveMissionCountsAMissionStillWaitingToBePushedDown)
+{
+  Fixture fixture;
+  EXPECT_FALSE(fixture.bridge_.hasActiveMission());
+
+  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2)));
+  EXPECT_TRUE(fixture.bridge_.hasActiveMission());
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/move_base2_scenario_driver.h

@@ -0,0 +1,294 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — hiện thực ScenarioDriver để chạy kịch bản khai báo của nav_test_harness.
+ *
+ * Đặt trong test/ của move_base2 chứ không trong harness: harness không được phụ thuộc ngược vào
+ * package tiêu thụ nào. Harness biết cách CHẠY và cách KIỂM; "chạy cái gì" do bên tiêu thụ hiện
+ * thực — đây chính là chỗ đó.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
+#define MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
+
+#include <cmath>
+#include <cstddef>
+#include <string>
+#include <vector>
+
+#include <nav_test_harness/scenario.h>
+#include <nav_test_harness/scenario_runner.h>
+
+#include <move_base2/control_loop.h>
+
+#include "fake_ports.h"
+
+namespace move_base2
+{
+namespace testing
+{
+
+/**
+ * @class MoveBase2ScenarioDriver
+ * @brief Chạy một kịch bản qua @ref ControlLoop với toàn bộ cổng giả.
+ *
+ * Kịch bản mô tả **hành vi của các cổng**, không mô tả thế giới vật lý: `planner_script` là chuỗi
+ * kết quả `makePlan`, `controller_script` là chuỗi kết quả `computeVelocityCommands`. Nhờ vậy các
+ * bất biến an toàn (mất TF -> dừng, NaN -> chặn, vượt trần -> clamp) kiểm được **xác định**, không
+ * phụ thuộc vào việc dựng đúng một tình huống hình học.
+ *
+ * @note `obstacles` trong kịch bản **chưa được dùng** ở driver này: cổng recovery là fake theo kịch
+ *       bản nên không tra costmap. Kịch bản cần va chạm thật (`obstacle_behind_during_backup`) phải
+ *       chạy qua một driver khác nạp `recovery_core` thật kèm `FakeCollisionChecker` — thuộc phần
+ *       sau của Phase 5. Driver này **báo lỗi setup** nếu kịch bản khai obstacle, thay vì lặng lẽ
+ *       chạy một ca test không kiểm đúng thứ nó nói là đang kiểm.
+ */
+class MoveBase2ScenarioDriver final : public nav_test_harness::ScenarioDriver
+{
+public:
+  bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
+  {
+    scenario_ = scenario;
+
+    if (!scenario.obstacles.empty())
+    {
+      error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); "
+              "dùng driver có recovery_core thật cho kịch bản va chạm";
+      return false;
+    }
+
+    std::vector<PlannerScript> planner_script;
+    for (const std::string& item : scenario.planner_script)
+    {
+      if (item == "ok")
+      {
+        planner_script.push_back(PlannerScript::kOk);
+      }
+      else if (item == "fail")
+      {
+        planner_script.push_back(PlannerScript::kFail);
+      }
+      else if (item == "empty")
+      {
+        planner_script.push_back(PlannerScript::kEmpty);
+      }
+      else
+      {
+        error = "planner_script không hiểu: '" + item + "'";
+        return false;
+      }
+    }
+
+    std::vector<ControllerScript> controller_script;
+    for (const std::string& item : scenario.controller_script)
+    {
+      if (item == "ok")
+      {
+        controller_script.push_back(ControllerScript::kOk);
+      }
+      else if (item == "fail")
+      {
+        controller_script.push_back(ControllerScript::kFail);
+      }
+      else if (item == "goal_reached")
+      {
+        controller_script.push_back(ControllerScript::kGoalReached);
+      }
+      else if (item == "nan")
+      {
+        controller_script.push_back(ControllerScript::kNaN);
+      }
+      else if (item == "too_fast")
+      {
+        controller_script.push_back(ControllerScript::kTooFast);
+      }
+      else
+      {
+        error = "controller_script không hiểu: '" + item + "'";
+        return false;
+      }
+    }
+
+    std::vector<RecoveryScript> recovery_script;
+    for (const std::string& item : scenario.recovery_script)
+    {
+      if (item == "running")
+      {
+        recovery_script.push_back(RecoveryScript::kRunning);
+      }
+      else if (item == "succeeded")
+      {
+        recovery_script.push_back(RecoveryScript::kSucceeded);
+      }
+      else if (item == "failed")
+      {
+        recovery_script.push_back(RecoveryScript::kFailed);
+      }
+      else
+      {
+        error = "recovery_script không hiểu: '" + item + "'";
+        return false;
+      }
+    }
+
+    for (const nav_test_harness::ScenarioEvent& event : scenario.events)
+    {
+      if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
+          event.action != "lose_pose" && event.action != "restore_pose")
+      {
+        error = "events: action không hiểu: '" + event.action + "'";
+        return false;
+      }
+    }
+
+    planner_.setScript(planner_script);
+    controller_.setScript(controller_script);
+    recovery_.setScript(recovery_script);
+
+    pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
+
+    ControlLoopConfig config;
+    config.nominal_control_period = scenario.control_period;  // [s]
+    config.state_machine.planner_patience = 0.5;              // [s]
+    config.state_machine.controller_patience = 0.5;           // [s]
+    config.state_machine.oscillation_timeout = 0.0;           // tắt trừ khi kịch bản cần
+    config.state_machine.oscillation_distance = 0.5;          // [m]
+    config.state_machine.max_planning_retries = -1;
+    config.state_machine.recovery_behavior_count = 2;
+    config.state_machine.recovery_enabled = true;
+
+    // Trần vận tốc lấy từ kỳ vọng của kịch bản: `expect_max_speed` vừa là điều được kiểm, vừa là
+    // trần mà arbiter phải áp — nếu hai con số đó lệch nhau thì ca test không kiểm đúng thứ nó nói.
+    config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
+    config.velocity.min_vel_x = -config.velocity.max_vel_x;
+    config.velocity.max_vel_theta =
+        scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
+    config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn: kịch bản kiểm state và trần, không kiểm ramp
+    config.velocity.max_accel_theta = 100.0;  // [rad/s^2]
+
+    config.position.global_planner_name = "ScenarioGlobalPlanner";
+    config.position.local_planner_name = "ScenarioLocalPlanner";
+    config.docking = config.position;
+    config.go_straight = config.position;
+    config.rotate = config.position;
+
+    deps_.clock = &clock_;
+    deps_.pose = &pose_;
+    deps_.planner = &planner_;
+    deps_.controller = &controller_;
+    deps_.recovery = &recovery_;
+    deps_.mission = &mission_;
+    deps_.action = &action_;
+
+    if (!loop_.configure(config, deps_, error))
+    {
+      return false;
+    }
+
+    NavigationRequest request;
+    request.profile = MotionProfile::kPosition;
+    request.goal.header.frame_id = "map";
+    request.goal.pose.position.x = scenario.goal.x;  // [m]
+    request.goal.pose.position.y = scenario.goal.y;  // [m]
+    request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
+    request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);
+
+    if (!loop_.submit(request, error))
+    {
+      return false;
+    }
+
+    cycle_ = 0;
+    started_ = false;
+    return true;
+  }
+
+  bool step(nav_test_harness::ScenarioStep& step) override
+  {
+    if (!started_)
+    {
+      // Ảnh chụp TRƯỚC khi cycle nào chạy. Không phải chi tiết kỹ thuật: nó là bằng chứng rằng lõi
+      // chưa phát lệnh nào trước khi nhận yêu cầu — và không có nó thì `expect_no_cmd_in_states`
+      // không kiểm được state ban đầu, vì state đó không bao giờ xuất hiện trong trace.
+      started_ = true;
+      step.cycle = 0;
+      step.state = toString(loop_.state());
+      step.linear_x = 0.0;
+      step.angular_z = 0.0;
+      return true;
+    }
+
+    applyEventsFor(cycle_);
+
+    const bool running = loop_.step();
+
+    step.cycle = cycle_;
+    step.state = toString(loop_.state());
+    step.linear_x = loop_.lastCommand().linear.x;    // [m/s]
+    step.angular_z = loop_.lastCommand().angular.z;  // [rad/s]
+
+    clock_.advance(scenario_.control_period);
+    ++cycle_;
+    return running;
+  }
+
+  std::string outcome() const override
+  {
+    const char* text = loop_.lastOutcome();
+    return text != nullptr ? std::string(text) : std::string();
+  }
+
+private:
+  void applyEventsFor(std::size_t cycle)
+  {
+    for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
+    {
+      if (event.cycle != cycle)
+      {
+        continue;
+      }
+      if (event.action == "cancel")
+      {
+        loop_.requestCancel();
+      }
+      else if (event.action == "pause")
+      {
+        loop_.requestPause();
+      }
+      else if (event.action == "resume")
+      {
+        loop_.requestResume();
+      }
+      else if (event.action == "lose_pose")
+      {
+        pose_.setAvailable(false);
+      }
+      else if (event.action == "restore_pose")
+      {
+        pose_.setAvailable(true);
+      }
+    }
+  }
+
+  nav_test_harness::Scenario scenario_;
+
+  ControlLoop loop_;
+  ControlLoopDeps deps_;
+  FakeClockPort clock_;
+  FakePosePort pose_;
+  FakePlannerPort planner_;
+  FakeControllerPort controller_;
+  FakeRecoveryPort recovery_{ 2 };
+  FakeMissionPort mission_;
+  FakeActionPort action_;
+
+  std::size_t cycle_ = 0;
+  bool started_ = false;
+};
+
+}  // namespace testing
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_TEST_SCENARIO_DRIVER_H_

test/move_base2_scenario_test.cpp

@@ -0,0 +1,115 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — chạy toàn bộ kịch bản khai báo trong nav_test_harness/scenarios.
+ *
+ * Test này **tự tìm** file trong thư mục kịch bản và sinh một ca gtest cho mỗi file. Đó là điều kiện
+ * để giữ đúng mục tiêu của Phase 5: thêm một ca test = thêm một file YAML, không sửa C++.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#include <nav_test_harness/scenario.h>
+#include <nav_test_harness/scenario_runner.h>
+
+#include "move_base2_scenario_driver.h"
+
+namespace
+{
+using move_base2::testing::MoveBase2ScenarioDriver;
+using nav_test_harness::Scenario;
+using nav_test_harness::ScenarioReport;
+using nav_test_harness::ScenarioRunner;
+
+/// @brief Thư mục kịch bản, do CMake truyền vào — test không được đoán đường dẫn.
+std::string scenarioDir()
+{
+  return MOVE_BASE2_SCENARIO_DIR;
+}
+
+std::vector<std::string> scenarioFiles()
+{
+  std::vector<std::string> files = nav_test_harness::listScenarioFiles(scenarioDir());
+  // Sắp xếp để thứ tự chạy ổn định giữa các máy — thứ tự đọc thư mục không được đảm bảo.
+  std::sort(files.begin(), files.end());
+  return files;
+}
+
+/// @brief Nạp, chạy, và khẳng định một kịch bản.
+void runScenarioFile(const std::string& path)
+{
+  Scenario scenario;
+  std::string error;
+  ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error))
+      << "không nạp được " << path << ": " << error;
+
+  MoveBase2ScenarioDriver driver;
+  ASSERT_TRUE(driver.setup(scenario, error)) << scenario.name << ": setup thất bại: " << error;
+
+  ScenarioRunner runner;
+  const ScenarioReport report = runner.run(scenario, driver);
+
+  EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report);
+}
+
+/**
+ * @class ScenarioFixture
+ * @brief Một ca gtest cho mỗi file kịch bản, tên ca lấy từ tên file.
+ */
+class ScenarioFixture : public ::testing::TestWithParam<std::string>
+{
+};
+
+TEST_P(ScenarioFixture, Passes)
+{
+  runScenarioFile(GetParam());
+}
+
+/// @brief Tên ca test lấy từ tên file, bỏ đuôi và ký tự không hợp lệ.
+std::string caseName(const ::testing::TestParamInfo<std::string>& info)
+{
+  std::string name = info.param;
+  const std::size_t slash = name.find_last_of('/');
+  if (slash != std::string::npos)
+  {
+    name = name.substr(slash + 1);
+  }
+  const std::size_t dot = name.find_last_of('.');
+  if (dot != std::string::npos)
+  {
+    name = name.substr(0, dot);
+  }
+  for (char& c : name)
+  {
+    if (!std::isalnum(static_cast<unsigned char>(c)))
+    {
+      c = '_';
+    }
+  }
+  return name;
+}
+
+INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenarioFiles()),
+                         caseName);
+
+/// @brief Thư mục kịch bản không được rỗng — rỗng thì mọi ca ở trên biến mất mà suite vẫn xanh.
+TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty)
+{
+  const std::vector<std::string> files = scenarioFiles();
+  EXPECT_FALSE(files.empty()) << "không tìm thấy kịch bản nào trong " << scenarioDir()
+                              << " — suite sẽ xanh mà không kiểm gì cả";
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/navigation_server_test.cpp

@@ -9,10 +9,13 @@
  *********************************************************************/
 #include <gtest/gtest.h>
 
+#include <atomic>
+#include <chrono>
 #include <cmath>
 #include <cstddef>
 #include <limits>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <robot_costmap_2d/layered_costmap.h>
@@ -236,8 +239,13 @@ TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
 {
   // Host loại lệnh quá hạn theo dấu này. Lấy giờ hệ thống lúc host hỏi sẽ làm một control loop đã
   // treo vẫn trông như đang phát lệnh tươi — đúng thứ dấu thời gian sinh ra để ngăn.
+  // Stamp chỉ tiến khi đang có yêu cầu (xem test StampFreezesWhenIdle...), nên phải có goal chạy.
   Fixture fixture;
   fixture.configure();
+  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
 
   fixture.spin(1);
   EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9);
@@ -247,6 +255,49 @@ TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
   EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9);
 }
 
+TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
+{
+  // Không có yêu cầu nào thì stamp phải ĐỨNG YÊN dù control loop vẫn chạy: host publish /cmd_vel
+  // qua cửa tươi 0.05 s (amr_publiser.cpp:361), stamp tươi mỗi cycle nghĩa là amr_node phát 0 ở
+  // 20 Hz vĩnh viễn và đè chết teleop/joystick (rqt_robot_steering 10 Hz — robot chỉ nhích rồi
+  // đứng im). Bản cũ chỉ đóng dấu trong executeCycle; đây là regression đã gặp trên sim.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.spin(3);
+  EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
+      << "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
+}
+
+TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
+{
+  // Lệnh 0 cuối cùng phải qua được cửa 0.05 s của host — kết thúc mà đóng băng stamp ngay thì
+  // robot giữ nguyên vận tốc chót. Cửa ân hạn 0.5 s; hết ân hạn stamp phải đứng yên trả /cmd_vel
+  // cho teleop.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kGoalReached });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+
+  fixture.spin(4);  // IDLE -> PLANNING -> CONTROLLING -> tới đích (terminal)
+  ASSERT_FALSE(fixture.server_.loop().hasActiveRequest());
+
+  // Ngay sau khi kết thúc: còn trong ân hạn, stamp vẫn tiến để host phát lệnh dừng.
+  const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
+  fixture.spin(1);
+  EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
+      << "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
+
+  // Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
+  fixture.spin(12);
+  const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
+  fixture.spin(3);
+  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
+      << "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
+}
+
 TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
 {
   Fixture fixture;
@@ -552,6 +603,257 @@ TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity)
   EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9);
 }
 
+// ================================================================================================
+// pause / resume / cancel — host gọi từ thread khác
+//
+// OPC-UA và VDA5050 chạy thread riêng (amr_control.cpp:159, 184) và gọi thẳng ba hàm này.
+// `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi HAI cờ không nguyên tử —
+// xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Nên chúng chỉ được ghi nhận ở đây,
+// rồi chuyển xuống lõi trên control thread.
+// ================================================================================================
+
+TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
+      << "pause() đi thẳng vào lõi từ thread host";
+
+  fixture.spin(1);
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+}
+
+TEST(NavigationServerLifecycle, ResumeAfterPauseReturnsToControlling)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+
+  fixture.server_.pause();
+  fixture.spin(1);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+
+  fixture.server_.resume();
+  fixture.spin(1);
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+}
+
+TEST(NavigationServerLifecycle, PauseThenResumeWithinOneCycleEndsResumed)
+{
+  // Hai cờ đối nghịch được đặt dưới cùng một lock, nên lệnh sau luôn thắng lệnh trước — không có
+  // trạng thái "cả hai cùng false" như bản ghi hai cờ rời rạc.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  fixture.server_.resume();
+  fixture.spin(1);
+
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+}
+
+TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
+{
+  // "Tạm dừng rồi huỷ" và "huỷ rồi tạm dừng" phải cho cùng kết quả: huỷ thắng.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  fixture.server_.cancel();
+  fixture.spin(1);
+
+  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
+}
+
+TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+
+  fixture.server_.pause();
+  fixture.spin(1);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+
+  // Không có lệnh mới: cờ đã bị tiêu thụ, các cycle sau không được tự tạm dừng lại lần nữa.
+  fixture.server_.resume();
+  fixture.spin(3);
+  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused);
+}
+
+// ================================================================================================
+// Control thread
+//
+// Contract `BaseNavigation` KHÔNG có hàm spin nào: host nạp plugin, gọi initialize(), rồi chỉ tương
+// tác qua moveTo/getTwist/getFeedback. Runtime vì thế phải TỰ LÁI mình, đúng như bản cũ (thread
+// planner + action server). Thiếu control thread thì goal nằm im trong chỗ chờ vĩnh viễn — không
+// cycle nào chạy, không state nào đổi, và không log gì cả. Đó là lỗi đã thật sự xảy ra trên sim.
+// ================================================================================================
+
+TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+
+  // Ở đây KHÔNG gọi spin() của fixture: chính control thread phải đẩy state đi.
+  ASSERT_TRUE(fixture.server_.startControlThread(200.0));  // [Hz]
+
+  bool left_idle = false;
+  for (int i = 0; i < 500 && !left_idle; ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    left_idle = fixture.server_.loop().state() != NavigationState::kIdle;
+  }
+  fixture.server_.stopControlThread();
+
+  EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
+}
+
+TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
+{
+  NavigationServer server;
+  EXPECT_FALSE(server.startControlThread(20.0));
+  EXPECT_FALSE(server.controlThreadRunning());
+}
+
+TEST(NavigationServerControlThread, RefusesNonPositiveFrequency)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  EXPECT_FALSE(fixture.server_.startControlThread(0.0));
+  EXPECT_FALSE(fixture.server_.startControlThread(-5.0));
+}
+
+TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
+  EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
+
+  fixture.server_.stopControlThread();
+  fixture.server_.stopControlThread();  // không được treo hay sập
+  EXPECT_FALSE(fixture.server_.controlThreadRunning());
+}
+
+TEST(NavigationServerControlThread, DestructorStopsTheThread)
+{
+  // Thread chạm loop_, runtime_ và sensors_ mỗi cycle; huỷ chúng khi thread còn sống là hỏng ở chỗ
+  // không truy được.
+  Fixture fixture;
+  fixture.configure();
+  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
+  SUCCEED();  // destructor của fixture phải join, không treo
+}
+
+// ================================================================================================
+// Dữ liệu hiển thị — host gọi từ BỐN ros::Timer khác nhau
+// ================================================================================================
+
+TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
+{
+  // `getGlobalData` và `getLocalData` mỗi cái được gọi từ HAI timer (costmap và plan). Ghi vào
+  // `global_data_`/`local_data_` dùng chung đã gây `std::bad_alloc` rồi hỏng heap và giết tiến trình.
+  Fixture fixture;
+  fixture.configure();
+  fixture.spin(1);
+
+  robot::move_base_core::PlannerDataOutput a = fixture.server_.getGlobalData();
+  robot::move_base_core::PlannerDataOutput b = fixture.server_.getGlobalData();
+
+  a.plan.poses.clear();
+  EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
+      << "hai lần gọi trả về cùng một vùng nhớ";
+  EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
+}
+
+TEST(NavigationServerPlannerData, ConcurrentGettersDoNotCorruptEachOther)
+{
+  // Không chứng minh được không có race (cần ThreadSanitizer), nhưng chạy đúng hình dạng lời gọi
+  // của host: bốn thread cùng đọc trong khi control thread cùng ghi bộ đệm.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  ASSERT_TRUE(fixture.server_.startControlThread(200.0));
+
+  std::atomic<bool> stop{ false };
+  std::vector<std::thread> readers;
+  for (int i = 0; i < 4; ++i)
+  {
+    readers.emplace_back([&fixture, &stop, i]() {
+      while (!stop.load())
+      {
+        if (i % 2 == 0)
+        {
+          (void)fixture.server_.getGlobalData();
+        }
+        else
+        {
+          (void)fixture.server_.getLocalData();
+        }
+      }
+    });
+  }
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  stop.store(true);
+  for (auto& t : readers)
+  {
+    t.join();
+  }
+  fixture.server_.stopControlThread();
+
+  SUCCEED();
+}
+
+TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.clock_.setTime(kClockStart + 7.0);
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
+      << "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
+}
+
 int main(int argc, char** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

test/plugins/test_local_planner.cpp

@@ -20,6 +20,7 @@
  *********************************************************************/
 #include <algorithm>
 #include <cmath>
+#include <cstddef>
 #include <limits>
 #include <memory>
 #include <stdexcept>
@@ -30,7 +31,8 @@
 #include <boost/config.hpp>
 #include <boost/dll/alias.hpp>
 
-#include <robot_nav_core/base_local_planner.h>
+#include <robot_nav_2d_utils/conversions.h>
+#include <robot_nav_core2/local_planner.h>
 
 namespace move_base2
 {
@@ -44,7 +46,7 @@ constexpr double kBaseYawRate = 0.40;  ///< [rad/s]
  * @class TestLocalPlanner
  * @brief Local planner giả, hành vi cố định theo tham số dựng.
  */
-class TestLocalPlanner : public robot_nav_core::BaseLocalPlanner
+class TestLocalPlanner : public robot_nav_core2::LocalPlanner
 {
 public:
   enum class Behavior
@@ -60,46 +62,56 @@ public:
   {
   }
 
-  void initialize(std::string name, tf3::BufferCore* /*tf*/,
-                  robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
+                  std::shared_ptr<tf3::BufferCore> /*tf*/,
+                  robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
   {
     // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
-    name_ = std::move(name);
+    name_ = name;
   }
 
-  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
   {
-    return !plan.empty();
+    saw_goal_ = true;
   }
 
-  void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  void setPlan(const robot_nav_2d_msgs::Path2D& path) override
   {
-    path.clear();
+    plan_size_ = path.poses.size();
   }
 
-  void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  void getPlan(robot_nav_2d_msgs::Path2D& path) override
   {
-    path.clear();
+    path = robot_nav_2d_msgs::Path2D();
   }
 
-  bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity,
-                               robot_geometry_msgs::Twist& cmd_vel) override
+  void getGlobalPlan(robot_nav_2d_msgs::Path2D& path) override
   {
+    path = robot_nav_2d_msgs::Path2D();
+  }
+
+  robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(
+      const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
+      const robot_nav_2d_msgs::Twist2D& velocity) override
+  {
+    robot_nav_2d_msgs::Twist2DStamped cmd;
+
     switch (behavior_)
     {
       case Behavior::kThrow:
         throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
       case Behavior::kNoCommand:
-        return false;
+        // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
+        throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
       case Behavior::kNaN:
-        cmd_vel.linear.x = std::numeric_limits<double>::quiet_NaN();
-        return true;
+        cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
+        return cmd;
       case Behavior::kOk:
       case Behavior::kRefusesLimits:
         break;
     }
 
-    double linear = kBaseSpeed + velocity.linear.x;
+    double linear = kBaseSpeed + velocity.x;
     if (has_limit_forward_)
     {
       linear = std::min(linear, limit_forward_);
@@ -111,12 +123,13 @@ public:
       yaw = std::min(yaw, limit_angular_);
     }
 
-    cmd_vel.linear.x = linear;
-    cmd_vel.angular.z = yaw;
-    return true;
+    cmd.velocity.x = linear;
+    cmd.velocity.theta = yaw;
+    return cmd;
   }
 
-  bool isGoalReached() override
+  bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& /*pose*/,
+                     const robot_nav_2d_msgs::Twist2D& /*velocity*/) override
   {
     return false;
   }
@@ -168,6 +181,8 @@ public:
 private:
   Behavior behavior_;
   std::string name_;
+  std::size_t plan_size_ = 0;
+  bool saw_goal_ = false;
   double limit_forward_ = 0.0;   ///< [m/s]
   double limit_backward_ = 0.0;  ///< [m/s], âm
   double limit_angular_ = 0.0;   ///< [rad/s]
@@ -175,32 +190,32 @@ private:
   bool has_limit_angular_ = false;
 };
 
-robot_nav_core::BaseLocalPlanner::Ptr createOk()
+robot_nav_core2::LocalPlanner::Ptr createOk()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
+robot_nav_core2::LocalPlanner::Ptr createSecondary()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
+robot_nav_core2::LocalPlanner::Ptr createNoCommand()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createNaN()
+robot_nav_core2::LocalPlanner::Ptr createNaN()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
+robot_nav_core2::LocalPlanner::Ptr createThrowing()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
 }
 
-robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
+robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
 {
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
 }

test/walking_skeleton_test.cpp

@@ -1002,6 +1002,115 @@ TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
       << join(fixture.states());
 }
 
+// ================================================================================================
+// Preempt — goal mới thay goal cũ NGAY
+//
+// Bấm goal mới nghĩa là goal cũ không còn muốn nữa. Xếp hàng chờ robot đi hết chặng cũ là hành vi
+// không ai mong đợi, và mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2).
+// ================================================================================================
+
+TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
+{
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  fixture.stepOnce();  // IDLE -> PLANNING
+  fixture.stepOnce();  // -> CONTROLLING
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  const std::size_t plans_before = fixture.planner_.makePlanCount();
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
+      << "goal mới nằm chờ thay vì thay goal cũ ngay";
+  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
+}
+
+TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
+{
+  Fixture fixture;
+  fixture.planner_.setLatencyCycles(50);  // plan cũ còn lâu mới xong
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  fixture.stepOnce();
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
+}
+
+TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
+{
+  // Bất biến quan trọng nhất: báo kết quả ĐÚNG MỘT LẦN và ĐÚNG ID. Preempt báo kết quả chặng cũ và
+  // nhận chặng mới trong cùng một cycle — dùng nhầm id thì mission layer mất dấu cả hai chặng.
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
+  fixture.stepOnce();
+  fixture.stepOnce();
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
+}
+
+TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
+{
+  // Preempt không được để lại trạng thái nửa vời: chặng mới phải chạy tới cùng như bình thường.
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk,
+                                 ControllerScript::kGoalReached});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
+  fixture.stepOnce();
+  fixture.stepOnce();
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
+  fixture.run();
+
+  EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states());
+  EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 1u);
+}
+
+TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
+{
+  ControlLoopConfig config = baseConfig();
+  config.state_machine.planner_patience = 0.05;  // [s] vào recovery nhanh
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail});
+  fixture.recovery_.setScript({RecoveryScript::kRunning});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  for (int i = 0; i < 6 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
+  {
+    fixture.stepOnce();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering) << join(fixture.states());
+
+  fixture.planner_.setScript({PlannerScript::kOk});
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);