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/timeout_test.cpp
+++ b/test/timeout_test.cpp
@@ -0,0 +1,132 @@
+/*********************************************************************
+ *
+ * Kiểm `elapsed` + `timeout` của base.
+ *
+ * Bản trước khai `RecoveryResult::elapsed` trong header nhưng KHÔNG nơi nào ghi vào nó, nên caller
+ * không có cách phát hiện recovery treo. Test này khoá cả hai chiều: elapsed phải bám đồng hồ thật,
+ * và quá timeout phải là kFailed kèm stop output đúng họ.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <string>
+
+#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;
+
+/// NodeHandle trỏ vào một namespace có sẵn khoá `timeout` trong config test.
+robot::NodeHandle timeoutNodeHandle(const std::string& ns)
+{
+  robot::NodeHandle root;
+  return robot::NodeHandle(root, ns);
+}
+
+TEST(Timeout, DisabledByDefault)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh;
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  // Không khai `timeout` trong namespace này -> 0 = không giới hạn.
+  EXPECT_DOUBLE_EQ(behavior.timeout(), 0.0);
+
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+  const auto result = behavior.update(robot::Time(1000.0 + 3600.0));
+  EXPECT_EQ(result.status, RecoveryStatus::kRunning);
+}
+
+TEST(Timeout, ReadFromConfiguredNamespace)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  EXPECT_DOUBLE_EQ(behavior.timeout(), 20.0);
+}
+
+TEST(Timeout, ExceededYieldsFailedAndStopsTicking)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");  // timeout: 20 s
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  ASSERT_EQ(behavior.update(robot::Time(1019.0)).status, RecoveryStatus::kRunning);
+  const int calls_before = behavior.update_calls;
+
+  const auto timed_out = behavior.update(robot::Time(1020.5));
+
+  EXPECT_EQ(timed_out.status, RecoveryStatus::kFailed);
+  EXPECT_NEAR(timed_out.elapsed, 20.5, 1e-6);
+  EXPECT_FALSE(timed_out.message.empty());
+  // Quá hạn thì KHÔNG giao quyền cho plugin nữa.
+  EXPECT_EQ(behavior.update_calls, calls_before);
+}
+
+TEST(Timeout, VelocityFamilyStopsWithExplicitZeroTwist)
+{
+  VelocityRig rig;
+  MockBehavior behavior(RecoveryOutputType::kVelocity);
+  robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");  // timeout: 20 s
+  ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
+  ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
+
+  const auto timed_out = behavior.update(robot::Time(1021.0));
+
+  ASSERT_EQ(timed_out.status, RecoveryStatus::kFailed);
+  ASSERT_NE(timed_out.velocity(), nullptr);
+  EXPECT_DOUBLE_EQ(timed_out.velocity()->linear.x, 0.0);
+  EXPECT_DOUBLE_EQ(timed_out.velocity()->angular.z, 0.0);
+}
+
+TEST(Timeout, ElapsedIsSetOnEveryResult)
+{
+  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)));
+
+  // Plugin không đặt elapsed; base phải điền vào.
+  behavior.next_result = recovery_core::RecoveryResult::Running();
+  EXPECT_NEAR(behavior.update(robot::Time(1002.0)).elapsed, 2.0, 1e-6);
+
+  behavior.next_result = recovery_core::RecoveryResult::Succeeded();
+  EXPECT_NEAR(behavior.update(robot::Time(1007.5)).elapsed, 7.5, 1e-6);
+}
+
+TEST(Timeout, OutOfRangeConfigFallsBackToDisabled)
+{
+  MockBehavior behavior(RecoveryOutputType::kNone);
+  // `recovery/bad_timeout` khai timeout âm -> phải cảnh báo và về 0 chứ không nhận giá trị âm.
+  robot::NodeHandle nh = timeoutNodeHandle("recovery_bad/bad_timeout");
+  recovery_core::RecoveryContext ctx;
+  ASSERT_TRUE(behavior.configure("mock", ctx, nh));
+
+  EXPECT_DOUBLE_EQ(behavior.timeout(), 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
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}