optimal & fix file cmake

test/move_base2_scenario_driver.h

@@ -54,8 +54,8 @@ public:
 
     if (!scenario.obstacles.empty())
     {
-      error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); "
-              "dùng driver có recovery_core thật cho kịch bản va chạm";
+      error = "this driver does not simulate obstacles (the recovery port is faked by the "
+              "scenario); use the driver with the real recovery_core for collision scenarios";
       return false;
     }
 
@@ -76,7 +76,7 @@ public:
       }
       else
       {
-        error = "planner_script không hiểu: '" + item + "'";
+        error = "unknown planner_script: '" + item + "'";
         return false;
       }
     }
@@ -106,7 +106,7 @@ public:
       }
       else
       {
-        error = "controller_script không hiểu: '" + item + "'";
+        error = "unknown controller_script: '" + item + "'";
         return false;
       }
     }
@@ -128,7 +128,7 @@ public:
       }
       else
       {
-        error = "recovery_script không hiểu: '" + item + "'";
+        error = "unknown recovery_script: '" + item + "'";
         return false;
       }
     }
@@ -136,9 +136,10 @@ public:
     for (const nav_test_harness::ScenarioEvent& event : scenario.events)
     {
       if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
-          event.action != "lose_pose" && event.action != "restore_pose")
+          event.action != "lose_pose" && event.action != "restore_pose" &&
+          event.action != "sensors_stale" && event.action != "sensors_ok")
       {
-        error = "events: action không hiểu: '" + event.action + "'";
+        error = "events: unknown action: '" + event.action + "'";
         return false;
       }
     }
@@ -147,6 +148,13 @@ public:
     controller_.setScript(controller_script);
     recovery_.setScript(recovery_script);
 
+    // Recovery thế hệ 2 có thể tự lái. Kịch bản nào khai `recovery_velocity` thì behavior được coi
+    // là họ velocity; 0 nghĩa là behavior chỉ đợi/xoá costmap và lõi phải giữ nguồn vận tốc kNone.
+    const bool recovery_drives = std::abs(scenario.recovery_velocity) > 0.0;
+    recovery_.setRecoveryVelocity(recovery_drives, scenario.recovery_velocity);  // [m/s]
+    recovery_.setDefaultOutputKind(recovery_drives ? RecoveryOutputKind::kVelocity
+                                                   : RecoveryOutputKind::kNone);
+
     pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
 
     ControlLoopConfig config;
@@ -181,6 +189,7 @@ public:
     deps_.recovery = &recovery_;
     deps_.mission = &mission_;
     deps_.action = &action_;
+    deps_.costmap_status = &costmap_status_;
 
     if (!loop_.configure(config, deps_, error))
     {
@@ -269,6 +278,15 @@ private:
       {
         pose_.setAvailable(true);
       }
+      else if (event.action == "sensors_stale")
+      {
+        // Observation buffer của costmap hết hạn — lõi phải ngừng cho lái bánh xe.
+        costmap_status_.setCurrent(false);
+      }
+      else if (event.action == "sensors_ok")
+      {
+        costmap_status_.setCurrent(true);
+      }
     }
   }
 
@@ -283,6 +301,7 @@ private:
   FakeRecoveryPort recovery_{ 2 };
   FakeMissionPort mission_;
   FakeActionPort action_;
+  FakeCostmapStatusPort costmap_status_;
 
   std::size_t cycle_ = 0;
   bool started_ = false;