optimal & fix file cmake

2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
--- a/test/wait_recovery_test.cpp
+++ b/test/wait_recovery_test.cpp
@@ -0,0 +1,179 @@
+/*********************************************************************
+ *
+ * Kiểm WaitRecovery — behavior mới của bộ default.
+ *
+ * Đây là recovery an toàn nhất (robot không di chuyển) và hữu dụng nhất cho AMR trong kho, nơi phần
+ * lớn tình huống chặn đường là vật cản động. Nó cũng là chỗ rẻ nhất để chứng minh đường `elapsed`
+ * của base chạy đúng theo đồng hồ thật.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#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::RecoveryOutputType;
+using recovery_core::RecoveryStatus;
+using recovery_test::VelocityRig;
+
+constexpr double kConfiguredWait = 3.0;  // [s] khớp `recovery/wait/wait_duration`
+
+struct WaitFixture
+{
+  WaitFixture()
+  {
+    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
+    wait = recovery_test::findBehavior(registry, "wait");
+  }
+
+  VelocityRig rig;
+  robot::NodeHandle nh;
+  recovery_core::RecoveryRegistry registry;
+  bool loaded = false;
+  recovery_core::RecoveryBehavior* wait = nullptr;
+};
+
+TEST(WaitRecovery, DeclaresNoOutputFamily)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  EXPECT_EQ(fixture.wait->outputKind(), RecoveryOutputType::kNone);
+}
+
+TEST(WaitRecovery, NeverEmitsVelocity)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  robot::Time now(1000.0);
+  for (int i = 0; i < 60; ++i)
+  {
+    now = robot::Time(now.toSec() + 0.1);
+    const auto result = fixture.wait->update(now);
+    // Behavior đứng yên tuyệt đối không được làm caller tưởng nó đang lái robot.
+    EXPECT_EQ(result.velocity(), nullptr);
+    EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
+    if (result.terminal())
+    {
+      break;
+    }
+  }
+}
+
+TEST(WaitRecovery, SucceedsAfterConfiguredDuration)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kRunning);
+  EXPECT_EQ(fixture.wait->update(robot::Time(1002.9)).status, RecoveryStatus::kRunning);
+  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
+}
+
+TEST(WaitRecovery, CountsByClockNotByTickCount)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  // Một tick duy nhất nhưng nhảy qua trọn thời lượng: phải xong ngay, không cần đủ số nhịp.
+  const auto result = fixture.wait->update(robot::Time(1000.0 + kConfiguredWait));
+
+  EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
+  EXPECT_NEAR(result.elapsed, kConfiguredWait, 1e-6);
+}
+
+TEST(WaitRecovery, ProgressAdvancesMonotonically)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  double last = -1.0;
+  for (int i = 1; i <= 5; ++i)
+  {
+    const auto result = fixture.wait->update(robot::Time(1000.0 + 0.5 * i));
+    EXPECT_GE(result.progress, last);
+    EXPECT_GE(result.remaining, 0.0);
+    last = result.progress;
+  }
+}
+
+TEST(WaitRecovery, PerRunDurationOverride)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  RecoveryGoal goal;
+  goal.params["wait_duration"] = 1.0;
+
+  ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
+
+  EXPECT_EQ(fixture.wait->update(robot::Time(1000.5)).status, RecoveryStatus::kRunning);
+  EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kSucceeded);
+}
+
+TEST(WaitRecovery, InvalidOverrideFallsBackToConfiguredDuration)
+{
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  RecoveryGoal goal;
+  goal.params["wait_duration"] = -5.0;  // vô lý -> phải cảnh báo và dùng default
+
+  ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
+
+  EXPECT_EQ(fixture.wait->update(robot::Time(1002.0)).status, RecoveryStatus::kRunning);
+  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
+}
+
+TEST(WaitRecovery, NeedsNoPoseOrCollisionPorts)
+{
+  // Điểm mạnh của WaitRecovery: chạy được cả khi TF hỏng, nên nó là đường phục hồi cuối cùng còn
+  // dùng được khi mọi thứ khác đã mất pose.
+  WaitFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.wait, nullptr);
+
+  fixture.rig.pose.setAvailable(false);
+
+  ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
+  EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
+}
+
+}  // 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();
+}