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/recovery_lifecycle_test.cpp
+++ b/test/recovery_lifecycle_test.cpp
@@ -0,0 +1,280 @@
+/*********************************************************************
+ *
+ * Kiểm bất biến vòng đời của RecoveryBehavior: guard configure/start/update, kiểm ngữ cảnh bắt
+ * buộc theo họ output, dt đo bằng đồng hồ thật, và đường cancel.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+
+#include <robot/node_handle.h>
+
+#include "recovery_test_utils.h"
+
+namespace
+{
+using recovery_core::RecoveryGoal;
+using recovery_core::RecoveryOutputType;
+using recovery_core::RecoveryStatus;
+using recovery_test::MockBehavior;
+using recovery_test::VelocityRig;
+
+TEST(RecoveryLifecycle, StartBeforeConfigureFails)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  const robot::Time now(1000.0);
+
+  EXPECT_FALSE(behavior.start(RecoveryGoal(), now));
+  EXPECT_EQ(behavior.start_calls, 0);
+}
+
+TEST(RecoveryLifecycle, UpdateBeforeStartReturnsFailedStopOutput)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  const robot::Time now(1000.0);
+
+  const auto result = behavior.update(now);
+
+  EXPECT_EQ(result.status, RecoveryStatus::kFailed);
+  EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
+  EXPECT_EQ(behavior.update_calls, 0);
+}
+
+TEST(RecoveryLifecycle, ConfigureTwiceRejected)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+
+  EXPECT_TRUE(behavior.configure("mock", ctx, nh));
+  // Gọi lần hai với ctx khác là lỗi lập trình của caller — phải báo, không được nuốt.
+  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
+  EXPECT_EQ(behavior.configure_calls, 1);
+}
+
+TEST(RecoveryLifecycle, EmptyInstanceNameRejected)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+
+  EXPECT_FALSE(behavior.configure("", ctx, nh));
+  EXPECT_EQ(behavior.configure_calls, 0);
+}
+
+TEST(RecoveryLifecycle, VelocityFamilyRequiresPoseProvider)
+{
+  VelocityRig rig;
+  MockBehavior behavior(RecoveryOutputType::kVelocity);
+  robot::NodeHandle nh;
+
+  recovery_core::RecoveryContext ctx = rig.ctx;
+  ctx.pose = nullptr;
+
+  // Không có pose thì tiến độ chỉ có thể dead-reckon — đúng lớp lỗi Phase 3 phải diệt.
+  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
+}
+
+TEST(RecoveryLifecycle, VelocityFamilyRequiresCollisionChecker)
+{
+  VelocityRig rig;
+  MockBehavior behavior(RecoveryOutputType::kVelocity);
+  robot::NodeHandle nh;
+
+  recovery_core::RecoveryContext ctx = rig.ctx;
+  ctx.collision = nullptr;
+
+  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
+}
+
+TEST(RecoveryLifecycle, PathFamilyRequiresPlanProvider)
+{
+  MockBehavior behavior(RecoveryOutputType::kPath);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+
+  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
+}
+
+TEST(RecoveryLifecycle, NoneFamilyNeedsNoPorts)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+
+  EXPECT_TRUE(behavior.configure("mock", ctx, nh));
+}
+
+TEST(RecoveryLifecycle, ConfigureFailsWhenPluginRejects)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  behavior.configure_ok = false;
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+
+  EXPECT_FALSE(behavior.configure("mock", ctx, nh));
+  // Không được coi là đã cấu hình: start() sau đó phải hỏng.
+  EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+}
+
+TEST(RecoveryLifecycle, StartRejectedWhenPluginRefuses)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  behavior.start_ok = false;
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+  EXPECT_EQ(behavior.status(), RecoveryStatus::kFailed);
+}
+
+TEST(RecoveryLifecycle, TerminalStateDoesNotTickAgain)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  behavior.next_result = recovery_core::RecoveryResult::Succeeded();
+  ASSERT_EQ(behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
+  const int calls_after_success = behavior.update_calls;
+
+  const auto again = behavior.update(robot::Time(1000.2));
+  EXPECT_EQ(again.status, RecoveryStatus::kSucceeded);
+  EXPECT_EQ(behavior.update_calls, calls_after_success);  // không gọi thêm onUpdate
+}
+
+TEST(RecoveryLifecycle, CancelYieldsCancelledStopOutput)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  behavior.cancel();
+  const auto result = behavior.update(robot::Time(1000.1));
+
+  EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
+  EXPECT_EQ(behavior.cancel_calls, 1);
+  EXPECT_EQ(behavior.update_calls, 0);  // cancel thay thế tick, không chạy logic plugin
+}
+
+TEST(RecoveryLifecycle, CancelOnVelocityFamilyEmitsExplicitZeroTwist)
+{
+  VelocityRig rig;
+  MockBehavior behavior(RecoveryOutputType::kVelocity);
+  robot::NodeHandle nh;
+  ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  behavior.cancel();
+  const auto result = behavior.update(robot::Time(1000.1));
+
+  // Họ velocity phải nhận lệnh dừng TƯỜNG MINH: caller đang lấy cmd_vel từ đây.
+  ASSERT_NE(result.velocity(), nullptr);
+  EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
+}
+
+TEST(RecoveryLifecycle, DtMeasuredFromRealClockNotConfiguredPeriod)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  behavior.update(robot::Time(1000.0));
+  EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9);  // tick đầu ngay sau start
+
+  behavior.update(robot::Time(1000.5));
+  EXPECT_NEAR(behavior.last_dt, 0.5, 1e-6);
+
+  behavior.update(robot::Time(1002.5));
+  EXPECT_NEAR(behavior.last_dt, 2.0, 1e-6);  // loop chạy chậm -> dt lớn, không phải hằng số config
+}
+
+TEST(RecoveryLifecycle, BackwardClockYieldsZeroDt)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  behavior.update(robot::Time(1001.0));
+  behavior.update(robot::Time(1000.5));  // đồng hồ đi lùi
+
+  EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9);
+}
+
+TEST(RecoveryLifecycle, ElapsedTracksClockFromStart)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  const auto first = behavior.update(robot::Time(1001.25));
+  EXPECT_NEAR(first.elapsed, 1.25, 1e-6);
+  EXPECT_NEAR(behavior.elapsed(), 1.25, 1e-6);
+
+  const auto second = behavior.update(robot::Time(1004.0));
+  EXPECT_NEAR(second.elapsed, 4.0, 1e-6);
+}
+
+TEST(RecoveryLifecycle, RestartResetsElapsed)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+  behavior.update(robot::Time(1005.0));
+  ASSERT_NEAR(behavior.elapsed(), 5.0, 1e-6);
+
+  behavior.next_result = recovery_core::RecoveryResult::Running();
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(2000.0)));
+  EXPECT_NEAR(behavior.elapsed(), 0.0, 1e-9);
+
+  const auto result = behavior.update(robot::Time(2000.5));
+  EXPECT_NEAR(result.elapsed, 0.5, 1e-6);
+}
+
+TEST(RecoveryLifecycle, GoalIsHandedToPluginVerbatim)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  RecoveryGoal goal;
+  goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
+  goal.angle = 1.5;
+  goal.params["custom"] = 7.0;
+
+  ASSERT_TRUE(behavior.start(goal, robot::Time(1000.0)));
+
+  EXPECT_EQ(behavior.last_goal.trigger, recovery_core::RecoveryTrigger::kOscillation);
+  ASSERT_TRUE(behavior.last_goal.angle.has_value());
+  EXPECT_DOUBLE_EQ(*behavior.last_goal.angle, 1.5);
+  EXPECT_DOUBLE_EQ(behavior.last_goal.param("custom", 0.0), 7.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
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}