first commit

2026-07-29 15:45:16 +07:00
commit 4762a3032c
56 changed files with 15310 additions and 0 deletions
--- a/test/config_validation_test.cpp
+++ b/test/config_validation_test.cpp
@@ -0,0 +1,245 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm việc đọc và validate cấu hình runtime.
+ *
+ * Bản cũ đọc param không kiểm miền giá trị: một `controller_frequency` bằng 0 hay một
+ * `max_planning_retries` âm đi thẳng vào vòng điều khiển. Ở đây cấu hình sai phải chặn runtime khởi
+ * động, chứ không phải hiện ra thành hành vi lạ lúc chạy.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <string>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/config/move_base2_config.h>
+
+namespace
+{
+using move_base2::MoveBase2Config;
+
+MoveBase2Config loadFrom(const std::string& ns)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle nh(root, ns);
+
+  MoveBase2Config config;
+  config.fromNodeHandle(nh);
+  return config;
+}
+
+/// Điền con số mà RecoveryRunner báo lại sau khi nạp behavior — bước bắt buộc trước validate().
+MoveBase2Config withRecoveryCount(MoveBase2Config config, std::size_t count)
+{
+  config.state_machine.recovery_behavior_count = count;
+  return config;
+}
+
+/// Cấu hình tối thiểu hợp lệ, dựng bằng tay (không qua YAML).
+MoveBase2Config minimalValid()
+{
+  MoveBase2Config config;
+  config.position.local_planner_name = "AnyLocalPlanner";
+  config.state_machine.recovery_behavior_count = 1;
+  return config;
+}
+
+TEST(MoveBase2Config, DefaultsAreValid)
+{
+  const MoveBase2Config config = minimalValid();
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
+}
+
+TEST(MoveBase2Config, RejectsRecoveryEnabledWithNoBehaviorLoaded)
+{
+  // Ràng buộc thứ tự khởi tạo: RecoveryRunner không nạp được behavior nào mà
+  // recovery_behavior_enabled vẫn true thì mọi lỗi dẫn thẳng tới ABORTED — chặn ngay lúc khởi động.
+  MoveBase2Config config = minimalValid();
+  config.state_machine.recovery_behavior_count = 0;
+  config.state_machine.recovery_enabled = true;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, ReadsSensorGatewayBlockFromYaml)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_TRUE(config.sensors.laser_sor_enabled);
+  EXPECT_EQ(config.sensors.laser_sor_mean_k, 8);
+  EXPECT_DOUBLE_EQ(config.sensors.laser_sor_stddev_mul, 1.5);
+}
+
+TEST(MoveBase2Config, LaserFilterIsOffWhenTheSensorsBlockIsAbsent)
+{
+  // Cây config gen-1 không có khoá nào cho bộ lọc. Thiếu khoá phải giữ hành vi host ROS đang chạy —
+  // tức là KHÔNG lọc — chứ không phải âm thầm bật một bộ lọc lên.
+  const MoveBase2Config config = loadFrom("move_base2_no_planner");
+
+  EXPECT_FALSE(config.sensors.laser_sor_enabled);
+}
+
+TEST(MoveBase2Config, RejectsSensorFilterParametersOutOfRange)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_sensors"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("laser_sor_mean_k"), std::string::npos) << error;
+}
+
+TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_DOUBLE_EQ(config.controller_frequency, 20.0);
+  EXPECT_DOUBLE_EQ(config.planner_timeout, 5.0);
+
+  EXPECT_DOUBLE_EQ(config.state_machine.planner_patience, 5.0);
+  EXPECT_DOUBLE_EQ(config.state_machine.controller_patience, 15.0);
+  EXPECT_EQ(config.state_machine.max_planning_retries, -1);
+  EXPECT_TRUE(config.state_machine.recovery_enabled);
+
+  EXPECT_DOUBLE_EQ(config.velocity.max_vel_x, 0.5);
+  EXPECT_DOUBLE_EQ(config.velocity.min_vel_x, -0.2);
+  EXPECT_DOUBLE_EQ(config.velocity.max_accel_x, 1.0);
+
+  EXPECT_EQ(config.global_frame, "map");
+  EXPECT_EQ(config.robot_base_frame, "base_link");
+  EXPECT_EQ(config.recovery_namespace, "recovery");
+}
+
+TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
+{
+  const MoveBase2Config config = loadFrom("move_base2");
+
+  EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
+  EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
+  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.15);
+
+  // Ghép nối cần sai số chặt hơn nhiều — đây chính là thứ sáu entry point cũ khác nhau ở.
+  EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
+  EXPECT_DOUBLE_EQ(config.docking.default_xy_tolerance, 0.02);
+  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.02);
+}
+
+TEST(MoveBase2Config, LoadedConfigValidates)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
+}
+
+TEST(MoveBase2Config, RejectsZeroControllerFrequency)
+{
+  const MoveBase2Config config = loadFrom("move_base2_bad_frequency");
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("controller_frequency"), std::string::npos);
+}
+
+TEST(MoveBase2Config, RejectsAbsurdlyHighControllerFrequency)
+{
+  MoveBase2Config config = minimalValid();
+  config.controller_frequency = 5000.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, RejectsGlobalFrameEqualToBaseFrame)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_frames"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("global_frame"), std::string::npos);
+}
+
+TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
+{
+  const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_no_planner"), 1);
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error))
+      << "cấu hình này lúc chạy sẽ từ chối MỌI yêu cầu — phải chặn ngay lúc khởi động";
+}
+
+TEST(MoveBase2Config, RejectsNonPositiveToleranceOnConfiguredProfile)
+{
+  MoveBase2Config config = minimalValid();
+  config.position.default_xy_tolerance = 0.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("xy_goal_tolerance"), std::string::npos);
+}
+
+TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
+{
+  MoveBase2Config config = minimalValid();
+  config.state_machine.oscillation_timeout = 5.0;
+  config.state_machine.oscillation_distance = 0.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("oscillation"), std::string::npos);
+}
+
+TEST(MoveBase2Config, PropagatesVelocityValidationFailure)
+{
+  MoveBase2Config config = minimalValid();
+  config.velocity.max_vel_x = -1.0;
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+}
+
+TEST(MoveBase2Config, ControlLoopConfigDerivesPeriodFromFrequency)
+{
+  MoveBase2Config config = minimalValid();
+  config.controller_frequency = 25.0;
+
+  const auto loop_config = config.toControlLoopConfig();
+
+  EXPECT_NEAR(loop_config.nominal_control_period, 0.04, 1e-9);  // [s]
+  EXPECT_EQ(loop_config.position.local_planner_name, "AnyLocalPlanner");
+}
+
+TEST(MoveBase2Config, RecoveryBehaviorCountIsNotReadFromYaml)
+{
+  // Số behavior phải là số nạp được THẬT, do RecoveryRunner báo lại. Đọc từ YAML thì một behavior
+  // hỏng vẫn khiến state machine tin là còn đường phục hồi.
+  const MoveBase2Config config = loadFrom("move_base2");
+  EXPECT_EQ(config.state_machine.recovery_behavior_count, 0u);
+}
+
+TEST(MoveBase2Config, DescribeMentionsEveryGroup)
+{
+  const std::string text = loadFrom("move_base2").describe();
+
+  EXPECT_NE(text.find("controller_frequency"), std::string::npos);
+  EXPECT_NE(text.find("position"), std::string::npos);
+  EXPECT_NE(text.find("docking"), std::string::npos);
+  EXPECT_NE(text.find("planner_patience"), std::string::npos);
+  EXPECT_NE(text.find("max_vel_x"), std::string::npos);
+}
+
+}  // 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
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}