optimal & fix file cmake

2026-08-03 22:41:32 +07:00
parent d8babff20b
commit 701d25f952
70 changed files with 5572 additions and 1146 deletions
--- a/test/walking_skeleton_test.cpp
+++ b/test/walking_skeleton_test.cpp
@@ -31,6 +31,7 @@ using move_base2::testing::ControllerScript;
 using move_base2::testing::FakeActionPort;
 using move_base2::testing::FakeClockPort;
 using move_base2::testing::FakeControllerPort;
+using move_base2::testing::FakeCostmapStatusPort;
 using move_base2::testing::FakeMissionPort;
 using move_base2::testing::FakePlannerPort;
 using move_base2::testing::FakePosePort;
@@ -65,8 +66,6 @@ ControlLoopConfig baseConfig()

  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.docking.local_planner_name = "FakeDockPlanner";
@@ -128,6 +127,7 @@ public:
    deps_.recovery = &recovery_;
    deps_.mission = &mission_;
    deps_.action = &action_;
+    deps_.costmap_status = &costmap_status_;

    std::string error;
    EXPECT_TRUE(loop_.configure(config, deps_, error)) << error;
@@ -175,6 +175,7 @@ public:
  FakeRecoveryPort recovery_;
  FakeMissionPort mission_;
  FakeActionPort action_;
+  FakeCostmapStatusPort costmap_status_;
  ControlLoopDeps deps_;

 private:
@@ -218,7 +219,7 @@ TEST(ControlLoop, RefusesToConfigureWithMissingPorts)

  std::string error;
  EXPECT_FALSE(loop.configure(baseConfig(), deps, error));
-  EXPECT_NE(error.find("cổng"), std::string::npos);
+  EXPECT_NE(error.find("port"), std::string::npos);
  EXPECT_FALSE(loop.initialized());
 }

@@ -286,24 +287,89 @@ TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded)
  EXPECT_NE(reason.find("global planner"), std::string::npos);
 }

-TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances)
+TEST(ControlLoop, ProfileSelectsItsOwnLocalPlanner)
 {
  Fixture fixture;

  NavigationRequest docking = makeRequest(1.0);
  docking.profile = MotionProfile::kDocking;
+  docking.marker = "dock_a";

  std::string reason;
  ASSERT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
  EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
-  EXPECT_NEAR(fixture.controller_.xyTolerance(), 0.15, 1e-9) << "tolerance 0 -> dùng default profile";
+  // Marker phải tới port TRƯỚC khi swap — planner đọc maker_name trong initialize().
+  EXPECT_EQ(fixture.controller_.lastDockingMarker(), "dock_a");

  NavigationRequest rotate = makeRequest(1.0);
  rotate.profile = MotionProfile::kRotate;
-  rotate.tolerance.yaw = 0.02;  // [rad]
  ASSERT_TRUE(fixture.loop_.submit(rotate, reason)) << reason;
  EXPECT_EQ(fixture.controller_.activeController(), "FakeRotatePlanner");
-  EXPECT_NEAR(fixture.controller_.yawTolerance(), 0.02, 1e-9);
+}
+
+TEST(ControlLoop, DockingMarkerProfileOverridesBothPlannersAndFallsBackToDefault)
+{
+  ControlLoopConfig config = baseConfig();
+  config.docking_marker_profiles["trolley"].global_planner_name = "FakeTrolleyGlobalPlanner";
+  config.docking_marker_profiles["trolley"].local_planner_name = "FakeTrolleyLocalPlanner";
+  Fixture fixture(config);
+
+  NavigationRequest trolley = makeRequest(1.0);
+  trolley.profile = MotionProfile::kDocking;
+  trolley.marker = "trolley";
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(trolley, reason)) << reason;
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeTrolleyGlobalPlanner");
+  EXPECT_EQ(fixture.controller_.activeController(), "FakeTrolleyLocalPlanner");
+
+  NavigationRequest unconfigured = makeRequest(1.0);
+  unconfigured.profile = MotionProfile::kDocking;
+  unconfigured.marker = "charger";
+  ASSERT_TRUE(fixture.loop_.submit(unconfigured, reason)) << reason;
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeGlobalPlanner");
+  EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
+}
+
+TEST(ControlLoop, RejectsDockingRequestWithoutMarker)
+{
+  Fixture fixture;
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+  // marker cố ý bỏ trống — bản cũ cũng chặn tại cửa dockTo.
+
+  std::string reason;
+  EXPECT_FALSE(fixture.loop_.submit(docking, reason));
+  EXPECT_NE(reason.find("marker"), std::string::npos) << reason;
+}
+
+TEST(ControlLoop, AllowsGoalFrameStyleDockingWithoutMarkerWhenConfigured)
+{
+  ControlLoopConfig config = baseConfig();
+  config.docking_requires_marker = false;
+  Fixture fixture(config);
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+
+  std::string reason;
+  EXPECT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
+  EXPECT_TRUE(fixture.controller_.lastDockingMarker().empty());
+}
+
+TEST(ControlLoop, RejectsDockingRequestWhenMarkerIsUnknown)
+{
+  Fixture fixture;
+  fixture.controller_.setDockingMarkerSucceeds(false);  // marker không có trong maker_sources
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+  docking.marker = "tram_la";
+
+  std::string reason;
+  EXPECT_FALSE(fixture.loop_.submit(docking, reason));
+  EXPECT_NE(reason.find("tram_la"), std::string::npos) << reason;
 }

 // ================================================================================================
@@ -329,7 +395,7 @@ TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce)
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_EQ(fixture.mission_.reportCountFor(42), 1u)
-      << "mỗi chặng chỉ được báo kết quả đúng một lần";
+      << "each leg may report its outcome exactly once";
 }

 TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
@@ -343,7 +409,7 @@ TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)

  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_TRUE(fixture.mission_.reports().empty())
-      << "goal trực tiếp không thuộc mission nào thì không báo lên mission layer";
+      << "a direct goal belongs to no mission so nothing is reported to the mission layer";
 }

 TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling)
@@ -397,9 +463,9 @@ TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering)
    if (move_base2::mustBeStopped(state))
    {
      ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
-          << "state " << move_base2::toString(state) << " ở cycle " << i;
+          << "state " << move_base2::toString(state) << " at cycle " << i;
      ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0)
-          << "state " << move_base2::toString(state) << " ở cycle " << i;
+          << "state " << move_base2::toString(state) << " at cycle " << i;
    }

    if (!running)
@@ -454,7 +520,8 @@ TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan)
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
  fixture.run();

-  EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "không được đẩy plan rỗng xuống controller";
+  EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "an empty plan must not be pushed down to "
+                                                       "the controller";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
 }

@@ -476,13 +543,63 @@ TEST(ControlLoop, LostPoseStopsTheRobotImmediately)
  fixture.loop_.step();

  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
-      << "mất TF thì phải dừng ngay, không đi tiếp bằng pose cũ";
+      << "losing TF must stop the robot immediately, not keep going on a stale pose";
 }

 // ================================================================================================
 // Recovery
 // ================================================================================================

+TEST(ControlLoop, PrimaryPlannerFailureUsesBackupBeforeRecovery)
+{
+  ControlLoopConfig config = baseConfig();
+  config.backup_global_planner_name = "FakeBackupGlobalPlanner";
+  // 0 nghĩa là không retry planner hiện tại. Backup vẫn phải có đúng một lượt riêng trước recovery.
+  config.state_machine.max_planning_retries = 0;
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kOk});
+  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
+
+  std::string reason;
+  NavigationRequest request = makeRequest(3.0, 71);
+  request.order = std::make_shared<robot_protocol_msgs::Order>();
+  ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason;
+  fixture.run();
+
+  EXPECT_FALSE(fixture.hitLimit());
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
+  EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
+  EXPECT_EQ(fixture.planner_.orderHistory(), (std::vector<bool>{true, false}));
+  EXPECT_EQ(fixture.recovery_.startCount(), 0u);
+  EXPECT_EQ(fixture.states(),
+            (std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING", "SUCCEEDED"}))
+      << join(fixture.states());
+}
+
+TEST(ControlLoop, BackupPlannerFailureEscalatesToRecovery)
+{
+  ControlLoopConfig config = baseConfig();
+  config.backup_global_planner_name = "FakeBackupGlobalPlanner";
+  config.state_machine.max_planning_retries = 0;
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kFail});
+  fixture.recovery_.setScript({RecoveryScript::kRunning});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 72), reason)) << reason;
+
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
+  {
+    fixture.stepOnce();
+  }
+
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
+  EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kRecovering);
+  EXPECT_EQ(fixture.recovery_.startCount(), 1u);
+  EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed);
+}
+
 TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
 {
  // Kịch bản thật: planner bế tắc cho tới khi recovery gỡ được thế, sau đó lập plan bình thường.
@@ -521,7 +638,7 @@ TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
    fixture.clock_.advance(kControlPeriod);
  }

-  EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery";
+  EXPECT_TRUE(planner_unblocked) << "never entered recovery";
  EXPECT_EQ(states, (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING",
                                              "CONTROLLING", "SUCCEEDED"}))
      << join(states);
@@ -548,7 +665,7 @@ TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport)
      << join(fixture.states());

  EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}))
-      << "phải chạy lần lượt từng behavior, không lặp lại behavior đầu";
+      << "behaviors must run one after another, the first one must not repeat";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
  EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u);
@@ -566,7 +683,7 @@ TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior)

  EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}));
  EXPECT_EQ(fixture.recovery_.updateCount(), 0u)
-      << "không được tick một behavior chưa start thành công";
+      << "a behavior that never started successfully must not be ticked";
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
 }

@@ -590,7 +707,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
        fixture.loop_.lastCommand().linear.x < -1e-6)
    {
      saw_reverse = true;
-      EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "lệnh lùi phải nằm trong trần";
+      EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "the reverse command must stay "
+                                                               "within the limit";
    }
    if (!running)
    {
@@ -599,7 +717,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
    fixture.clock_.advance(kControlPeriod);
  }

-  EXPECT_TRUE(saw_reverse) << "recovery phát vận tốc nhưng lệnh không tới được đầu ra";
+  EXPECT_TRUE(saw_reverse) << "recovery published a velocity but the command never reached the "
+                              "output";
 }

 TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure)
@@ -652,7 +771,7 @@ TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled)
    }
  }

-  EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ";
+  EXPECT_LE(cycles_to_zero, 2) << "cmd_vel must reach 0 within 2 cycles after a cancel";
  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
  EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED");
@@ -742,8 +861,8 @@ TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce)
      }
    }

-    ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg;
-    ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg;
+    ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "leg " << leg;
+    ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "leg " << leg;
  }

  EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u);
@@ -813,7 +932,7 @@ TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce)
  ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted);
  EXPECT_EQ(fixture.mission_.reportCountFor(13), 1u);
-  EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "action hỏng không được kéo recovery vào";
+  EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "a failed action must not drag recovery in";
 }

 TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
@@ -833,7 +952,7 @@ TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)

  // Không goal lẫn action thì bị từ chối bất kể có port hay không.
  EXPECT_FALSE(fixture.loop_.submit(makeActionOnlyRequest(0), reason));
-  EXPECT_NE(reason.find("goal lẫn action"), std::string::npos) << reason;
+  EXPECT_NE(reason.find("neither goal nor action"), std::string::npos) << reason;
 }

 // ================================================================================================
@@ -859,7 +978,7 @@ TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking)
  {
    fixture.stepOnce();
    EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
-        << "rời PLANNING khi planner còn đang tính, cycle " << i;
+        << "left PLANNING while the planner was still computing, cycle " << i;
  }

  fixture.stepOnce();
@@ -882,7 +1001,7 @@ TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning)
  }

  EXPECT_EQ(fixture.planner_.makePlanCount(), 1u)
-      << "lượt lập plan bị khởi động lại mỗi cycle";
+      << "the planning attempt was restarted every cycle";
 }

 TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
@@ -908,7 +1027,7 @@ TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
  // recovery -> ABORTED. Điều phải khoá lại là nó KHÔNG đứng im ở PLANNING.
  EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
            fixture.states().end())
-      << "planner treo mà không ai escalate — robot đứng ở PLANNING vĩnh viễn: "
+      << "the planner hung and nobody escalated — the robot would sit in PLANNING forever: "
      << join(fixture.states());
  EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states());
 }
@@ -935,7 +1054,7 @@ TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackgroun
    fixture.stepOnce();
    EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
    EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9)
-        << "cmd_vel gián đoạn trong lúc lập lại plan, cycle " << i;
+        << "cmd_vel was interrupted while replanning, cycle " << i;
  }
 }

@@ -978,7 +1097,7 @@ TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan)
  fixture.stepOnce();

  EXPECT_GT(fixture.planner_.cancelCount(), before)
-      << "yêu cầu mới không huỷ lượt lập plan của goal cũ";
+      << "a new request did not cancel the planning attempt of the old goal";
 }

 TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
@@ -1026,8 +1145,8 @@ TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
-      << "goal mới nằm chờ thay vì thay goal cũ ngay";
-  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
+      << "the new goal was queued instead of replacing the old one right away";
+  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "did not replan for the new goal";
 }

 TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
@@ -1044,7 +1163,8 @@ TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
-  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
+  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "the planning attempt for the old goal was not "
+                                                   "cancelled";
 }

 TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
@@ -1063,8 +1183,10 @@ TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
  fixture.stepOnce();

-  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
-  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
+  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "the preempted leg was not reported "
+                                                        "exactly once";
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "the NEW leg got its outcome reported the "
+                                                        "moment it was accepted";
 }

 TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
@@ -1108,7 +1230,120 @@ TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
  fixture.stepOnce();

  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
-  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
+  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "the running behavior was not told to stop";
+}
+
+// ================================================================================================
+// Guard "không đi mù" — dữ liệu quan sát của costmap quá hạn
+// ================================================================================================
+//
+// move_base thế hệ 1 có đúng guard này (`move_base.cpp:2720`) và move_base2 trước đây KHÔNG có:
+// costmap hết hạn nghĩa là robot đang tránh vật cản trên một bản đồ của quá khứ.
+
+TEST(StaleCostmap, BlocksWheelsWhileControlling)
+{
+  Fixture fixture;
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+
+  // Chạy tới khi đang bám plan và thực sự có lệnh khác 0.
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+  fixture.loop_.step();
+  ASSERT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0) << "no command to block yet";
+
+  const std::size_t controller_calls_before = fixture.controller_.computeCount();
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+
+  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0) << "sensor data is stale yet it kept "
+                                                                 "driving";
+  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0);
+  EXPECT_EQ(fixture.controller_.computeCount(), controller_calls_before)
+      << "the controller must not compute a command on stale data";
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling)
+      << "the guard only blocks the wheels, it does not change state — exactly like the old "
+         "version";
+}
+
+TEST(StaleCostmap, ResumesWhenSensorDataBecomesCurrentAgain)
+{
+  Fixture fixture;
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+  ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
+
+  fixture.costmap_status_.setCurrent(true);
+  fixture.loop_.step();
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
+      << "sensors are fresh again yet the robot stays still";
+}
+
+TEST(StaleCostmap, DisabledByConfigLetsTheRobotDrive)
+{
+  ControlLoopConfig config = baseConfig();
+  config.require_current_costmap = false;
+
+  Fixture fixture(config);
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
+      << "the guard is disabled by config yet it still blocked";
+}
+
+TEST(StaleCostmap, NullPortMeansNoGuard)
+{
+  // Đường đi của mọi test cổng-giả có sẵn: không ai bơm costmap_status thì lõi coi là còn hạn.
+  Fixture fixture;
+  fixture.deps_.costmap_status = nullptr;
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.configure(baseConfig(), fixture.deps_, error)) << error;
+
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);  // không ai hỏi nó cả
+  fixture.loop_.step();
+
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0);
+  EXPECT_EQ(fixture.costmap_status_.queryCount(), 0u) << "the port was detached yet the core still "
+                                                         "asked it";
 }

 int main(int argc, char** argv)