move_base2/test/config_validation_test.cpp

/*********************************************************************
 *
 * 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");
  EXPECT_FALSE(config.docking_requires_marker);
}

TEST(MoveBase2Config, MissionLayerIsEnabledByDefault)
{
  // Mặc định bật: order VDA5050 đi qua mission layer và được cắt thành chặng. Đổi mặc định này là
  // đổi hành vi của mọi order trên robot, nên nó được khoá lại bằng test.
  const MoveBase2Config defaults;
  EXPECT_TRUE(defaults.mission_layer_enabled);
  EXPECT_EQ(defaults.mission_namespace, "mission_adapters");
}

TEST(MoveBase2Config, RejectsEnabledMissionLayerWithoutNamespace)
{
  MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
  config.mission_layer_enabled = true;
  config.mission_namespace.clear();

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("mission_namespace"), std::string::npos) << error;
}

TEST(MoveBase2Config, DisabledMissionLayerDoesNotNeedANamespace)
{
  MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
  config.mission_layer_enabled = false;
  config.mission_namespace.clear();

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

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_EQ(config.docking.global_planner_name, "TestDockPlanner");
  EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
  ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
  const auto trolley = config.docking_marker_profiles.find("trolley");
  ASSERT_NE(trolley, config.docking_marker_profiles.end());
  EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
  EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
}

TEST(MoveBase2ConfigRootProfile, LoadsIndependentPlannerPairsWithoutTheLegacyAdapter)
{
  robot::NodeHandle root;
  robot::NodeHandle profile_nh(root, "root_profiles");
  const MoveBase2Config config = MoveBase2Config::load(profile_nh);

  EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
  EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
  EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
  EXPECT_EQ(config.docking.local_planner_name, "TestLocalPlanner");
  EXPECT_EQ(config.go_straight.global_planner_name, "TestStraightPlanner");
  EXPECT_EQ(config.go_straight.local_planner_name, "TestStraightLocalPlanner");
  EXPECT_EQ(config.rotate.global_planner_name, "TestRotatePlanner");
  EXPECT_EQ(config.rotate.local_planner_name, "TestRotateLocalPlanner");
  EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
  ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
  const auto trolley = config.docking_marker_profiles.find("trolley");
  ASSERT_NE(trolley, config.docking_marker_profiles.end());
  EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
  EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
  EXPECT_FALSE(config.docking_requires_marker);
}

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))
      << "this config would reject EVERY request at runtime — it must be caught at startup";
}

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

// ================================================================================================
// Schema gen-1 (legacy)
//
// Cây config production hiện tại là schema gen-1. Không đọc được nó thì move_base2 không chạy được
// trên cấu hình thật, và bước 11 (chạy song song bản cũ/mới) không thực hiện được.
// ================================================================================================

TEST(MoveBase2ConfigLegacy, MapsPlannerNamesToProfiles)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
  EXPECT_EQ(config.docking.local_planner_name, "DockLocalPlanner");

  // Profile không khai global planner riêng thì rơi về khoá ở root.
  EXPECT_EQ(config.position.global_planner_name, "SBPLLatticePlanner");
  // Profile có khai riêng thì dùng bản riêng.
  EXPECT_EQ(config.docking.global_planner_name, "DockPlanner");
}

TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

}

TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
{
  // Khác biệt NGỮ NGHĨA, không phải khác biệt giá trị. Gen-1: 0 = "fail -> recovery ngay". Gen-2:
  // 0 = "tắt đồng hồ". Dịch nhầm là biến một cấu hình phản ứng-ngay thành cấu hình không-bao-giờ.
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_GT(config.state_machine.planner_patience, 0.0);
  EXPECT_NEAR(config.state_machine.planner_patience, 1.0 / 20.0, 1e-9);
}

TEST(MoveBase2ConfigLegacy, KeepsGen1DefaultForRobotBaseFrame)
{
  // Gen-1 mặc định "base_footprint", gen-2 "base_link". Chế độ legacy giữ default gen-1 để không
  // đổi hành vi của một hệ đang chạy chỉ vì đổi runtime.
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);

  EXPECT_EQ(config.robot_base_frame, "base_footprint");
}

TEST(MoveBase2ConfigLegacy, LoadedLegacyConfigPassesValidation)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  MoveBase2Config config;
  config.fromLegacyNodeHandle(nh);
  config.state_machine.recovery_behavior_count = 2;

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
{
  // Không trộn từng khoá giữa hai schema: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu
  // lỗi "sửa config mãi không ăn" mà hai cây config trùng tên của workspace đã gây ra một lần.
  robot::NodeHandle root;
  const MoveBase2Config config = MoveBase2Config::load(root);

  EXPECT_EQ(config.robot_base_frame, "base_link") << "picked the gen-1 schema even though the new "
                                                     "namespace exists";
  EXPECT_TRUE(config.sensors.laser_sor_enabled) << "a key that only exists in the new schema was "
                                                   "not read";
}

TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace)
{
  robot::NodeHandle root;
  robot::NodeHandle nh(root, "legacy_move_base");

  const MoveBase2Config config = MoveBase2Config::load(nh);

  EXPECT_EQ(config.robot_base_frame, "base_footprint");
  EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner");
}

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();
}