first commit

test/action_runner_test.cpp

@@ -0,0 +1,363 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm ActionRunner: định tuyến theo actionType, vòng đời tick, và timeout tầng 1.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/action_runner.h>
+
+#include "fake_ports.h"
+
+namespace
+{
+using move_base2::ActionHandler;
+using move_base2::ActionRunner;
+using move_base2::ActionTick;
+using move_base2::testing::FakeClockPort;
+
+robot_protocol_msgs::Action makeAction(const std::string& type, const std::string& id = "a1")
+{
+  robot_protocol_msgs::Action action;
+  action.actionType = type;
+  action.actionId = id;
+  return action;
+}
+
+/**
+ * @brief Handler 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 action mới mà không sửa
+ * file nào trong `src/` của gói.
+ */
+class ScriptedHandler final : public ActionHandler
+{
+public:
+  explicit ScriptedHandler(std::vector<std::string> types) : types_(std::move(types))
+  {
+  }
+
+  bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
+  {
+    name_ = name;
+    return configure_ok;
+  }
+
+  std::vector<std::string> supportedActionTypes() const override
+  {
+    return types_;
+  }
+
+  bool start(const robot_protocol_msgs::Action& action, const robot::Time& /*now*/) override
+  {
+    ++start_count;
+    last_action_id = action.actionId;
+    return start_ok;
+  }
+
+  ActionTick update(const robot::Time& /*now*/) override
+  {
+    ++update_count;
+    ActionTick tick;
+    tick.status = next_status;
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count;
+  }
+
+  bool configure_ok = true;
+  bool start_ok = true;
+  ActionTick::Status next_status = ActionTick::Status::kRunning;
+
+  int start_count = 0;
+  int update_count = 0;
+  int cancel_count = 0;
+  std::string last_action_id;
+
+private:
+  std::vector<std::string> types_;
+  std::string name_;
+};
+
+struct Rig
+{
+  Rig()
+  {
+    runner.setClock(&clock);
+  }
+
+  bool load(const std::string& ns)
+  {
+    runner.setNamespace(ns);
+    robot::NodeHandle nh;
+    return runner.configure(nh);
+  }
+
+  FakeClockPort clock{1000.0};
+  ActionRunner runner;
+};
+
+// ------------------------------------------------------------------------------------------------
+// Đăng ký thủ công — kiểm định tuyến và vòng đời mà không cần .so
+// ------------------------------------------------------------------------------------------------
+
+TEST(ActionRunner, RoutesByActionType)
+{
+  Rig rig;
+  auto pick = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  auto drop = std::make_shared<ScriptedHandler>(std::vector<std::string>{"drop"});
+  ASSERT_TRUE(rig.runner.registerHandler(pick));
+  ASSERT_TRUE(rig.runner.registerHandler(drop));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  ASSERT_TRUE(rig.runner.start(makeAction("drop", "d7")));
+
+  EXPECT_EQ(drop->start_count, 1);
+  EXPECT_EQ(pick->start_count, 0);
+  EXPECT_EQ(drop->last_action_id, "d7");
+}
+
+TEST(ActionRunner, OneHandlerCanTakeSeveralTypes)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick", "drop"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_TRUE(rig.runner.start(makeAction("pick")));
+  EXPECT_TRUE(rig.runner.start(makeAction("drop")));
+  EXPECT_EQ(handler->start_count, 2);
+}
+
+TEST(ActionRunner, RejectsDuplicateActionType)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+
+  // Hai handler cùng nhận một type thì định tuyến phụ thuộc thứ tự nạp — phải từ chối, không ghi đè.
+  EXPECT_FALSE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+}
+
+TEST(ActionRunner, RejectsHandlerWithoutTypes)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.runner.registerHandler(std::make_shared<ScriptedHandler>(
+      std::vector<std::string>{})));
+  EXPECT_FALSE(rig.runner.registerHandler(nullptr));
+}
+
+TEST(ActionRunner, UnknownActionTypeFailsStart)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("charge")));
+}
+
+TEST(ActionRunner, EmptyActionTypeFailsStart)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("")));
+}
+
+TEST(ActionRunner, StartBeforeConfigureFails)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+}
+
+TEST(ActionRunner, HandlerRefusingStartIsReported)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  handler->start_ok = false;
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+  // Không được tick tiếp sau khi start hỏng.
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+  EXPECT_EQ(handler->update_count, 0);
+}
+
+TEST(ActionRunner, UpdateWithoutActiveActionFailsInsteadOfCrashing)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  const ActionTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, ActionTick::Status::kFailed);
+  EXPECT_FALSE(tick.message.empty());
+}
+
+TEST(ActionRunner, TicksUntilHandlerFinishes)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+  ASSERT_TRUE(rig.runner.start(makeAction("pick")));
+
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  handler->next_status = ActionTick::Status::kSucceeded;
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
+
+  // Sau khi kết thúc, action không còn active: tick thêm là lỗi thứ tự gọi, không phải kRunning.
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+  EXPECT_EQ(handler->update_count, 3);
+}
+
+TEST(ActionRunner, CancelReachesHandlerAndClearsActive)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+  ASSERT_TRUE(rig.runner.start(makeAction("pick")));
+
+  rig.runner.cancel();
+
+  EXPECT_EQ(handler->cancel_count, 1);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+}
+
+TEST(ActionRunner, CancelWithoutActiveActionIsSafe)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_empty"));
+  rig.runner.cancel();
+  SUCCEED();
+}
+
+TEST(ActionRunner, ConfigureRequiresClock)
+{
+  ActionRunner runner;  // không setClock
+  robot::NodeHandle nh;
+  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort thì handler không có mốc timeout";
+}
+
+TEST(ActionRunner, EmptyHandlerListIsValid)
+{
+  // Hệ không có thiết bị nào: mọi mission đều nav-only, và ControlLoop::submit đã từ chối yêu cầu
+  // mang action ngay tại cửa.
+  Rig rig;
+  EXPECT_TRUE(rig.load("actions_empty"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+}
+
+// ------------------------------------------------------------------------------------------------
+// Nạp thật qua Boost.DLL — đúng đường runtime đi
+// ------------------------------------------------------------------------------------------------
+
+TEST(ActionRunner, LoadsHandlerPluginFromConfig)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+
+  ASSERT_EQ(rig.runner.handlerCount(), 1u);
+  EXPECT_NE(rig.runner.find("wait"), nullptr);
+  EXPECT_NE(rig.runner.find("pick"), nullptr);
+  EXPECT_EQ(rig.runner.find("charge"), nullptr);
+}
+
+TEST(ActionRunner, LoadedHandlerRunsForConfiguredDuration)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+  ASSERT_TRUE(rig.runner.start(makeAction("wait", "w1")));  // duration: 2.0 s
+
+  rig.clock.advance(1.0);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  rig.clock.advance(1.0);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
+}
+
+TEST(ActionRunner, HandlerTimesOutOnItsOwnWithoutStateMachineHelp)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_slow"));  // hang: true, timeout 3 s
+  ASSERT_TRUE(rig.runner.start(makeAction("wait")));
+
+  rig.clock.advance(2.0);
+  ASSERT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  rig.clock.advance(1.5);
+  const ActionTick tick = rig.runner.update();
+
+  // Timeout TẦNG 1: handler tự chịu trách nhiệm, không dựa vào action_patience (mặc định tắt).
+  EXPECT_EQ(tick.status, ActionTick::Status::kFailed);
+  EXPECT_NE(tick.message.find("timeout"), std::string::npos);
+}
+
+TEST(ActionRunner, HangingHandlerWithoutTimeoutIsRejectedAtConfigure)
+{
+  // Treo + tắt timeout = action chạy vĩnh viễn. Contract cấm, nên phải chặn lúc khởi động.
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_hang_forever"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, MissingLibraryPathFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_missing_library"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, HandlerWithTimeoutBelowDurationIsRejectedAtConfigure)
+{
+  // Cấu hình khiến action LUÔN hỏng vì timeout — phải chặn lúc khởi động, không phải lúc chạy.
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_bad"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, ConfigureTwiceRejected)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(rig.runner.configure(nh));
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/config/move_base2_params.yaml

@@ -0,0 +1,171 @@
+# Config CHỈ dùng cho test của gói. Bản runtime nằm ở `pnkx_nav_core/config/` (C2).
+#
+# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/move_base2/test/config
+
+# --- Tham số runtime, dùng cho config_validation_test ----------------------------------------
+move_base2:
+  controller_frequency: 20.0      # [Hz]
+  planner_frequency: 0.0          # [Hz] 0 = chỉ lập plan khi cần
+  planner_timeout: 5.0            # [s]
+
+  planner_patience: 5.0           # [s]
+  controller_patience: 15.0       # [s]
+  oscillation_timeout: 0.0        # [s] 0 = tắt
+  oscillation_distance: 0.5       # [m]
+  action_patience: 0.0            # [s] 0 = tắt; lưới cuối, không phải cơ chế timeout chính
+  max_planning_retries: -1        # < 0 = không giới hạn
+  recovery_behavior_enabled: true
+
+  max_vel_x: 0.5                  # [m/s] tiến
+  min_vel_x: -0.2                 # [m/s] lùi, ÂM
+  max_vel_theta: 1.0              # [rad/s]
+  acc_lim_x: 1.0                  # [m/s^2]
+  acc_lim_theta: 2.0              # [rad/s^2]
+
+  global_frame: map
+  robot_base_frame: base_link
+
+  sensors:
+    laser_sor_enabled: true         # lọc outlier laser TRƯỚC khi vào costmap
+    laser_sor_mean_k: 8             # [điểm] số láng giềng gần nhất dùng để ước lượng
+    laser_sor_stddev_mul: 1.5       # [-] ngưỡng = mean + hệ_số * stddev
+
+  recovery_namespace: recovery
+  action_namespace: actions
+  mission_namespace: mission_adapters
+
+  position:
+    base_global_planner: TestGlobalPlanner
+    base_local_planner: TestLocalPlanner
+    xy_goal_tolerance: 0.15       # [m]
+    yaw_goal_tolerance: 0.10      # [rad]
+
+  docking:
+    base_global_planner: TestDockPlanner
+    base_local_planner: TestLocalPlanner
+    xy_goal_tolerance: 0.02       # [m] ghép nối cần chính xác hơn nhiều
+    yaw_goal_tolerance: 0.02      # [rad]
+
+# --- Cấu hình sai, dùng cho test đường lỗi -----------------------------------------------------
+move_base2_bad_frequency:
+  controller_frequency: 0.0       # phải bị từ chối
+
+move_base2_bad_frames:
+  controller_frequency: 20.0
+  global_frame: base_link         # trùng robot_base_frame -> pose robot luôn là gốc toạ độ
+  robot_base_frame: base_link
+  position:
+    base_local_planner: TestLocalPlanner
+
+move_base2_no_planner:
+  controller_frequency: 20.0      # không profile nào có base_local_planner
+
+move_base2_bad_sensors:
+  controller_frequency: 20.0
+  position:
+    base_local_planner: TestLocalPlanner
+  sensors:
+    laser_sor_enabled: true
+    laser_sor_mean_k: 1           # < 2 -> vô nghĩa, phải bị từ chối
+
+# --- Recovery behavior cho recovery_runner_test ------------------------------------------------
+#
+# Chỉ khai behavior họ kNone: họ velocity cần Costmap2DROBOT thật (TF + chuỗi layer) nên được kiểm
+# ở tầng tích hợp, không kiểm bằng fake ở đây.
+recovery:
+  behaviors:
+    - {name: wait_short, type: WaitRecovery}
+    - {name: wait_long,  type: WaitRecovery}
+
+  wait_short:
+    wait_duration: 1.0            # [s]
+  wait_long:
+    wait_duration: 5.0            # [s]
+    timeout: 3.0                  # [s] cố ý NGẮN HƠN wait_duration -> lượt này luôn kết thúc bằng timeout
+
+recovery_empty:
+  behaviors: []
+
+recovery_missing_library:
+  behaviors:
+    - {name: ghost, type: GhostRecovery}
+
+WaitRecovery:
+  library_path: librecovery_core_wait_recovery
+
+# GhostRecovery cố ý KHÔNG khai library_path.
+
+# --- Action handler cho action_runner_test -----------------------------------------------------
+actions:
+  handlers:
+    - {name: noop, type: NoopActionHandler}
+  noop:
+    action_types: [wait, pick, drop]
+    duration: 2.0                 # [s]
+    timeout: 10.0                 # [s] tầng 1 — trách nhiệm của chính handler
+
+actions_slow:
+  handlers:
+    - {name: noop, type: NoopActionHandler}
+  noop:
+    action_types: [wait]
+    hang: true                    # mô phỏng thiết bị không bao giờ trả lời
+    timeout: 3.0                  # [s] handler tự cắt, không chờ state machine
+
+actions_hang_forever:
+  handlers:
+    - {name: noop, type: NoopActionHandler}
+  noop:
+    action_types: [wait]
+    hang: true
+    timeout: 0.0                  # tắt timeout + treo = chạy vĩnh viễn -> phải bị chặn lúc configure
+
+actions_bad:
+  handlers:
+    - {name: noop, type: NoopActionHandler}
+  noop:
+    action_types: [wait]
+    duration: 10.0                # [s]
+    timeout: 5.0                  # [s] <= duration -> action LUÔN hỏng; phải bị chặn lúc configure
+
+actions_empty:
+  handlers: []
+
+actions_missing_library:
+  handlers:
+    - {name: ghost, type: GhostActionHandler}
+
+NoopActionHandler:
+  library_path: libmove_base2_noop_action_handler
+
+# --- Global planner giả cho planner_runner_test -------------------------------------------------
+#
+# Bốn alias cùng nằm trong một thư viện; mỗi alias là một hành vi mà PlannerRunner phải xử lý đúng.
+TestPlannerOk:
+  library_path: libmove_base2_test_global_planner
+TestPlannerEmptyPlan:
+  library_path: libmove_base2_test_global_planner
+TestPlannerThrowing:
+  library_path: libmove_base2_test_global_planner
+TestPlannerInitFails:
+  library_path: libmove_base2_test_global_planner
+
+# TestPlannerMissingLibrary cố ý KHÔNG khai library_path.
+
+# GhostActionHandler cố ý KHÔNG khai library_path.
+
+# --- Local planner giả cho controller_runner_test ------------------------------------------------
+TestControllerOk:
+  library_path: libmove_base2_test_local_planner
+TestControllerSecondary:
+  library_path: libmove_base2_test_local_planner
+TestControllerNoCommand:
+  library_path: libmove_base2_test_local_planner
+TestControllerNaN:
+  library_path: libmove_base2_test_local_planner
+TestControllerThrowing:
+  library_path: libmove_base2_test_local_planner
+TestControllerRefusesLimits:
+  library_path: libmove_base2_test_local_planner
+
+# TestControllerMissing cố ý KHÔNG khai library_path.

test/config_validation_test.cpp

@@ -0,0 +1,245 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm việc đọc và validate cấu hình runtime.
+ *
+ * Bản cũ đọc param không kiểm miền giá trị: một `controller_frequency` bằng 0 hay một
+ * `max_planning_retries` âm đi thẳng vào vòng điều khiển. Ở đây cấu hình sai phải chặn runtime khởi
+ * động, chứ không phải hiện ra thành hành vi lạ lúc chạy.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <string>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/config/move_base2_config.h>
+
+namespace
+{
+using move_base2::MoveBase2Config;
+
+MoveBase2Config loadFrom(const std::string& ns)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, ns);
+
+  MoveBase2Config config;
+  config.fromNodeHandle(nh);
+  return config;
+}
+
+/// Điền con số mà RecoveryRunner báo lại sau khi nạp behavior — bước bắt buộc trước validate().
+MoveBase2Config withRecoveryCount(MoveBase2Config config, std::size_t count)
+{
+  config.state_machine.recovery_behavior_count = count;
+  return config;
+}
+
+/// Cấu hình tối thiểu hợp lệ, dựng bằng tay (không qua YAML).
+MoveBase2Config minimalValid()
+{
+  MoveBase2Config config;
+  config.position.local_planner_name = "AnyLocalPlanner";
+  config.state_machine.recovery_behavior_count = 1;
+  return config;
+}
+
+TEST(MoveBase2Config, DefaultsAreValid)
+{
+  const MoveBase2Config config = minimalValid();
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
+}
+
+TEST(MoveBase2Config, RejectsRecoveryEnabledWithNoBehaviorLoaded)
+{
+  // Ràng buộc thứ tự khởi tạo: RecoveryRunner không nạp được behavior nào mà
+  // recovery_behavior_enabled vẫn true thì mọi lỗi dẫn thẳng tới ABORTED — chặn ngay lúc khởi động.
+  MoveBase2Config config = minimalValid();
+  config.state_machine.recovery_behavior_count = 0;
+  config.state_machine.recovery_enabled = true;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, ReadsSensorGatewayBlockFromYaml)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_TRUE(config.sensors.laser_sor_enabled);
+  EXPECT_EQ(config.sensors.laser_sor_mean_k, 8);
+  EXPECT_DOUBLE_EQ(config.sensors.laser_sor_stddev_mul, 1.5);
+}
+
+TEST(MoveBase2Config, LaserFilterIsOffWhenTheSensorsBlockIsAbsent)
+{
+  // Cây config gen-1 không có khoá nào cho bộ lọc. Thiếu khoá phải giữ hành vi host ROS đang chạy —
+  // tức là KHÔNG lọc — chứ không phải âm thầm bật một bộ lọc lên.
+  const MoveBase2Config config = loadFrom("move_base2_no_planner");
+
+  EXPECT_FALSE(config.sensors.laser_sor_enabled);
+}
+
+TEST(MoveBase2Config, RejectsSensorFilterParametersOutOfRange)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_sensors"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("laser_sor_mean_k"), std::string::npos) << error;
+}
+
+TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_DOUBLE_EQ(config.controller_frequency, 20.0);
+  EXPECT_DOUBLE_EQ(config.planner_timeout, 5.0);
+
+  EXPECT_DOUBLE_EQ(config.state_machine.planner_patience, 5.0);
+  EXPECT_DOUBLE_EQ(config.state_machine.controller_patience, 15.0);
+  EXPECT_EQ(config.state_machine.max_planning_retries, -1);
+  EXPECT_TRUE(config.state_machine.recovery_enabled);
+
+  EXPECT_DOUBLE_EQ(config.velocity.max_vel_x, 0.5);
+  EXPECT_DOUBLE_EQ(config.velocity.min_vel_x, -0.2);
+  EXPECT_DOUBLE_EQ(config.velocity.max_accel_x, 1.0);
+
+  EXPECT_EQ(config.global_frame, "map");
+  EXPECT_EQ(config.robot_base_frame, "base_link");
+  EXPECT_EQ(config.recovery_namespace, "recovery");
+}
+
+TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
+  EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
+  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.15);
+
+  // Ghép nối cần sai số chặt hơn nhiều — đây chính là thứ sáu entry point cũ khác nhau ở.
+  EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
+  EXPECT_DOUBLE_EQ(config.docking.default_xy_tolerance, 0.02);
+  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.02);
+}
+
+TEST(MoveBase2Config, LoadedConfigValidates)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
+}
+
+TEST(MoveBase2Config, RejectsZeroControllerFrequency)
+{
+  const MoveBase2Config config = loadFrom("move_base2_bad_frequency");
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("controller_frequency"), std::string::npos);
+}
+
+TEST(MoveBase2Config, RejectsAbsurdlyHighControllerFrequency)
+{
+  MoveBase2Config config = minimalValid();
+  config.controller_frequency = 5000.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, RejectsGlobalFrameEqualToBaseFrame)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_frames"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("global_frame"), std::string::npos);
+}
+
+TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_no_planner"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error))
+      << "cấu hình này lúc chạy sẽ từ chối MỌI yêu cầu — phải chặn ngay lúc khởi động";
+}
+
+TEST(MoveBase2Config, RejectsNonPositiveToleranceOnConfiguredProfile)
+{
+  MoveBase2Config config = minimalValid();
+  config.position.default_xy_tolerance = 0.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("xy_goal_tolerance"), std::string::npos);
+}
+
+TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
+{
+  MoveBase2Config config = minimalValid();
+  config.state_machine.oscillation_timeout = 5.0;
+  config.state_machine.oscillation_distance = 0.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("oscillation"), std::string::npos);
+}
+
+TEST(MoveBase2Config, PropagatesVelocityValidationFailure)
+{
+  MoveBase2Config config = minimalValid();
+  config.velocity.max_vel_x = -1.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, ControlLoopConfigDerivesPeriodFromFrequency)
+{
+  MoveBase2Config config = minimalValid();
+  config.controller_frequency = 25.0;
+
+  const auto loop_config = config.toControlLoopConfig();
+
+  EXPECT_NEAR(loop_config.nominal_control_period, 0.04, 1e-9);  // [s]
+  EXPECT_EQ(loop_config.position.local_planner_name, "AnyLocalPlanner");
+}
+
+TEST(MoveBase2Config, RecoveryBehaviorCountIsNotReadFromYaml)
+{
+  // Số behavior phải là số nạp được THẬT, do RecoveryRunner báo lại. Đọc từ YAML thì một behavior
+  // hỏng vẫn khiến state machine tin là còn đường phục hồi.
+  const MoveBase2Config config = loadFrom("move_base2");
+  EXPECT_EQ(config.state_machine.recovery_behavior_count, 0u);
+}
+
+TEST(MoveBase2Config, DescribeMentionsEveryGroup)
+{
+  const std::string text = loadFrom("move_base2").describe();
+
+  EXPECT_NE(text.find("controller_frequency"), std::string::npos);
+  EXPECT_NE(text.find("position"), std::string::npos);
+  EXPECT_NE(text.find("docking"), std::string::npos);
+  EXPECT_NE(text.find("planner_patience"), std::string::npos);
+  EXPECT_NE(text.find("max_vel_x"), std::string::npos);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/controller_runner_test.cpp

@@ -0,0 +1,381 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test ControllerRunner: nạp plugin thật qua Boost.DLL, trần vận tốc phải tới được
+ * plugin, và mọi đường lỗi phải trả về "không có lệnh" chứ không để dữ liệu hỏng đi tiếp.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <cstdint>
+#include <cstdlib>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/controller_runner.h>
+
+namespace
+{
+using move_base2::ControllerRunner;
+
+/// [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;
+/// [rad/s]
+constexpr double kBaseYawRate = 0.40;
+
+/**
+ * @brief Con trỏ costmap giả.
+ *
+ * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy
+ * đủ). An toàn ở đây vì `test_local_planner.cpp` không alias nào chạm vào con trỏ này — nó chỉ đi
+ * qua `initialize()` rồi bị bỏ. Đường có costmap thật thuộc test tích hợp (Phase 5).
+ */
+robot_costmap_2d::Costmap2DROBOT* dummyCostmap()
+{
+  static std::uintptr_t placeholder = 0;
+  return reinterpret_cast<robot_costmap_2d::Costmap2DROBOT*>(&placeholder);
+}
+
+std::vector<robot_geometry_msgs::PoseStamped> makePlan(std::size_t poses = 3)
+{
+  std::vector<robot_geometry_msgs::PoseStamped> plan;
+  for (std::size_t i = 0; i < poses; ++i)
+  {
+    robot_geometry_msgs::PoseStamped pose;
+    pose.header.frame_id = "map";
+    pose.pose.position.x = static_cast<double>(i);  // [m]
+    pose.pose.orientation.w = 1.0;
+    plan.push_back(pose);
+  }
+  return plan;
+}
+
+robot_geometry_msgs::Vector3 vec(double x, double y = 0.0, double z = 0.0)
+{
+  robot_geometry_msgs::Vector3 v;
+  v.x = x;
+  v.y = y;
+  v.z = z;
+  return v;
+}
+
+robot_geometry_msgs::Twist twist(double vx, double wz = 0.0)
+{
+  robot_geometry_msgs::Twist t;
+  t.linear.x = vx;   // [m/s]
+  t.angular.z = wz;  // [rad/s]
+  return t;
+}
+
+class Fixture
+{
+public:
+  explicit Fixture(const std::string& name = "TestControllerOk")
+  {
+    robot::NodeHandle nh;
+    std::string error;
+    ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error);
+    error_ = error;
+  }
+
+  bool ok() const
+  {
+    return ok_;
+  }
+
+  const std::string& error() const
+  {
+    return error_;
+  }
+
+  ControllerRunner runner_;
+
+private:
+  bool ok_ = false;
+  std::string error_;
+};
+
+}  // namespace
+
+// ================================================================================================
+// Cấu hình và nạp plugin
+// ================================================================================================
+
+TEST(ControllerRunner, RefusesNullCostmap)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error));
+  EXPECT_FALSE(error.empty());
+  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.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+}
+
+TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 1U);
+}
+
+TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
+{
+  // swapPlanner chạy ở cửa vào mỗi yêu cầu (profile position/docking/...). Đổi qua lại không được
+  // dlopen lại.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U);
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
+}
+
+TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.swapPlanner("TestControllerMissing"));
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+}
+
+TEST(ControllerRunner, SwapBeforeConfigureIsRefused)
+{
+  ControllerRunner runner;
+  EXPECT_FALSE(runner.swapPlanner("TestControllerOk"));
+}
+
+// ================================================================================================
+// Trần vận tốc — đường tầng an toàn hạ tốc độ robot (bước 12)
+// ================================================================================================
+
+TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
+{
+  // Nếu lời gọi này không tới được plugin thì tầng an toàn yêu cầu giảm tốc mà robot vẫn chạy
+  // nguyên tốc độ planner — và không có dấu hiệu nào cả.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  ASSERT_NEAR(cmd.linear.x, kBaseSpeed, 1e-9);
+
+  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10)));  // [m/s]
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
+}
+
+TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  ASSERT_NEAR(cmd.angular.z, kBaseYawRate, 1e-9);
+
+  ASSERT_TRUE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.15)));  // [rad/s]
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.angular.z, 0.15, 1e-9);
+}
+
+TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
+{
+  // Thứ tự khởi tạo không do move_base2 quyết: host có thể đặt trần trước khi controller được nạp.
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  EXPECT_TRUE(runner.setTwistLinear(vec(0.08)));  // [m/s], chưa có controller nào
+  ASSERT_TRUE(runner.swapPlanner("TestControllerOk"));
+  ASSERT_TRUE(runner.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(runner.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
+}
+
+TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
+{
+  // Trần thuộc về YÊU CẦU chứ không thuộc về instance planner. Instance mới không biết gì về trần
+  // đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.07)));  // [m/s]
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
+}
+
+TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
+{
+  // Host phải biết trần của nó không có hiệu lực, thay vì tưởng đã đặt được.
+  Fixture fixture("TestControllerRefusesLimits");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(0.10)));
+  EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
+}
+
+TEST(ControllerRunner, NonFiniteLimitIsRejected)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  const double nan = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(nan)));
+  EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, nan)));
+}
+
+// ================================================================================================
+// Vận tốc đo được
+// ================================================================================================
+
+TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
+{
+  // Interface gen-1 nhận vận tốc hiện tại làm tham số của computeVelocityCommands. Bản cũ đưa nó
+  // vào bằng con trỏ tới bộ nhớ host ghi (`setOdom(&odometry_)`) — một data race không có gì bảo
+  // vệ. Ở đây truyền theo giá trị.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  fixture.runner_.setMeasuredVelocity(twist(0.30));  // [m/s]
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
+}
+
+TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
+{
+  // Nhiều local planner dùng vận tốc hiện tại làm mốc giới hạn gia tốc; NaN ở đó lan ra toàn bộ
+  // cost function.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  fixture.runner_.setMeasuredVelocity(twist(0.20));
+  fixture.runner_.setMeasuredVelocity(twist(std::numeric_limits<double>::quiet_NaN()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.20, 1e-9);
+}
+
+// ================================================================================================
+// Đường lỗi
+// ================================================================================================
+
+TEST(ControllerRunner, EmptyPlanIsRefused)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setPlan({}));
+}
+
+TEST(ControllerRunner, SetPlanWithoutAControllerFails)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  EXPECT_FALSE(runner.setPlan(makePlan()));
+}
+
+TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(runner.computeVelocityCommands(cmd));
+}
+
+TEST(ControllerRunner, PluginReturningNoCommandIsPassedThroughAsFalse)
+{
+  Fixture fixture("TestControllerNoCommand");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+}
+
+TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
+{
+  // VelocityArbiter cũng chặn NaN, nhưng chặn tại nguồn cho biết ĐÚNG plugin nào đang trả dữ liệu
+  // hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
+  Fixture fixture("TestControllerNaN");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
+}
+
+TEST(ControllerRunner, ExceptionFromThePluginIsContained)
+{
+  Fixture fixture("TestControllerThrowing");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_NO_THROW({ EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); });
+}
+
+TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
+{
+  // Bên gọi dùng lại cùng một biến qua nhiều cycle. Trả false mà để nguyên lệnh cũ trong đó là mời
+  // tầng trên phát lại một lệnh đã hết hạn.
+  Fixture fixture("TestControllerNoCommand");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  robot_geometry_msgs::Twist cmd = twist(9.0, 9.0);
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.0, 1e-9);
+  EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
+}
+
+int main(int argc, char** argv)
+{
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/fake_ports.h

@@ -0,0 +1,870 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — hiện thực giả của các port, kịch bản hoá bằng chuỗi kết quả định sẵn.
+ *
+ * Đặt trong test/ của chính move_base2 chứ không đặt trong nav_test_harness: các fake này hiện thực
+ * port CỦA move_base2, nếu để trong harness thì harness phải phụ thuộc ngược vào move_base2 và
+ * chiều phụ thuộc một chiều bị phá vỡ. Phần fake thực sự dùng chung (đồng hồ, costmap, pose,
+ * kiểm va chạm, kịch bản) nằm ở nav_test_harness.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_TEST_FAKE_PORTS_H_
+#define MOVE_BASE2_TEST_FAKE_PORTS_H_
+
+#include <cstddef>
+#include <cstdint>
+#include <map>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <robot/time.h>
+
+#include <move_base2/ports/action_port.h>
+#include <move_base2/ports/clock_port.h>
+#include <move_base2/ports/controller_port.h>
+#include <move_base2/ports/mission_port.h>
+#include <move_base2/ports/planner_port.h>
+#include <move_base2/ports/pose_port.h>
+#include <move_base2/ports/recovery_port.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+/// @brief Kết quả một lần lập plan trong kịch bản.
+enum class PlannerScript
+{
+  kOk,     ///< Trả plan hợp lệ.
+  kFail,   ///< makePlan trả false.
+  kEmpty   ///< makePlan trả true nhưng plan rỗng — bẫy front()/back() trên vector rỗng.
+};
+
+/// @brief Kết quả một lần gọi controller trong kịch bản.
+enum class ControllerScript
+{
+  kOk,           ///< Sinh lệnh hợp lệ.
+  kFail,         ///< Không sinh được lệnh.
+  kGoalReached,  ///< Báo đã tới đích.
+  kNaN,          ///< Sinh lệnh chứa NaN — phải bị bộ trọng tài chặn.
+  kTooFast       ///< Sinh lệnh vượt trần vận tốc — phải bị clamp.
+};
+
+/// @brief Kết quả một tick recovery trong kịch bản.
+enum class RecoveryScript
+{
+  kRunning,
+  kSucceeded,
+  kFailed
+};
+
+/// @brief Kết quả một tick action trong kịch bản (D8).
+enum class ActionScript
+{
+  kRunning,
+  kSucceeded,
+  kFailed
+};
+
+// ------------------------------------------------------------------------------------------------
+
+/// @brief Đồng hồ do test điều khiển, chuyển tiếp một FakeClock của harness qua ClockPort.
+class FakeClockPort final : public ClockPort
+{
+public:
+  explicit FakeClockPort(double start_sec = 1000.0) : now_(start_sec)
+  {
+  }
+
+  robot::Time now() const override
+  {
+    return now_;
+  }
+
+  /// @param seconds [s] Lượng thời gian trôi. Giá trị âm bị bỏ qua.
+  void advance(double seconds)
+  {
+    if (seconds > 0.0)
+    {
+      now_ = robot::Time(now_.toSec() + seconds);
+    }
+  }
+
+  void setTime(double seconds)
+  {
+    now_ = robot::Time(seconds);
+  }
+
+private:
+  robot::Time now_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakePosePort final : public PosePort
+{
+public:
+  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
+  {
+    ++call_count_;
+    if (!available_)
+    {
+      return false;
+    }
+    pose = pose_;
+    return true;
+  }
+
+  /// @param x,y [m]
+  void setPosition(double x, double y)
+  {
+    pose_.header.frame_id = "map";
+    pose_.pose.position.x = x;
+    pose_.pose.position.y = y;
+    pose_.pose.orientation.w = 1.0;
+  }
+
+  /// @brief false = mô phỏng TF thiếu/stale.
+  void setAvailable(bool available)
+  {
+    available_ = available;
+  }
+
+  std::size_t callCount() const
+  {
+    return call_count_;
+  }
+
+private:
+  robot_geometry_msgs::PoseStamped pose_;
+  bool available_ = true;
+  mutable std::size_t call_count_ = 0;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakePlannerPort final : public PlannerPort
+{
+public:
+  bool swapPlanner(const std::string& planner_name) override
+  {
+    if (!swap_succeeds_)
+    {
+      return false;
+    }
+    active_ = planner_name;
+    ++swap_count_;
+    return true;
+  }
+
+  bool startPlan(const robot_geometry_msgs::PoseStamped& /*start*/,
+                 const robot_geometry_msgs::PoseStamped& goal,
+                 const robot_protocol_msgs::Order* order, std::uint64_t tag) override
+  {
+    if (in_flight_)
+    {
+      return false;  // Đúng như PlannerRunner: một lượt tại một thời điểm.
+    }
+
+    ++make_plan_count_;
+    saw_order_ = saw_order_ || order != nullptr;
+
+    in_flight_ = true;
+    pending_tag_ = tag;
+    pending_goal_ = goal;
+    cycles_left_ = latency_cycles_;
+    pending_action_ = nextAction();
+    return true;
+  }
+
+  bool isPlanning() const override
+  {
+    return in_flight_;
+  }
+
+  bool pollPlan(PlanResult& result) override
+  {
+    if (!in_flight_)
+    {
+      return false;
+    }
+    if (cycles_left_ > 0)
+    {
+      --cycles_left_;
+      return false;  // Còn "đang tính" — bên gọi phải thấy kBusy.
+    }
+
+    in_flight_ = false;
+    result.tag = pending_tag_;
+    result.plan.clear();
+
+    switch (pending_action_)
+    {
+      case PlannerScript::kFail:
+        result.succeeded = false;
+        return true;
+      case PlannerScript::kEmpty:
+        // Contract nói thành công phải kèm plan không rỗng; fake cố ý vi phạm để kiểm guard của
+        // bên gọi.
+        result.succeeded = true;
+        return true;
+      case PlannerScript::kOk:
+        break;
+    }
+
+    result.succeeded = true;
+    result.plan.push_back(pending_goal_);
+    return true;
+  }
+
+  void cancelPlan() override
+  {
+    in_flight_ = false;
+    ++cancel_count_;
+  }
+
+  std::string activePlanner() const override
+  {
+    return active_;
+  }
+
+  /**
+   * @brief Số cycle mà một lượt lập plan "mất" trước khi có kết quả.
+   *
+   * 0 (mặc định) = kết quả có ngay ở lần poll kế tiếp, tức đúng nhịp của bản lập plan đồng bộ cũ:
+   * kick ở cuối cycle N, state machine thấy plan ở cycle N+1. Nhờ vậy mọi test viết cho bản đồng bộ
+   * giữ nguyên ý nghĩa.
+   */
+  void setLatencyCycles(std::size_t cycles)
+  {
+    latency_cycles_ = cycles;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  void setScript(std::vector<PlannerScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setSwapSucceeds(bool succeeds)
+  {
+    swap_succeeds_ = succeeds;
+  }
+
+  std::size_t makePlanCount() const
+  {
+    return make_plan_count_;
+  }
+
+  std::size_t swapCount() const
+  {
+    return swap_count_;
+  }
+
+  bool sawOrder() const
+  {
+    return saw_order_;
+  }
+
+private:
+  /// Hết kịch bản thì giữ kết quả cuối; kịch bản rỗng thì luôn thành công.
+  PlannerScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return PlannerScript::kOk;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<PlannerScript> script_;
+  std::size_t index_ = 0;
+  std::string active_;
+  bool swap_succeeds_ = true;
+
+  bool in_flight_ = false;
+  std::uint64_t pending_tag_ = 0;
+  robot_geometry_msgs::PoseStamped pending_goal_;
+  PlannerScript pending_action_ = PlannerScript::kOk;
+  std::size_t latency_cycles_ = 0;
+  std::size_t cycles_left_ = 0;
+  std::size_t cancel_count_ = 0;
+
+  std::size_t make_plan_count_ = 0;
+  std::size_t swap_count_ = 0;
+  bool saw_order_ = false;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeControllerPort final : public ControllerPort
+{
+public:
+  bool swapPlanner(const std::string& planner_name) override
+  {
+    if (!swap_succeeds_)
+    {
+      return false;
+    }
+    active_ = planner_name;
+    return true;
+  }
+
+  void setTolerance(double xy_m, double yaw_rad) override
+  {
+    xy_tolerance_ = xy_m;
+    yaw_tolerance_ = yaw_rad;
+  }
+
+  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    ++set_plan_count_;
+    last_plan_size_ = plan.size();
+    return set_plan_succeeds_ && !plan.empty();
+  }
+
+  bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override
+  {
+    ++compute_count_;
+    switch (current_action_)
+    {
+      case ControllerScript::kFail:
+        return false;
+      case ControllerScript::kNaN:
+        cmd.linear.x = std::numeric_limits<double>::quiet_NaN();
+        cmd.angular.z = 0.0;
+        return true;
+      case ControllerScript::kTooFast:
+        cmd.linear.x = 99.0;
+        cmd.angular.z = 99.0;
+        return true;
+      case ControllerScript::kGoalReached:
+      case ControllerScript::kOk:
+        break;
+    }
+    cmd.linear.x = nominal_speed_;
+    cmd.angular.z = 0.0;
+    return true;
+  }
+
+  bool isGoalReached() override
+  {
+    // Lấy hành động cho cycle này ở đây vì đây là lời gọi ĐẦU TIÊN của một cycle controller, đúng
+    // thứ tự mà control loop dùng.
+    current_action_ = nextAction();
+    ++goal_check_count_;
+    return current_action_ == ControllerScript::kGoalReached;
+  }
+
+  void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override
+  {
+    measured_velocity_ = velocity;
+  }
+
+  bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override
+  {
+    if (linear.x < 0.0)
+    {
+      limit_backward_ = linear.x;  // [m/s], âm
+    }
+    else
+    {
+      limit_forward_ = linear.x;  // [m/s]
+    }
+    return true;
+  }
+
+  bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override
+  {
+    limit_angular_ = angular.z;  // [rad/s]
+    return true;
+  }
+
+  const robot_geometry_msgs::Twist& measuredVelocity() const
+  {
+    return measured_velocity_;
+  }
+
+  double limitForward() const
+  {
+    return limit_forward_;
+  }
+
+  double limitBackward() const
+  {
+    return limit_backward_;
+  }
+
+  double limitAngular() const
+  {
+    return limit_angular_;
+  }
+
+  std::string activeController() const override
+  {
+    return active_;
+  }
+
+  void setScript(std::vector<ControllerScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setSwapSucceeds(bool succeeds)
+  {
+    swap_succeeds_ = succeeds;
+  }
+
+  void setSetPlanSucceeds(bool succeeds)
+  {
+    set_plan_succeeds_ = succeeds;
+  }
+
+  /// @param speed [m/s] Tốc độ dài của lệnh khi kịch bản là kOk.
+  void setNominalSpeed(double speed)
+  {
+    nominal_speed_ = speed;
+  }
+
+  std::size_t setPlanCount() const
+  {
+    return set_plan_count_;
+  }
+
+  std::size_t computeCount() const
+  {
+    return compute_count_;
+  }
+
+  std::size_t goalCheckCount() const
+  {
+    return goal_check_count_;
+  }
+
+  std::size_t lastPlanSize() const
+  {
+    return last_plan_size_;
+  }
+
+  double xyTolerance() const
+  {
+    return xy_tolerance_;
+  }
+
+  double yawTolerance() const
+  {
+    return yaw_tolerance_;
+  }
+
+private:
+  robot_geometry_msgs::Twist measured_velocity_;
+  double limit_forward_ = 0.0;   ///< [m/s]
+  double limit_backward_ = 0.0;  ///< [m/s], âm
+  double limit_angular_ = 0.0;   ///< [rad/s]
+
+  ControllerScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return ControllerScript::kOk;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<ControllerScript> script_;
+  std::size_t index_ = 0;
+  ControllerScript current_action_ = ControllerScript::kOk;
+
+  std::string active_;
+  bool swap_succeeds_ = true;
+  bool set_plan_succeeds_ = true;
+  double nominal_speed_ = 0.3;  ///< [m/s]
+  double xy_tolerance_ = 0.0;   ///< [m]
+  double yaw_tolerance_ = 0.0;  ///< [rad]
+
+  std::size_t set_plan_count_ = 0;
+  std::size_t compute_count_ = 0;
+  std::size_t goal_check_count_ = 0;
+  std::size_t last_plan_size_ = 0;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeRecoveryPort final : public RecoveryPort
+{
+public:
+  explicit FakeRecoveryPort(std::size_t behavior_count = 2) : behavior_count_(behavior_count)
+  {
+  }
+
+  bool configure(robot::NodeHandle& /*nh*/) override
+  {
+    return true;
+  }
+
+  std::size_t behaviorCount() const override
+  {
+    return behavior_count_;
+  }
+
+  RecoveryOutputKind outputKind(std::size_t index) const override
+  {
+    if (index >= behavior_count_)
+    {
+      return RecoveryOutputKind::kNone;
+    }
+    const auto it = output_kinds_.find(index);
+    return it == output_kinds_.end() ? default_output_kind_ : it->second;
+  }
+
+  /// @brief Đặt họ output cho behavior thứ @p index (mặc định mọi behavior đều lái robot).
+  void setOutputKind(std::size_t index, RecoveryOutputKind kind)
+  {
+    output_kinds_[index] = kind;
+  }
+
+  void setDefaultOutputKind(RecoveryOutputKind kind)
+  {
+    default_output_kind_ = kind;
+  }
+
+  bool start(std::size_t index, RecoveryTrigger trigger) override
+  {
+    ++start_count_;
+    last_start_index_ = index;
+    last_trigger_ = trigger;
+    started_indices_.push_back(index);
+
+    if (index >= behavior_count_ || !start_succeeds_)
+    {
+      return false;
+    }
+    active_ = true;
+    return true;
+  }
+
+  RecoveryTick update() override
+  {
+    ++update_count_;
+
+    RecoveryTick tick;
+    switch (nextAction())
+    {
+      case RecoveryScript::kRunning:
+        tick.status = RecoveryTick::Status::kRunning;
+        tick.has_velocity = emits_velocity_;
+        tick.cmd.linear.x = recovery_speed_;
+        break;
+      case RecoveryScript::kSucceeded:
+        tick.status = RecoveryTick::Status::kSucceeded;
+        active_ = false;
+        break;
+      case RecoveryScript::kFailed:
+        tick.status = RecoveryTick::Status::kFailed;
+        active_ = false;
+        break;
+    }
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count_;
+    active_ = false;
+  }
+
+  std::string behaviorName(std::size_t index) const override
+  {
+    return index < behavior_count_ ? "fake_behavior_" + std::to_string(index) : std::string();
+  }
+
+  void setScript(std::vector<RecoveryScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setStartSucceeds(bool succeeds)
+  {
+    start_succeeds_ = succeeds;
+  }
+
+  /// @param speed [m/s] Vận tốc behavior phát khi đang chạy. Dấu âm nghĩa là lùi.
+  void setRecoveryVelocity(bool emits, double speed)
+  {
+    emits_velocity_ = emits;
+    recovery_speed_ = speed;
+  }
+
+  std::size_t startCount() const
+  {
+    return start_count_;
+  }
+
+  std::size_t updateCount() const
+  {
+    return update_count_;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  std::size_t lastStartIndex() const
+  {
+    return last_start_index_;
+  }
+
+  RecoveryTrigger lastTrigger() const
+  {
+    return last_trigger_;
+  }
+
+  const std::vector<std::size_t>& startedIndices() const
+  {
+    return started_indices_;
+  }
+
+  bool active() const
+  {
+    return active_;
+  }
+
+private:
+  RecoveryScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return RecoveryScript::kSucceeded;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::size_t behavior_count_;
+  std::vector<RecoveryScript> script_;
+  std::size_t index_ = 0;
+
+  bool start_succeeds_ = true;
+  bool active_ = false;
+  bool emits_velocity_ = false;
+  double recovery_speed_ = -0.1;  ///< [m/s], âm = lùi
+
+  /// Mặc định coi mọi behavior đều lái robot — giữ nguyên hành vi của các test viết trước khi
+  /// RecoveryPort có outputKind().
+  RecoveryOutputKind default_output_kind_ = RecoveryOutputKind::kVelocity;
+  std::map<std::size_t, RecoveryOutputKind> output_kinds_;
+
+  std::size_t start_count_ = 0;
+  std::size_t update_count_ = 0;
+  std::size_t cancel_count_ = 0;
+  std::size_t last_start_index_ = 0;
+  RecoveryTrigger last_trigger_ = RecoveryTrigger::kPlanningFailed;
+  std::vector<std::size_t> started_indices_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeActionPort final : public ActionPort
+{
+public:
+  bool configure(robot::NodeHandle& /*nh*/) override
+  {
+    return true;
+  }
+
+  bool start(const robot_protocol_msgs::Action& action) override
+  {
+    ++start_count_;
+    started_action_types_.push_back(action.actionType);
+    if (!start_succeeds_)
+    {
+      return false;
+    }
+    active_ = true;
+    return true;
+  }
+
+  ActionTick update() override
+  {
+    ++update_count_;
+
+    ActionTick tick;
+    switch (nextAction())
+    {
+      case ActionScript::kRunning:
+        tick.status = ActionTick::Status::kRunning;
+        break;
+      case ActionScript::kSucceeded:
+        tick.status = ActionTick::Status::kSucceeded;
+        active_ = false;
+        break;
+      case ActionScript::kFailed:
+        tick.status = ActionTick::Status::kFailed;
+        tick.message = "fake action failed";
+        active_ = false;
+        break;
+    }
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count_;
+    active_ = false;
+  }
+
+  void setScript(std::vector<ActionScript> script)
+  {
+    script_ = std::move(script);
+    index_ = 0;
+  }
+
+  void setStartSucceeds(bool succeeds)
+  {
+    start_succeeds_ = succeeds;
+  }
+
+  std::size_t startCount() const
+  {
+    return start_count_;
+  }
+
+  std::size_t updateCount() const
+  {
+    return update_count_;
+  }
+
+  std::size_t cancelCount() const
+  {
+    return cancel_count_;
+  }
+
+  /// @brief actionType của từng lần start, theo thứ tự — kiểm "actions đi nguyên vẹn, đúng thứ tự".
+  const std::vector<std::string>& startedActionTypes() const
+  {
+    return started_action_types_;
+  }
+
+  bool active() const
+  {
+    return active_;
+  }
+
+private:
+  ActionScript nextAction()
+  {
+    if (script_.empty())
+    {
+      return ActionScript::kSucceeded;
+    }
+    if (index_ >= script_.size())
+    {
+      return script_.back();
+    }
+    return script_[index_++];
+  }
+
+  std::vector<ActionScript> script_;
+  std::size_t index_ = 0;
+
+  bool start_succeeds_ = true;
+  bool active_ = false;
+
+  std::size_t start_count_ = 0;
+  std::size_t update_count_ = 0;
+  std::size_t cancel_count_ = 0;
+  std::vector<std::string> started_action_types_;
+};
+
+// ------------------------------------------------------------------------------------------------
+
+class FakeMissionPort final : public MissionPort
+{
+public:
+  void setRequestCallback(RequestCallback callback) override
+  {
+    callback_ = std::move(callback);
+  }
+
+  void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) override
+  {
+    reports_.emplace_back(mission_sequence_id, outcome);
+  }
+
+  bool hasActiveMission() const override
+  {
+    return active_;
+  }
+
+  void start() override
+  {
+    active_ = true;
+  }
+
+  void stop() override
+  {
+    active_ = false;
+  }
+
+  /// @brief Giả lập mission layer đẩy một chặng xuống.
+  void emit(const NavigationRequest& request)
+  {
+    if (callback_)
+    {
+      callback_(request);
+    }
+  }
+
+  const std::vector<std::pair<std::uint64_t, NavigationOutcome>>& reports() const
+  {
+    return reports_;
+  }
+
+  /// @brief Số lần đã báo kết quả cho một sequence id — bất biến là phải bằng 1.
+  std::size_t reportCountFor(std::uint64_t mission_sequence_id) const
+  {
+    std::size_t count = 0;
+    for (const auto& report : reports_)
+    {
+      if (report.first == mission_sequence_id)
+      {
+        ++count;
+      }
+    }
+    return count;
+  }
+
+private:
+  RequestCallback callback_;
+  std::vector<std::pair<std::uint64_t, NavigationOutcome>> reports_;
+  bool active_ = false;
+};
+
+}  // namespace testing
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_TEST_FAKE_PORTS_H_

test/navigation_server_test.cpp

@@ -0,0 +1,559 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test facade `NavigationServer`: đường lệnh vận tốc ra host, và đường dữ liệu cảm
+ * biến từ host vào costmap.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <robot_costmap_2d/layered_costmap.h>
+
+#include <move_base2/navigation_server.h>
+
+#include "fake_ports.h"
+#include "spy_layer.h"
+
+using move_base2::ControlLoopConfig;
+using move_base2::ControlLoopDeps;
+using move_base2::MotionProfile;
+using move_base2::NavigationRequest;
+using move_base2::NavigationServer;
+using move_base2::NavigationState;
+using move_base2::SensorGatewayConfig;
+using move_base2::testing::attachSpy;
+using move_base2::testing::ControllerScript;
+using move_base2::testing::FakeActionPort;
+using move_base2::testing::FakeClockPort;
+using move_base2::testing::FakeControllerPort;
+using move_base2::testing::FakeMissionPort;
+using move_base2::testing::FakePlannerPort;
+using move_base2::testing::FakePosePort;
+using move_base2::testing::FakeRecoveryPort;
+using move_base2::testing::PlannerScript;
+using move_base2::testing::SpyPtr;
+using robot_costmap_2d::LayerType;
+
+namespace
+{
+
+constexpr double kControlPeriod = 0.05;  ///< [s]
+constexpr double kClockStart = 1000.0;   ///< [s]
+
+ControlLoopConfig baseConfig()
+{
+  ControlLoopConfig config;
+
+  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
+  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;
+
+  config.velocity.max_vel_x = 0.5;          // [m/s]
+  config.velocity.min_vel_x = -0.2;         // [m/s]
+  config.velocity.max_vel_theta = 1.0;      // [rad/s]
+  config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn để test không vướng ramp
+  config.velocity.max_accel_theta = 100.0;  // [rad/s^2]
+
+  config.nominal_control_period = kControlPeriod;
+  config.robot_base_frame = "base_link";
+
+  config.position.global_planner_name = "FakeGlobalPlanner";
+  config.position.local_planner_name = "FakeLocalPlanner";
+  config.position.default_xy_tolerance = 0.15;   // [m]
+  config.position.default_yaw_tolerance = 0.10;  // [rad]
+
+  config.docking = config.position;
+  config.go_straight = config.position;
+  config.rotate = config.position;
+
+  return config;
+}
+
+NavigationRequest makeRequest(double goal_x)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kPosition;
+  request.goal.header.frame_id = "map";
+  request.goal.pose.position.x = goal_x;
+  request.goal.pose.orientation.w = 1.0;
+  return request;
+}
+
+robot_geometry_msgs::Vector3 makeVector(double x, double y = 0.0, double z = 0.0)
+{
+  robot_geometry_msgs::Vector3 v;
+  v.x = x;
+  v.y = y;
+  v.z = z;
+  return v;
+}
+
+robot_nav_msgs::Odometry makeOdometry(double vx, double wz)
+{
+  robot_nav_msgs::Odometry odom;
+  odom.header.frame_id = "odom";
+  odom.twist.twist.linear.x = vx;   // [m/s]
+  odom.twist.twist.angular.z = wz;  // [rad/s]
+  return odom;
+}
+
+robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F)
+{
+  robot_sensor_msgs::LaserScan scan;
+  scan.header.frame_id = "laser";
+  scan.angle_min = -1.5F;                                  // [rad]
+  scan.angle_max = 1.5F;                                   // [rad]
+  scan.angle_increment = 3.0F / static_cast<float>(rays);  // [rad]
+  scan.range_min = 0.05F;                                  // [m]
+  scan.range_max = 10.0F;                                  // [m]
+  scan.ranges.assign(rays, range);
+  return scan;
+}
+
+/**
+ * @class Fixture
+ * @brief `NavigationServer` nối đủ cổng giả, cộng hai costmap thật để kiểm đường cảm biến.
+ */
+class Fixture
+{
+public:
+  Fixture()
+    : clock_(kClockStart)
+    , recovery_(2)
+    , global_("map", false, true)
+    , local_("odom", true, false)
+  {
+    pose_.setPosition(0.0, 0.0);
+
+    deps_.clock = &clock_;
+    deps_.pose = &pose_;
+    deps_.planner = &planner_;
+    deps_.controller = &controller_;
+    deps_.recovery = &recovery_;
+    deps_.mission = &mission_;
+    deps_.action = &action_;
+  }
+
+  void configure(const ControlLoopConfig& config = baseConfig())
+  {
+    std::string error;
+    ASSERT_TRUE(server_.configureLoop(config, deps_, error)) << error;
+  }
+
+  void configureSensors(const SensorGatewayConfig& config)
+  {
+    std::string error;
+    ASSERT_TRUE(server_.configureSensors(config, error)) << error;
+  }
+
+  /// @brief Chạy @p cycles control cycle, mỗi cycle nhích đồng hồ giả một chu kỳ.
+  void spin(std::size_t cycles)
+  {
+    for (std::size_t i = 0; i < cycles; ++i)
+    {
+      server_.spinOnce();
+      clock_.advance(kControlPeriod);
+    }
+  }
+
+  void attachCostmaps()
+  {
+    server_.attachCostmaps(&global_, &local_);
+  }
+
+  NavigationServer server_;
+  FakeClockPort clock_;
+  FakePosePort pose_;
+  FakePlannerPort planner_;
+  FakeControllerPort controller_;
+  FakeRecoveryPort recovery_;
+  FakeMissionPort mission_;
+  FakeActionPort action_;
+  ControlLoopDeps deps_;
+
+  robot_costmap_2d::LayeredCostmap global_;
+  robot_costmap_2d::LayeredCostmap local_;
+};
+
+}  // namespace
+
+// ================================================================================================
+// getTwist() — LỆNH vận tốc, không phải vận tốc đo được
+//
+// Host lấy getTwist() rồi publish thẳng ra /cmd_vel. Nếu giá trị đó đến từ odometry thì có một vòng
+// lặp dương: robot chạy 0.5 m/s -> đọc odom 0.5 -> phát lệnh 0.5 -> mãi mãi. VelocityArbiter — toàn
+// bộ hàng rào an toàn của gói — cũng bị bỏ qua hoàn toàn. Các test dưới đây khoá lại điều đó.
+// ================================================================================================
+
+TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
+
+  // Odometry báo robot đang chạy nhanh hơn hẳn lệnh mà controller muốn phát.
+  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+
+  fixture.spin(2);  // IDLE -> PLANNING -> CONTROLLING (controller chạy ngay ở cycle này)
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
+  EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
+  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
+}
+
+TEST(NavigationServerTwist, OdometryAloneNeverProducesACommand)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9));
+  fixture.spin(1);  // IDLE, không có yêu cầu nào
+
+  const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
+  EXPECT_NEAR(twist.velocity.x, 0.0, 1e-9);
+  EXPECT_NEAR(twist.velocity.y, 0.0, 1e-9);
+  EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
+}
+
+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.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.spin(1);
+  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9);
+
+  fixture.clock_.setTime(kClockStart + 12.0);
+  fixture.spin(1);
+  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9);
+}
+
+TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.spin(1);
+  const double stamp_after_first = fixture.server_.getTwist().header.stamp.toSec();
+
+  // Đồng hồ chạy tiếp nhưng KHÔNG có cycle nào — mô phỏng control thread treo.
+  fixture.clock_.setTime(kClockStart + 30.0);
+  fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));
+
+  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
+      << "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
+}
+
+TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
+{
+  ControlLoopConfig config = baseConfig();
+  config.robot_base_frame = "base_footprint";
+
+  Fixture fixture;
+  fixture.configure(config);
+  fixture.spin(1);
+
+  EXPECT_EQ(fixture.server_.getTwist().header.frame_id, "base_footprint");
+}
+
+TEST(NavigationServerTwist, ConfigureIsRefusedWhenRobotBaseFrameIsEmpty)
+{
+  ControlLoopConfig config = baseConfig();
+  config.robot_base_frame.clear();
+
+  Fixture fixture;
+  std::string error;
+  EXPECT_FALSE(fixture.server_.configureLoop(config, fixture.deps_, error));
+  EXPECT_FALSE(error.empty());
+}
+
+// ================================================================================================
+// Đường dữ liệu cảm biến từ host vào costmap
+// ================================================================================================
+
+TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
+  SpyPtr local_voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
+  fixture.attachCostmaps();
+
+  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+  fixture.server_.addLaserScan("/b_scan", makeScan());
+  fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());
+
+  EXPECT_EQ(static_layer->count(), 1U);
+  EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
+  EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
+  EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
+}
+
+TEST(NavigationServerSensors, StoringStillWorksWhenNoCostmapIsAttachedYet)
+{
+  // Trạng thái bình thường lúc khởi động: host đã bắt đầu bơm dữ liệu trước khi costmap được dựng.
+  // Dữ liệu vẫn phải đọc lại được qua getter của contract host, và số mẫu mất phải đếm được.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(fixture.server_.getLaserScan("/b_scan").ranges.size(), 40U);
+  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);
+  EXPECT_EQ(fixture.server_.sensors().stats().delivered, 0U);
+}
+
+TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
+{
+  // Không có phần phát lại này thì thứ tự "map tới trước, costmap dựng sau" — thứ tự thường gặp
+  // nhất khi khởi động — để global costmap trắng vĩnh viễn: /map là topic latched, host không gửi
+  // lại. Bản cũ bù bằng cặp biến public map_save_/map_name_save_.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+  ASSERT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U);
+
+  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
+  fixture.attachCostmaps();
+
+  ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
+  EXPECT_EQ(static_layer->records()[0].topic, "/map");
+}
+
+TEST(NavigationServerSensors, LegacyMapSavePublicMemberIsAlsoReplayed)
+{
+  // `map_save_`/`map_name_save_` là biến PUBLIC của BaseNavigation mà host tự gán
+  // (sensor_converter.cpp). Giữ đường này để host không phải sửa gì khi đổi sang move_base2.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.map_name_save_ = "/map";
+  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();
+
+  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
+  fixture.attachCostmaps();
+
+  EXPECT_EQ(static_layer->count(), 1U);
+  EXPECT_EQ(static_layer->records()[0].topic, "/map");
+}
+
+TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTheApi)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+  fixture.server_.map_name_save_ = "/map";  // host gán cả hai đường, như bản cũ đang làm
+  fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid();
+
+  SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
+  fixture.attachCostmaps();
+
+  EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
+}
+
+TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
+{
+  // Cố ý: phát lại một scan cũ là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu. Mẫu kế tiếp
+  // chỉ cách vài chục ms — chờ nó an toàn hơn hẳn.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addLaserScan("/b_scan", makeScan());
+
+  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
+  fixture.attachCostmaps();
+
+  EXPECT_EQ(voxel->count(), 0U);
+}
+
+TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
+{
+  // Bản cũ cất bản ĐÃ LỌC. Nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật
+  // lệch nhau, và mọi chẩn đoán dựa trên getter sẽ nói dối về thứ costmap thật sự dùng.
+  SensorGatewayConfig sensors;
+  sensors.laser_sor_enabled = true;
+  sensors.laser_sor_mean_k = 5;
+  sensors.laser_sor_stddev_mul = 1.0;
+
+  Fixture fixture;
+  fixture.configure();
+  fixture.configureSensors(sensors);
+
+  std::vector<float> seen_by_layer;
+  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
+  voxel->setObserver([&seen_by_layer](const void* data, const std::type_info& type,
+                                      const std::string&) {
+    if (type == typeid(robot_sensor_msgs::LaserScan))
+    {
+      seen_by_layer = static_cast<const robot_sensor_msgs::LaserScan*>(data)->ranges;
+    }
+  });
+  fixture.attachCostmaps();
+
+  fixture.server_.addLaserScan("/b_scan", makeScan());
+
+  const std::vector<float> stored = fixture.server_.getLaserScan("/b_scan").ranges;
+  ASSERT_FALSE(seen_by_layer.empty());
+  ASSERT_EQ(stored.size(), seen_by_layer.size());
+
+  // So từng phần tử chứ không so cả vector: bộ lọc biến outlier thành NaN để giữ nguyên cấu trúc
+  // scan, mà NaN != NaN nên operator== của vector sẽ báo khác nhau dù nội dung giống hệt.
+  for (std::size_t i = 0; i < stored.size(); ++i)
+  {
+    if (std::isnan(stored[i]))
+    {
+      EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
+    }
+    else
+    {
+      EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
+    }
+  }
+}
+
+TEST(NavigationServerSensors, DepthCameraDataIsStoredAndForwardedAsConstPtr)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
+  fixture.attachCostmaps();
+
+  robot_sensor_msgs::DepthCameraData::Ptr data =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  data->header.frame_id = "camera_optical";
+  fixture.server_.addDepthCameraData("/camera/depth/data", data);
+
+  ASSERT_EQ(voxel->count(), 1U);
+  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr));
+  EXPECT_EQ(voxel->records()[0].topic, "/camera/depth/data");
+}
+
+TEST(NavigationServerSensors, NullDepthPointerIsRejectedAtTheDoor)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles");
+  fixture.attachCostmaps();
+
+  fixture.server_.addDepthCameraData("/camera/depth/data",
+                                     robot_sensor_msgs::DepthCameraData::ConstPtr());
+
+  EXPECT_EQ(voxel->count(), 0U);
+  EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 0U);
+}
+
+// ================================================================================================
+// Trần vận tốc (bước 12) — đường tầng an toàn hạ tốc độ robot
+//
+// `setTwistLinear` không phải lệnh jog dù tên nghe như vậy: host gọi nó theo cặp +v/-v để đặt trần
+// cho hai chiều, và giá trị truyền xuống mang theo tốc độ đã bị tầng an toàn hạ
+// (amr_control.cpp:561, 671-680). Trước đây `NavigationServer` trả false và không làm gì.
+// ================================================================================================
+
+TEST(NavigationServerLimits, ForwardAndBackwardLimitsReachTheController)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(0.30)));   // [m/s] trần tiến
+  EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(-0.15)));  // [m/s] trần lùi, ÂM
+
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.controller_.limitForward(), 0.30, 1e-9);
+  EXPECT_NEAR(fixture.controller_.limitBackward(), -0.15, 1e-9);
+}
+
+TEST(NavigationServerLimits, AngularLimitReachesTheController)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  EXPECT_TRUE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, 0.45)));  // [rad/s]
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.controller_.limitAngular(), 0.45, 1e-9);
+}
+
+TEST(NavigationServerLimits, LimitTakesEffectInTheSameCycleItIsPushed)
+{
+  // Chậm một cycle nghĩa là một chu kỳ nữa robot chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
+  Fixture fixture;
+  fixture.configure();
+
+  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.12)));
+  fixture.spin(1);
+  EXPECT_NEAR(fixture.controller_.limitForward(), 0.12, 1e-9);
+}
+
+TEST(NavigationServerLimits, NonFiniteLimitIsRejectedAtTheDoor)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  const double nan = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_FALSE(fixture.server_.setTwistLinear(makeVector(nan)));
+  EXPECT_FALSE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, nan)));
+
+  fixture.spin(1);
+  EXPECT_NEAR(fixture.controller_.limitForward(), 0.0, 1e-9);
+}
+
+TEST(NavigationServerLimits, LatestLimitWinsWhenSetSeveralTimesWithinOneCycle)
+{
+  // Host gọi từ thread của nó với nhịp riêng; nhiều lời gọi giữa hai cycle là bình thường. Thứ phải
+  // có hiệu lực là giá trị MỚI NHẤT, không phải giá trị đầu tiên.
+  Fixture fixture;
+  fixture.configure();
+
+  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.40)));
+  ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.10)));  // tầng an toàn vừa hạ tiếp
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.controller_.limitForward(), 0.10, 1e-9);
+}
+
+TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity)
+{
+  // Bản cũ đưa vận tốc đo được vào controller bằng con trỏ tới bộ nhớ host ghi
+  // (`tc_->setOdom(&odometry_)`) — data race không có gì bảo vệ. Ở đây truyền theo giá trị, qua
+  // control thread.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.server_.addOdometry("/odom", makeOdometry(0.42, -0.17));
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.controller_.measuredVelocity().linear.x, 0.42, 1e-9);
+  EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9);
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/planner_runner_test.cpp

@@ -0,0 +1,422 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test PlannerRunner: nạp plugin thật qua Boost.DLL, và mọi đường lỗi phải trả false
+ * chứ không được để dữ liệu hỏng đi tiếp.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <cstdlib>
+#include <cstdint>
+#include <limits>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/planner_runner.h>
+
+namespace
+{
+using move_base2::PlannerRunner;
+
+/**
+ * @brief Con trỏ costmap giả.
+ *
+ * `PlannerRunner::configure` từ chối costmap null — đúng, vì mọi plugin thật đều dùng nó. Nhưng
+ * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy
+ * đủ), nên test dùng một địa chỉ hợp lệ nhưng không phải costmap.
+ *
+ * An toàn ở đây vì `test_global_planner.cpp` **không alias nào chạm vào con trỏ này** — nó chỉ được
+ * chuyển tiếp qua `initialize()` rồi bị bỏ qua. Đường có costmap thật thuộc test tích hợp (Phase 5).
+ */
+robot_costmap_2d::Costmap2DROBOT* dummyCostmap()
+{
+  static std::uintptr_t placeholder = 0;
+  return reinterpret_cast<robot_costmap_2d::Costmap2DROBOT*>(&placeholder);
+}
+
+/// @brief Chạy trọn một lượt lập plan đồng bộ hoá lại cho test: kick, chờ thread, lấy kết quả.
+bool runOnePlan(move_base2::PlannerRunner& runner, const robot_geometry_msgs::PoseStamped& start,
+                const robot_geometry_msgs::PoseStamped& goal,
+                const robot_protocol_msgs::Order* order, move_base2::PlanResult& result)
+{
+  if (!runner.startPlan(start, goal, order, /*tag=*/1))
+  {
+    return false;
+  }
+  // Thread planner là thread thật; test phải chờ nó. Vòng quay ngắn thay vì sleep cố định để test
+  // không phụ thuộc vào tốc độ máy.
+  for (int i = 0; i < 10000 && runner.isPlanning(); ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  return runner.pollPlan(result);
+}
+
+robot_geometry_msgs::PoseStamped makePose(double x, double y)
+{
+  robot_geometry_msgs::PoseStamped pose;
+  pose.header.frame_id = "map";
+  pose.pose.position.x = x;  // [m]
+  pose.pose.position.y = y;  // [m]
+  pose.pose.orientation.w = 1.0;
+  return pose;
+}
+
+/// @brief Runner đã configure với planner @p name; ASSERT nếu không nạp được.
+class Fixture
+{
+public:
+  explicit Fixture(const std::string& name = "TestPlannerOk")
+  {
+    robot::NodeHandle nh;
+    std::string error;
+    ok_ = runner_.configure(nh, dummyCostmap(), name, error);
+    error_ = error;
+  }
+
+  bool ok() const
+  {
+    return ok_;
+  }
+
+  const std::string& error() const
+  {
+    return error_;
+  }
+
+  PlannerRunner runner_;
+
+private:
+  bool ok_ = false;
+  std::string error_;
+};
+
+}  // namespace
+
+// ================================================================================================
+// Cấu hình
+// ================================================================================================
+
+TEST(PlannerRunner, RefusesNullCostmap)
+{
+  // Plugin thật nào cũng dùng costmap. Nhận null rồi chuyển tiếp xuống `initialize()` là đẩy quyết
+  // định "sập hay không" cho từng plugin tự lo.
+  robot::NodeHandle nh;
+  PlannerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, nullptr, "TestPlannerOk", error));
+  EXPECT_FALSE(error.empty());
+  EXPECT_FALSE(runner.configured());
+}
+
+TEST(PlannerRunner, ConfiguresWithoutAnInitialPlanner)
+{
+  robot::NodeHandle nh;
+  PlannerRunner runner;
+  std::string error;
+
+  ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error;
+  EXPECT_TRUE(runner.configured());
+  EXPECT_TRUE(runner.activePlanner().empty());
+  EXPECT_EQ(runner.loadedCount(), 0U);
+}
+
+TEST(PlannerRunner, RefusesSecondConfigure)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  robot::NodeHandle nh;
+  std::string error;
+  EXPECT_FALSE(fixture.runner_.configure(nh, dummyCostmap(), "TestPlannerOk", error));
+}
+
+TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded)
+{
+  robot::NodeHandle nh;
+  PlannerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerMissingLibrary", error));
+  EXPECT_FALSE(error.empty());
+  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+}
+
+// ================================================================================================
+// Nạp plugin qua Boost.DLL
+// ================================================================================================
+
+TEST(PlannerRunner, LoadsTheInitialPlannerAndReportsItAsActive)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 1U);
+}
+
+TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries)
+{
+  // swapPlanner chạy ở CỬA VÀO mỗi yêu cầu. Đổi qua lại giữa hai profile không được dlopen lại.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerEmptyPlan"));
+  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerEmptyPlan");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U);
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerOk"));
+  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại planner cũ mà vẫn nạp lại thư viện";
+}
+
+TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive)
+{
+  // Bên gọi từ chối yêu cầu dựa trên giá trị trả về. Chuyển sang trạng thái "không có planner" sẽ
+  // giết luôn yêu cầu đang chạy dở, dù nó chẳng liên quan gì tới planner vừa nạp hỏng.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerMissingLibrary"));
+  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
+  EXPECT_TRUE(result.succeeded);
+}
+
+TEST(PlannerRunner, PlannerReportingInitializeFailureIsRejected)
+{
+  // Bản cũ chỉ log rồi đi tiếp với planner chưa khởi tạo xong.
+  robot::NodeHandle nh;
+  PlannerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerInitFails", error));
+}
+
+TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerInitFails"));
+  EXPECT_EQ(fixture.runner_.loadedCount(), 1U)
+      << "instance hỏng bị cache lại — mọi lần thử sau sẽ nhận lại đúng cái hỏng đó";
+}
+
+TEST(PlannerRunner, RefusesEmptyPlannerName)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.swapPlanner(""));
+  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
+}
+
+TEST(PlannerRunner, SwapBeforeConfigureIsRefused)
+{
+  PlannerRunner runner;
+  EXPECT_FALSE(runner.swapPlanner("TestPlannerOk"));
+}
+
+// ================================================================================================
+// Một lượt lập plan — mọi đường lỗi phải báo thất bại, plan phải rỗng
+// ================================================================================================
+
+TEST(PlannerRunner, ProducesANonEmptyPlanOnTheHappyPath)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 1.0), nullptr, result));
+  ASSERT_TRUE(result.succeeded);
+  ASSERT_FALSE(result.plan.empty());
+  EXPECT_EQ(result.tag, 1U);
+  EXPECT_DOUBLE_EQ(result.plan.back().pose.position.x, 2.0);
+  EXPECT_DOUBLE_EQ(result.plan.back().pose.position.y, 1.0);
+}
+
+TEST(PlannerRunner, OrderIsForwardedToTheOrderAwareOverload)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  const robot_protocol_msgs::Order order;
+  move_base2::PlanResult result;
+  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), &order, result));
+  EXPECT_TRUE(result.succeeded);
+  EXPECT_FALSE(result.plan.empty());
+}
+
+TEST(PlannerRunner, OrderIsCopiedSoItMayDieBeforeThePlanFinishes)
+{
+  // Con trỏ Order chỉ hợp lệ trong lời gọi startPlan, nhưng lượt lập plan sống lâu hơn thế. Không
+  // sao chép là thread planner đọc bộ nhớ đã chết.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  {
+    const robot_protocol_msgs::Order order;
+    ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), &order, 7));
+  }  // order chết ở đây
+
+  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(fixture.runner_.pollPlan(result));
+  EXPECT_TRUE(result.succeeded);
+  EXPECT_EQ(result.tag, 7U);
+}
+
+TEST(PlannerRunner, PlannerReturningTrueWithAnEmptyPlanIsTreatedAsFailure)
+{
+  // Contract của PlannerPort: thành công nghĩa là plan KHÔNG rỗng. Lọt qua thì tầng trên gọi
+  // front()/back() trên vector rỗng.
+  Fixture fixture("TestPlannerEmptyPlan");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
+  EXPECT_FALSE(result.succeeded);
+  EXPECT_TRUE(result.plan.empty());
+}
+
+TEST(PlannerRunner, ExceptionFromThePluginIsContained)
+{
+  // Plugin là code bên thứ ba nạp lúc chạy. Exception thoát khỏi thân thread là std::terminate —
+  // mất cả tiến trình navigation vì một lượt lập plan hỏng.
+  Fixture fixture("TestPlannerThrowing");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
+  EXPECT_FALSE(result.succeeded);
+  EXPECT_TRUE(result.plan.empty());
+}
+
+TEST(PlannerRunner, NonFiniteStartOrGoalIsRejectedBeforeStartingTheThread)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  const double nan = std::numeric_limits<double>::quiet_NaN();
+  const double inf = std::numeric_limits<double>::infinity();
+
+  EXPECT_FALSE(fixture.runner_.startPlan(makePose(nan, 0.0), makePose(2.0, 0.0), nullptr, 1));
+  EXPECT_FALSE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(inf, 0.0), nullptr, 1));
+  EXPECT_FALSE(fixture.runner_.isPlanning());
+}
+
+TEST(PlannerRunner, StartPlanWithoutAnActivePlannerFails)
+{
+  robot::NodeHandle nh;
+  PlannerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error;
+
+  EXPECT_FALSE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
+}
+
+TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused)
+{
+  // Một lượt tại một thời điểm. Nhận thêm sẽ đè lên yêu cầu đang chạy và làm mất công đã bỏ ra.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
+  const bool refused = !fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(3.0, 0.0), nullptr, 2);
+
+  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  // Lượt đầu có thể đã xong trước lời gọi thứ hai (planner giả rất nhanh), nên chỉ khẳng định điều
+  // luôn đúng: không bao giờ có hai lượt cùng chạy, và kết quả thu về là của MỘT lượt.
+  move_base2::PlanResult result;
+  ASSERT_TRUE(fixture.runner_.pollPlan(result));
+  EXPECT_TRUE(result.tag == 1U || (!refused && result.tag == 2U));
+  EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "còn kết quả thứ hai trong hộp thư";
+}
+
+TEST(PlannerRunner, CancelledPlanProducesNoResult)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
+  fixture.runner_.cancelPlan();
+
+  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  move_base2::PlanResult result;
+  EXPECT_FALSE(fixture.runner_.pollPlan(result))
+      << "lượt đã huỷ vẫn trả kết quả — bên gọi sẽ bám theo plan tới goal không còn ai yêu cầu";
+}
+
+TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 42));
+  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  move_base2::PlanResult result;
+  ASSERT_TRUE(fixture.runner_.pollPlan(result));
+  EXPECT_EQ(result.tag, 42U);
+}
+
+TEST(PlannerRunner, PollOnAnIdleRunnerReturnsNothing)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  move_base2::PlanResult result;
+  EXPECT_FALSE(fixture.runner_.pollPlan(result));
+  EXPECT_FALSE(fixture.runner_.isPlanning());
+}
+
+TEST(PlannerRunner, DestructorJoinsWhileAPlanIsInFlight)
+{
+  // Detach thay vì join sẽ để thread chạm vào buffer đã bị huỷ. Test này chạy sạch dưới sanitizer
+  // là bằng chứng; ở đây nó ít nhất khẳng định destructor không treo.
+  robot::NodeHandle nh;
+  std::string error;
+  {
+    PlannerRunner runner;
+    ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "TestPlannerOk", error)) << error;
+    EXPECT_TRUE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
+  }
+  SUCCEED();
+}
+
+int main(int argc, char** argv)
+{
+  // ctest không mang theo biến môi trường của shell; binary tự trỏ vào cây config và thư viện của
+  // gói, đúng cách recovery_runner_test và action_runner_test đang làm.
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/plugins/test_global_planner.cpp

@@ -0,0 +1,128 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — plugin global planner CHỈ dùng cho test.
+ *
+ * Bốn alias được export từ cùng một thư viện, mỗi alias là một hành vi mà `PlannerRunner` phải xử lý
+ * đúng. Chọn cách này thay vì một plugin đọc config: nó làm test không phụ thuộc vào cây config, và
+ * mỗi kịch bản gọi tên đúng thứ nó kiểm.
+ *
+ * Không alias nào chạm vào con trỏ costmap — đó là điều kiện để test truyền vào một con trỏ giả
+ * thay vì phải dựng `Costmap2DROBOT` thật (cần tf3::BufferCore và cây config đầy đủ).
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <boost/config.hpp>
+#include <boost/dll/alias.hpp>
+#include <boost/make_shared.hpp>
+
+#include <robot_nav_core/base_global_planner.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+/// @brief Số pose trong plan mà `TestPlannerOk` sinh ra.
+constexpr std::size_t kPlanLength = 3;
+
+/**
+ * @class TestGlobalPlanner
+ * @brief Planner giả, hành vi cố định theo tham số dựng.
+ */
+class TestGlobalPlanner : public robot_nav_core::BaseGlobalPlanner
+{
+public:
+  enum class Behavior
+  {
+    kOk,          ///< Trả plan hợp lệ.
+    kEmptyPlan,   ///< Trả true kèm plan RỖNG — bẫy mà PlannerRunner phải quy về false.
+    kThrow,       ///< Ném exception giữa lúc lập plan.
+    kInitFails    ///< initialize() trả false.
+  };
+
+  explicit TestGlobalPlanner(Behavior behavior) : behavior_(behavior)
+  {
+  }
+
+  bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  {
+    // Cố ý KHÔNG chạm costmap_robot — xem chú thích đầu file.
+    name_ = std::move(name);
+    return behavior_ != Behavior::kInitFails;
+  }
+
+  bool makePlan(const robot_geometry_msgs::PoseStamped& start,
+                const robot_geometry_msgs::PoseStamped& goal,
+                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    ++call_count_;
+
+    if (behavior_ == Behavior::kThrow)
+    {
+      throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
+    }
+
+    plan.clear();
+    if (behavior_ == Behavior::kEmptyPlan)
+    {
+      return true;  // true + rỗng: đúng thứ contract PlannerPort cấm lọt qua.
+    }
+
+    plan.push_back(start);
+    for (std::size_t i = plan.size(); i + 1 < kPlanLength; ++i)
+    {
+      plan.push_back(start);
+    }
+    plan.push_back(goal);
+    return true;
+  }
+
+  bool makePlan(const robot_protocol_msgs::Order& /*msg*/,
+                const robot_geometry_msgs::PoseStamped& start,
+                const robot_geometry_msgs::PoseStamped& goal,
+                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    saw_order_ = true;
+    return makePlan(start, goal, plan);
+  }
+
+private:
+  Behavior behavior_;
+  std::string name_;
+  std::size_t call_count_ = 0;
+  bool saw_order_ = false;
+};
+
+robot_nav_core::BaseGlobalPlanner::Ptr createOk()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kEmptyPlan);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createThrowing()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kThrow);
+}
+
+robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing()
+{
+  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kInitFails);
+}
+
+}  // namespace testing
+}  // namespace move_base2
+
+BOOST_DLL_ALIAS(move_base2::testing::createOk, TestPlannerOk)
+BOOST_DLL_ALIAS(move_base2::testing::createEmptyPlan, TestPlannerEmptyPlan)
+BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestPlannerThrowing)
+BOOST_DLL_ALIAS(move_base2::testing::createInitFailing, TestPlannerInitFails)

test/plugins/test_local_planner.cpp

@@ -0,0 +1,216 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — plugin local planner CHỈ dùng cho test.
+ *
+ * Instance được nạp qua Boost.DLL nên test không giữ được con trỏ tới nó. Thay vì mở một cửa hậu để
+ * đọc trạng thái, planner này **phản ánh** thứ nó nhận được vào chính lệnh vận tốc nó trả về:
+ *
+ *   cmd.linear.x  = clamp(kBaseSpeed + vận_tốc_đo_được.x, trần_tiến)
+ *   cmd.angular.z = clamp(kBaseYawRate, trần_góc)
+ *
+ * Nhờ vậy "trần đã tới plugin chưa" và "vận tốc đo được đã tới plugin chưa" kiểm được qua đúng API
+ * mà runtime dùng, không cần cơ chế quan sát riêng nào.
+ *
+ * Không alias nào chạm vào con trỏ TF hay costmap — đó là điều kiện để test truyền con trỏ giả thay
+ * vì phải dựng `tf3::BufferCore` và `Costmap2DROBOT` thật.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <boost/config.hpp>
+#include <boost/dll/alias.hpp>
+
+#include <robot_nav_core/base_local_planner.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+constexpr double kBaseSpeed = 0.25;    ///< [m/s] lệnh nền khi chưa có trần nào
+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
+{
+public:
+  enum class Behavior
+  {
+    kOk,            ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được.
+    kNoCommand,     ///< computeVelocityCommands trả false.
+    kNaN,           ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
+    kThrow,         ///< Ném exception khi tính lệnh.
+    kRefusesLimits  ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
+  };
+
+  explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
+  {
+  }
+
+  void initialize(std::string name, tf3::BufferCore* /*tf*/,
+                  robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
+  {
+    // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
+    name_ = std::move(name);
+  }
+
+  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
+  {
+    return !plan.empty();
+  }
+
+  void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  {
+    path.clear();
+  }
+
+  void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
+  {
+    path.clear();
+  }
+
+  bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity,
+                               robot_geometry_msgs::Twist& cmd_vel) override
+  {
+    switch (behavior_)
+    {
+      case Behavior::kThrow:
+        throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
+      case Behavior::kNoCommand:
+        return false;
+      case Behavior::kNaN:
+        cmd_vel.linear.x = std::numeric_limits<double>::quiet_NaN();
+        return true;
+      case Behavior::kOk:
+      case Behavior::kRefusesLimits:
+        break;
+    }
+
+    double linear = kBaseSpeed + velocity.linear.x;
+    if (has_limit_forward_)
+    {
+      linear = std::min(linear, limit_forward_);
+    }
+
+    double yaw = kBaseYawRate;
+    if (has_limit_angular_)
+    {
+      yaw = std::min(yaw, limit_angular_);
+    }
+
+    cmd_vel.linear.x = linear;
+    cmd_vel.angular.z = yaw;
+    return true;
+  }
+
+  bool isGoalReached() override
+  {
+    return false;
+  }
+
+  bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override
+  {
+    if (behavior_ == Behavior::kRefusesLimits)
+    {
+      return false;
+    }
+    // Dấu chọn chiều, đúng quy ước của interface gen-1.
+    if (linear.x < 0.0)
+    {
+      limit_backward_ = linear.x;
+    }
+    else
+    {
+      limit_forward_ = linear.x;
+      has_limit_forward_ = true;
+    }
+    return true;
+  }
+
+  robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override
+  {
+    robot_geometry_msgs::Vector3 out;
+    out.x = direct ? limit_forward_ : limit_backward_;
+    return out;
+  }
+
+  bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override
+  {
+    if (behavior_ == Behavior::kRefusesLimits)
+    {
+      return false;
+    }
+    limit_angular_ = angular.z;
+    has_limit_angular_ = true;
+    return true;
+  }
+
+  robot_geometry_msgs::Vector3 getTwistAngular(bool /*direct*/) override
+  {
+    robot_geometry_msgs::Vector3 out;
+    out.z = limit_angular_;
+    return out;
+  }
+
+private:
+  Behavior behavior_;
+  std::string name_;
+  double limit_forward_ = 0.0;   ///< [m/s]
+  double limit_backward_ = 0.0;  ///< [m/s], âm
+  double limit_angular_ = 0.0;   ///< [rad/s]
+  bool has_limit_forward_ = false;
+  bool has_limit_angular_ = false;
+};
+
+robot_nav_core::BaseLocalPlanner::Ptr createOk()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createNaN()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
+}
+
+robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
+}
+
+}  // namespace testing
+}  // namespace move_base2
+
+BOOST_DLL_ALIAS(move_base2::testing::createOk, TestControllerOk)
+BOOST_DLL_ALIAS(move_base2::testing::createSecondary, TestControllerSecondary)
+BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
+BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
+BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
+BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)

test/recovery_runner_test.cpp

@@ -0,0 +1,237 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm chỗ nối RecoveryPort <-> recovery_core.
+ *
+ * Đây là seam giữa hai gói: nếu nó đúng thì mọi behavior của recovery_core dùng được từ lõi mà lõi
+ * không biết gì về recovery_core. Test nạp plugin qua đúng đường Boost.DLL mà runtime đi.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <string>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/recovery_runner.h>
+
+#include "fake_ports.h"
+
+namespace
+{
+using move_base2::RecoveryOutputKind;
+using move_base2::RecoveryRunner;
+using move_base2::RecoveryTick;
+using move_base2::RecoveryTrigger;
+using move_base2::testing::FakeClockPort;
+using move_base2::testing::FakePosePort;
+
+/// Bộ đồ nghề tối thiểu: đồng hồ giả + pose giả, không costmap (behavior họ kNone không cần).
+struct Rig
+{
+  Rig()
+  {
+    pose.setPosition(0.0, 0.0);
+
+    RecoveryRunner::Deps deps;
+    deps.clock = &clock;
+    deps.pose = &pose;
+    runner.setDeps(deps);
+  }
+
+  bool load(const std::string& ns)
+  {
+    runner.setNamespace(ns);
+    robot::NodeHandle nh;
+    return runner.configure(nh);
+  }
+
+  FakeClockPort clock{1000.0};
+  FakePosePort pose;
+  RecoveryRunner runner;
+};
+
+TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  ASSERT_EQ(rig.runner.behaviorCount(), 2u);
+  EXPECT_EQ(rig.runner.behaviorName(0), "wait_short");
+  EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
+}
+
+TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  EXPECT_EQ(rig.runner.outputKind(0), RecoveryOutputKind::kNone);
+  EXPECT_EQ(rig.runner.outputKind(1), RecoveryOutputKind::kNone);
+}
+
+TEST(RecoveryRunner, OutOfRangeIndexIsSafeAndNeverClaimsVelocity)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  // Giả định an toàn: không biết là gì thì không cấp quyền phát vận tốc.
+  EXPECT_EQ(rig.runner.outputKind(99), RecoveryOutputKind::kNone);
+  EXPECT_TRUE(rig.runner.behaviorName(99).empty());
+  EXPECT_FALSE(rig.runner.start(99, RecoveryTrigger::kPlanningFailed));
+}
+
+TEST(RecoveryRunner, StartBeforeConfigureFails)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+}
+
+TEST(RecoveryRunner, UpdateWithoutActiveBehaviorFailsInsteadOfCrashing)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  const RecoveryTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+  EXPECT_FALSE(tick.has_velocity);
+  EXPECT_FALSE(tick.message.empty());
+}
+
+TEST(RecoveryRunner, RunsBehaviorToSuccessOnRealClock)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));  // wait_duration: 1.0 s
+
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);
+}
+
+TEST(RecoveryRunner, NoneFamilyNeverReportsVelocityToTheCore)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+
+  for (int i = 0; i < 5; ++i)
+  {
+    rig.clock.advance(0.3);
+    const RecoveryTick tick = rig.runner.update();
+    // Lõi dùng has_velocity để quyết định có lấy cmd hay không; behavior đứng yên không được bật.
+    EXPECT_FALSE(tick.has_velocity);
+    EXPECT_FALSE(tick.has_path);
+    if (tick.status != RecoveryTick::Status::kRunning)
+    {
+      break;
+    }
+  }
+}
+
+TEST(RecoveryRunner, BehaviorTimeoutSurfacesAsFailed)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  // wait_long: wait_duration 5 s nhưng timeout 3 s -> phải kết thúc bằng kFailed, không treo.
+  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kControllingFailed));
+
+  rig.clock.advance(2.0);
+  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+
+  rig.clock.advance(1.5);
+  const RecoveryTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+  EXPECT_NE(tick.message.find("timeout"), std::string::npos);
+}
+
+TEST(RecoveryRunner, CancelIsSafeWithoutActiveBehavior)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  rig.runner.cancel();  // không được crash
+  SUCCEED();
+}
+
+TEST(RecoveryRunner, CancelledTickSurfacesAsFailedNotRunning)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+
+  rig.runner.cancel();
+  rig.clock.advance(0.1);
+  const RecoveryTick tick = rig.runner.update();
+
+  // State machine hiện tại không tick sau cancel, nên nhánh này không đạt tới trong runtime thật.
+  // Nhưng nếu ai đó nới điều kiện tick, kCancelled phải thành kFailed chứ không im lặng thành
+  // kRunning — đó là lý do nhánh dịch được giữ lại.
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+}
+
+TEST(RecoveryRunner, RestartingSecondBehaviorWorks)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+  rig.clock.advance(1.0);
+  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);
+
+  // State machine chuyển sang behavior kế tiếp sau khi cái trước kết thúc.
+  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kOscillation));
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+}
+
+TEST(RecoveryRunner, EmptyBehaviorListFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("recovery_empty"));
+  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
+}
+
+TEST(RecoveryRunner, MissingLibraryPathFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("recovery_missing_library"));
+  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
+}
+
+TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
+{
+  RecoveryRunner runner;
+  runner.setNamespace("recovery");
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort/PosePort phải hỏng ngay, không phải lúc tick";
+}
+
+TEST(RecoveryRunner, ConfigureTwiceRejected)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(rig.runner.configure(nh));
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/sensor_gateway_test.cpp

@@ -0,0 +1,393 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test đường vào cảm biến.
+ *
+ * Test này dựng `LayeredCostmap` THẬT và cắm vào đó các layer gián điệp. Lý do không dùng costmap
+ * giả: thứ cần khoá lại ở đây chính là ba contract ẩn của `robot_costmap_2d`, và chúng chỉ tồn tại
+ * trong lớp thật —
+ *   1. `Layer::dataCallBack<T>` xoá kiểu về `void*` + `std::type_info`, sai kiểu KHÔNG gây lỗi biên
+ *      dịch mà rơi im lặng;
+ *   2. tham số `name` là khoá topic mà layer so lại, không phải nhãn tự do;
+ *   3. bộ lọc chọn layer quyết định layer nào thấy dữ liệu.
+ * Một costmap giả sẽ mô phỏng lại các contract đó theo cách tôi *nghĩ* chúng hoạt động — đúng loại
+ * test không phát hiện được gì.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+#include <typeinfo>
+#include <vector>
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+
+#include <move_base2/io/sensor_gateway.h>
+
+#include "spy_layer.h"
+
+using move_base2::SensorGateway;
+using move_base2::SensorGatewayConfig;
+using move_base2::testing::attachSpy;
+using move_base2::testing::SpyPtr;
+using robot_costmap_2d::LayerType;
+
+namespace
+{
+
+/**
+ * @class Bench
+ * @brief Một cặp costmap thật + cổng cảm biến đã gắn.
+ */
+class Bench
+{
+public:
+  Bench()
+    : global_("map", false, true)
+    , local_("odom", true, false)
+  {
+    std::string error;
+    EXPECT_TRUE(gateway_.configure(SensorGatewayConfig(), error)) << error;
+  }
+
+  /// @brief Cắm một layer gián điệp vào costmap global và trả con trỏ để kiểm tra sau.
+  SpyPtr addGlobal(LayerType type, const std::string& name, bool enabled = true, bool explode = false)
+  {
+    return attachSpy(global_, type, name, enabled, explode);
+  }
+
+  SpyPtr addLocal(LayerType type, const std::string& name, bool enabled = true, bool explode = false)
+  {
+    return attachSpy(local_, type, name, enabled, explode);
+  }
+
+  void attach()
+  {
+    gateway_.attach(&global_, &local_);
+  }
+
+  SensorGateway& gateway()
+  {
+    return gateway_;
+  }
+
+private:
+  robot_costmap_2d::LayeredCostmap global_;
+  robot_costmap_2d::LayeredCostmap local_;
+  SensorGateway gateway_;
+};
+
+robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F)
+{
+  robot_sensor_msgs::LaserScan scan;
+  scan.header.frame_id = "laser";
+  scan.angle_min = -1.5F;                                             // [rad]
+  scan.angle_max = 1.5F;                                              // [rad]
+  scan.angle_increment = 3.0F / static_cast<float>(rays);             // [rad]
+  scan.range_min = 0.05F;                                             // [m]
+  scan.range_max = 10.0F;                                             // [m]
+  scan.ranges.assign(rays, range);
+  return scan;
+}
+
+robot_sensor_msgs::DepthCameraData::ConstPtr makeDepth()
+{
+  robot_sensor_msgs::DepthCameraData::Ptr data =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  data->header.frame_id = "camera_optical";
+  return data;
+}
+
+}  // namespace
+
+// ================================================================================================
+// Contract ẩn #1 — kiểu phải tới nơi ĐÚNG NHƯ khi gửi đi
+// ================================================================================================
+
+TEST(SensorGateway, LaserScanArrivesWithLaserScanType)
+{
+  Bench bench;
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  ASSERT_EQ(voxel->count(), 1U);
+  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::LaserScan));
+}
+
+TEST(SensorGateway, DepthCameraDataArrivesAsConstPtrNotAsValue)
+{
+  // Đây là test đắt nhất của file. `ObstacleLayer::handleImpl` so
+  // `typeid(DepthCameraData::ConstPtr)`; gửi đi dạng giá trị sẽ khớp `typeid(DepthCameraData)` và
+  // rơi qua MỌI nhánh if mà không có lỗi biên dịch, không có log — depth camera lặng lẽ ngừng hoạt
+  // động. Không có cách nào bắt được lỗi đó ngoài việc kiểm đúng type_info tới nơi.
+  Bench bench;
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushDepthCameraData("/camera/depth/data", makeDepth());
+
+  ASSERT_EQ(voxel->count(), 1U);
+  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr));
+  EXPECT_FALSE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData));
+}
+
+TEST(SensorGateway, StaticMapArrivesWithOccupancyGridType)
+{
+  Bench bench;
+  SpyPtr layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map");
+  bench.attach();
+
+  bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+
+  ASSERT_EQ(layer->count(), 1U);
+  EXPECT_TRUE(*layer->records()[0].type == typeid(robot_nav_msgs::OccupancyGrid));
+}
+
+TEST(SensorGateway, PointCloudAndPointCloud2AreDistinctTypes)
+{
+  Bench bench;
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushPointCloud("/pc", robot_sensor_msgs::PointCloud());
+  bench.gateway().pushPointCloud2("/pc2", robot_sensor_msgs::PointCloud2());
+
+  ASSERT_EQ(voxel->count(), 2U);
+  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::PointCloud));
+  EXPECT_TRUE(*voxel->records()[1].type == typeid(robot_sensor_msgs::PointCloud2));
+}
+
+// ================================================================================================
+// Contract ẩn #2 — `name` là khoá topic, phải tới nguyên văn
+// ================================================================================================
+
+TEST(SensorGateway, TopicNameReachesTheLayerVerbatim)
+{
+  // Layer so chuỗi này với `map_topic` / `observation_sources[i].topic` trong YAML. Sửa nó dù chỉ
+  // một ký tự — kể cả thêm/bớt dấu '/' — là mất hẳn một cảm biến, không cảnh báo.
+  Bench bench;
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+  bench.gateway().pushDepthCameraData("/camera_right/depth/data", makeDepth());
+
+  ASSERT_EQ(voxel->count(), 2U);
+  EXPECT_EQ(voxel->records()[0].topic, "/b_scan");
+  EXPECT_EQ(voxel->records()[1].topic, "/camera_right/depth/data");
+}
+
+// ================================================================================================
+// Contract ẩn #3 — bộ lọc chọn layer (C5)
+// ================================================================================================
+
+TEST(SensorGateway, StaticMapGoesOnlyToStaticLayers)
+{
+  Bench bench;
+  SpyPtr static_layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map");
+  SpyPtr voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles");
+  SpyPtr inflation = bench.addGlobal(LayerType::INFLATION_LAYER, "inflation");
+  bench.attach();
+
+  bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+
+  EXPECT_EQ(static_layer->count(), 1U);
+  EXPECT_EQ(voxel->count(), 0U);
+  EXPECT_EQ(inflation->count(), 0U);
+}
+
+TEST(SensorGateway, ObstacleDataGoesOnlyToVoxelLayers)
+{
+  Bench bench;
+  SpyPtr static_layer = bench.addLocal(LayerType::STATIC_LAYER, "navigation_map");
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  SpyPtr inflation = bench.addLocal(LayerType::INFLATION_LAYER, "inflation");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(voxel->count(), 1U);
+  EXPECT_EQ(static_layer->count(), 0U);
+  EXPECT_EQ(inflation->count(), 0U);
+}
+
+TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample)
+{
+  // Hồi quy cho C5. Bản `move_base` cũ lọc bằng
+  //     getType() == layer_type || getName() == name
+  // Vế thứ hai là bẫy: đặt tên một layer trùng tên topic thì nó nhận dữ liệu nó không hiểu. Với
+  // InflationLayer — có handleImpl chỉ biết log error — hậu quả là spam log ở đúng tần số cảm biến.
+  Bench bench;
+  SpyPtr trap = bench.addLocal(LayerType::INFLATION_LAYER, "/b_scan");
+  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(trap->count(), 0U) << "layer trùng TÊN topic nhưng sai KIỂU vẫn nhận được dữ liệu";
+  EXPECT_EQ(voxel->count(), 1U);
+}
+
+TEST(SensorGateway, SampleReachesBothGlobalAndLocalCostmaps)
+{
+  Bench bench;
+  SpyPtr global_voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles");
+  SpyPtr local_voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(global_voxel->count(), 1U);
+  EXPECT_EQ(local_voxel->count(), 1U);
+  EXPECT_EQ(bench.gateway().stats().delivered, 2U);
+}
+
+// ================================================================================================
+// Layer tắt (C8) và các nhánh bỏ mẫu — phải ĐẾM được, không im lặng
+// ================================================================================================
+
+TEST(SensorGateway, DisabledLayerIsSkippedAndCounted)
+{
+  Bench bench;
+  SpyPtr disabled = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles", /*enabled=*/false);
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(disabled->count(), 0U);
+  EXPECT_EQ(bench.gateway().stats().skipped_disabled, 1U);
+  EXPECT_EQ(bench.gateway().stats().delivered, 0U);
+}
+
+TEST(SensorGateway, SamplesBeforeAnyCostmapIsAttachedAreCounted)
+{
+  // Bản cũ mở đầu bằng `if (!costmap) return;` không log. Mọi mẫu tới trước khi costmap tồn tại
+  // biến mất không dấu vết, và đó là trạng thái BÌNH THƯỜNG lúc khởi động.
+  SensorGateway gateway;
+  std::string error;
+  ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error;
+  ASSERT_FALSE(gateway.attached());
+
+  gateway.pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());
+  gateway.pushLaserScan("/b_scan", makeScan());
+  gateway.pushDepthCameraData("/camera/depth/data", makeDepth());
+
+  EXPECT_EQ(gateway.stats().dropped_no_costmap, 3U);
+  EXPECT_EQ(gateway.stats().delivered, 0U);
+}
+
+TEST(SensorGateway, NullDepthPointerIsIgnoredWithoutCountingAsADrop)
+{
+  Bench bench;
+  bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushDepthCameraData("/camera/depth/data",
+                                      robot_sensor_msgs::DepthCameraData::ConstPtr());
+
+  EXPECT_EQ(bench.gateway().stats().delivered, 0U);
+  EXPECT_EQ(bench.gateway().stats().dropped_no_costmap, 0U);
+}
+
+TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes)
+{
+  // Bản cũ bọc try/catch quanh CẢ vòng lặp rồi `return`, nên một layer ném exception làm mọi layer
+  // đứng sau nó mất luôn mẫu đó — và với một layer hỏng cố định thì mất vĩnh viễn.
+  Bench bench;
+  SpyPtr exploding = bench.addLocal(LayerType::VOXEL_LAYER, "broken", true, /*explode=*/true);
+  SpyPtr healthy = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
+  bench.attach();
+
+  bench.gateway().pushLaserScan("/b_scan", makeScan());
+
+  EXPECT_EQ(exploding->count(), 0U);
+  EXPECT_EQ(healthy->count(), 1U) << "layer lành bị bỏ qua vì layer trước nó ném exception";
+  EXPECT_EQ(bench.gateway().stats().layer_exceptions, 1U);
+  EXPECT_EQ(bench.gateway().stats().delivered, 1U);
+}
+
+// ================================================================================================
+// Lọc laser (C7)
+// ================================================================================================
+
+TEST(SensorGatewayConfigTest, LaserFilterIsOffByDefaultAndLeavesTheScanUntouched)
+{
+  SensorGateway gateway;
+  std::string error;
+  ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error;
+
+  const robot_sensor_msgs::LaserScan scan = makeScan();
+  const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan);
+
+  EXPECT_EQ(prepared.ranges, scan.ranges);
+  EXPECT_EQ(prepared.angle_increment, scan.angle_increment);
+}
+
+TEST(SensorGatewayConfigTest, EnabledLaserFilterKeepsScanStructure)
+{
+  // Bộ lọc giữ nguyên cấu trúc scan (outlier thành NaN) — chỉ số tia phải khớp một-một với góc, nếu
+  // không thì mọi phép chiếu tia sang điểm sau đó đều lệch.
+  SensorGatewayConfig config;
+  config.laser_sor_enabled = true;
+  config.laser_sor_mean_k = 5;
+  config.laser_sor_stddev_mul = 1.0;
+
+  SensorGateway gateway;
+  std::string error;
+  ASSERT_TRUE(gateway.configure(config, error)) << error;
+
+  const robot_sensor_msgs::LaserScan scan = makeScan();
+  const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan);
+
+  EXPECT_EQ(prepared.ranges.size(), scan.ranges.size());
+  EXPECT_EQ(prepared.angle_increment, scan.angle_increment);
+  EXPECT_EQ(prepared.header.frame_id, scan.header.frame_id);
+}
+
+TEST(SensorGatewayConfigTest, RejectsOutOfRangeFilterParametersOnlyWhenFilterIsOn)
+{
+  std::string error;
+
+  SensorGatewayConfig off;
+  off.laser_sor_enabled = false;
+  off.laser_sor_mean_k = 0;         // vô nghĩa, nhưng tính năng đang tắt
+  off.laser_sor_stddev_mul = -1.0;  // vô nghĩa, nhưng tính năng đang tắt
+  EXPECT_TRUE(off.validate(error)) << error;
+
+  SensorGatewayConfig bad_k;
+  bad_k.laser_sor_enabled = true;
+  bad_k.laser_sor_mean_k = 1;
+  EXPECT_FALSE(bad_k.validate(error));
+
+  SensorGatewayConfig bad_mul;
+  bad_mul.laser_sor_enabled = true;
+  bad_mul.laser_sor_stddev_mul = 0.0;
+  EXPECT_FALSE(bad_mul.validate(error));
+}
+
+TEST(SensorGatewayConfigTest, ConfigureFailsAndReportsWhyOnInvalidParameters)
+{
+  SensorGatewayConfig config;
+  config.laser_sor_enabled = true;
+  config.laser_sor_mean_k = 0;
+
+  SensorGateway gateway;
+  std::string error;
+  EXPECT_FALSE(gateway.configure(config, error));
+  EXPECT_FALSE(error.empty());
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/spy_layer.h

@@ -0,0 +1,116 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — layer gián điệp dùng chung cho các test đường vào cảm biến.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_TEST_SPY_LAYER_H_
+#define MOVE_BASE2_TEST_SPY_LAYER_H_
+
+#include <cstddef>
+#include <functional>
+#include <stdexcept>
+#include <string>
+#include <typeinfo>
+#include <vector>
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+
+namespace move_base2
+{
+namespace testing
+{
+
+/**
+ * @class SpyLayer
+ * @brief Layer chỉ ghi lại những gì nó nhận được.
+ *
+ * Dùng với `LayeredCostmap` **thật**: ba contract ẩn cần khoá lại (type erasure qua `void*` +
+ * `type_info`, `name` là khoá topic, bộ lọc chọn layer) đều nằm trong lớp thật, nên một costmap giả
+ * chỉ mô phỏng lại chúng theo cách người viết test *nghĩ* chúng hoạt động.
+ *
+ * @note `Layer::Layer()` đặt `enabled_ = false`; layer thật bật cờ này khi đọc config. Ở đây phải tự
+ *       bật — và chính chỗ đó cho phép test nhánh "bỏ qua layer đang tắt".
+ */
+class SpyLayer : public robot_costmap_2d::Layer
+{
+public:
+  struct Record
+  {
+    const std::type_info* type = nullptr;  ///< type_info có storage tĩnh nên giữ con trỏ là an toàn.
+    std::string topic;
+  };
+
+  /// @brief Hook để test tự sao chép phần dữ liệu nó quan tâm — con trỏ `data` treo sau khi trả về.
+  using Observer = std::function<void(const void*, const std::type_info&, const std::string&)>;
+
+  explicit SpyLayer(robot_costmap_2d::LayerType type, bool enabled = true, bool explode = false)
+    : type_(type), explode_(explode)
+  {
+    enabled_ = enabled;
+  }
+
+  robot_costmap_2d::LayerType getType() const override
+  {
+    return type_;
+  }
+
+  void setObserver(Observer observer)
+  {
+    observer_ = std::move(observer);
+  }
+
+  const std::vector<Record>& records() const
+  {
+    return records_;
+  }
+
+  std::size_t count() const
+  {
+    return records_.size();
+  }
+
+protected:
+  void handleImpl(const void* data, const std::type_info& type, const std::string& topic) override
+  {
+    if (explode_)
+    {
+      throw std::runtime_error("SpyLayer được yêu cầu ném exception");
+    }
+    records_.push_back(Record{ &type, topic });
+    if (observer_)
+    {
+      observer_(data, type, topic);
+    }
+  }
+
+private:
+  robot_costmap_2d::LayerType type_;
+  bool explode_;
+  Observer observer_;
+  std::vector<Record> records_;
+};
+
+using SpyPtr = boost::shared_ptr<SpyLayer>;
+
+/// @brief Cắm một layer gián điệp vào @p costmap và trả con trỏ để kiểm tra sau.
+inline SpyPtr attachSpy(robot_costmap_2d::LayeredCostmap& costmap,
+                        robot_costmap_2d::LayerType type, const std::string& name,
+                        bool enabled = true, bool explode = false)
+{
+  SpyPtr spy = boost::make_shared<SpyLayer>(type, enabled, explode);
+  spy->initialize(&costmap, name, nullptr);
+  costmap.addPlugin(spy);
+  return spy;
+}
+
+}  // namespace testing
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_TEST_SPY_LAYER_H_

test/velocity_arbiter_test.cpp

@@ -0,0 +1,398 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test bộ trọng tài vận tốc.
+ *
+ * Ba quy tắc phải được chứng minh chứ không chỉ được ghi trong comment: kNone phát 0, mọi lệnh đi
+ * qua sanitize, và đổi nguồn luôn chèn một cycle 0.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <limits>
+#include <string>
+
+#include <move_base2/core/velocity_arbiter.h>
+
+using move_base2::VelocityArbiter;
+using move_base2::VelocityLimits;
+using move_base2::VelocitySource;
+
+namespace
+{
+
+constexpr double kDt = 0.05;  ///< [s] chu kỳ dùng trong test
+
+VelocityLimits baseLimits()
+{
+  VelocityLimits limits;
+  limits.max_vel_x = 0.5;         // [m/s]
+  limits.min_vel_x = -0.2;        // [m/s]
+  limits.max_vel_theta = 1.0;     // [rad/s]
+  limits.max_accel_x = 100.0;     // [m/s^2] rất lớn: mặc định tắt ảnh hưởng của giới hạn gia tốc
+  limits.max_accel_theta = 100.0; // [rad/s^2]
+  limits.zero_velocity_epsilon = 1e-3;
+  return limits;
+}
+
+robot_geometry_msgs::Twist twist(double linear_x, double angular_z)
+{
+  robot_geometry_msgs::Twist cmd;
+  cmd.linear.x = linear_x;
+  cmd.angular.z = angular_z;
+  return cmd;
+}
+
+VelocityArbiter makeArbiter(const VelocityLimits& limits = baseLimits())
+{
+  VelocityArbiter arbiter;
+  std::string error;
+  EXPECT_TRUE(arbiter.configure(limits, error)) << error;
+  return arbiter;
+}
+
+}  // namespace
+
+// ================================================================================================
+// Cấu hình
+// ================================================================================================
+
+TEST(VelocityLimits, RejectsNonPositiveMaxVelX)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_vel_x = 0.0;
+
+  std::string error;
+  EXPECT_FALSE(limits.validate(error));
+  EXPECT_NE(error.find("max_vel_x"), std::string::npos);
+}
+
+TEST(VelocityLimits, RejectsPositiveMinVelXBecauseItIsTheReverseLimit)
+{
+  VelocityLimits limits = baseLimits();
+  limits.min_vel_x = 0.3;
+
+  std::string error;
+  EXPECT_FALSE(limits.validate(error));
+  EXPECT_NE(error.find("min_vel_x"), std::string::npos);
+}
+
+TEST(VelocityLimits, RejectsNonPositiveAccelerations)
+{
+  std::string error;
+
+  VelocityLimits linear = baseLimits();
+  linear.max_accel_x = 0.0;
+  EXPECT_FALSE(linear.validate(error));
+
+  VelocityLimits angular = baseLimits();
+  angular.max_accel_theta = -1.0;
+  EXPECT_FALSE(angular.validate(error));
+}
+
+TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero)
+{
+  VelocityLimits limits = baseLimits();
+  limits.min_vel_x = 0.0;
+  EXPECT_NE(limits.describe().find("cấm lùi"), std::string::npos);
+}
+
+TEST(VelocityArbiter, RefusesToEmitBeforeConfigure)
+{
+  VelocityArbiter arbiter;
+  EXPECT_FALSE(arbiter.initialized());
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
+}
+
+TEST(VelocityArbiter, ConfigureFailsLoudlyOnBadLimits)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_vel_theta = -1.0;
+
+  VelocityArbiter arbiter;
+  std::string error;
+  EXPECT_FALSE(arbiter.configure(limits, error));
+  EXPECT_FALSE(arbiter.initialized());
+}
+
+// ================================================================================================
+// Quy tắc 1 — nguồn kNone phát 0
+// ================================================================================================
+
+TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.8), kDt);
+  ASSERT_GT(arbiter.lastCommand().linear.x, 0.0);
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(0.5, 0.8), kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "lệnh 0 phải tức thì, không giảm tốc dần";
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
+  EXPECT_TRUE(arbiter.stopped());
+}
+
+TEST(VelocityArbiter, NoneSourceIgnoresCandidateEntirely)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(99.0, 99.0), kDt);
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
+  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
+}
+
+// ================================================================================================
+// Quy tắc 2 — sanitize
+// ================================================================================================
+
+TEST(VelocityArbiter, NaNIsBlockedAndCounted)
+{
+  VelocityArbiter arbiter = makeArbiter();
+  const double nan_value = std::numeric_limits<double>::quiet_NaN();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(nan_value, 0.3), kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "một trục hỏng làm hỏng cả lệnh, không sửa từng phần";
+  EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
+}
+
+TEST(VelocityArbiter, InfinityIsBlockedToo)
+{
+  VelocityArbiter arbiter = makeArbiter();
+  const double inf_value = std::numeric_limits<double>::infinity();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.2, inf_value), kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
+  EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
+}
+
+TEST(VelocityArbiter, ForwardVelocityIsClampedToMax)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.5);
+  EXPECT_EQ(arbiter.velocityClamps(), 1u);
+}
+
+TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-9.0, 0.0), kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x là trần LÙI, không phải cận dưới bằng 0";
+}
+
+TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero)
+{
+  VelocityLimits limits = baseLimits();
+  limits.min_vel_x = 0.0;
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-0.5, 0.0), kDt);
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+}
+
+TEST(VelocityArbiter, YawRateIsClampedBothDirections)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, 5.0), kDt).angular.z,
+                   1.0);
+  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, -5.0), kDt).angular.z,
+                   -1.0);
+}
+
+TEST(VelocityArbiter, LateralAndUnusedAxesAreDropped)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  robot_geometry_msgs::Twist candidate = twist(0.2, 0.1);
+  candidate.linear.y = 0.7;
+  candidate.linear.z = 0.7;
+  candidate.angular.x = 0.7;
+  candidate.angular.y = 0.7;
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, candidate, kDt);
+
+  EXPECT_DOUBLE_EQ(cmd.linear.y, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.linear.z, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.x, 0.0);
+  EXPECT_DOUBLE_EQ(cmd.angular.y, 0.0);
+}
+
+TEST(VelocityArbiter, AccelerationIsLimitedByRealDtNotNominalPeriod)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_accel_x = 1.0;  // [m/s^2]
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  // dt = 0.05 s -> bước nhảy tối đa 0.05 m/s.
+  const auto small_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.05);
+  EXPECT_NEAR(small_step.linear.x, 0.05, 1e-9);
+  EXPECT_EQ(arbiter.accelerationClamps(), 1u);
+
+  // Cycle chậm gấp 10: dt = 0.5 s -> bước nhảy tối đa 0.5 m/s, nên đạt luôn trần vận tốc.
+  const auto big_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.5);
+  EXPECT_NEAR(big_step.linear.x, 0.5, 1e-9);
+}
+
+TEST(VelocityArbiter, NonPositiveDtSkipsAccelerationLimitInsteadOfInventingOne)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_accel_x = 1.0;
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.0);
+  EXPECT_NEAR(cmd.linear.x, 0.5, 1e-9);
+}
+
+TEST(VelocityArbiter, DecelerationIsAlsoLimited)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_accel_x = 1.0;
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  // Tăng dần tới 0.3 m/s.
+  for (int i = 0; i < 20; ++i)
+  {
+    arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), 0.05);
+  }
+  ASSERT_NEAR(arbiter.lastCommand().linear.x, 0.3, 1e-6);
+
+  // Yêu cầu về 0 ngay: vẫn cùng nguồn nên bị giới hạn gia tốc chặn lại.
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.0, 0.0), 0.05);
+  EXPECT_NEAR(cmd.linear.x, 0.25, 1e-6);
+}
+
+// ================================================================================================
+// Quy tắc 3 — đổi nguồn chèn một cycle 0
+// ================================================================================================
+
+TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  const auto controlling = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
+  ASSERT_NEAR(controlling.linear.x, 0.4, 1e-9);
+
+  const auto handover = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
+  EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "cycle bàn giao phải là 0";
+  EXPECT_EQ(arbiter.handoverCycles(), 1u);
+
+  const auto recovering = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
+  EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "chỉ đúng MỘT cycle 0, không nhiều hơn";
+}
+
+TEST(VelocityArbiter, HandoverWorksInBothDirections)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt);
+  arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt);
+  ASSERT_NEAR(arbiter.lastCommand().linear.x, -0.1, 1e-9);
+
+  EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x,
+                   0.0);
+  EXPECT_NEAR(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x, 0.3,
+              1e-9);
+  EXPECT_EQ(arbiter.handoverCycles(), 1u);
+}
+
+TEST(VelocityArbiter, GoingThroughNoneDoesNotCountAsHandover)
+{
+  // kController -> kNone -> kController: cycle kNone đã ép về 0 rồi, không cần chèn thêm.
+  VelocityArbiter arbiter = makeArbiter();
+
+  arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
+  arbiter.arbitrate(VelocitySource::kNone, twist(0.0, 0.0), kDt);
+
+  const auto resumed = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt);
+  EXPECT_NEAR(resumed.linear.x, 0.4, 1e-9);
+  EXPECT_EQ(arbiter.handoverCycles(), 0u);
+}
+
+TEST(VelocityArbiter, FirstCommandAfterConfigureNeedsNoHandover)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt);
+  EXPECT_NEAR(cmd.linear.x, 0.3, 1e-9);
+  EXPECT_EQ(arbiter.handoverCycles(), 0u);
+}
+
+// ================================================================================================
+// Dừng khẩn và reset
+// ================================================================================================
+
+TEST(VelocityArbiter, EmergencyStopIgnoresAccelerationLimit)
+{
+  VelocityLimits limits = baseLimits();
+  limits.max_accel_x = 0.01;  // giảm tốc bình thường sẽ mất rất nhiều cycle
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  for (int i = 0; i < 100; ++i)
+  {
+    arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), kDt);
+  }
+  ASSERT_GT(arbiter.lastCommand().linear.x, 0.0);
+
+  const auto cmd = arbiter.emergencyStop();
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
+  EXPECT_TRUE(arbiter.stopped());
+  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
+}
+
+TEST(VelocityArbiter, ResetClearsCountersAndHistory)
+{
+  VelocityArbiter arbiter = makeArbiter();
+
+  arbiter.arbitrate(VelocitySource::kController,
+                    twist(std::numeric_limits<double>::quiet_NaN(), 0.0), kDt);
+  arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt);
+  arbiter.arbitrate(VelocitySource::kRecovery, twist(0.1, 0.0), kDt);
+  ASSERT_GT(arbiter.nonFiniteRejections(), 0u);
+  ASSERT_GT(arbiter.velocityClamps(), 0u);
+  ASSERT_GT(arbiter.handoverCycles(), 0u);
+
+  arbiter.reset();
+
+  EXPECT_EQ(arbiter.nonFiniteRejections(), 0u);
+  EXPECT_EQ(arbiter.velocityClamps(), 0u);
+  EXPECT_EQ(arbiter.accelerationClamps(), 0u);
+  EXPECT_EQ(arbiter.handoverCycles(), 0u);
+  EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone);
+  EXPECT_TRUE(arbiter.stopped());
+}
+
+TEST(VelocityArbiter, StoppedUsesEpsilonNotExactZero)
+{
+  VelocityLimits limits = baseLimits();
+  limits.zero_velocity_epsilon = 0.01;
+  VelocityArbiter arbiter = makeArbiter(limits);
+
+  arbiter.arbitrate(VelocitySource::kController, twist(0.005, 0.0), kDt);
+  EXPECT_TRUE(arbiter.stopped());
+
+  arbiter.arbitrate(VelocitySource::kController, twist(0.05, 0.0), kDt);
+  EXPECT_FALSE(arbiter.stopped());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}