DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
test/CMakeLists.txt
View File

@@ -1,26 +0,0 @@
# Test target cho recovery_core contract.
# Build khi cấu hình standalone và khi catkin bật test target của package.
add_executable(recovery_core_plugin_loader_test plugin_loader_contract_test.cpp)
target_include_directories(recovery_core_plugin_loader_test
PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}
${STANDALONE_INCLUDE_DIRS}
)
target_link_libraries(recovery_core_plugin_loader_test
PRIVATE
recovery_core
yaml-cpp
${Boost_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
${CMAKE_DL_LIBS}
)
add_dependencies(recovery_core_plugin_loader_test ${RECOVERY_CORE_PLUGIN_TARGETS})
target_compile_definitions(recovery_core_plugin_loader_test
PRIVATE
RECOVERY_CORE_PLUGIN_DIR=\"$<TARGET_FILE_DIR:recovery_core_rotate_recovery>\"
)

186
test/backup_safety_test.cpp Normal file
View File

@@ -0,0 +1,186 @@
/*********************************************************************
*
* Kiểm ba lớp an toàn của BackUpRecovery.
*
* Bản trước không dùng collision checker (chỉ null-check con trỏ, và chỉ khi `require_costmap` bật
* — mặc định TẮT), nên mặc định robot lùi mù. Lùi là hướng robot thường không có sensor, nên đây là
* bộ test quan trọng nhất của Phase 3.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
struct BackUpFixture
{
BackUpFixture()
{
// Robot nhìn theo +x tại gốc; lùi nghĩa là đi về phía -x.
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
back_up = recovery_test::findBehavior(registry, "back_up");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* back_up = nullptr;
};
TEST(BackupSafety, RefusesToStartWhenObstacleIsBehind)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
// Vật cản chạm biên sau của footprint (footprint 0.6 x 0.4 -> biên sau ở x = -0.3).
fixture.rig.costmap.setLethalCircle(-0.35, 0.0, 0.10);
EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(BackupSafety, StopsWithZeroCommandWhenObstacleAppearsMidRun)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// Vài tick đầu chạy bình thường.
robot::Time now(1000.0);
for (int i = 0; i < 3; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_LT(result.velocity()->linear.x, 0.0); // âm = lùi
fixture.rig.applyCommand(result.command, 0.1);
}
// Có người bước vào phía sau robot.
const double robot_x = fixture.rig.pose.rawPose().x;
fixture.rig.costmap.setLethalCircle(robot_x - 0.36, 0.0, 0.10);
now = robot::Time(now.toSec() + 0.1);
const auto blocked = fixture.back_up->update(now);
EXPECT_EQ(blocked.status, RecoveryStatus::kFailed);
ASSERT_NE(blocked.velocity(), nullptr);
EXPECT_DOUBLE_EQ(blocked.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(blocked.velocity()->angular.z, 0.0);
}
TEST(BackupSafety, StopsWhenPoseIsLost)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
// TF quá hạn / thiếu frame.
fixture.rig.pose.setAvailable(false);
const auto result = fixture.back_up->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}
TEST(BackupSafety, RefusesToStartWhenPoseIsUnavailable)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
fixture.rig.pose.setAvailable(false);
EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(BackupSafety, CommandNeverExceedsConfiguredSpeed)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < 40; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
break;
}
ASSERT_NE(result.velocity(), nullptr);
// linear_speed: 0.1 m/s trong config test.
EXPECT_LE(std::abs(result.velocity()->linear.x), 0.1 + 1e-9);
fixture.rig.applyCommand(result.command, 0.1);
}
}
TEST(BackupSafety, RampsUpInsteadOfSteppingToFullSpeed)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// acc_lim_x: 0.3 m/s^2 -> sau 0.05 s không thể vượt 0.015 m/s.
const auto first = fixture.back_up->update(robot::Time(1000.05));
ASSERT_NE(first.velocity(), nullptr);
EXPECT_LE(std::abs(first.velocity()->linear.x), 0.015 + 1e-9);
}
TEST(BackupSafety, CancelEmitsZeroCommand)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.back_up->cancel();
const auto result = fixture.back_up->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

68
test/config/recovery_behaviors_params.yaml Normal file
View File

@@ -0,0 +1,68 @@
# Config CHỈ dùng cho test của gói. Bản runtime nằm ở
# `pnkx_nav_core/config/recovery_behaviors_params.yaml` (C2) — sửa tham số vận hành thì sửa ở đó.
#
# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/recovery_core/test/config
#
# Chỉ khai các behavior chạy được với fake của nav_test_harness. ClearCostmapRecovery cần
# Costmap2DROBOT thật (TF + layer), nên nó được kiểm ở tầng tích hợp chứ không ở đây.
recovery:
# Thứ tự CHÍNH LÀ hành vi. Bản test giữ đúng thứ tự tương đối của bộ default.
behaviors:
- {name: wait, type: WaitRecovery}
- {name: rotate, type: RotateRecovery}
- {name: back_up, type: BackUpRecovery}
wait:
wait_duration: 3.0 # [s]
rotate:
full_rotation: true # quay đủ 2*pi để costmap thấy xung quanh
angular_speed: 0.4 # [rad/s] độ lớn; dấu do goal.angle quyết định
acc_lim_theta: 0.8 # [rad/s^2]
sim_granularity: 0.1 # [rad] bước quét cung lúc start
timeout: 20.0 # [s]
back_up:
backup_distance: 0.28 # [m] cố ý KHÔNG chia hết cho quãng đi mỗi tick của test
backup_distance_max: 1.0 # [m] trần cứng
linear_speed: 0.1 # [m/s] độ lớn; dấu âm do plugin đặt
acc_lim_x: 0.3 # [m/s^2]
timeout: 15.0 # [s]
# Namespace dành riêng cho test đường lỗi: param ngoài dải cho phép phải bị từ chối kèm cảnh báo,
# không được nhận nguyên giá trị.
recovery_bad:
bad_timeout:
timeout: -5.0 # [s] âm -> base phải quay về 0 (không giới hạn)
# Namespace dành riêng cho test đường lỗi: type không có khoá library_path tương ứng bên dưới.
recovery_missing_library:
behaviors:
- {name: ghost, type: GhostRecovery}
# Namespace dành riêng cho test đường lỗi: một behavior tốt, một behavior hỏng — behavior tốt vẫn
# phải được giữ lại.
recovery_partial:
behaviors:
- {name: wait, type: WaitRecovery}
- {name: ghost, type: GhostRecovery}
wait:
wait_duration: 1.0 # [s]
# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
WaitRecovery:
library_path: librecovery_core_wait_recovery
RotateRecovery:
library_path: librecovery_core_rotate_recovery
BackUpRecovery:
library_path: librecovery_core_back_up_recovery
ClearCostmapRecovery:
library_path: librecovery_core_clear_costmap_recovery
# GhostRecovery cố ý KHÔNG khai library_path — registry_test dựa vào đó để kiểm thông báo lỗi có
# nêu đích danh khoá bị thiếu hay không.

239
test/goal_semantics_test.cpp Normal file
View File

@@ -0,0 +1,239 @@
/*********************************************************************
*
* Kiểm ngữ nghĩa RecoveryGoal sau khi bỏ sentinel "0 = dùng default".
*
* Bản trước dùng `std::abs(goal.angle) > 0.0` để quyết định "caller có đặt góc không", nên một góc
* tính từ hình học ra đúng 0 bị âm thầm thay bằng pi/2 — robot quay 90 độ mà không log gì. Test này
* khoá lại: `std::optional` phân biệt được "không đặt" với "đặt bằng 0".
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kTwoPi = 2.0 * M_PI;
/// Nạp bộ behavior test qua đúng đường Boost.DLL mà runtime dùng.
struct PluginFixture
{
PluginFixture()
{
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
}
recovery_core::RecoveryBehavior* behavior(const std::string& name)
{
return recovery_test::findBehavior(registry, name);
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
};
TEST(GoalSemantics, ExplicitZeroAngleDoesNotRotate)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 0.0; // "đừng quay" — một yêu cầu hợp lệ
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(GoalSemantics, UnsetAngleUsesFullRotationDefault)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal; // angle không đặt
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}
TEST(GoalSemantics, TinyAngleIsRespectedNotReplaced)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 0.05; // nhỏ nhưng khác 0
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 0.05, 1e-3);
}
TEST(GoalSemantics, NegativeAngleRotatesClockwise)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = -1.0;
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
fixture.rig.clock.advance(0.1);
const auto result = rotate->update(fixture.rig.clock.now());
ASSERT_NE(result.velocity(), nullptr);
EXPECT_LT(result.velocity()->angular.z, 0.0);
}
TEST(GoalSemantics, AngleBeyondTwoPiIsClamped)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 100.0; // ~16 vòng — bản cũ nhận nguyên
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}
TEST(GoalSemantics, UnsetDistanceUsesConfiguredDefault)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal; // distance không đặt -> backup_distance: 0.28 trong config test
ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
const auto result = back_up->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 0.28, 1e-3);
}
TEST(GoalSemantics, DistanceBeyondMaxIsClamped)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal;
goal.distance = 50.0; // backup_distance_max: 1.0
ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
const auto result = back_up->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 1.0, 1e-3);
}
TEST(GoalSemantics, NonPositiveDistanceIsRejectedByBase)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal;
goal.distance = -0.1;
// distance có giá trị thì phải > 0. Base bắt trước khi plugin nhìn thấy goal.
EXPECT_FALSE(back_up->start(goal, robot::Time(1000.0)));
}
TEST(GoalSemantics, NonFiniteGoalIsRejectedByBase)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal nan_angle;
nan_angle.angle = std::numeric_limits<double>::quiet_NaN();
EXPECT_FALSE(rotate->start(nan_angle, robot::Time(1000.0)));
RecoveryGoal nan_param;
nan_param.params["angular_speed"] = std::numeric_limits<double>::infinity();
EXPECT_FALSE(rotate->start(nan_param, robot::Time(1000.0)));
}
TEST(GoalSemantics, PerRunSpeedOverrideIsApplied)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
goal.params["angular_speed"] = 0.2;
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
// Đi đủ lâu để ramp gia tốc đạt trần tốc độ yêu cầu.
robot::Time now(1000.0);
double commanded = 0.0;
for (int i = 0; i < 20; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = rotate->update(now);
ASSERT_NE(result.velocity(), nullptr);
commanded = result.velocity()->angular.z;
fixture.rig.applyCommand(result.command, 0.1);
}
EXPECT_NEAR(commanded, 0.2, 1e-6);
}
} // 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

190
test/output_kind_test.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*********************************************************************
*
* Kiểm base CƯỠNG CHẾ bất biến họ output.
*
* Bản trước để plugin tự đặt `output_type` mỗi tick, nên nó không dùng được để route: một plugin họ
* path trả `kVelocity` ở tick đầu, còn base thì tự sinh `Velocity(zero)` cho mọi họ ở nhánh
* terminal. Test này khoá lại hành vi đúng.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <limits>
#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;
/// Dựng một MockBehavior đã configure + start, sẵn sàng nhận update().
struct Fixture
{
explicit Fixture(RecoveryOutputType kind) : behavior(kind)
{
ctx = rig.ctx;
EXPECT_TRUE(behavior.configure("mock", ctx, nh));
EXPECT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
MockBehavior behavior;
};
TEST(OutputKind, NoneFamilyNeverReportsVelocity)
{
Fixture fixture(RecoveryOutputType::kNone);
// Plugin cố tình trả output vận tốc dù nó khai họ kNone.
robot_geometry_msgs::Twist rogue;
rogue.linear.x = 0.5;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.velocity(), nullptr);
// Quan trọng nhất: giá trị rogue không được rò ra ngoài dưới bất kỳ dạng nào.
EXPECT_DOUBLE_EQ(result.command.linear.x, 0.0);
}
TEST(OutputKind, PathFamilyNeverReportsVelocity)
{
Fixture fixture(RecoveryOutputType::kPath);
robot_geometry_msgs::Twist rogue;
rogue.angular.z = 1.0;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.velocity(), nullptr);
EXPECT_EQ(result.pathOut(), nullptr);
}
TEST(OutputKind, VelocityFamilyNeverReportsPath)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_nav_msgs::Path rogue;
rogue.poses.resize(2);
fixture.behavior.next_result =
recovery_core::RecoveryResult::PathOut(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.pathOut(), nullptr);
EXPECT_TRUE(result.path.poses.empty());
}
TEST(OutputKind, MatchingKindPassesThrough)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist cmd;
cmd.linear.x = -0.1;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(cmd, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, -0.1);
}
TEST(OutputKind, NoneIsAlwaysAllowed)
{
// Một behavior họ velocity vẫn được phép nói "tick này không có output".
Fixture fixture(RecoveryOutputType::kVelocity);
fixture.behavior.next_result = recovery_core::RecoveryResult::Running();
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.status, RecoveryStatus::kRunning);
}
TEST(OutputKind, TerminalStopOutputMatchesFamily)
{
{
Fixture none_family(RecoveryOutputType::kNone);
none_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
ASSERT_EQ(none_family.behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
// Tick sau khi đã kết thúc: họ kNone KHÔNG được báo cáo mình phát vận tốc.
const auto after = none_family.behavior.update(robot::Time(1000.2));
EXPECT_EQ(after.output_type, RecoveryOutputType::kNone);
}
{
Fixture velocity_family(RecoveryOutputType::kVelocity);
velocity_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
ASSERT_EQ(velocity_family.behavior.update(robot::Time(1000.1)).status,
RecoveryStatus::kSucceeded);
// Họ velocity thì ngược lại: phải có lệnh dừng tường minh để caller publish.
const auto after = velocity_family.behavior.update(robot::Time(1000.2));
ASSERT_NE(after.velocity(), nullptr);
EXPECT_DOUBLE_EQ(after.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(after.velocity()->angular.z, 0.0);
}
}
TEST(OutputKind, NonFiniteVelocityIsForcedToStop)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist bad;
bad.linear.x = std::numeric_limits<double>::quiet_NaN();
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
// NaN lọt ra cmd_vel là không được phép đi tiếp.
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_TRUE(std::isfinite(result.velocity()->linear.x));
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}
TEST(OutputKind, InfiniteAngularVelocityIsForcedToStop)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist bad;
bad.angular.z = std::numeric_limits<double>::infinity();
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 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();
}

288
test/plugin_loader_contract_test.cpp
View File

@@ -1,288 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — Boost.DLL plugin contract test.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>
#include <robot/robot.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <boost/dll/import.hpp>
#include <boost/dll/shared_library.hpp>
#include <boost/filesystem/path.hpp>
namespace
{
using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr();
struct PluginConfig
{
std::string name;
std::string type;
};
std::vector<boost::dll::shared_library> libraries_;
std::vector<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr> creators_;
std::vector<std::string> name_plugins_;
std::vector<robot_geometry_msgs::PoseStamped> global_path_;
void expect(bool condition, const std::string& message)
{
if (!condition)
{
std::cerr << "[FAIL] " << message << std::endl;
std::exit(1);
}
}
std::vector<PluginConfig> getListRecoveryPlugins()
{
std::vector<PluginConfig> my_list;
robot::NodeHandle priv_nh;
if (priv_nh.hasParam("recovery_behaviors"))
{
YAML::Node my_plugins = priv_nh.getParamValue("recovery_behaviors");
if (my_plugins.IsDefined() && my_plugins.IsSequence())
{
std::set<std::string> name_plugins;
for (std::size_t i = 0; i < my_plugins.size(); ++i)
{
YAML::Node plugin_i = my_plugins[i];
// 1. Phải là map
if (!plugin_i.IsMap())
{
std::cerr<< "Recovery plugin at index " << i << " must be a map." << std::endl;
continue;
}
// 2. Phải có name và type
if (!plugin_i["name"].IsDefined() || !plugin_i["type"].IsDefined())
{
std::cerr << "Recovery plugin at index " << i << " must have 'name' and 'type'." << std::endl;
continue;
}
PluginConfig p;
try
{
p.name = plugin_i["name"].as<std::string>();
p.type = plugin_i["type"].as<std::string>();
}
catch (const YAML::Exception& e)
{
std::cerr << "Invalid recovery plugin at index " << i << ": " << e.what() << std::endl;
continue;
}
// 3. Kiểm tra duplicate name
const auto result = name_plugins.insert(p.name);
if (!result.second)
{
std::cerr << "A recovery plugin with name '" << p.name << "' already exists." << std::endl;
continue;
}
// 4. Chỉ thêm sau khi validate hoàn toàn
my_list.push_back(p);
name_plugins_.push_back(p.name);
robot::log_warning("Load plugin: name: %s, type: %s", p.name.c_str(), p.type.c_str());
}
}
else
{
std::cerr << "'recovery_behaviors' must be a sequence." << std::endl;
}
}
return my_list;
}
void testLoadPlugins()
{
std::vector<PluginConfig> my_list_plugin = getListRecoveryPlugins();
if(my_list_plugin.empty())
{
robot::log_error("No recovery plugins found in configuration.");
return;
}
for(const auto& plugin : my_list_plugin)
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(plugin.type);
if(path_file_so == "")
{
robot::log_error("Cannot find library for recovery behavior type '%s'", plugin.type.c_str());
return;
}
robot::log_info("Loading recovery behavior type '%s' from '%s'", plugin.type.c_str(), path_file_so.c_str());
try
{
// 1. Load library vào local handle.
boost::dll::shared_library library(path_file_so);
// 2. Lấy factory alias.
auto& factory = library.get_alias<Factory>(plugin.type);
// 3. Factory tạo behavior object.
recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = factory();
if (!behavior)
{
robot::log_error("Factory returned nullptr for '%s'", plugin.type.c_str());
return;
}
// 4. Chuyển quyền giữ library vào storage sống lâu dài.
libraries_.push_back(std::move(library));
recovery_core::RecoveryContext ctx;
ctx.global_path = &global_path_;
behavior->configure(plugin.name, ctx);
creators_.push_back(behavior);
}
catch (const std::exception& e)
{
robot::log_error("Failed to load recovery behavior '%s': %s", plugin.type.c_str(), e.what());
return;
}
// expect(static_cast<bool>(behavior), "Failed to load plugin: " + plugin.name + " of type: " + plugin.type);
}
}
void testRotatePlugin()
{
for(const auto& behavior : creators_)
{
if(behavior->getNameRecoveryBehavior() == "rotation_rc")
{
// Caller đặt góc quay RUNTIME = pi/2 (90 độ) cho lượt này.
recovery_core::RecoveryGoal goal;
goal.angle = 1.57079632679;
const recovery_core::RecoveryResult started = behavior->start(goal);
expect(started.status == recovery_core::RecoveryStatus::kRunning,
"rotate must be running right after start");
const recovery_core::RecoveryResult first = behavior->update();
expect(first.status == recovery_core::RecoveryStatus::kRunning,
"rotate first cycle must be running");
expect(first.output_type == recovery_core::RecoveryOutputType::kVelocity,
"rotate must return velocity output");
expect(first.command.angular.z > 0.0, "rotate must command positive angular.z for +angle");
expect(first.progress >= 0.0 && first.progress < 1.0,
"rotate progress must advance within [0,1)");
expect(first.remaining > 0.0, "rotate must report remaining angle while running");
recovery_core::RecoveryResult last = first;
for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
{
last = behavior->update();
}
expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
"rotate must finish within bounded cycles");
expect(last.output_type == recovery_core::RecoveryOutputType::kVelocity,
"rotate final result must still be a velocity output");
expect(std::abs(last.command.angular.z) < 1e-9,
"rotate must return a zero angular command when complete");
expect(std::abs(last.progress - 1.0) < 1e-9, "rotate must report full progress on success");
expect(last.remaining < 1e-9, "rotate must report zero remaining on success");
}
}
}
void testBackupPlugin()
{
for(const auto& behavior : creators_)
{
if(behavior->getNameRecoveryBehavior() == "backward_rc")
{
// Caller đặt khoảng lùi RUNTIME = 0.3 m cho lượt này.
recovery_core::RecoveryGoal goal;
goal.distance = 0.3;
behavior->start(goal);
const recovery_core::RecoveryResult first = behavior->update();
expect(first.status == recovery_core::RecoveryStatus::kRunning,
"backup first cycle must be running");
expect(first.command.linear.x < 0.0, "backup must command negative linear.x");
recovery_core::RecoveryResult last = first;
for (int i = 0; i < 1000 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
{
last = behavior->update();
}
expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
"backup must finish within bounded cycles");
expect(std::abs(last.command.linear.x) < 1e-9,
"backup must return a zero command when complete");
expect(std::abs(last.progress - 1.0) < 1e-9, "backup must report full progress on success");
}
}
}
void testRegenPathPlugin()
{
robot_geometry_msgs::PoseStamped pose1;
pose1.pose.position.x = 1.0;
robot_geometry_msgs::PoseStamped pose2;
pose2.pose.position.x = 2.0;
global_path_.push_back(pose1);
global_path_.push_back(pose2);
for(const auto& behavior : creators_)
{
if(behavior->getNameRecoveryBehavior() == "regen_path_rc")
{
behavior->start(recovery_core::RecoveryGoal());
const recovery_core::RecoveryResult result = behavior->update();
expect(result.status == recovery_core::RecoveryStatus::kSucceeded,
"regen path must succeed with a non-empty global path");
expect(result.output_type == recovery_core::RecoveryOutputType::kPath,
"regen path must return path output");
expect(result.path.poses.size() == global_path_.size(),
"regen path must preserve the global path size");
global_path_ = result.path.poses;
}
}
}
} // namespace
int main()
{
testLoadPlugins();
if(creators_.empty()) return 0;
// for(auto& behavior : creators_)
// {
// std::cout<<behavior->getNameRecoveryBehavior()<<std::endl;
// }
testRegenPathPlugin();
testRotatePlugin();
testBackupPlugin();
std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
return 0;
}

188
test/pose_progress_test.cpp Normal file
View File

@@ -0,0 +1,188 @@
/*********************************************************************
*
* Kiểm tiến độ đo bằng POSE THẬT, không dead-reckon theo chu kỳ cấu hình.
*
* Đây là test cho lỗi nặng nhất của bản trước: quãng đi được tính bằng
* `|cmd.linear.x| * control_period` với `control_period` lấy từ YAML. Control loop chạy chậm gấp N
* lần là robot đi quá quãng gấp N lần, còn bánh trượt thì vẫn báo hoàn thành.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kConfiguredDistance = 0.28; // [m] khớp `recovery/back_up/backup_distance`
struct BackUpFixture
{
BackUpFixture()
{
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
back_up = recovery_test::findBehavior(registry, "back_up");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* back_up = nullptr;
};
/**
* @brief Chạy trọn một lượt lùi với chu kỳ @p dt, mô phỏng robot đi đúng lệnh phát ra.
* @return quãng đường thực tế robot đã lùi [m].
*/
double runBackup(BackUpFixture& fixture, double dt, int max_ticks = 2000)
{
EXPECT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < max_ticks; ++i)
{
now = robot::Time(now.toSec() + dt);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
break;
}
fixture.rig.applyCommand(result.command, dt);
}
return -fixture.rig.pose.rawPose().x; // lùi theo -x
}
TEST(PoseProgress, NominalRateStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 0.1);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, FiveTimesSlowerLoopStillStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
// Control loop chạy chậm gấp 5. Bản cũ sai ~400% ở đây vì nhân với hằng số config.
const double traveled = runBackup(fixture, 0.5);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, TwentyTimesSlowerLoopStillStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 2.0);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, FasterLoopStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 0.02);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, StalledRobotNeverReportsSuccess)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// Bánh trượt hoàn toàn: lệnh vẫn phát nhưng pose không đổi. Bản cũ tích phân vận tốc LỆNH nên vẫn
// báo kSucceeded; bản này phải chạy tới khi timeout chứ không được nói dối.
robot::Time now(1000.0);
bool reported_success = false;
for (int i = 0; i < 200; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.status == RecoveryStatus::kSucceeded)
{
reported_success = true;
break;
}
if (result.terminal())
{
break; // timeout -> kFailed, đúng như mong đợi
}
// KHÔNG applyCommand: robot không nhúc nhích.
}
EXPECT_FALSE(reported_success);
}
TEST(PoseProgress, ProgressAndRemainingTrackRealPose)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
double last_progress = -1.0;
for (int i = 0; i < 10; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
break;
}
const double traveled = -fixture.rig.pose.rawPose().x;
EXPECT_NEAR(result.remaining, kConfiguredDistance - traveled, 1e-6);
EXPECT_GE(result.progress, last_progress);
EXPECT_GE(result.progress, 0.0);
EXPECT_LE(result.progress, 1.0);
last_progress = result.progress;
fixture.rig.applyCommand(result.command, 0.1);
}
}
} // 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

280
test/recovery_lifecycle_test.cpp Normal file
View File

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

213
test/recovery_test_utils.h Normal file
View File

@@ -0,0 +1,213 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — tiện ích dùng chung cho test.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_
#define RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_
#include <cmath>
#include <string>
#include <utility>
#include <vector>
#include <nav_test_harness/fake_clock.h>
#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_math.h>
#include <recovery_core/recovery_registry.h>
namespace recovery_test
{
/// @brief Behavior trong @p registry mang tên @p name, hoặc nullptr.
inline recovery_core::RecoveryBehavior* findBehavior(const recovery_core::RecoveryRegistry& registry,
const std::string& name)
{
for (std::size_t i = 0; i < registry.size(); ++i)
{
if (registry.nameAt(i) == name)
{
return registry.at(i);
}
}
return nullptr;
}
/**
* @brief Nối `nav_test_harness::FakePoseProvider` vào cổng của recovery_core.
*
* Hai interface cố ý tách nhau: `recovery_core` không được phụ thuộc gói test harness, và
* `nav_test_harness` phục vụ nhiều gói khác nhau. Adapter mỏng ở đây chính là thứ `RecoveryRunner`
* sẽ làm với `move_base2::PosePort` ở Phase 4.
*/
class HarnessPoseProvider final : public recovery_core::PoseProvider
{
public:
explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
{
}
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
{
return fake_ != nullptr && fake_->getRobotPose(pose);
}
private:
nav_test_harness::FakePoseProvider* fake_ = nullptr;
};
/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng của recovery_core.
class HarnessCollisionChecker final : public recovery_core::CollisionChecker
{
public:
explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
{
}
double footprintCost(double x, double y, double theta) const override
{
return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
}
private:
nav_test_harness::FakeCollisionChecker* fake_ = nullptr;
};
/// @brief Nguồn plan đơn giản do test bơm thẳng.
class StubPlanProvider final : public recovery_core::PlanProvider
{
public:
void setPlan(std::vector<robot_geometry_msgs::PoseStamped> plan)
{
plan_ = std::move(plan);
}
bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const override
{
if (plan_.empty())
{
return false;
}
out = plan_;
return true;
}
private:
std::vector<robot_geometry_msgs::PoseStamped> plan_;
};
/**
* @brief Behavior giả, cho phép test điều khiển từng hook.
*
* Dùng để kiểm phần **base** (guard vòng đời, timeout, cưỡng chế họ output) mà không phụ thuộc vào
* hành vi của plugin thật.
*/
class MockBehavior final : public recovery_core::RecoveryBehavior
{
public:
explicit MockBehavior(recovery_core::RecoveryOutputType kind) : kind_(kind)
{
next_result = recovery_core::RecoveryResult::Running();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return kind_;
}
// Núm điều khiển cho test.
bool configure_ok = true;
bool start_ok = true;
recovery_core::RecoveryResult next_result;
// Ghi nhận để assert.
int configure_calls = 0;
int start_calls = 0;
int update_calls = 0;
int cancel_calls = 0;
double last_dt = -1.0;
recovery_core::RecoveryGoal last_goal;
protected:
bool onConfigure(robot::NodeHandle& /*nh*/) override
{
++configure_calls;
return configure_ok;
}
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
++start_calls;
last_goal = goal;
return start_ok;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{
++update_calls;
last_dt = dt;
return next_result;
}
recovery_core::RecoveryResult onCancel() override
{
++cancel_calls;
return recovery_core::RecoveryBehavior::onCancel();
}
private:
recovery_core::RecoveryOutputType kind_;
};
/**
* @brief Bộ đồ nghề đầy đủ cho một test plugin họ velocity.
*
* Gom costmap giả, pose giả, collision checker giả và đồng hồ giả, kèm hàm mô phỏng robot chạy
* theo đúng lệnh vận tốc mà behavior phát ra.
*/
struct VelocityRig
{
VelocityRig(double span_m = 8.0, double resolution = 0.05,
double footprint_length = 0.6, double footprint_width = 0.4)
: costmap(nav_test_harness::FakeCostmap::centered(span_m, resolution))
, checker(&costmap,
nav_test_harness::FakeCollisionChecker::rectangleFootprint(footprint_length,
footprint_width))
, pose_port(&pose)
, collision_port(&checker)
{
ctx.pose = &pose_port;
ctx.collision = &collision_port;
ctx.plan = &plan;
}
/// @brief Cho robot chạy theo @p command trong @p dt giây, cập nhật pose giả.
void applyCommand(const robot_geometry_msgs::Twist& command, double dt)
{
const double yaw = pose.rawPose().theta;
pose.moveBy(command.linear.x * std::cos(yaw) * dt, command.linear.x * std::sin(yaw) * dt,
command.angular.z * dt);
}
nav_test_harness::FakeCostmap costmap;
nav_test_harness::FakeCollisionChecker checker;
nav_test_harness::FakePoseProvider pose;
nav_test_harness::FakeClock clock;
StubPlanProvider plan;
HarnessPoseProvider pose_port;
HarnessCollisionChecker collision_port;
recovery_core::RecoveryContext ctx;
};
} // namespace recovery_test
#endif // RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_

181
test/registry_test.cpp Normal file
View File

@@ -0,0 +1,181 @@
/*********************************************************************
*
* Kiểm đường nạp plugin thật: YAML -> library_path -> Boost.DLL -> configure.
*
* Bản test cũ của gói KHÔNG kiểm được gì: nó `return` khi không nạp được plugin nào và `main` trả 0,
* nên "không plugin nào chạy" cũng in [PASS]. Test này phải fail được.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <memory>
#include <string>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryOutputType;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;
TEST(Registry, LoadsDeclaredBehaviorsInOrder)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
// Thứ tự CHÍNH LÀ hành vi: caller thử behavior 0 trước.
ASSERT_EQ(registry.size(), 3u);
EXPECT_EQ(registry.nameAt(0), "wait");
EXPECT_EQ(registry.nameAt(1), "rotate");
EXPECT_EQ(registry.nameAt(2), "back_up");
}
TEST(Registry, LoadedBehaviorsReportCorrectOutputKind)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
ASSERT_EQ(registry.size(), 3u);
EXPECT_EQ(registry.at(0)->outputKind(), RecoveryOutputType::kNone); // wait
EXPECT_EQ(registry.at(1)->outputKind(), RecoveryOutputType::kVelocity); // rotate
EXPECT_EQ(registry.at(2)->outputKind(), RecoveryOutputType::kVelocity); // back_up
}
TEST(Registry, PerInstanceParamsComeFromItsOwnNamespace)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
// `recovery/rotate/timeout: 20.0`, `recovery/back_up/timeout: 15.0`, `recovery/wait` không khai.
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "rotate")->timeout(), 20.0);
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "back_up")->timeout(), 15.0);
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "wait")->timeout(), 0.0);
}
TEST(Registry, MissingLibraryPathIsReportedAndFails)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
// `GhostRecovery` được khai trong danh sách nhưng không có khoá library_path.
EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_missing_library", rig.ctx));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, OneBadBehaviorDoesNotDropTheGoodOnes)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
// Một đường phục hồi hỏng không nên xoá sạch các đường còn lại.
EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_partial", rig.ctx));
ASSERT_EQ(registry.size(), 1u);
EXPECT_EQ(registry.nameAt(0), "wait");
}
TEST(Registry, MissingBehaviorListFails)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
EXPECT_FALSE(registry.loadFromConfig(nh, "namespace_khong_ton_tai", rig.ctx));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, IndexOutOfRangeIsSafe)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
EXPECT_EQ(registry.at(99), nullptr);
EXPECT_TRUE(registry.nameAt(99).empty());
}
TEST(Registry, RegisterBehaviorRejectsNull)
{
recovery_core::RecoveryRegistry registry;
EXPECT_FALSE(registry.registerBehavior(nullptr));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, RegisterBehaviorAppendsInOrder)
{
recovery_core::RecoveryRegistry registry;
auto first = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);
auto second = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);
ASSERT_TRUE(registry.registerBehavior(first));
ASSERT_TRUE(registry.registerBehavior(second));
ASSERT_EQ(registry.size(), 2u);
EXPECT_EQ(registry.at(0), first.get());
EXPECT_EQ(registry.at(1), second.get());
}
TEST(Registry, ClearReleasesBehaviors)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
ASSERT_GT(registry.size(), 0u);
registry.clear();
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, BehaviorsStayUsableAfterLoaderScopeEnds)
{
VelocityRig rig;
recovery_core::RecoveryRegistry registry;
{
// NodeHandle chết trước registry: behavior vẫn phải sống, vì registry mới là thứ giữ .so.
robot::NodeHandle scoped_nh;
ASSERT_TRUE(registry.loadFromConfig(scoped_nh, "recovery", rig.ctx));
}
auto* wait = recovery_test::findBehavior(registry, "wait");
ASSERT_NE(wait, nullptr);
ASSERT_TRUE(wait->start(recovery_core::RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(wait->update(robot::Time(1003.0)).status, recovery_core::RecoveryStatus::kSucceeded);
}
} // 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

216
test/rotate_safety_test.cpp Normal file
View File

@@ -0,0 +1,216 @@
/*********************************************************************
*
* Kiểm quét cung và đo góc bằng pose thật của RotateRecovery.
*
* Bản trước không dùng `ctx()` một lần nào trong toàn file: quay mù, và đếm góc bằng
* `angular_speed * control_period`.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kTwoPi = 2.0 * M_PI;
struct RotateFixture
{
RotateFixture()
{
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
rotate = recovery_test::findBehavior(registry, "rotate");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* rotate = nullptr;
};
TEST(RotateSafety, RefusesToStartWhenArcIsBlocked)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Footprint 0.6 x 0.4 quanh gốc: khi quay 90 độ, mũi robot quét tới y ~ +/-0.3.
// Đặt vật cản ở đó -> cung quay bị chặn dù vị trí hiện tại vẫn trống.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_FALSE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, StartsWhenArcIsClear)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, PartialArcAvoidsBlockedSector)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Vật cản chỉ chặn khi robot đã quay đáng kể; cung nhỏ vẫn phải đi được.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal small_arc;
small_arc.angle = 0.05;
EXPECT_TRUE(fixture.rotate->start(small_arc, robot::Time(1000.0)));
}
TEST(RotateSafety, StopsWithZeroCommandWhenPoseIsLost)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rig.pose.setAvailable(false);
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(RotateSafety, RefusesToStartWhenPoseIsUnavailable)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
fixture.rig.pose.setAvailable(false);
EXPECT_FALSE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(RotateSafety, FullRotationCountsPastPiCorrectly)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
// Cộng dồn góc quay THẬT do test tự đo, độc lập với con số plugin báo.
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
robot::Time now(1000.0);
bool finished = false;
for (int i = 0; i < 2000; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
finished = true;
break;
}
fixture.rig.applyCommand(result.command, 0.1);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
ASSERT_TRUE(finished);
// Quay đủ vòng: phép chuẩn hoá từng bước phải đếm đúng qua mốc pi, không bị wrap về 0.
EXPECT_NEAR(swept, kTwoPi, 0.05 * kTwoPi);
}
TEST(RotateSafety, SlowLoopDoesNotOvershoot)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = 1.0;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
// dt gấp 5 lần nhịp thường.
robot::Time now(1000.0);
for (int i = 0; i < 200; ++i)
{
now = robot::Time(now.toSec() + 0.5);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
break;
}
fixture.rig.applyCommand(result.command, 0.5);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
EXPECT_NEAR(swept, 1.0, 0.05);
}
TEST(RotateSafety, CancelEmitsZeroCommand)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rotate->cancel();
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

132
test/timeout_test.cpp Normal file
View File

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

179
test/wait_recovery_test.cpp Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* Kiểm WaitRecovery — behavior mới của bộ default.
*
* Đây là recovery an toàn nhất (robot không di chuyển) và hữu dụng nhất cho AMR trong kho, nơi phần
* lớn tình huống chặn đường là vật cản động. Nó cũng là chỗ rẻ nhất để chứng minh đường `elapsed`
* của base chạy đúng theo đồng hồ thật.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kConfiguredWait = 3.0; // [s] khớp `recovery/wait/wait_duration`
struct WaitFixture
{
WaitFixture()
{
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
wait = recovery_test::findBehavior(registry, "wait");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* wait = nullptr;
};
TEST(WaitRecovery, DeclaresNoOutputFamily)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
EXPECT_EQ(fixture.wait->outputKind(), RecoveryOutputType::kNone);
}
TEST(WaitRecovery, NeverEmitsVelocity)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < 60; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.wait->update(now);
// Behavior đứng yên tuyệt đối không được làm caller tưởng nó đang lái robot.
EXPECT_EQ(result.velocity(), nullptr);
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
if (result.terminal())
{
break;
}
}
}
TEST(WaitRecovery, SucceedsAfterConfiguredDuration)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1002.9)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, CountsByClockNotByTickCount)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
// Một tick duy nhất nhưng nhảy qua trọn thời lượng: phải xong ngay, không cần đủ số nhịp.
const auto result = fixture.wait->update(robot::Time(1000.0 + kConfiguredWait));
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
EXPECT_NEAR(result.elapsed, kConfiguredWait, 1e-6);
}
TEST(WaitRecovery, ProgressAdvancesMonotonically)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
double last = -1.0;
for (int i = 1; i <= 5; ++i)
{
const auto result = fixture.wait->update(robot::Time(1000.0 + 0.5 * i));
EXPECT_GE(result.progress, last);
EXPECT_GE(result.remaining, 0.0);
last = result.progress;
}
}
TEST(WaitRecovery, PerRunDurationOverride)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
RecoveryGoal goal;
goal.params["wait_duration"] = 1.0;
ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1000.5)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, InvalidOverrideFallsBackToConfiguredDuration)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
RecoveryGoal goal;
goal.params["wait_duration"] = -5.0; // vô lý -> phải cảnh báo và dùng default
ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1002.0)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, NeedsNoPoseOrCollisionPorts)
{
// Điểm mạnh của WaitRecovery: chạy được cả khi TF hỏng, nên nó là đường phục hồi cuối cùng còn
// dùng được khi mọi thứ khác đã mất pose.
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
fixture.rig.pose.setAvailable(false);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
} // 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
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}