optimal & fix file cmake

test/pose_progress_test.cpp

@@ -0,0 +1,188 @@
+/*********************************************************************
+ *
+ * Kiểm tiến độ đo bằng POSE THẬT, không dead-reckon theo chu kỳ cấu hình.
+ *
+ * Đây là test cho lỗi nặng nhất của bản trước: quãng đi được tính bằng
+ * `|cmd.linear.x| * control_period` với `control_period` lấy từ YAML. Control loop chạy chậm gấp N
+ * lần là robot đi quá quãng gấp N lần, còn bánh trượt thì vẫn báo hoàn thành.
+ *
+ * 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;
+
+constexpr double kConfiguredDistance = 0.28;  // [m] khớp `recovery/back_up/backup_distance`
+
+struct BackUpFixture
+{
+  BackUpFixture()
+  {
+    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;
+};
+
+/**
+ * @brief Chạy trọn một lượt lùi với chu kỳ @p dt, mô phỏng robot đi đúng lệnh phát ra.
+ * @return quãng đường thực tế robot đã lùi [m].
+ */
+double runBackup(BackUpFixture& fixture, double dt, int max_ticks = 2000)
+{
+  EXPECT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  robot::Time now(1000.0);
+  for (int i = 0; i < max_ticks; ++i)
+  {
+    now = robot::Time(now.toSec() + dt);
+    const auto result = fixture.back_up->update(now);
+    if (result.terminal())
+    {
+      EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
+      break;
+    }
+    fixture.rig.applyCommand(result.command, dt);
+  }
+
+  return -fixture.rig.pose.rawPose().x;  // lùi theo -x
+}
+
+TEST(PoseProgress, NominalRateStopsAtRequestedDistance)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  const double traveled = runBackup(fixture, 0.1);
+
+  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
+}
+
+TEST(PoseProgress, FiveTimesSlowerLoopStillStopsAtRequestedDistance)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  // Control loop chạy chậm gấp 5. Bản cũ sai ~400% ở đây vì nhân với hằng số config.
+  const double traveled = runBackup(fixture, 0.5);
+
+  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
+}
+
+TEST(PoseProgress, TwentyTimesSlowerLoopStillStopsAtRequestedDistance)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  const double traveled = runBackup(fixture, 2.0);
+
+  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
+}
+
+TEST(PoseProgress, FasterLoopStopsAtRequestedDistance)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  const double traveled = runBackup(fixture, 0.02);
+
+  EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
+}
+
+TEST(PoseProgress, StalledRobotNeverReportsSuccess)
+{
+  BackUpFixture fixture;
+  ASSERT_TRUE(fixture.loaded);
+  ASSERT_NE(fixture.back_up, nullptr);
+
+  ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
+
+  // Bánh trượt hoàn toàn: lệnh vẫn phát nhưng pose không đổi. Bản cũ tích phân vận tốc LỆNH nên vẫn
+  // báo kSucceeded; bản này phải chạy tới khi timeout chứ không được nói dối.
+  robot::Time now(1000.0);
+  bool reported_success = false;
+  for (int i = 0; i < 200; ++i)
+  {
+    now = robot::Time(now.toSec() + 0.1);
+    const auto result = fixture.back_up->update(now);
+    if (result.status == RecoveryStatus::kSucceeded)
+    {
+      reported_success = true;
+      break;
+    }
+    if (result.terminal())
+    {
+      break;  // timeout -> kFailed, đúng như mong đợi
+    }
+    // KHÔNG applyCommand: robot không nhúc nhích.
+  }
+
+  EXPECT_FALSE(reported_success);
+}
+
+TEST(PoseProgress, ProgressAndRemainingTrackRealPose)
+{
+  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);
+  double last_progress = -1.0;
+  for (int i = 0; i < 10; ++i)
+  {
+    now = robot::Time(now.toSec() + 0.1);
+    const auto result = fixture.back_up->update(now);
+    if (result.terminal())
+    {
+      break;
+    }
+
+    const double traveled = -fixture.rig.pose.rawPose().x;
+    EXPECT_NEAR(result.remaining, kConfiguredDistance - traveled, 1e-6);
+    EXPECT_GE(result.progress, last_progress);
+    EXPECT_GE(result.progress, 0.0);
+    EXPECT_LE(result.progress, 1.0);
+    last_progress = result.progress;
+
+    fixture.rig.applyCommand(result.command, 0.1);
+  }
+}
+
+}  // 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();
+}