optimal & fix file cmake

test/controller_runner_test.cpp

@@ -153,7 +153,8 @@ TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
   std::string error;
 
   EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error));
-  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+  EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
+                                       "configured";
 }
 
 TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
@@ -177,7 +178,8 @@ TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
 
   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";
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous controller yet "
+                                                  "reloaded the library";
 }
 
 TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
@@ -213,7 +215,7 @@ TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
 
   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";
+  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "velocity limit never reached the plugin";
 }
 
 TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
@@ -245,7 +247,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
 
   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";
+  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "a limit set before the controller was loaded got lost";
 }
 
 TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
@@ -261,7 +263,8 @@ TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
 
   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";
+  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "swapping the controller dropped the active velocity "
+                                           "limit";
 }
 
 TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
@@ -274,6 +277,26 @@ TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
   EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
 }
 
+TEST(ControllerRunner, RefreshActivePlannerReinitializesItAndRestoresThePlan)
+{
+  // Local planners như HybridController copy footprint vào cache trong initialize(). Refresh phải
+  // tạo instance mới, nhưng không được làm mất goal/plan giữa mission đang chạy.
+  Fixture fixture("TestControllerFootprintProbe");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist before;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(before));
+  ASSERT_GT(before.linear.x, 0.0);
+
+  ASSERT_TRUE(fixture.runner_.refreshActivePlanner());
+
+  robot_geometry_msgs::Twist after;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(after));
+  EXPECT_GT(after.linear.x, before.linear.x)
+      << "planner was not recreated, or its active goal/plan was not restored";
+}
+
 TEST(ControllerRunner, NonFiniteLimitIsRejected)
 {
   Fixture fixture;
@@ -301,7 +324,8 @@ TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
 
   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";
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "measured velocity never reached the "
+                                                        "plugin";
 }
 
 TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
@@ -373,7 +397,7 @@ TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
 
   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";
+  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "a command containing NaN was still written out";
 }
 
 TEST(ControllerRunner, ExceptionFromThePluginIsContained)
@@ -399,6 +423,46 @@ TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
   EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
 }
 
+// ================================================================================================
+// setDockingMarker — kênh marker của chặng docking
+//
+// Docking planner đọc `maker_name` đúng MỘT lần trong initialize() (getMaker). Bản cũ dlopen lại
+// planner mỗi lần dock nên luôn thấy giá trị mới; ControllerRunner cache instance nên phải tự dựng
+// lại khi marker đổi — không làm là robot dock vào marker của chặng TRƯỚC, lặng lẽ.
+// ================================================================================================
+
+TEST(ControllerRunnerDockingMarker, RejectsMarkerNotInMakerSources)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setDockingMarker("khong_ton_tai"));
+  // Marker hợp lệ (test/config: `maker_sources: dock_a dock_b`) phải qua.
+  EXPECT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
+}
+
+TEST(ControllerRunnerDockingMarker, CachedPlannerIsReinitializedWhenMarkerChanges)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.11, 1e-9) << "probe could not read maker_name='dock_a' at init";
+
+  // Đổi marker rồi swap lại CÙNG planner: instance cache phải được dựng lại để initialize() đọc
+  // giá trị mới. Nếu không, lệnh vẫn mang mã của dock_a — chính là "dock vào nhầm trạm".
+  ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_b"));
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.22, 1e-9)
+      << "instance cache kept the old maker_name — it must re-init when the marker changes";
+}
+
 int main(int argc, char** argv)
 {
   setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);