optimal & fix file cmake

test/plugins/test_local_planner.cpp

@@ -19,6 +19,7 @@
  * Author: DuongTD
  *********************************************************************/
 #include <algorithm>
+#include <atomic>
 #include <cmath>
 #include <cstddef>
 #include <limits>
@@ -51,23 +52,32 @@ class TestLocalPlanner : public robot_nav_core2::LocalPlanner
 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).
+    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).
+    kMarkerProbe,    ///< Đọc `maker_name` MỘT lần lúc initialize, mã hoá vào lệnh — mô phỏng
+                     ///< getMaker() của docking planner để test đường re-init khi đổi marker.
+    kFootprintProbe  ///< Mỗi initialize có generation mới; cần reapply goal/plan mới sinh lệnh.
   };
 
   explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
   {
   }
 
-  void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
+  void initialize(robot::NodeHandle& parent, const std::string& name,
                   std::shared_ptr<tf3::BufferCore> /*tf*/,
                   robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
   {
     // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
     name_ = name;
+    // Như PNKXDockingLocalPlanner::getMaker(): đọc đúng MỘT lần, không bao giờ đọc lại.
+    parent.param("maker_name", marker_at_init_, std::string(""));
+    if (behavior_ == Behavior::kFootprintProbe)
+    {
+      footprint_generation_ = ++footprint_probe_generation_;
+    }
   }
 
   void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
@@ -99,13 +109,22 @@ public:
     switch (behavior_)
     {
       case Behavior::kThrow:
-        throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
+        throw std::runtime_error("TestLocalPlanner was asked to throw an exception");
       case Behavior::kNoCommand:
         // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
-        throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
+        throw std::runtime_error("TestLocalPlanner: could not produce a command");
       case Behavior::kNaN:
         cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
         return cmd;
+      case Behavior::kMarkerProbe:
+        // Mã hoá marker đọc được lúc initialize vào lệnh — bảng cố định, test đối chiếu.
+        cmd.velocity.x = marker_at_init_ == "dock_a" ? 0.11 : marker_at_init_ == "dock_b" ? 0.22 : 0.0;
+        return cmd;
+      case Behavior::kFootprintProbe:
+        // Nếu refresh chỉ dựng instance mà quên setGoalPose/setPlan lại, probe trả 0 thay vì lệnh
+        // mang generation mới. Như vậy test kiểm đồng thời cache footprint và khôi phục chặng.
+        cmd.velocity.x = (saw_goal_ && plan_size_ != 0) ? 0.01 * footprint_generation_ : 0.0;
+        return cmd;
       case Behavior::kOk:
       case Behavior::kRefusesLimits:
         break;
@@ -181,6 +200,9 @@ public:
 private:
   Behavior behavior_;
   std::string name_;
+  std::string marker_at_init_;  ///< `maker_name` tại thời điểm initialize — không bao giờ đọc lại.
+  inline static std::atomic<unsigned int> footprint_probe_generation_{ 0 };
+  unsigned int footprint_generation_ = 0;
   std::size_t plan_size_ = 0;
   bool saw_goal_ = false;
   double limit_forward_ = 0.0;   ///< [m/s]
@@ -220,6 +242,16 @@ robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
 }
 
+robot_nav_core2::LocalPlanner::Ptr createMarkerProbe()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kMarkerProbe);
+}
+
+robot_nav_core2::LocalPlanner::Ptr createFootprintProbe()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kFootprintProbe);
+}
+
 }  // namespace testing
 }  // namespace move_base2
 
@@ -229,3 +261,5 @@ 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)
+BOOST_DLL_ALIAS(move_base2::testing::createMarkerProbe, TestControllerMarkerProbe)
+BOOST_DLL_ALIAS(move_base2::testing::createFootprintProbe, TestControllerFootprintProbe)