optimal & fix file cmake

test/backup_safety_test.cpp

@@ -0,0 +1,186 @@
+/*********************************************************************
+ *
+ * Kiểm ba lớp an toàn của BackUpRecovery.
+ *
+ * Bản trước không dùng collision checker (chỉ null-check con trỏ, và chỉ khi `require_costmap` bật
+ * — mặc định TẮT), nên mặc định robot lùi mù. Lùi là hướng robot thường không có sensor, nên đây là
+ * bộ test quan trọng nhất của Phase 3.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <cstdlib>
+
+#include <robot/node_handle.h>
+
+#include <recovery_core/recovery_registry.h>
+
+#include "recovery_test_utils.h"
+
+namespace
+{
+using recovery_core::RecoveryGoal;
+using recovery_core::RecoveryStatus;
+using recovery_test::VelocityRig;
+
+struct BackUpFixture
+{
+  BackUpFixture()
+  {
+    // Robot nhìn theo +x tại gốc; lùi nghĩa là đi về phía -x.
+    rig.pose.setPose(0.0, 0.0, 0.0);
+    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
+    back_up = recovery_test::findBehavior(registry, "back_up");
+  }
+
+  VelocityRig rig;
+  robot::NodeHandle nh;
+  recovery_core::RecoveryRegistry registry;
+  bool loaded = false;
+  recovery_core::RecoveryBehavior* back_up = nullptr;
+};
+
+TEST(BackupSafety, RefusesToStartWhenObstacleIsBehind)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  // Vật cản chạm biên sau của footprint (footprint 0.6 x 0.4 -> biên sau ở x = -0.3).
+  fixture.rig.costmap.setLethalCircle(-0.35, 0.0, 0.10);
+
+  EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+}
+
+TEST(BackupSafety, StopsWithZeroCommandWhenObstacleAppearsMidRun)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  // Vài tick đầu chạy bình thường.
+  robot::Time now(1000.0);
+  for (int i = 0; i < 3; ++i)
+  {
+    now = robot::Time(now.toSec() + 0.1);
+    const auto result = fixture.back_up->update(now);
+    ASSERT_EQ(result.status, RecoveryStatus::kRunning);
+    ASSERT_NE(result.velocity(), nullptr);
+    EXPECT_LT(result.velocity()->linear.x, 0.0);  // âm = lùi
+    fixture.rig.applyCommand(result.command, 0.1);
+  }
+
+  // Có người bước vào phía sau robot.
+  const double robot_x = fixture.rig.pose.rawPose().x;
+  fixture.rig.costmap.setLethalCircle(robot_x - 0.36, 0.0, 0.10);
+
+  now = robot::Time(now.toSec() + 0.1);
+  const auto blocked = fixture.back_up->update(now);
+
+  EXPECT_EQ(blocked.status, RecoveryStatus::kFailed);
+  ASSERT_NE(blocked.velocity(), nullptr);
+  EXPECT_DOUBLE_EQ(blocked.velocity()->linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(blocked.velocity()->angular.z, 0.0);
+}
+
+TEST(BackupSafety, StopsWhenPoseIsLost)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+  ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
+
+  // TF quá hạn / thiếu frame.
+  fixture.rig.pose.setAvailable(false);
+  const auto result = fixture.back_up->update(robot::Time(1000.2));
+
+  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
+  ASSERT_NE(result.velocity(), nullptr);
+  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
+}
+
+TEST(BackupSafety, RefusesToStartWhenPoseIsUnavailable)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  fixture.rig.pose.setAvailable(false);
+
+  EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+}
+
+TEST(BackupSafety, CommandNeverExceedsConfiguredSpeed)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  robot::Time now(1000.0);
+  for (int i = 0; i < 40; ++i)
+  {
+    now = robot::Time(now.toSec() + 0.1);
+    const auto result = fixture.back_up->update(now);
+    if (result.terminal())
+    {
+      break;
+    }
+    ASSERT_NE(result.velocity(), nullptr);
+    // linear_speed: 0.1 m/s trong config test.
+    EXPECT_LE(std::abs(result.velocity()->linear.x), 0.1 + 1e-9);
+    fixture.rig.applyCommand(result.command, 0.1);
+  }
+}
+
+TEST(BackupSafety, RampsUpInsteadOfSteppingToFullSpeed)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  // acc_lim_x: 0.3 m/s^2 -> sau 0.05 s không thể vượt 0.015 m/s.
+  const auto first = fixture.back_up->update(robot::Time(1000.05));
+  ASSERT_NE(first.velocity(), nullptr);
+  EXPECT_LE(std::abs(first.velocity()->linear.x), 0.015 + 1e-9);
+}
+
+TEST(BackupSafety, CancelEmitsZeroCommand)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+  ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
+
+  fixture.back_up->cancel();
+  const auto result = fixture.back_up->update(robot::Time(1000.2));
+
+  EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
+  ASSERT_NE(result.velocity(), nullptr);
+  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}