first commit

2026-07-29 15:45:16 +07:00
commit 4762a3032c
56 changed files with 15310 additions and 0 deletions
--- a/test/recovery_runner_test.cpp
+++ b/test/recovery_runner_test.cpp
@@ -0,0 +1,237 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm chỗ nối RecoveryPort <-> recovery_core.
+ *
+ * Đây là seam giữa hai gói: nếu nó đúng thì mọi behavior của recovery_core dùng được từ lõi mà lõi
+ * không biết gì về recovery_core. Test nạp plugin qua đúng đường Boost.DLL mà runtime đi.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <string>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/recovery_runner.h>
+
+#include "fake_ports.h"
+
+namespace
+{
+using move_base2::RecoveryOutputKind;
+using move_base2::RecoveryRunner;
+using move_base2::RecoveryTick;
+using move_base2::RecoveryTrigger;
+using move_base2::testing::FakeClockPort;
+using move_base2::testing::FakePosePort;
+
+/// Bộ đồ nghề tối thiểu: đồng hồ giả + pose giả, không costmap (behavior họ kNone không cần).
+struct Rig
+{
+  Rig()
+  {
+    pose.setPosition(0.0, 0.0);
+
+    RecoveryRunner::Deps deps;
+    deps.clock = &clock;
+    deps.pose = &pose;
+    runner.setDeps(deps);
+  }
+
+  bool load(const std::string& ns)
+  {
+    runner.setNamespace(ns);
+    robot::NodeHandle nh;
+    return runner.configure(nh);
+  }
+
+  FakeClockPort clock{1000.0};
+  FakePosePort pose;
+  RecoveryRunner runner;
+};
+
+TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  ASSERT_EQ(rig.runner.behaviorCount(), 2u);
+  EXPECT_EQ(rig.runner.behaviorName(0), "wait_short");
+  EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
+}
+
+TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  EXPECT_EQ(rig.runner.outputKind(0), RecoveryOutputKind::kNone);
+  EXPECT_EQ(rig.runner.outputKind(1), RecoveryOutputKind::kNone);
+}
+
+TEST(RecoveryRunner, OutOfRangeIndexIsSafeAndNeverClaimsVelocity)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  // Giả định an toàn: không biết là gì thì không cấp quyền phát vận tốc.
+  EXPECT_EQ(rig.runner.outputKind(99), RecoveryOutputKind::kNone);
+  EXPECT_TRUE(rig.runner.behaviorName(99).empty());
+  EXPECT_FALSE(rig.runner.start(99, RecoveryTrigger::kPlanningFailed));
+}
+
+TEST(RecoveryRunner, StartBeforeConfigureFails)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+}
+
+TEST(RecoveryRunner, UpdateWithoutActiveBehaviorFailsInsteadOfCrashing)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  const RecoveryTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+  EXPECT_FALSE(tick.has_velocity);
+  EXPECT_FALSE(tick.message.empty());
+}
+
+TEST(RecoveryRunner, RunsBehaviorToSuccessOnRealClock)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));  // wait_duration: 1.0 s
+
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);
+}
+
+TEST(RecoveryRunner, NoneFamilyNeverReportsVelocityToTheCore)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+
+  for (int i = 0; i < 5; ++i)
+  {
+    rig.clock.advance(0.3);
+    const RecoveryTick tick = rig.runner.update();
+    // Lõi dùng has_velocity để quyết định có lấy cmd hay không; behavior đứng yên không được bật.
+    EXPECT_FALSE(tick.has_velocity);
+    EXPECT_FALSE(tick.has_path);
+    if (tick.status != RecoveryTick::Status::kRunning)
+    {
+      break;
+    }
+  }
+}
+
+TEST(RecoveryRunner, BehaviorTimeoutSurfacesAsFailed)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  // wait_long: wait_duration 5 s nhưng timeout 3 s -> phải kết thúc bằng kFailed, không treo.
+  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kControllingFailed));
+
+  rig.clock.advance(2.0);
+  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+
+  rig.clock.advance(1.5);
+  const RecoveryTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+  EXPECT_NE(tick.message.find("timeout"), std::string::npos);
+}
+
+TEST(RecoveryRunner, CancelIsSafeWithoutActiveBehavior)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  rig.runner.cancel();  // không được crash
+  SUCCEED();
+}
+
+TEST(RecoveryRunner, CancelledTickSurfacesAsFailedNotRunning)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+
+  rig.runner.cancel();
+  rig.clock.advance(0.1);
+  const RecoveryTick tick = rig.runner.update();
+
+  // State machine hiện tại không tick sau cancel, nên nhánh này không đạt tới trong runtime thật.
+  // Nhưng nếu ai đó nới điều kiện tick, kCancelled phải thành kFailed chứ không im lặng thành
+  // kRunning — đó là lý do nhánh dịch được giữ lại.
+  EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed);
+}
+
+TEST(RecoveryRunner, RestartingSecondBehaviorWorks)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed));
+  rig.clock.advance(1.0);
+  ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded);
+
+  // State machine chuyển sang behavior kế tiếp sau khi cái trước kết thúc.
+  ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kOscillation));
+  rig.clock.advance(0.5);
+  EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning);
+}
+
+TEST(RecoveryRunner, EmptyBehaviorListFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("recovery_empty"));
+  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
+}
+
+TEST(RecoveryRunner, MissingLibraryPathFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("recovery_missing_library"));
+  EXPECT_EQ(rig.runner.behaviorCount(), 0u);
+}
+
+TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
+{
+  RecoveryRunner runner;
+  runner.setNamespace("recovery");
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort/PosePort phải hỏng ngay, không phải lúc tick";
+}
+
+TEST(RecoveryRunner, ConfigureTwiceRejected)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(rig.runner.configure(nh));
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}