246 lines
7.8 KiB
C++
246 lines
7.8 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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();
|
|
}
|