DuongTD/move_base2
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:41:32 +07:00
parent d8babff20b
commit 701d25f952
70 changed files with 5572 additions and 1146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
test/action_runner_test.cpp
View File

@@ -13,13 +13,14 @@
#include <robot/node_handle.h>
#include <action_core/action_handler.h>
#include <move_base2/runners/action_runner.h>
#include "fake_ports.h"
namespace
{
using move_base2::ActionHandler;
using action_core::ActionHandler;
using move_base2::ActionRunner;
using move_base2::ActionTick;
using move_base2::testing::FakeClockPort;
@@ -45,7 +46,8 @@ public:
{
}
bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
bool configure(const std::string& name, const action_core::ActionContext& /*ctx*/,
robot::NodeHandle& /*nh*/) override
{
name_ = name;
return configure_ok;
@@ -63,10 +65,10 @@ public:
return start_ok;
}
ActionTick update(const robot::Time& /*now*/) override
action_core::ActionTick update(const robot::Time& /*now*/) override
{
++update_count;
ActionTick tick;
action_core::ActionTick tick;
tick.status = next_status;
return tick;
}
@@ -78,7 +80,7 @@ public:
bool configure_ok = true;
bool start_ok = true;
ActionTick::Status next_status = ActionTick::Status::kRunning;
action_core::ActionStatus next_status = action_core::ActionStatus::kRunning;
int start_count = 0;
int update_count = 0;
@@ -224,7 +226,7 @@ TEST(ActionRunner, TicksUntilHandlerFinishes)
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
handler->next_status = ActionTick::Status::kSucceeded;
handler->next_status = action_core::ActionStatus::kSucceeded;
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
// Sau khi kết thúc, action không còn active: tick thêm là lỗi thứ tự gọi, không phải kRunning.
@@ -258,7 +260,8 @@ TEST(ActionRunner, ConfigureRequiresClock)
{
ActionRunner runner; // không setClock
robot::NodeHandle nh;
EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort thì handler không có mốc timeout";
EXPECT_FALSE(runner.configure(nh)) << "missing ClockPort means handlers have no timeout "
"reference";
}
TEST(ActionRunner, EmptyHandlerListIsValid)

93
test/config/move_base2_params.yaml
View File

@@ -4,6 +4,7 @@
# --- Tham số runtime, dùng cho config_validation_test ----------------------------------------
move_base2:
docking_requires_marker: false
controller_frequency: 20.0 # [Hz]
planner_frequency: 0.0 # [Hz] 0 = chỉ lập plan khi cần
planner_timeout: 5.0 # [s]
@@ -33,18 +34,53 @@ move_base2:
recovery_namespace: recovery
action_namespace: actions
mission_namespace: mission_adapters
backup_global_planner: TestBackupGlobalPlanner
position:
base_global_planner: TestGlobalPlanner
base_local_planner: TestLocalPlanner
xy_goal_tolerance: 0.15 # [m]
yaw_goal_tolerance: 0.10 # [rad]
docking:
base_global_planner: TestDockPlanner
base_local_planner: TestLocalPlanner
xy_goal_tolerance: 0.02 # [m] ghép nối cần chính xác hơn nhiều
yaw_goal_tolerance: 0.02 # [rad]
docking_marker_profiles:
trolley:
global_planner: TestTrolleyDockPlanner
local_planner: TestTrolleyDockLocalPlanner
# --- Schema runtime root-profile ---------------------------------------------------------------
# Đây là schema dùng bởi move_base_common_params.yaml của move_base2. Không có adapter gen-1.
root_profiles:
controller_frequency: 30.0
planner_frequency: 0.0
planner_patience: 2.0
controller_patience: 0.033333333
max_planning_retries: 0
recovery_behavior_enabled: true
docking_requires_marker: false
backup_global_planner: TestBackupGlobalPlanner
position:
global_planner: TestGlobalPlanner
local_planner: TestLocalPlanner
docking:
global_planner: TestDockPlanner
local_planner: TestLocalPlanner
docking_marker_profiles:
trolley:
global_planner: TestTrolleyDockPlanner
local_planner: TestTrolleyDockLocalPlanner
go_straight:
global_planner: TestStraightPlanner
local_planner: TestStraightLocalPlanner
rotate:
global_planner: TestRotatePlanner
local_planner: TestRotateLocalPlanner
# --- Cấu hình sai, dùng cho test đường lỗi -----------------------------------------------------
move_base2_bad_frequency:
@@ -77,6 +113,11 @@ recovery:
- {name: wait_short, type: WaitRecovery}
- {name: wait_long, type: WaitRecovery}
routes:
planning_failed: [wait_short]
controlling_failed: [wait_long, wait_short]
oscillation: [wait_long]
wait_short:
wait_duration: 1.0 # [s]
wait_long:
@@ -90,9 +131,40 @@ recovery_missing_library:
behaviors:
- {name: ghost, type: GhostRecovery}
# Mô phỏng config được commit trước plugin DetourPathRecovery. `configure()` báo partial failure,
# nhưng configureRoutes() phải bỏ detour_path thay vì đưa index không tồn tại sang StateMachine.
recovery_missing_detour:
behaviors:
- {name: wait_short, type: WaitRecovery}
- {name: detour_path, type: DetourPathRecovery}
routes:
planning_failed: [wait_short]
controlling_failed: [detour_path, wait_short]
oscillation: [detour_path, wait_short]
# --- Recovery THẬT cho kịch bản có vật cản (RecoveryScenarioDriver) ----------------------------
#
# Namespace riêng, KHÔNG dùng chung với `recovery` ở trên: hai bộ test hỏi hai câu khác nhau. Bộ kia
# kiểm chỗ nối RecoveryPort <-> recovery_core bằng behavior họ kNone; bộ này kiểm hành vi an toàn
# THẬT của BackUpRecovery trên lưới có vật cản.
recovery_scenario:
behaviors:
- {name: back_up, type: BackUpRecovery}
back_up:
backup_distance: 0.50 # [m] quãng lùi mong muốn
backup_distance_max: 1.0 # [m] trần cứng
linear_speed: 0.15 # [m/s] độ lớn; dấu âm do plugin đặt (lùi)
acc_lim_x: 1.0 # [m/s^2]
timeout: 15.0 # [s]
WaitRecovery:
library_path: librecovery_core_wait_recovery
BackUpRecovery:
library_path: librecovery_core_back_up_recovery
# GhostRecovery cố ý KHÔNG khai library_path.
# --- Action handler cho action_runner_test -----------------------------------------------------
@@ -136,7 +208,7 @@ actions_missing_library:
- {name: ghost, type: GhostActionHandler}
NoopActionHandler:
library_path: libmove_base2_noop_action_handler
library_path: libaction_core_noop_action_handler
# --- Global planner giả cho planner_runner_test -------------------------------------------------
#
@@ -167,6 +239,14 @@ TestControllerThrowing:
library_path: libmove_base2_test_local_planner
TestControllerRefusesLimits:
library_path: libmove_base2_test_local_planner
TestControllerMarkerProbe:
library_path: libmove_base2_test_local_planner
TestControllerFootprintProbe:
library_path: libmove_base2_test_local_planner
# Danh sách marker cho test đường docking (setDockingMarker) — format chuỗi cách nhau bằng space,
# đúng như maker_sources.yaml production.
maker_sources: dock_a dock_b
# TestControllerMissing cố ý KHÔNG khai library_path.
@@ -181,9 +261,6 @@ legacy_move_base:
max_planning_retries: 0
recovery_behavior_enabled: true
xy_goal_tolerance: 0.25 # [m] default chung cho cả bốn profile
yaw_goal_tolerance: 0.30 # [rad]
base_global_planner: SBPLLatticePlanner
base_local_planner: LocalPlannerAdapter # phải bị BỎ QUA có log

85
test/config_validation_test.cpp
View File

@@ -113,6 +113,37 @@ TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
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)
@@ -121,12 +152,37 @@ TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
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);
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)
@@ -170,17 +226,7 @@ TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
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);
<< "this config would reject EVERY request at runtime — it must be caught at startup";
}
TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
@@ -267,9 +313,6 @@ TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
MoveBase2Config config;
config.fromLegacyNodeHandle(nh);
EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25);
EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30);
EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25);
}
TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
@@ -319,8 +362,10 @@ TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
robot::NodeHandle root;
const MoveBase2Config config = MoveBase2Config::load(root);
EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới";
EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc";
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)

78
test/controller_runner_test.cpp
View File

@@ -153,7 +153,8 @@ TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
std::string error;
EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error));
EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
"configured";
}
TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
@@ -177,7 +178,8 @@ TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous controller yet "
"reloaded the library";
}
TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
@@ -213,7 +215,7 @@ TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10))); // [m/s]
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "velocity limit never reached the plugin";
}
TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
@@ -245,7 +247,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(runner.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "a limit set before the controller was loaded got lost";
}
TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
@@ -261,7 +263,8 @@ TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "swapping the controller dropped the active velocity "
"limit";
}
TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
@@ -274,6 +277,26 @@ TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
}
TEST(ControllerRunner, RefreshActivePlannerReinitializesItAndRestoresThePlan)
{
// Local planners như HybridController copy footprint vào cache trong initialize(). Refresh phải
// tạo instance mới, nhưng không được làm mất goal/plan giữa mission đang chạy.
Fixture fixture("TestControllerFootprintProbe");
ASSERT_TRUE(fixture.ok()) << fixture.error();
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
robot_geometry_msgs::Twist before;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(before));
ASSERT_GT(before.linear.x, 0.0);
ASSERT_TRUE(fixture.runner_.refreshActivePlanner());
robot_geometry_msgs::Twist after;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(after));
EXPECT_GT(after.linear.x, before.linear.x)
<< "planner was not recreated, or its active goal/plan was not restored";
}
TEST(ControllerRunner, NonFiniteLimitIsRejected)
{
Fixture fixture;
@@ -301,7 +324,8 @@ TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "measured velocity never reached the "
"plugin";
}
TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
@@ -373,7 +397,7 @@ TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
robot_geometry_msgs::Twist cmd;
EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "a command containing NaN was still written out";
}
TEST(ControllerRunner, ExceptionFromThePluginIsContained)
@@ -399,6 +423,46 @@ TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
}
// ================================================================================================
// setDockingMarker — kênh marker của chặng docking
//
// Docking planner đọc `maker_name` đúng MỘT lần trong initialize() (getMaker). Bản cũ dlopen lại
// planner mỗi lần dock nên luôn thấy giá trị mới; ControllerRunner cache instance nên phải tự dựng
// lại khi marker đổi — không làm là robot dock vào marker của chặng TRƯỚC, lặng lẽ.
// ================================================================================================
TEST(ControllerRunnerDockingMarker, RejectsMarkerNotInMakerSources)
{
Fixture fixture;
ASSERT_TRUE(fixture.ok()) << fixture.error();
EXPECT_FALSE(fixture.runner_.setDockingMarker("khong_ton_tai"));
// Marker hợp lệ (test/config: `maker_sources: dock_a dock_b`) phải qua.
EXPECT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
}
TEST(ControllerRunnerDockingMarker, CachedPlannerIsReinitializedWhenMarkerChanges)
{
Fixture fixture;
ASSERT_TRUE(fixture.ok()) << fixture.error();
ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.11, 1e-9) << "probe could not read maker_name='dock_a' at init";
// Đổi marker rồi swap lại CÙNG planner: instance cache phải được dựng lại để initialize() đọc
// giá trị mới. Nếu không, lệnh vẫn mang mã của dock_a — chính là "dock vào nhầm trạm".
ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_b"));
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.22, 1e-9)
<< "instance cache kept the old maker_name — it must re-init when the marker changes";
}
int main(int argc, char** argv)
{
setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);

70
test/fake_ports.h
View File

@@ -27,6 +27,7 @@
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/costmap_status_port.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/planner_port.h>
#include <move_base2/ports/pose_port.h>
@@ -173,6 +174,7 @@ public:
++make_plan_count_;
saw_order_ = saw_order_ || order != nullptr;
order_history_.push_back(order != nullptr);
in_flight_ = true;
pending_tag_ = tag;
@@ -276,6 +278,11 @@ public:
return saw_order_;
}
const std::vector<bool>& orderHistory() const
{
return order_history_;
}
private:
/// Hết kịch bản thì giữ kết quả cuối; kịch bản rỗng thì luôn thành công.
PlannerScript nextAction()
@@ -307,6 +314,7 @@ private:
std::size_t make_plan_count_ = 0;
std::size_t swap_count_ = 0;
bool saw_order_ = false;
std::vector<bool> order_history_;
};
// ------------------------------------------------------------------------------------------------
@@ -324,10 +332,21 @@ public:
return true;
}
void setTolerance(double xy_m, double yaw_rad) override
bool setDockingMarker(const std::string& marker) override
{
xy_tolerance_ = xy_m;
yaw_tolerance_ = yaw_rad;
last_docking_marker_ = marker;
return docking_marker_succeeds_;
}
void setDockingMarkerSucceeds(bool succeeds)
{
docking_marker_succeeds_ = succeeds;
}
/// @brief Marker của lời gọi setDockingMarker gần nhất; rỗng nếu chưa từng gọi.
const std::string& lastDockingMarker() const
{
return last_docking_marker_;
}
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
@@ -472,16 +491,6 @@ public:
return last_plan_size_;
}
double xyTolerance() const
{
return xy_tolerance_;
}
double yawTolerance() const
{
return yaw_tolerance_;
}
private:
robot_nav_2d_msgs::Path2D local_plan_;
robot_geometry_msgs::Twist measured_velocity_;
@@ -509,9 +518,9 @@ private:
std::string active_;
bool swap_succeeds_ = true;
bool set_plan_succeeds_ = true;
bool docking_marker_succeeds_ = true;
std::string last_docking_marker_;
double nominal_speed_ = 0.3; ///< [m/s]
double xy_tolerance_ = 0.0; ///< [m]
double yaw_tolerance_ = 0.0; ///< [rad]
std::size_t set_plan_count_ = 0;
std::size_t compute_count_ = 0;
@@ -814,6 +823,37 @@ private:
// ------------------------------------------------------------------------------------------------
/**
* @class FakeCostmapStatusPort
* @brief Costmap "còn hạn / hết hạn" bật tắt được, cho guard không-đi-mù.
*
* Mặc định **còn hạn**: một cổng giả im lặng chặn robot sẽ làm mọi test khác fail vì lý do không
* liên quan tới thứ chúng đang kiểm.
*/
class FakeCostmapStatusPort final : public CostmapStatusPort
{
public:
bool isCurrent() const override
{
++query_count_;
return current_;
}
void setCurrent(bool current)
{
current_ = current;
}
std::size_t queryCount() const
{
return query_count_;
}
private:
bool current_ = true;
mutable std::size_t query_count_ = 0;
};
class FakeMissionPort final : public MissionPort
{
public:

81
test/mission_adapter_bridge_test.cpp
View File

@@ -96,6 +96,71 @@ TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse)
EXPECT_EQ(request.actions.size(), 3U);
}
TEST(MissionAdapterBridgeConversion, CarriesDockingMarkerIndependentlyOfGoalFrame)
{
auto mission = makeMission(10);
mission->motion_hint = "docking";
mission->marker = "charger";
mission->goal_frame = "charger_02_goal";
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.profile, move_base2::MotionProfile::kDocking);
EXPECT_EQ(request.marker, "charger");
EXPECT_EQ(request.goal_frame, "charger_02_goal");
}
TEST(MissionAdapterBridgeConversion, OrderLegCarriesItsOwnNodesAndEdges)
{
// Global planner của profile position (`CustomPlanner`) CHỈ hiện thực nhánh
// makePlan(Order, ...); nhánh ba tham số của nó là stub trả false. Chặng đi xuống mà không mang
// order thì fail ngay lượt lập plan đầu và chạy thẳng vào recovery cho tới ABORTED — đã xảy ra
// thật ngày 2026-07-31.
auto mission = makeMission(4, 2.0);
mission->type = mission_adapters::MissionType::VDA5050_ORDER;
robot_protocol_msgs::Node n0;
n0.nodeId = "n0";
robot_protocol_msgs::Node n1;
n1.nodeId = "n1";
mission->nodes = { n0, n1 };
robot_protocol_msgs::Edge e0;
e0.edgeId = "e0";
e0.startNodeId = "n0";
e0.endNodeId = "n1";
e0.trajectory.degree = 1;
mission->edges = { e0 };
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
ASSERT_NE(request.order, nullptr);
ASSERT_EQ(request.order->nodes.size(), 2U);
EXPECT_EQ(request.order->nodes[1].nodeId, "n1");
// Edge của CHẶNG, không phải của cả order: planner tra edge theo startNodeId/endNodeId trong tập
// node nó nhận được, nên tập hai bên phải khớp nhau.
ASSERT_EQ(request.order->edges.size(), 1U);
EXPECT_EQ(request.order->edges[0].startNodeId, "n0");
EXPECT_EQ(request.order->edges[0].trajectory.degree, 1U);
}
TEST(MissionAdapterBridgeConversion, SimpleGoalMissionCarriesNoOrder)
{
const auto mission = makeMission(2); // mặc định SIMPLE_GOAL
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.order, nullptr);
}
TEST(MissionAdapterBridgeConversion, ActionOnlyLegCarriesNoOrder)
{
// Không có quãng đường nào để lập plan, nên cũng không có gì để đưa cho planner.
auto mission = makeMission(3, 0.0, 1, /*has_goal=*/false);
mission->type = mission_adapters::MissionType::VDA5050_ORDER;
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.order, nullptr);
}
TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
{
// Quy ước của NavigationRequest: sai số <= 0 nghĩa "dùng default của profile trong config".
@@ -103,8 +168,6 @@ TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
const auto mission = makeMission(1);
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_FALSE(request.tolerance.hasXy());
EXPECT_FALSE(request.tolerance.hasYaw());
}
// ================================================================================================
@@ -118,7 +181,8 @@ TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped)
Fixture fixture;
ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3)));
EXPECT_TRUE(fixture.received_.empty()) << "dispatch đi thẳng xuống navigation, bỏ qua biên thread";
EXPECT_TRUE(fixture.received_.empty()) << "dispatch went straight down to navigation, skipping "
"the thread boundary";
EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
ASSERT_EQ(fixture.received_.size(), 1U);
@@ -139,7 +203,7 @@ TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce)
ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4)));
EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "cùng một mission bị đẩy xuống hai lần";
EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "the same mission was pushed down twice";
EXPECT_EQ(fixture.received_.size(), 1U);
}
@@ -192,7 +256,7 @@ TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted)
ASSERT_TRUE(fixture.bridge_.pumpPendingRequest());
ASSERT_EQ(fixture.received_.size(), 1U);
EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "mission cũ thắng mission mới";
EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "the old mission won over the new one";
}
// ================================================================================================
@@ -238,7 +302,8 @@ TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported)
fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded);
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "goal trực tiếp bị đem báo lên mission layer";
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "a direct goal was reported up to the mission "
"layer";
}
TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
@@ -249,13 +314,13 @@ TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
manager.submit({ makeMission(0) });
const auto running = manager.nextMission();
ASSERT_TRUE(running) << "manager không giao mission nào để chạy";
ASSERT_TRUE(running) << "manager handed over no mission to run";
fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
<< "mission vẫn còn đang chạy sau khi đã báo hoàn tất";
<< "mission is still running after completion was reported";
}
TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted)

368
test/mission_layer_test.cpp Normal file
View File

@@ -0,0 +1,368 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — test MissionLayer lắp với MissionAdapterBridge thật.
*
* Đây là chỗ kiểm thứ mà `mission_adapter_bridge_test` không kiểm được: bridge có được nối vào một
* mission layer ĐANG CHẠY hay không, và một yêu cầu nhiều chặng có thật sự đi hết chặng này tới
* chặng kia hay không. Bridge đúng mà layer không được dựng thì mọi test bridge vẫn xanh trong khi
* robot chỉ chạy được chặng đầu — đúng trạng thái của gói trước lần sửa này.
*
* Nguồn mission ở đây được đăng ký thẳng vào registry (không qua Boost.DLL): đường nạp `.so` đã có
* `plugin_registry_test` của `mission_adapters` lo, còn thứ cần khoá tại đây là chuỗi sự kiện.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <mission_adapters/mission_request.h>
#include <mission_adapters/types.h>
#include <move_base2/bridges/mission_layer.h>
namespace
{
using mission_adapters::ConversionResult;
using mission_adapters::Mission;
using mission_adapters::MissionRequest;
using mission_adapters::MissionSourceAdapter;
using mission_adapters::MissionState;
using mission_adapters::SubmitMode;
using move_base2::MissionAdapterBridge;
using move_base2::MissionLayer;
using move_base2::NavigationOutcome;
using move_base2::NavigationRequest;
constexpr auto kTimeout = std::chrono::seconds(2);
constexpr auto kPollStep = std::chrono::milliseconds(2);
/**
* @brief Nguồn giả cắt một pose thành @c legs chặng, mô phỏng đúng hình dạng của một VDA5050 order
* nhiều node có action.
*/
class SplittingAdapter : public MissionSourceAdapter
{
public:
explicit SplittingAdapter(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
: legs_(legs), mode_(mode)
{
}
bool configure(const std::string&, robot::NodeHandle&) override
{
return true;
}
std::string schema() const override
{
return mission_adapters::schema::kPoseStamped;
}
bool validate(const MissionRequest& request, std::string& reason) const override
{
if (!request.pose)
{
reason = "request carries no pose";
return false;
}
return true;
}
ConversionResult convert(const MissionRequest& request) override
{
ConversionResult result;
result.mode = mode_;
for (std::size_t i = 0; i < legs_; ++i)
{
auto mission = std::make_shared<Mission>();
mission->has_goal = true;
mission->goal = *request.pose;
// Mỗi chặng xa hơn chặng trước một mét — đủ để test phân biệt được chặng nào đang chạy.
mission->goal.pose.position.x = static_cast<double>(i + 1); // [m]
result.missions.push_back(mission);
}
return result;
}
private:
std::size_t legs_;
SubmitMode mode_;
};
robot_geometry_msgs::PoseStamped makeGoal()
{
robot_geometry_msgs::PoseStamped goal;
goal.header.frame_id = "map";
goal.pose.orientation.w = 1.0;
return goal;
}
/// @brief Layer + bridge đã nối, cộng chỗ nhận yêu cầu như control thread thật.
class Fixture
{
public:
explicit Fixture(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
{
adapter_ = std::make_shared<SplittingAdapter>(legs, mode);
EXPECT_TRUE(layer_.registry().registerAdapter(adapter_));
layer_.markActiveForTesting();
layer_.attach(bridge_);
bridge_.setRequestCallback([this](const NavigationRequest& request) {
received_.push_back(request);
});
bridge_.setCancelCallback([this]() { ++cancel_calls_; });
bridge_.start();
layer_.start();
}
~Fixture()
{
layer_.stop();
bridge_.stop();
}
/**
* @brief Quay control thread cho tới khi một chặng được đẩy xuống, hoặc hết thời gian chờ.
* @return false nếu không có chặng nào tới — dùng để khẳng định "KHÔNG được có chặng mới".
*/
bool pumpUntilRequest()
{
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (std::chrono::steady_clock::now() < deadline)
{
if (bridge_.pumpPendingRequest())
{
return true;
}
std::this_thread::sleep_for(kPollStep);
}
return false;
}
/// @brief Chờ mission layer đạt tới trạng thái mong đợi.
bool waitForState(MissionState expected)
{
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (std::chrono::steady_clock::now() < deadline)
{
if (layer_.state() == expected)
{
return true;
}
std::this_thread::sleep_for(kPollStep);
}
return false;
}
/// @brief Báo kết cục của chặng vừa nhận, đúng như ControlLoop làm ở cuối một phiên.
void finishLastLeg(NavigationOutcome outcome)
{
ASSERT_FALSE(received_.empty());
bridge_.reportOutcome(received_.back().mission_sequence_id, outcome);
}
MissionLayer layer_;
MissionAdapterBridge bridge_;
std::vector<NavigationRequest> received_;
int cancel_calls_ = 0;
std::shared_ptr<SplittingAdapter> adapter_;
};
// ================================================================================================
// Chuỗi nhiều chặng — lý do lớp này tồn tại
// ================================================================================================
TEST(MissionLayerTest, ThreeLegOrderReachesNavigationOneLegAtATime)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
// Chặng 1
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(1u, fixture.received_.size());
EXPECT_DOUBLE_EQ(1.0, fixture.received_[0].goal.pose.position.x);
EXPECT_NE(0u, fixture.received_[0].mission_sequence_id);
// Chặng 2 chỉ được giao SAU khi chặng 1 báo xong: hàng đợi tuần tự, không phải bắn hết một lượt.
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(2u, fixture.received_.size());
EXPECT_DOUBLE_EQ(2.0, fixture.received_[1].goal.pose.position.x);
// Chặng 3
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(3u, fixture.received_.size());
EXPECT_DOUBLE_EQ(3.0, fixture.received_[2].goal.pose.position.x);
// Mỗi chặng một id riêng: outcome trễ của chặng cũ không thể được tính cho chặng mới.
EXPECT_NE(fixture.received_[0].mission_sequence_id, fixture.received_[1].mission_sequence_id);
EXPECT_NE(fixture.received_[1].mission_sequence_id, fixture.received_[2].mission_sequence_id);
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
EXPECT_FALSE(fixture.layer_.hasMission());
}
TEST(MissionLayerTest, MissionStillPendingBetweenLegs)
{
Fixture fixture(2);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
// Đây là tín hiệu mà NavigationServer dùng để KHÔNG báo SUCCEEDED cho host giữa hai chặng. Sai ở
// đây nghĩa là fleet master nghe "đã tới node cuối" khi robot mới đi được nửa tuyến.
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.bridge_.hasActiveMission());
ASSERT_TRUE(fixture.pumpUntilRequest());
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
EXPECT_FALSE(fixture.bridge_.hasActiveMission());
}
// ================================================================================================
// Đường lỗi và đường huỷ
// ================================================================================================
TEST(MissionLayerTest, FailedLegClearsTheRestOfTheQueue)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
// clear_queue_on_failure mặc định true: không tới được node n thì chạy tiếp chặng n+1 là cắt ngang
// đoạn đường fleet manager chưa cho phép đi.
fixture.finishLastLeg(NavigationOutcome::kFailed);
EXPECT_TRUE(fixture.waitForState(MissionState::FAILED));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
TEST(MissionLayerTest, CancelStopsNavigationAndDropsTheQueue)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
fixture.layer_.cancel();
EXPECT_TRUE(fixture.waitForState(MissionState::CANCELLED));
// Huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ (A3).
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (fixture.cancel_calls_ == 0 && std::chrono::steady_clock::now() < deadline)
{
std::this_thread::sleep_for(kPollStep);
}
EXPECT_GE(fixture.cancel_calls_, 1);
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
TEST(MissionLayerTest, PausedLayerHoldsTheQueueUntilResume)
{
Fixture fixture(2);
fixture.layer_.pause();
ASSERT_TRUE(fixture.waitForState(MissionState::PAUSED));
// Yêu cầu tới trong lúc người vận hành đang chủ động dừng: nhận vào hàng đợi nhưng KHÔNG tự chạy.
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_TRUE(fixture.received_.empty());
fixture.layer_.resume();
EXPECT_TRUE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
// ================================================================================================
// Định tuyến: layer chỉ nhận thứ nó có nguồn để xử lý
// ================================================================================================
TEST(MissionLayerTest, RejectsSchemaWithNoRegisteredSource)
{
Fixture fixture(1);
// Không có nguồn nào khai schema `vda5050.order` — layer phải TỪ CHỐI để NavigationServer biết
// đường mà rơi về nhánh trực tiếp, thay vì nuốt order rồi im lặng.
robot_protocol_msgs::Order order;
EXPECT_FALSE(fixture.layer_.handles(mission_adapters::schema::kVda5050Order));
EXPECT_FALSE(fixture.layer_.submitOrder(order));
}
TEST(MissionLayerTest, RejectsEverythingBeforeStartAndAfterStop)
{
MissionLayer layer;
MissionAdapterBridge bridge;
auto adapter = std::make_shared<SplittingAdapter>(1);
ASSERT_TRUE(layer.registry().registerAdapter(adapter));
layer.markActiveForTesting();
layer.attach(bridge);
// Chưa start: không được nhận việc mà sẽ không ai chạy.
EXPECT_FALSE(layer.submitGoal(makeGoal()));
bridge.start();
layer.start();
EXPECT_TRUE(layer.submitGoal(makeGoal()));
layer.stop();
bridge.stop();
EXPECT_FALSE(layer.submitGoal(makeGoal()));
}
TEST(MissionLayerTest, InactiveLayerNeverStarts)
{
// Không nguồn nào -> configure sẽ hỏng ở runtime thật; ở đây kiểm bất biến tương ứng: layer không
// active thì start() là no-op và mọi đường vào đều đóng.
MissionLayer layer;
MissionAdapterBridge bridge;
layer.attach(bridge);
layer.start();
EXPECT_FALSE(layer.active());
EXPECT_FALSE(layer.started());
EXPECT_EQ(0u, layer.sourceCount());
EXPECT_FALSE(layer.submitGoal(makeGoal()));
}
// ================================================================================================
// Order update — phần release thêm nối tiếp, không chạy lại từ đầu
// ================================================================================================
TEST(MissionLayerTest, AppendModeDoesNotPreemptTheRunningLeg)
{
Fixture fixture(1, SubmitMode::kAppend);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
const std::uint64_t running_id = fixture.received_.back().mission_sequence_id;
// Bản cập nhật tới trong lúc chặng cũ đang chạy: nó phải nằm chờ, không được huỷ chặng đang đi.
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(0, fixture.cancel_calls_);
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
EXPECT_NE(running_id, fixture.received_.back().mission_sequence_id);
}
} // namespace
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

33
test/move_base2_scenario_driver.h
View File

@@ -54,8 +54,8 @@ public:
if (!scenario.obstacles.empty())
{
error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); "
"dùng driver có recovery_core thật cho kịch bản va chạm";
error = "this driver does not simulate obstacles (the recovery port is faked by the "
"scenario); use the driver with the real recovery_core for collision scenarios";
return false;
}
@@ -76,7 +76,7 @@ public:
}
else
{
error = "planner_script không hiểu: '" + item + "'";
error = "unknown planner_script: '" + item + "'";
return false;
}
}
@@ -106,7 +106,7 @@ public:
}
else
{
error = "controller_script không hiểu: '" + item + "'";
error = "unknown controller_script: '" + item + "'";
return false;
}
}
@@ -128,7 +128,7 @@ public:
}
else
{
error = "recovery_script không hiểu: '" + item + "'";
error = "unknown recovery_script: '" + item + "'";
return false;
}
}
@@ -136,9 +136,10 @@ public:
for (const nav_test_harness::ScenarioEvent& event : scenario.events)
{
if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
event.action != "lose_pose" && event.action != "restore_pose")
event.action != "lose_pose" && event.action != "restore_pose" &&
event.action != "sensors_stale" && event.action != "sensors_ok")
{
error = "events: action không hiểu: '" + event.action + "'";
error = "events: unknown action: '" + event.action + "'";
return false;
}
}
@@ -147,6 +148,13 @@ public:
controller_.setScript(controller_script);
recovery_.setScript(recovery_script);
// Recovery thế hệ 2 có thể tự lái. Kịch bản nào khai `recovery_velocity` thì behavior được coi
// là họ velocity; 0 nghĩa là behavior chỉ đợi/xoá costmap và lõi phải giữ nguồn vận tốc kNone.
const bool recovery_drives = std::abs(scenario.recovery_velocity) > 0.0;
recovery_.setRecoveryVelocity(recovery_drives, scenario.recovery_velocity); // [m/s]
recovery_.setDefaultOutputKind(recovery_drives ? RecoveryOutputKind::kVelocity
: RecoveryOutputKind::kNone);
pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
ControlLoopConfig config;
@@ -181,6 +189,7 @@ public:
deps_.recovery = &recovery_;
deps_.mission = &mission_;
deps_.action = &action_;
deps_.costmap_status = &costmap_status_;
if (!loop_.configure(config, deps_, error))
{
@@ -269,6 +278,15 @@ private:
{
pose_.setAvailable(true);
}
else if (event.action == "sensors_stale")
{
// Observation buffer của costmap hết hạn — lõi phải ngừng cho lái bánh xe.
costmap_status_.setCurrent(false);
}
else if (event.action == "sensors_ok")
{
costmap_status_.setCurrent(true);
}
}
}
@@ -283,6 +301,7 @@ private:
FakeRecoveryPort recovery_{ 2 };
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
std::size_t cycle_ = 0;
bool started_ = false;

46
test/move_base2_scenario_test.cpp
View File

@@ -12,6 +12,7 @@
#include <gtest/gtest.h>
#include <algorithm>
#include <cstdlib>
#include <string>
#include <vector>
@@ -19,10 +20,12 @@
#include <nav_test_harness/scenario_runner.h>
#include "move_base2_scenario_driver.h"
#include "recovery_scenario_driver.h"
namespace
{
using move_base2::testing::MoveBase2ScenarioDriver;
using move_base2::testing::RecoveryScenarioDriver;
using nav_test_harness::Scenario;
using nav_test_harness::ScenarioReport;
using nav_test_harness::ScenarioRunner;
@@ -47,13 +50,34 @@ void runScenarioFile(const std::string& path)
Scenario scenario;
std::string error;
ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error))
<< "không nạp được " << path << ": " << error;
MoveBase2ScenarioDriver driver;
ASSERT_TRUE(driver.setup(scenario, error)) << scenario.name << ": setup thất bại: " << error;
<< "could not load " << path << ": " << error;
// Chọn driver theo DỮ LIỆU của kịch bản, không theo một khoá cấu hình riêng: kịch bản khai vật
// cản nghĩa là nó nói về va chạm thật, và chỉ driver nạp recovery_core thật mới trả lời được. Đây
// cũng là lý do MoveBase2ScenarioDriver báo lỗi setup khi thấy `obstacles` thay vì chạy lặng lẽ.
ScenarioRunner runner;
const ScenarioReport report = runner.run(scenario, driver);
ScenarioReport report;
// KHÔNG gọi setup() ở đây: `ScenarioRunner::run` đã tự gọi. Gọi hai lần từng làm driver nạp
// plugin THẬT dựng registry hai lượt, và lượt cũ giữ con trỏ tới cầu nối vừa bị huỷ — use after
// free, biểu hiện ra ngoài chỉ là một dòng "không lấy được pose" trông như lỗi TF.
(void)error;
if (scenario.obstacles.empty())
{
MoveBase2ScenarioDriver driver;
report = runner.run(scenario, driver);
}
else
{
RecoveryScenarioDriver driver;
report = runner.run(scenario, driver);
EXPECT_GT(driver.startRejections(), 0u)
<< scenario.name
<< ": no behavior REFUSED to start — a test case with obstacles that never reaches a "
"safety branch is checking something other than what it describes";
}
EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report);
}
@@ -102,14 +126,22 @@ INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenari
TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty)
{
const std::vector<std::string> files = scenarioFiles();
EXPECT_FALSE(files.empty()) << "không tìm thấy kịch bản nào trong " << scenarioDir()
<< " — suite sẽ xanh mà không kiểm gì cả";
EXPECT_FALSE(files.empty()) << "no scenario found in " << scenarioDir()
<< " — the suite would go green without checking anything";
}
} // namespace
int main(int argc, char** argv)
{
// Kịch bản có vật cản nạp plugin recovery THẬT qua Boost.DLL; ctest không mang theo biến môi
// trường của shell nên binary phải tự trỏ, giống recovery_runner_test.
#ifdef MOVE_BASE2_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
#endif
#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
#endif
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

42
test/navigation_server_test.cpp
View File

@@ -74,8 +74,6 @@ ControlLoopConfig baseConfig()
config.position.global_planner_name = "FakeGlobalPlanner";
config.position.local_planner_name = "FakeLocalPlanner";
config.position.default_xy_tolerance = 0.15; // [m]
config.position.default_yaw_tolerance = 0.10; // [rad]
config.docking = config.position;
config.go_straight = config.position;
@@ -217,7 +215,8 @@ TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist returned the measured velocity instead of "
"the command that was published";
EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}
@@ -266,7 +265,8 @@ TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
fixture.spin(3);
EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
<< "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
<< "no request has ever arrived yet the stamp is fresh — the host would publish 0 over "
"teleop";
}
TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
@@ -288,14 +288,16 @@ TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(1);
EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
<< "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
<< "the stamp freezes as soon as the leg ends — the final stop command would never be "
"published";
// Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
fixture.spin(12);
const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(3);
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
<< "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
<< "the grace period is over yet the stamp is still fresh — teleop would never get /cmd_vel "
"back";
}
TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
@@ -311,7 +313,8 @@ TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
<< "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
<< "the timestamp refreshes itself although the control loop is not running — the host would "
"think the command is still valid";
}
TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
@@ -355,7 +358,7 @@ TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());
EXPECT_EQ(static_layer->count(), 1U);
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 must both reach the VoxelLayer";
EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
}
@@ -388,7 +391,8 @@ TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
ASSERT_EQ(static_layer->count(), 1U) << "a static map received before a costmap was attached "
"must not be replayed";
EXPECT_EQ(static_layer->records()[0].topic, "/map");
}
@@ -421,7 +425,7 @@ TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTh
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
EXPECT_EQ(static_layer->count(), 1U) << "the same map was replayed twice";
}
TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
@@ -475,11 +479,11 @@ TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
{
if (std::isnan(stored[i]))
{
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "mismatch at ray " << i;
}
else
{
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "mismatch at ray " << i;
}
}
}
@@ -625,7 +629,7 @@ TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
fixture.server_.pause();
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
<< "pause() đi thẳng vào lõi từ thread host";
<< "pause() goes straight into the core from the host thread";
fixture.spin(1);
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
@@ -686,7 +690,7 @@ TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
fixture.server_.cancel();
fixture.spin(1);
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause won over cancel";
}
TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
@@ -738,7 +742,8 @@ TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
}
fixture.server_.stopControlThread();
EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
EXPECT_TRUE(left_idle) << "the goal was accepted but no cycle ran — the control thread is "
"missing";
}
TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
@@ -763,7 +768,7 @@ TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
fixture.configure();
ASSERT_TRUE(fixture.server_.startControlThread(100.0));
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "started a second thread";
fixture.server_.stopControlThread();
fixture.server_.stopControlThread(); // không được treo hay sập
@@ -797,7 +802,7 @@ TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
a.plan.poses.clear();
EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
<< "hai lần gọi trả về cùng một vùng nhớ";
<< "two calls returned the same memory";
EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
}
@@ -851,7 +856,8 @@ TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
fixture.spin(1);
EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
<< "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
<< "the plan carries a timestamp from another clock than the control loop — the host would "
"treat it as stale and drop it";
}
int main(int argc, char** argv)

14
test/planner_runner_test.cpp
View File

@@ -147,7 +147,8 @@ TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded)
EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerMissingLibrary", error));
EXPECT_FALSE(error.empty());
EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
"configured";
}
// ================================================================================================
@@ -175,7 +176,8 @@ TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries)
ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerOk"));
EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại planner cũ mà vẫn nạp lại thư viện";
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous planner yet "
"reloaded the library";
}
TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive)
@@ -210,7 +212,7 @@ TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached)
EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerInitFails"));
EXPECT_EQ(fixture.runner_.loadedCount(), 1U)
<< "instance hỏng bị cache lại — mọi lần thử sau sẽ nhận lại đúng cái hỏng đó";
<< "a broken instance was cached — every later attempt would get that same broken one back";
}
TEST(PlannerRunner, RefusesEmptyPlannerName)
@@ -349,7 +351,8 @@ TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused)
move_base2::PlanResult result;
ASSERT_TRUE(fixture.runner_.pollPlan(result));
EXPECT_TRUE(result.tag == 1U || (!refused && result.tag == 2U));
EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "còn kết quả thứ hai trong hộp thư";
EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "a second result is still sitting in the "
"mailbox";
}
TEST(PlannerRunner, CancelledPlanProducesNoResult)
@@ -367,7 +370,8 @@ TEST(PlannerRunner, CancelledPlanProducesNoResult)
move_base2::PlanResult result;
EXPECT_FALSE(fixture.runner_.pollPlan(result))
<< "lượt đã huỷ vẫn trả kết quả — bên gọi sẽ bám theo plan tới goal không còn ai yêu cầu";
<< "a cancelled attempt still returned a result — the caller would follow a plan to a goal "
"nobody asks for anymore";
}
TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith)

2
test/plugins/test_global_planner.cpp
View File

@@ -65,7 +65,7 @@ public:
if (behavior_ == Behavior::kThrow)
{
throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
throw std::runtime_error("TestGlobalPlanner was asked to throw an exception");
}
plan.clear();

50
test/plugins/test_local_planner.cpp
View File

@@ -19,6 +19,7 @@
* Author: DuongTD
*********************************************************************/
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstddef>
#include <limits>
@@ -51,23 +52,32 @@ class TestLocalPlanner : public robot_nav_core2::LocalPlanner
public:
enum class Behavior
{
kOk, ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được.
kNoCommand, ///< computeVelocityCommands trả false.
kNaN, ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
kThrow, ///< Ném exception khi tính lệnh.
kRefusesLimits ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
kOk, ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được.
kNoCommand, ///< computeVelocityCommands trả false.
kNaN, ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
kThrow, ///< Ném exception khi tính lệnh.
kRefusesLimits, ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
kMarkerProbe, ///< Đọc `maker_name` MỘT lần lúc initialize, mã hoá vào lệnh — mô phỏng
///< getMaker() của docking planner để test đường re-init khi đổi marker.
kFootprintProbe ///< Mỗi initialize có generation mới; cần reapply goal/plan mới sinh lệnh.
};
explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
{
}
void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
void initialize(robot::NodeHandle& parent, const std::string& name,
std::shared_ptr<tf3::BufferCore> /*tf*/,
robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
{
// Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
name_ = name;
// Như PNKXDockingLocalPlanner::getMaker(): đọc đúng MỘT lần, không bao giờ đọc lại.
parent.param("maker_name", marker_at_init_, std::string(""));
if (behavior_ == Behavior::kFootprintProbe)
{
footprint_generation_ = ++footprint_probe_generation_;
}
}
void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
@@ -99,13 +109,22 @@ public:
switch (behavior_)
{
case Behavior::kThrow:
throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
throw std::runtime_error("TestLocalPlanner was asked to throw an exception");
case Behavior::kNoCommand:
// Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
throw std::runtime_error("TestLocalPlanner: could not produce a command");
case Behavior::kNaN:
cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
return cmd;
case Behavior::kMarkerProbe:
// Mã hoá marker đọc được lúc initialize vào lệnh — bảng cố định, test đối chiếu.
cmd.velocity.x = marker_at_init_ == "dock_a" ? 0.11 : marker_at_init_ == "dock_b" ? 0.22 : 0.0;
return cmd;
case Behavior::kFootprintProbe:
// Nếu refresh chỉ dựng instance mà quên setGoalPose/setPlan lại, probe trả 0 thay vì lệnh
// mang generation mới. Như vậy test kiểm đồng thời cache footprint và khôi phục chặng.
cmd.velocity.x = (saw_goal_ && plan_size_ != 0) ? 0.01 * footprint_generation_ : 0.0;
return cmd;
case Behavior::kOk:
case Behavior::kRefusesLimits:
break;
@@ -181,6 +200,9 @@ public:
private:
Behavior behavior_;
std::string name_;
std::string marker_at_init_; ///< `maker_name` tại thời điểm initialize — không bao giờ đọc lại.
inline static std::atomic<unsigned int> footprint_probe_generation_{ 0 };
unsigned int footprint_generation_ = 0;
std::size_t plan_size_ = 0;
bool saw_goal_ = false;
double limit_forward_ = 0.0; ///< [m/s]
@@ -220,6 +242,16 @@ robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
}
robot_nav_core2::LocalPlanner::Ptr createMarkerProbe()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kMarkerProbe);
}
robot_nav_core2::LocalPlanner::Ptr createFootprintProbe()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kFootprintProbe);
}
} // namespace testing
} // namespace move_base2
@@ -229,3 +261,5 @@ BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)
BOOST_DLL_ALIAS(move_base2::testing::createMarkerProbe, TestControllerMarkerProbe)
BOOST_DLL_ALIAS(move_base2::testing::createFootprintProbe, TestControllerFootprintProbe)

44
test/recovery_runner_test.cpp
View File

@@ -44,7 +44,12 @@ struct Rig
{
runner.setNamespace(ns);
robot::NodeHandle nh;
return runner.configure(nh);
if (!runner.configure(nh))
{
return false;
}
std::string error;
return runner.configureRoutes(nh, error);
}
FakeClockPort clock{1000.0};
@@ -62,6 +67,40 @@ TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
}
TEST(RecoveryRunner, ResolvesPerTriggerRoutesByBehaviorName)
{
Rig rig;
ASSERT_TRUE(rig.load("recovery"));
const move_base2::RecoveryRoutes& routes = rig.runner.routes();
ASSERT_EQ(routes.planning_failed.size(), 1u);
EXPECT_EQ(routes.planning_failed[0], 0u); // wait_short
ASSERT_EQ(routes.controlling_failed.size(), 2u);
EXPECT_EQ(routes.controlling_failed[0], 1u); // wait_long
EXPECT_EQ(routes.controlling_failed[1], 0u); // wait_short
ASSERT_EQ(routes.oscillation.size(), 1u);
EXPECT_EQ(routes.oscillation[0], 1u); // wait_long
}
TEST(RecoveryRunner, SkipsFuturePluginFromRoutesWithoutCreatingInvalidIndexes)
{
Rig rig;
rig.runner.setNamespace("recovery_missing_detour");
robot::NodeHandle nh;
// Registry báo false vì plugin chưa có, nhưng wait vẫn được nạp và phải dùng được.
EXPECT_FALSE(rig.runner.configure(nh));
ASSERT_EQ(rig.runner.behaviorCount(), 1u);
std::string error;
ASSERT_TRUE(rig.runner.configureRoutes(nh, error)) << error;
const move_base2::RecoveryRoutes& routes = rig.runner.routes();
ASSERT_EQ(routes.controlling_failed.size(), 1u);
EXPECT_EQ(routes.controlling_failed[0], 0u);
ASSERT_EQ(routes.oscillation.size(), 1u);
EXPECT_EQ(routes.oscillation[0], 0u);
}
TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
{
Rig rig;
@@ -210,7 +249,8 @@ TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
runner.setNamespace("recovery");
robot::NodeHandle nh;
EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort/PosePort phải hỏng ngay, không phải lúc tick";
EXPECT_FALSE(runner.configure(nh)) << "a missing ClockPort/PosePort must fail right away, not at "
"tick time";
}
TEST(RecoveryRunner, ConfigureTwiceRejected)

555
test/recovery_scenario_driver.h Normal file
View File

@@ -0,0 +1,555 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — ScenarioDriver thứ hai: recovery_core THẬT trên một thế giới giả có vật cản.
*
* Vì sao cần driver thứ hai. @ref MoveBase2ScenarioDriver mô tả **hành vi của các cổng**: recovery
* của nó là một script, nên nó kiểm được "lõi phản ứng đúng khi recovery báo hỏng" nhưng không bao
* giờ kiểm được "recovery có tự phát hiện ra vật cản sau lưng hay không" — câu hỏi đó chỉ có nghĩa
* khi plugin thật chạy trên một lưới thật có vật cản thật.
*
* Ba thứ driver này có mà driver kia không có:
* 1. **Plugin thật**, nạp qua đúng đường Boost.DLL mà runtime đi.
* 2. **Lưới và va chạm thật** — `FakeCostmap` + `FakeCollisionChecker` của harness, vật cản lấy
* thẳng từ `Scenario::obstacles`.
* 3. **Robot thật sự di chuyển**: lệnh vận tốc phát ra được tích phân vào pose mỗi cycle. Không có
* phần này thì `BackUpRecovery` không bao giờ tiến tới vật cản, và ca test "lùi vào vật cản" chỉ
* là một cái tên.
*
* Author: DuongTD
*********************************************************************/
#ifndef MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
#define MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
#include <cmath>
#include <cstddef>
#include <memory>
#include <string>
#include <vector>
#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>
#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_registry.h>
#include <move_base2/control_loop.h>
#include "fake_ports.h"
namespace move_base2
{
namespace testing
{
/// @brief Nối `nav_test_harness::FakePoseProvider` vào cổng pose của recovery_core.
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; ///< non-owning
};
/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng va chạm 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
{
// Không có checker thì coi như không đặt được — giả định an toàn, không phải 0.
return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
}
private:
nav_test_harness::FakeCollisionChecker* fake_ = nullptr; ///< non-owning
};
/**
* @class RegistryRecoveryPort
* @brief Hiện thực @ref RecoveryPort trên một `recovery_core::RecoveryRegistry` đã nạp sẵn.
*
* Là bản rút gọn của @ref RecoveryRunner: cùng ánh xạ enum, cùng ngữ nghĩa tick, nhưng lấy context
* từ ngoài thay vì dựng từ `Costmap2DROBOT`. Nhờ vậy plugin thật chạy được trên lưới giả.
*
* @note Cố ý **không** dùng lại `RecoveryRunner`: cổng pose/va chạm của nó được dựng từ costmap thật
* và không bơm được từ ngoài. Thêm một seam chỉ-dành-cho-test vào lớp runtime để test dễ hơn
* là đổi runtime vì test — hướng phụ thuộc sai.
*/
class RegistryRecoveryPort final : public RecoveryPort
{
public:
explicit RegistryRecoveryPort(recovery_core::RecoveryRegistry* registry, ClockPort* clock)
: registry_(registry), clock_(clock)
{
}
bool configure(robot::NodeHandle& /*nh*/) override
{
// Registry đã được nạp bởi driver trước khi control loop chạy.
return registry_ != nullptr && registry_->size() > 0;
}
std::size_t behaviorCount() const override
{
return registry_ == nullptr ? 0 : registry_->size();
}
RecoveryOutputKind outputKind(std::size_t index) const override
{
recovery_core::RecoveryBehavior* behavior = behaviorAt(index);
if (behavior == nullptr)
{
return RecoveryOutputKind::kNone;
}
switch (behavior->outputKind())
{
case recovery_core::RecoveryOutputType::kVelocity:
return RecoveryOutputKind::kVelocity;
case recovery_core::RecoveryOutputType::kPath:
return RecoveryOutputKind::kPath;
case recovery_core::RecoveryOutputType::kNone:
break;
}
return RecoveryOutputKind::kNone;
}
bool start(std::size_t index, RecoveryTrigger trigger) override
{
active_ = behaviorAt(index);
if (active_ == nullptr || clock_ == nullptr)
{
return false;
}
recovery_core::RecoveryGoal goal;
switch (trigger)
{
case RecoveryTrigger::kPlanningFailed:
goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;
break;
case RecoveryTrigger::kControllingFailed:
goal.trigger = recovery_core::RecoveryTrigger::kControllingFailed;
break;
case RecoveryTrigger::kOscillation:
goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
break;
}
if (!active_->start(goal, clock_->now()))
{
// Đây chính là đường mà ca "vật cản sau lưng" đi qua: BackUpRecovery quét trước quãng lùi và
// TỪ CHỐI khởi động nếu đã có va chạm — robot không được nhúc nhích lấy một cycle.
++start_rejections_;
active_ = nullptr;
return false;
}
return true;
}
RecoveryTick update() override
{
RecoveryTick tick;
if (active_ == nullptr || clock_ == nullptr)
{
tick.status = RecoveryTick::Status::kFailed;
return tick;
}
const recovery_core::RecoveryResult result = active_->update(clock_->now());
switch (result.status)
{
case recovery_core::RecoveryStatus::kRunning:
tick.status = RecoveryTick::Status::kRunning;
break;
case recovery_core::RecoveryStatus::kSucceeded:
tick.status = RecoveryTick::Status::kSucceeded;
break;
case recovery_core::RecoveryStatus::kIdle:
case recovery_core::RecoveryStatus::kCancelled:
case recovery_core::RecoveryStatus::kFailed:
tick.status = RecoveryTick::Status::kFailed;
break;
}
if (const robot_geometry_msgs::Twist* velocity = result.velocity())
{
tick.has_velocity = true;
tick.cmd = *velocity;
}
tick.message = result.message;
return tick;
}
void cancel() override
{
if (active_ != nullptr)
{
active_->cancel();
}
}
std::string behaviorName(std::size_t index) const override
{
return registry_ != nullptr && index < registry_->size() ? registry_->nameAt(index)
: std::string();
}
/// @brief Số lần behavior từ chối khởi động — bằng chứng ca test đi đúng nhánh nó nói là đang kiểm.
std::size_t startRejections() const
{
return start_rejections_;
}
private:
recovery_core::RecoveryBehavior* behaviorAt(std::size_t index) const
{
return registry_ == nullptr ? nullptr : registry_->at(index);
}
recovery_core::RecoveryRegistry* registry_ = nullptr; ///< non-owning
ClockPort* clock_ = nullptr; ///< non-owning
recovery_core::RecoveryBehavior* active_ = nullptr; ///< non-owning
std::size_t start_rejections_ = 0;
};
/**
* @class RecoveryScenarioDriver
* @brief Chạy kịch bản qua @ref ControlLoop với recovery_core THẬT trên lưới giả có vật cản.
*
* Planner và controller vẫn là cổng giả theo script: ca test ở đây nói về **recovery**, và một
* planner thật sẽ làm kết quả phụ thuộc vào chất lượng đường đi thay vì vào thứ đang được kiểm.
*/
class RecoveryScenarioDriver final : public nav_test_harness::ScenarioDriver
{
public:
bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
{
scenario_ = scenario;
// Gọi setup() lần thứ hai phải dựng lại từ đầu, và THỨ TỰ tháo là bắt buộc: behavior trong
// registry giữ con trỏ tới pose/collision bridge qua RecoveryContext, nên registry phải chết
// TRƯỚC chúng. Tháo ngược lại là use-after-free, và triệu chứng của nó ("không lấy được pose")
// trông y hệt một lỗi TF bình thường.
recovery_port_.reset();
registry_.reset();
collision_bridge_.reset();
pose_bridge_.reset();
checker_.reset();
costmap_.reset();
if (scenario.obstacles.empty())
{
// Không phải lỗi chết người, nhưng nói ra: driver này tồn tại vì vật cản. Kịch bản không có
// vật cản nào chạy ở đây là đang trả giá dựng plugin thật mà không kiểm thêm được gì.
error = "RecoveryScenarioDriver is for scenarios WITH obstacles; use MoveBase2ScenarioDriver";
return false;
}
// --- Thế giới: lưới, vật cản, pose ban đầu --------------------------------------------------
costmap_.reset(new nav_test_harness::FakeCostmap(nav_test_harness::FakeCostmap::centered(
kWorldSpan, kResolution)));
for (const nav_test_harness::ScenarioObstacle& obstacle : scenario.obstacles)
{
if (costmap_->setLethalCircle(obstacle.x, obstacle.y, obstacle.radius) == 0) // [m]
{
// Vật cản nằm ngoài lưới = kịch bản dựng sai. Im lặng ở đây nghĩa là ca test chạy trên một
// thế giới không có vật cản nào và vẫn xanh.
error = "the obstacle lies outside the fake grid — re-check the coordinates in the "
"scenario";
return false;
}
}
checker_.reset(new nav_test_harness::FakeCollisionChecker(
costmap_.get(),
nav_test_harness::FakeCollisionChecker::rectangleFootprint(kFootprintLength,
kFootprintWidth)));
pose_source_.setPose(scenario.initial_pose.x, scenario.initial_pose.y,
scenario.initial_pose.theta);
pose_bridge_.reset(new HarnessPoseProvider(&pose_source_));
collision_bridge_.reset(new HarnessCollisionChecker(checker_.get()));
recovery_core::RecoveryContext ctx;
ctx.pose = pose_bridge_.get();
ctx.collision = collision_bridge_.get();
// --- Recovery THẬT --------------------------------------------------------------------------
registry_.reset(new recovery_core::RecoveryRegistry());
robot::NodeHandle nh;
if (!registry_->loadFromConfig(nh, kRecoveryNamespace, ctx))
{
error = "could not load the real recovery behaviors from namespace '" +
std::string(kRecoveryNamespace) + "' — check library_path in the test config";
return false;
}
if (registry_->size() == 0)
{
error = "the recovery namespace is empty — the scenario would run without checking anything";
return false;
}
recovery_port_.reset(new RegistryRecoveryPort(registry_.get(), &clock_));
// --- Cổng giả cho phần còn lại --------------------------------------------------------------
std::vector<PlannerScript> planner_script;
for (const std::string& item : scenario.planner_script)
{
if (item == "ok")
{
planner_script.push_back(PlannerScript::kOk);
}
else if (item == "fail")
{
planner_script.push_back(PlannerScript::kFail);
}
else if (item == "empty")
{
planner_script.push_back(PlannerScript::kEmpty);
}
else
{
error = "unknown planner_script: '" + item + "'";
return false;
}
}
planner_.setScript(planner_script);
std::vector<ControllerScript> controller_script;
for (const std::string& item : scenario.controller_script)
{
if (item == "ok")
{
controller_script.push_back(ControllerScript::kOk);
}
else if (item == "fail")
{
controller_script.push_back(ControllerScript::kFail);
}
else if (item == "goal_reached")
{
controller_script.push_back(ControllerScript::kGoalReached);
}
else
{
error = "controller_script is not supported by this driver: '" + item + "'";
return false;
}
}
controller_.setScript(controller_script);
if (!scenario.recovery_script.empty())
{
error = "recovery_script cannot be used here: recovery is a REAL plugin, its result is "
"decided by collisions and geometry, not assigned by the scenario";
return false;
}
pose_port_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
ControlLoopConfig config;
config.nominal_control_period = scenario.control_period; // [s]
config.state_machine.planner_patience = 0.5; // [s]
config.state_machine.controller_patience = 0.5; // [s]
config.state_machine.oscillation_timeout = 0.0; // tắt
config.state_machine.oscillation_distance = 0.5; // [m]
config.state_machine.max_planning_retries = -1;
config.state_machine.recovery_behavior_count = registry_->size();
config.state_machine.recovery_enabled = true;
config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
config.velocity.min_vel_x = -config.velocity.max_vel_x;
config.velocity.max_vel_theta =
scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
config.velocity.max_accel_x = 100.0; // [m/s^2] lớn: ca test kiểm va chạm, không kiểm ramp
config.velocity.max_accel_theta = 100.0; // [rad/s^2]
config.position.global_planner_name = "ScenarioGlobalPlanner";
config.position.local_planner_name = "ScenarioLocalPlanner";
config.docking = config.position;
config.go_straight = config.position;
config.rotate = config.position;
deps_.clock = &clock_;
deps_.pose = &pose_port_;
deps_.planner = &planner_;
deps_.controller = &controller_;
deps_.recovery = recovery_port_.get();
deps_.mission = &mission_;
deps_.action = &action_;
if (!loop_.configure(config, deps_, error))
{
return false;
}
NavigationRequest request;
request.profile = MotionProfile::kPosition;
request.goal.header.frame_id = "map";
request.goal.pose.position.x = scenario.goal.x; // [m]
request.goal.pose.position.y = scenario.goal.y; // [m]
request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);
if (!loop_.submit(request, error))
{
return false;
}
cycle_ = 0;
started_ = false;
return true;
}
bool step(nav_test_harness::ScenarioStep& step) override
{
if (!started_)
{
started_ = true;
step.cycle = 0;
step.state = toString(loop_.state());
step.linear_x = 0.0;
step.angular_z = 0.0;
return true;
}
applyEventsFor(cycle_);
const bool running = loop_.step();
const robot_geometry_msgs::Twist& command = loop_.lastCommand();
step.cycle = cycle_;
step.state = toString(loop_.state());
step.linear_x = command.linear.x; // [m/s]
step.angular_z = command.angular.z; // [rad/s]
// Robot thật sự đi theo lệnh vừa phát. Đây là điểm khác biệt của driver này: không có tích phân
// chuyển động thì pose đứng yên, `BackUpRecovery` không bao giờ đo được quãng đã lùi, và ca test
// "lùi vào vật cản" sẽ kết thúc vì hết giờ chứ không vì va chạm — xanh vì lý do sai.
integrateMotion(command, scenario_.control_period);
clock_.advance(scenario_.control_period);
++cycle_;
return running;
}
std::string outcome() const override
{
const char* text = loop_.lastOutcome();
return text != nullptr ? std::string(text) : std::string();
}
/// @brief Số lần behavior từ chối khởi động — dùng để khẳng định ca test đi đúng nhánh.
std::size_t startRejections() const
{
return recovery_port_ == nullptr ? 0 : recovery_port_->startRejections();
}
private:
/// [m] Cạnh của lưới giả, đủ rộng để quãng lùi và footprint không chạm biên.
static constexpr double kWorldSpan = 8.0;
static constexpr double kResolution = 0.05; ///< [m/ô] khớp costmap thật của workspace
static constexpr double kFootprintLength = 0.6; ///< [m]
static constexpr double kFootprintWidth = 0.4; ///< [m]
static constexpr const char* kRecoveryNamespace = "recovery_scenario";
void integrateMotion(const robot_geometry_msgs::Twist& command, double dt)
{
const double yaw = pose_source_.rawPose().theta; // [rad]
pose_source_.moveBy(command.linear.x * std::cos(yaw) * dt,
command.linear.x * std::sin(yaw) * dt, command.angular.z * dt);
// Hai nguồn pose phải đi cùng nhau: `pose_source_` là thứ recovery_core nhìn thấy, `pose_port_`
// là thứ lõi nhìn thấy. Lệch nhau thì chống quẩn và recovery nói về hai robot khác nhau.
const auto& pose = pose_source_.rawPose();
pose_port_.setPosition(pose.x, pose.y);
}
void applyEventsFor(std::size_t cycle)
{
for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
{
if (event.cycle != cycle)
{
continue;
}
if (event.action == "cancel")
{
loop_.requestCancel();
}
else if (event.action == "pause")
{
loop_.requestPause();
}
else if (event.action == "resume")
{
loop_.requestResume();
}
else if (event.action == "lose_pose")
{
pose_port_.setAvailable(false);
}
else if (event.action == "restore_pose")
{
pose_port_.setAvailable(true);
}
else if (event.action == "sensors_stale")
{
costmap_status_.setCurrent(false);
}
else if (event.action == "sensors_ok")
{
costmap_status_.setCurrent(true);
}
}
}
nav_test_harness::Scenario scenario_;
std::unique_ptr<nav_test_harness::FakeCostmap> costmap_;
std::unique_ptr<nav_test_harness::FakeCollisionChecker> checker_;
nav_test_harness::FakePoseProvider pose_source_;
std::unique_ptr<HarnessPoseProvider> pose_bridge_;
std::unique_ptr<HarnessCollisionChecker> collision_bridge_;
// Khai SAU các cầu nối: thành viên bị huỷ theo thứ tự ngược, nên registry (giữ con trỏ tới chúng)
// chết trước — cùng lý do với thứ tự tháo trong setup().
std::unique_ptr<recovery_core::RecoveryRegistry> registry_;
std::unique_ptr<RegistryRecoveryPort> recovery_port_;
ControlLoop loop_;
ControlLoopDeps deps_;
FakeClockPort clock_;
FakePosePort pose_port_;
FakePlannerPort planner_;
FakeControllerPort controller_;
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
std::size_t cycle_ = 0;
bool started_ = false;
};
} // namespace testing
} // namespace move_base2
#endif // MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_

184
test/runtime_stats_test.cpp Normal file
View File

@@ -0,0 +1,184 @@
/**
* @file runtime_stats_test.cpp
* @brief Kiểm @ref move_base2::RuntimeStats.
*
* Ba tính chất được khoá lại ở đây, đều là thứ mà một lỗi ở chúng sẽ làm bảng thống kê nói dối chứ
* không làm chương trình chết:
*
* 1. **Tắt là tắt hẳn** — `period <= 0` thì không đăng ký được đoạn nào, không đo, không in.
* Telemetry bật ngoài ý muốn trên robot thật nghĩa là log chen vào vòng điều khiển.
* 2. **Cửa sổ được reset sau mỗi lần in** — số liệu là của cửa sổ vừa qua, không phải tích luỹ từ
* lúc khởi động. Cộng dồn mãi thì mọi đỉnh tức thời sẽ bị pha loãng và không bao giờ thấy lại.
* 3. **Thread tự tạo được gán nhãn qua cửa sổ chụp** — đây là cơ chế duy nhất gọi tên được thread
* của costmap mà không phải sửa gói costmap.
*/
#include <move_base2/io/runtime_stats.h>
#include <atomic>
#include <chrono>
#include <thread>
#include <gtest/gtest.h>
namespace
{
using move_base2::RuntimeStats;
using move_base2::ScopedSection;
/// Đốt CPU thật trong khoảng @p ms — sleep không làm tăng bộ đếm CPU của thread.
void burnCpu(int ms)
{
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(ms);
volatile double sink = 0.0;
while (std::chrono::steady_clock::now() < deadline)
{
for (int i = 0; i < 1000; ++i)
{
sink += static_cast<double>(i) * 1.000001;
}
}
(void)sink;
}
TEST(RuntimeStatsTest, DisabledMeansNoWork)
{
RuntimeStats stats(0.0);
EXPECT_FALSE(stats.enabled());
EXPECT_EQ(stats.section("anything"), RuntimeStats::kInvalidSection);
EXPECT_FALSE(stats.tick()) << "period = 0 yet it still printed to the terminal";
// Không được crash dù chỉ số không hợp lệ.
stats.record(RuntimeStats::kInvalidSection, 1000);
stats.registerCurrentThread("must not be recorded");
const std::string table = stats.render();
EXPECT_EQ(table.find("anything"), std::string::npos);
}
TEST(RuntimeStatsTest, NegativePeriodDisables)
{
RuntimeStats stats(-1.0);
EXPECT_FALSE(stats.enabled());
}
TEST(RuntimeStatsTest, SectionAccumulatesAndResetsPerWindow)
{
RuntimeStats stats(3600.0); // chu kỳ dài: chỉ render() thủ công mới đóng cửa sổ
ASSERT_TRUE(stats.enabled());
const RuntimeStats::SectionId id = stats.section("test.section");
ASSERT_NE(id, RuntimeStats::kInvalidSection);
stats.record(id, 1'000'000); // 1 ms
stats.record(id, 3'000'000); // 3 ms
const std::string first = stats.render();
EXPECT_NE(first.find("test.section"), std::string::npos);
EXPECT_NE(first.find("2.00"), std::string::npos) << "average must be 2.00 ms:\n" << first;
EXPECT_NE(first.find("3.00"), std::string::npos) << "peak must be 3.00 ms:\n" << first;
// Cửa sổ mới: đoạn vẫn còn trong bảng nhưng số liệu về 0.
const std::string second = stats.render();
EXPECT_NE(second.find("test.section"), std::string::npos);
EXPECT_NE(second.find("0.00"), std::string::npos) << "a new window must reset:\n" << second;
}
TEST(RuntimeStatsTest, SameNameReturnsSameId)
{
RuntimeStats stats(3600.0);
EXPECT_EQ(stats.section("loop"), stats.section("loop"));
}
TEST(RuntimeStatsTest, TickOnlyPrintsWhenPeriodElapsed)
{
RuntimeStats stats(3600.0);
EXPECT_FALSE(stats.tick()) << "printed before the period elapsed";
RuntimeStats fast(0.001); // [s]
std::this_thread::sleep_for(std::chrono::milliseconds(5));
EXPECT_TRUE(fast.tick());
EXPECT_FALSE(fast.tick()) << "the window must be reopened right after printing";
}
TEST(RuntimeStatsTest, RegisteredThreadAppearsWithCpuTime)
{
RuntimeStats stats(3600.0);
std::atomic<bool> registered{ false };
std::atomic<bool> stop{ false };
std::thread worker([&stats, &registered, &stop]() {
stats.registerCurrentThread("test/worker");
registered.store(true);
while (!stop.load())
{
burnCpu(5);
}
});
while (!registered.load())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
burnCpu(60);
const std::string table = stats.render();
stop.store(true);
worker.join();
EXPECT_NE(table.find("test/worker"), std::string::npos) << table;
EXPECT_NE(table.find("(unregistered)"), std::string::npos)
<< "CPU outside the registered threads must show up, otherwise the table hides the culprit:\n"
<< table;
}
TEST(RuntimeStatsTest, ThreadCaptureLabelsThreadsCreatedInsideWindow)
{
RuntimeStats stats(3600.0);
std::atomic<bool> stop{ false };
std::thread created;
stats.beginThreadCapture();
created = std::thread([&stop]() {
while (!stop.load())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
});
// Thread phải thực sự tồn tại trong /proc trước khi đóng cửa sổ chụp.
std::this_thread::sleep_for(std::chrono::milliseconds(20));
stats.endThreadCapture("simulated/costmap");
const std::string table = stats.render();
stop.store(true);
created.join();
#ifdef __linux__
EXPECT_NE(table.find("simulated/costmap"), std::string::npos) << table;
#else
GTEST_SKIP() << "thread capture windows rely on /proc, Linux only";
#endif
}
TEST(RuntimeStatsTest, ScopedSectionToleratesNullStats)
{
// Đây là đường đi bình thường của mọi test dùng cổng giả: runner không được gắn telemetry.
{
ScopedSection timer(nullptr, RuntimeStats::kInvalidSection);
}
RuntimeStats stats(0.0);
{
ScopedSection timer(&stats, stats.section("skipped"));
}
SUCCEED();
}
} // namespace
int main(int argc, char** argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

6
test/sensor_gateway_test.cpp
View File

@@ -233,7 +233,8 @@ TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample)
bench.gateway().pushLaserScan("/b_scan", makeScan());
EXPECT_EQ(trap->count(), 0U) << "layer trùng TÊN topic nhưng sai KIỂU vẫn nhận được dữ liệu";
EXPECT_EQ(trap->count(), 0U) << "a layer matching the topic NAME but with the wrong TYPE still "
"received data";
EXPECT_EQ(voxel->count(), 1U);
}
@@ -310,7 +311,8 @@ TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes)
bench.gateway().pushLaserScan("/b_scan", makeScan());
EXPECT_EQ(exploding->count(), 0U);
EXPECT_EQ(healthy->count(), 1U) << "layer lành bị bỏ qua vì layer trước nó ném exception";
EXPECT_EQ(healthy->count(), 1U) << "a healthy layer was skipped because the layer before it "
"threw an exception";
EXPECT_EQ(bench.gateway().stats().layer_exceptions, 1U);
EXPECT_EQ(bench.gateway().stats().delivered, 1U);
}

2
test/spy_layer.h
View File

@@ -81,7 +81,7 @@ protected:
{
if (explode_)
{
throw std::runtime_error("SpyLayer được yêu cầu ném exception");
throw std::runtime_error("SpyLayer was asked to throw an exception");
}
records_.push_back(Record{ &type, topic });
if (observer_)

115
test/state_machine_test.cpp
View File

@@ -193,7 +193,8 @@ TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors)
config.recovery_enabled = true;
std::string error;
EXPECT_FALSE(config.validate(error)) << "cấu hình này lúc chạy sẽ ABORTED ngay ở lỗi đầu tiên";
EXPECT_FALSE(config.validate(error)) << "this config would go ABORTED at runtime on the very "
"first failure";
}
TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
@@ -206,6 +207,18 @@ TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
EXPECT_TRUE(config.validate(error)) << error;
}
TEST(StateMachineConfig, RejectsResolvedRouteWithAnOutOfRangeBehavior)
{
StateMachineConfig config = baseConfig();
config.recovery_routes.planning_failed = {0};
config.recovery_routes.controlling_failed = {1};
config.recovery_routes.oscillation = {2}; // behavior_count chỉ là 2
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("loaded behavior"), std::string::npos);
}
TEST(StateMachineConfig, DescribeMentionsEveryParameter)
{
const std::string text = baseConfig().describe();
@@ -341,6 +354,27 @@ TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires)
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}
TEST(StateMachinePlanning, UsesPlanningRouteInsteadOfRegistryOrder)
{
StateMachineConfig config = baseConfig();
config.max_planning_retries = 0;
config.planner_patience = 100.0;
config.recovery_routes.planning_failed = {1};
config.recovery_routes.controlling_failed = {0};
config.recovery_routes.oscillation = {0};
Driver driver(config);
StateMachineInput request;
request.has_pending_request = true;
driver.tick(request);
const StateMachineOutput out = driver.tick(plannerFailed());
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_TRUE(out.start_recovery);
EXPECT_EQ(out.recovery_index, 1u);
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}
TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
{
// `planner_patience = 0` vẫn nghĩa là "tắt đồng hồ kiên nhẫn". Nhưng từ khi planner chạy trên
@@ -359,7 +393,7 @@ TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
for (int i = 0; i < 100; ++i)
{
driver.advance(1.0);
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "vòng " << i;
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "round " << i;
}
}
@@ -475,13 +509,13 @@ TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop)
SUCCEED();
return;
}
ASSERT_EQ(state, NavigationState::kPlanning) << "vòng " << i;
ASSERT_EQ(state, NavigationState::kPlanning) << "round " << i;
driver.advance(0.05);
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "vòng " << i;
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "round " << i;
}
FAIL() << "controller_patience không bao giờ hết hạn — vòng lặp lập-plan đã làm mới đồng hồ";
FAIL() << "controller_patience never expires — the replanning loop kept refreshing the clock";
}
TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
@@ -504,6 +538,24 @@ TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
}
TEST(StateMachineControlling, UsesOscillationRouteIndependently)
{
StateMachineConfig config = baseConfig();
config.oscillation_timeout = 1.0;
config.oscillation_distance = 0.5;
config.recovery_routes.planning_failed = {0};
config.recovery_routes.controlling_failed = {0};
config.recovery_routes.oscillation = {1};
Driver driver(config);
driver.driveToControlling();
driver.advance(1.1);
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
EXPECT_EQ(out.recovery_index, 1u);
}
TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
{
StateMachineConfig config = baseConfig();
@@ -519,8 +571,8 @@ TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
input.travelled_since_oscillation_reset = 0.6; // [m] đi đủ xa mỗi lần
const StateMachineOutput out = driver.tick(input);
ASSERT_EQ(out.state, NavigationState::kControlling) << "vòng " << i;
ASSERT_TRUE(out.reset_oscillation_origin) << "vòng " << i;
ASSERT_EQ(out.state, NavigationState::kControlling) << "round " << i;
ASSERT_TRUE(out.reset_oscillation_origin) << "round " << i;
}
}
@@ -597,7 +649,7 @@ TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers)
const StateMachineOutput first = driver.tick(blind);
EXPECT_EQ(first.velocity_source, VelocitySource::kNone)
<< "không biết robot ở đâu thì không nguồn nào được phát vận tốc";
<< "when the robot position is unknown no source may publish velocity";
EXPECT_FALSE(first.run_controller);
// Mất TF kéo dài phải dẫn tới recovery, không được treo im lặng.
@@ -764,7 +816,7 @@ TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning
EXPECT_EQ(paused.state, NavigationState::kPaused);
EXPECT_TRUE(paused.cancel_recovery)
<< "giữ một behavior dở dang qua quãng dừng dài là không an toàn";
<< "keeping a behavior half-finished across a long pause is unsafe";
StateMachineInput resume;
resume.resume_requested = true;
@@ -785,7 +837,8 @@ TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity)
const StateMachineOutput out = driver.tick(blind);
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_TRUE(out.tick_recovery) << "vẫn tick để behavior tự báo lỗi theo contract của nó";
EXPECT_TRUE(out.tick_recovery) << "still ticked so the behavior reports its own failure per its "
"contract";
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}
@@ -819,7 +872,7 @@ TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume)
for (int i = 0; i < 50; ++i)
{
driver.advance(1.0);
ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "vòng " << i;
ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "round " << i;
ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone);
}
}
@@ -859,7 +912,7 @@ TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce)
for (int i = 0; i < 10; ++i)
{
const StateMachineOutput out = driver.idleTick();
ASSERT_FALSE(out.report_outcome) << "báo lại ở vòng " << i;
ASSERT_FALSE(out.report_outcome) << "reported again at round " << i;
ASSERT_EQ(out.state, NavigationState::kIdle);
}
}
@@ -910,7 +963,7 @@ TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely)
EXPECT_TRUE(accepted.accept_request);
EXPECT_TRUE(accepted.start_action);
EXPECT_EQ(accepted.action_index, 0u);
EXPECT_FALSE(accepted.start_planner) << "không có goal thì không có gì để lập plan";
EXPECT_FALSE(accepted.start_planner) << "with no goal there is nothing to plan";
EXPECT_EQ(accepted.velocity_source, VelocitySource::kNone);
ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
@@ -973,7 +1026,7 @@ TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery)
EXPECT_EQ(out.state, NavigationState::kAborted);
EXPECT_TRUE(out.report_outcome);
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
EXPECT_FALSE(out.start_recovery) << "recovery là công cụ phục hồi navigation, không cứu được action";
EXPECT_FALSE(out.start_recovery) << "recovery repairs navigation, it cannot rescue an action";
}
TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled)
@@ -1005,15 +1058,15 @@ TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling)
pause.pause_requested = true;
const StateMachineOutput paused = driver.tick(pause);
EXPECT_EQ(paused.state, NavigationState::kPaused);
EXPECT_FALSE(paused.cancel_action) << "action không idempotent — tạm dừng không được huỷ nó";
EXPECT_FALSE(paused.cancel_action) << "the action is not idempotent — pausing must not cancel it";
EXPECT_FALSE(paused.tick_action);
StateMachineInput resume;
resume.resume_requested = true;
const StateMachineOutput resumed = driver.tick(resume);
EXPECT_EQ(resumed.state, NavigationState::kExecutingActions);
EXPECT_TRUE(resumed.tick_action) << "resume tick tiếp action dở dang";
EXPECT_FALSE(resumed.start_action) << "không được start lại action đã chạy dở";
EXPECT_TRUE(resumed.tick_action) << "resume keeps ticking the unfinished action";
EXPECT_FALSE(resumed.start_action) << "an action already in progress must not be started again";
}
TEST(StateMachineActions, ActionPatienceIsDisabledByDefault)
@@ -1045,7 +1098,8 @@ TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort)
driver.advance(1.0); // Tổng 1.5 s > action_patience.
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
EXPECT_EQ(out.state, NavigationState::kAborted);
EXPECT_TRUE(out.cancel_action) << "phải bảo port dừng thiết bị an toàn trước khi kết thúc";
EXPECT_TRUE(out.cancel_action) << "the port must be told to stop the device safely before "
"finishing";
EXPECT_TRUE(out.report_outcome);
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
}
@@ -1071,7 +1125,8 @@ TEST(StateMachineActions, PauseRearmsActionPatienceClock)
driver.advance(0.5); // Mới 0.5 s sau resume, chưa chạm trần.
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "resume xong không được ABORTED oan";
EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "must not be wrongly ABORTED after "
"resume";
EXPECT_TRUE(out.tick_action);
}
@@ -1086,7 +1141,7 @@ TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero)
const StateMachineOutput out = driver.tick(no_pose);
EXPECT_EQ(out.state, NavigationState::kExecutingActions);
EXPECT_TRUE(out.tick_action) << "thao tác thiết bị tại chỗ không cần định vị";
EXPECT_TRUE(out.tick_action) << "operating a device in place needs no localization";
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}
@@ -1121,16 +1176,18 @@ TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously)
const StateMachineOutput out = driver.tick(input);
ASSERT_FALSE(out.run_controller && out.tick_recovery)
<< "state " << move_base2::toString(out.state) << ": hai nguồn lệnh cùng chạy";
<< "state " << move_base2::toString(out.state) << ": two command sources running at once";
ASSERT_FALSE(out.run_controller && out.tick_action)
<< "state " << move_base2::toString(out.state) << ": controller chạy cùng action";
<< "state " << move_base2::toString(out.state) << ": controller running together with an "
"action";
ASSERT_FALSE(out.tick_recovery && out.tick_action)
<< "state " << move_base2::toString(out.state) << ": recovery chạy cùng action";
<< "state " << move_base2::toString(out.state) << ": recovery running together with an "
"action";
if (move_base2::mustBeStopped(out.state))
{
ASSERT_EQ(out.velocity_source, VelocitySource::kNone)
<< "state " << move_base2::toString(out.state) << " phải dừng";
<< "state " << move_base2::toString(out.state) << " must be stopped";
}
if (out.velocity_source == VelocitySource::kController)
{
@@ -1192,7 +1249,8 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning)
for (int i = 1; i <= 5; ++i)
{
driver.advance(0.05);
EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning) << "rời PLANNING ở cycle " << i;
EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning)
<< "left PLANNING at cycle " << i;
}
}
@@ -1218,7 +1276,7 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt)
}
EXPECT_EQ(driver.machine().state(), NavigationState::kPlanning)
<< "kBusy bị tính là lượt lập plan hỏng nên đã cạn max_planning_retries";
<< "kBusy was counted as a failed planning attempt so max_planning_retries ran out";
}
TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
@@ -1234,7 +1292,8 @@ TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
const StateMachineOutput out = driver.tick(busy);
EXPECT_EQ(out.state, NavigationState::kControlling);
EXPECT_FALSE(out.apply_plan) << "đẩy plan xuống controller khi planner chưa có plan nào";
EXPECT_FALSE(out.apply_plan) << "pushed a plan down to the controller while the planner had no "
"plan yet";
}
TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
@@ -1247,7 +1306,7 @@ TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("planner treo"), std::string::npos) << error;
EXPECT_NE(error.find("hung planner"), std::string::npos) << error;
}
int main(int argc, char** argv)

14
test/velocity_arbiter_test.cpp
View File

@@ -97,7 +97,7 @@ TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero)
{
VelocityLimits limits = baseLimits();
limits.min_vel_x = 0.0;
EXPECT_NE(limits.describe().find("cấm lùi"), std::string::npos);
EXPECT_NE(limits.describe().find("reversing forbidden"), std::string::npos);
}
TEST(VelocityArbiter, RefusesToEmitBeforeConfigure)
@@ -134,7 +134,7 @@ TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately)
const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(0.5, 0.8), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "lệnh 0 phải tức thì, không giảm tốc dần";
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "a zero command must be immediate, not ramped down";
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
EXPECT_TRUE(arbiter.stopped());
}
@@ -161,7 +161,8 @@ TEST(VelocityArbiter, NaNIsBlockedAndCounted)
const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(nan_value, 0.3), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "một trục hỏng làm hỏng cả lệnh, không sửa từng phần";
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "one broken axis breaks the whole command, it is not "
"patched per axis";
EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
}
@@ -193,7 +194,8 @@ TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero)
const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-9.0, 0.0), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x là trần LÙI, không phải cận dưới bằng 0";
EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x is the REVERSE limit, not a lower bound of "
"zero";
}
TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero)
@@ -290,11 +292,11 @@ TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle)
ASSERT_NEAR(controlling.linear.x, 0.4, 1e-9);
const auto handover = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "cycle bàn giao phải là 0";
EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "the handover cycle must be 0";
EXPECT_EQ(arbiter.handoverCycles(), 1u);
const auto recovering = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "chỉ đúng MỘT cycle 0, không nhiều hơn";
EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "exactly ONE zero cycle, no more";
}
TEST(VelocityArbiter, HandoverWorksInBothDirections)

303
test/walking_skeleton_test.cpp
View File

@@ -31,6 +31,7 @@ using move_base2::testing::ControllerScript;
using move_base2::testing::FakeActionPort;
using move_base2::testing::FakeClockPort;
using move_base2::testing::FakeControllerPort;
using move_base2::testing::FakeCostmapStatusPort;
using move_base2::testing::FakeMissionPort;
using move_base2::testing::FakePlannerPort;
using move_base2::testing::FakePosePort;
@@ -65,8 +66,6 @@ ControlLoopConfig baseConfig()
config.position.global_planner_name = "FakeGlobalPlanner";
config.position.local_planner_name = "FakeLocalPlanner";
config.position.default_xy_tolerance = 0.15; // [m]
config.position.default_yaw_tolerance = 0.10; // [rad]
config.docking = config.position;
config.docking.local_planner_name = "FakeDockPlanner";
@@ -128,6 +127,7 @@ public:
deps_.recovery = &recovery_;
deps_.mission = &mission_;
deps_.action = &action_;
deps_.costmap_status = &costmap_status_;
std::string error;
EXPECT_TRUE(loop_.configure(config, deps_, error)) << error;
@@ -175,6 +175,7 @@ public:
FakeRecoveryPort recovery_;
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
ControlLoopDeps deps_;
private:
@@ -218,7 +219,7 @@ TEST(ControlLoop, RefusesToConfigureWithMissingPorts)
std::string error;
EXPECT_FALSE(loop.configure(baseConfig(), deps, error));
EXPECT_NE(error.find("cổng"), std::string::npos);
EXPECT_NE(error.find("port"), std::string::npos);
EXPECT_FALSE(loop.initialized());
}
@@ -286,24 +287,89 @@ TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded)
EXPECT_NE(reason.find("global planner"), std::string::npos);
}
TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances)
TEST(ControlLoop, ProfileSelectsItsOwnLocalPlanner)
{
Fixture fixture;
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
docking.marker = "dock_a";
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
EXPECT_NEAR(fixture.controller_.xyTolerance(), 0.15, 1e-9) << "tolerance 0 -> dùng default profile";
// Marker phải tới port TRƯỚC khi swap — planner đọc maker_name trong initialize().
EXPECT_EQ(fixture.controller_.lastDockingMarker(), "dock_a");
NavigationRequest rotate = makeRequest(1.0);
rotate.profile = MotionProfile::kRotate;
rotate.tolerance.yaw = 0.02; // [rad]
ASSERT_TRUE(fixture.loop_.submit(rotate, reason)) << reason;
EXPECT_EQ(fixture.controller_.activeController(), "FakeRotatePlanner");
EXPECT_NEAR(fixture.controller_.yawTolerance(), 0.02, 1e-9);
}
TEST(ControlLoop, DockingMarkerProfileOverridesBothPlannersAndFallsBackToDefault)
{
ControlLoopConfig config = baseConfig();
config.docking_marker_profiles["trolley"].global_planner_name = "FakeTrolleyGlobalPlanner";
config.docking_marker_profiles["trolley"].local_planner_name = "FakeTrolleyLocalPlanner";
Fixture fixture(config);
NavigationRequest trolley = makeRequest(1.0);
trolley.profile = MotionProfile::kDocking;
trolley.marker = "trolley";
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(trolley, reason)) << reason;
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeTrolleyGlobalPlanner");
EXPECT_EQ(fixture.controller_.activeController(), "FakeTrolleyLocalPlanner");
NavigationRequest unconfigured = makeRequest(1.0);
unconfigured.profile = MotionProfile::kDocking;
unconfigured.marker = "charger";
ASSERT_TRUE(fixture.loop_.submit(unconfigured, reason)) << reason;
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeGlobalPlanner");
EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
}
TEST(ControlLoop, RejectsDockingRequestWithoutMarker)
{
Fixture fixture;
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
// marker cố ý bỏ trống — bản cũ cũng chặn tại cửa dockTo.
std::string reason;
EXPECT_FALSE(fixture.loop_.submit(docking, reason));
EXPECT_NE(reason.find("marker"), std::string::npos) << reason;
}
TEST(ControlLoop, AllowsGoalFrameStyleDockingWithoutMarkerWhenConfigured)
{
ControlLoopConfig config = baseConfig();
config.docking_requires_marker = false;
Fixture fixture(config);
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
std::string reason;
EXPECT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
EXPECT_TRUE(fixture.controller_.lastDockingMarker().empty());
}
TEST(ControlLoop, RejectsDockingRequestWhenMarkerIsUnknown)
{
Fixture fixture;
fixture.controller_.setDockingMarkerSucceeds(false); // marker không có trong maker_sources
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
docking.marker = "tram_la";
std::string reason;
EXPECT_FALSE(fixture.loop_.submit(docking, reason));
EXPECT_NE(reason.find("tram_la"), std::string::npos) << reason;
}
// ================================================================================================
@@ -329,7 +395,7 @@ TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce)
EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_EQ(fixture.mission_.reportCountFor(42), 1u)
<< "mỗi chặng chỉ được báo kết quả đúng một lần";
<< "each leg may report its outcome exactly once";
}
TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
@@ -343,7 +409,7 @@ TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_TRUE(fixture.mission_.reports().empty())
<< "goal trực tiếp không thuộc mission nào thì không báo lên mission layer";
<< "a direct goal belongs to no mission so nothing is reported to the mission layer";
}
TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling)
@@ -397,9 +463,9 @@ TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering)
if (move_base2::mustBeStopped(state))
{
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
<< "state " << move_base2::toString(state) << " ở cycle " << i;
<< "state " << move_base2::toString(state) << " at cycle " << i;
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0)
<< "state " << move_base2::toString(state) << " ở cycle " << i;
<< "state " << move_base2::toString(state) << " at cycle " << i;
}
if (!running)
@@ -454,7 +520,8 @@ TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan)
ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
fixture.run();
EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "không được đẩy plan rỗng xuống controller";
EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "an empty plan must not be pushed down to "
"the controller";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}
@@ -476,13 +543,63 @@ TEST(ControlLoop, LostPoseStopsTheRobotImmediately)
fixture.loop_.step();
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
<< "mất TF thì phải dừng ngay, không đi tiếp bằng pose cũ";
<< "losing TF must stop the robot immediately, not keep going on a stale pose";
}
// ================================================================================================
// Recovery
// ================================================================================================
TEST(ControlLoop, PrimaryPlannerFailureUsesBackupBeforeRecovery)
{
ControlLoopConfig config = baseConfig();
config.backup_global_planner_name = "FakeBackupGlobalPlanner";
// 0 nghĩa là không retry planner hiện tại. Backup vẫn phải có đúng một lượt riêng trước recovery.
config.state_machine.max_planning_retries = 0;
Fixture fixture(config);
fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kOk});
fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
std::string reason;
NavigationRequest request = makeRequest(3.0, 71);
request.order = std::make_shared<robot_protocol_msgs::Order>();
ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason;
fixture.run();
EXPECT_FALSE(fixture.hitLimit());
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
EXPECT_EQ(fixture.planner_.orderHistory(), (std::vector<bool>{true, false}));
EXPECT_EQ(fixture.recovery_.startCount(), 0u);
EXPECT_EQ(fixture.states(),
(std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING", "SUCCEEDED"}))
<< join(fixture.states());
}
TEST(ControlLoop, BackupPlannerFailureEscalatesToRecovery)
{
ControlLoopConfig config = baseConfig();
config.backup_global_planner_name = "FakeBackupGlobalPlanner";
config.state_machine.max_planning_retries = 0;
Fixture fixture(config);
fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kFail});
fixture.recovery_.setScript({RecoveryScript::kRunning});
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 72), reason)) << reason;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
{
fixture.stepOnce();
}
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
EXPECT_EQ(fixture.loop_.state(), NavigationState::kRecovering);
EXPECT_EQ(fixture.recovery_.startCount(), 1u);
EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed);
}
TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
{
// Kịch bản thật: planner bế tắc cho tới khi recovery gỡ được thế, sau đó lập plan bình thường.
@@ -521,7 +638,7 @@ TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
fixture.clock_.advance(kControlPeriod);
}
EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery";
EXPECT_TRUE(planner_unblocked) << "never entered recovery";
EXPECT_EQ(states, (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING",
"CONTROLLING", "SUCCEEDED"}))
<< join(states);
@@ -548,7 +665,7 @@ TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport)
<< join(fixture.states());
EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}))
<< "phải chạy lần lượt từng behavior, không lặp lại behavior đầu";
<< "behaviors must run one after another, the first one must not repeat";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u);
@@ -566,7 +683,7 @@ TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior)
EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}));
EXPECT_EQ(fixture.recovery_.updateCount(), 0u)
<< "không được tick một behavior chưa start thành công";
<< "a behavior that never started successfully must not be ticked";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}
@@ -590,7 +707,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
fixture.loop_.lastCommand().linear.x < -1e-6)
{
saw_reverse = true;
EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "lệnh lùi phải nằm trong trần";
EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "the reverse command must stay "
"within the limit";
}
if (!running)
{
@@ -599,7 +717,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
fixture.clock_.advance(kControlPeriod);
}
EXPECT_TRUE(saw_reverse) << "recovery phát vận tốc nhưng lệnh không tới được đầu ra";
EXPECT_TRUE(saw_reverse) << "recovery published a velocity but the command never reached the "
"output";
}
TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure)
@@ -652,7 +771,7 @@ TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled)
}
}
EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ";
EXPECT_LE(cycles_to_zero, 2) << "cmd_vel must reach 0 within 2 cycles after a cancel";
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED");
@@ -742,8 +861,8 @@ TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce)
}
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg;
ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg;
ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "leg " << leg;
ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "leg " << leg;
}
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u);
@@ -813,7 +932,7 @@ TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce)
ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted);
EXPECT_EQ(fixture.mission_.reportCountFor(13), 1u);
EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "action hỏng không được kéo recovery vào";
EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "a failed action must not drag recovery in";
}
TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
@@ -833,7 +952,7 @@ TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
// Không goal lẫn action thì bị từ chối bất kể có port hay không.
EXPECT_FALSE(fixture.loop_.submit(makeActionOnlyRequest(0), reason));
EXPECT_NE(reason.find("goal lẫn action"), std::string::npos) << reason;
EXPECT_NE(reason.find("neither goal nor action"), std::string::npos) << reason;
}
// ================================================================================================
@@ -859,7 +978,7 @@ TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking)
{
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
<< "rời PLANNING khi planner còn đang tính, cycle " << i;
<< "left PLANNING while the planner was still computing, cycle " << i;
}
fixture.stepOnce();
@@ -882,7 +1001,7 @@ TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning)
}
EXPECT_EQ(fixture.planner_.makePlanCount(), 1u)
<< "lượt lập plan bị khởi động lại mỗi cycle";
<< "the planning attempt was restarted every cycle";
}
TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
@@ -908,7 +1027,7 @@ TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
// recovery -> ABORTED. Điều phải khoá lại là nó KHÔNG đứng im ở PLANNING.
EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
fixture.states().end())
<< "planner treo mà không ai escalate — robot đứng ở PLANNING vĩnh viễn: "
<< "the planner hung and nobody escalated — the robot would sit in PLANNING forever: "
<< join(fixture.states());
EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states());
}
@@ -935,7 +1054,7 @@ TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackgroun
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9)
<< "cmd_vel gián đoạn trong lúc lập lại plan, cycle " << i;
<< "cmd_vel was interrupted while replanning, cycle " << i;
}
}
@@ -978,7 +1097,7 @@ TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan)
fixture.stepOnce();
EXPECT_GT(fixture.planner_.cancelCount(), before)
<< "yêu cầu mới không huỷ lượt lập plan của goal cũ";
<< "a new request did not cancel the planning attempt of the old goal";
}
TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
@@ -1026,8 +1145,8 @@ TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
<< "goal mới nằm chờ thay vì thay goal cũ ngay";
EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
<< "the new goal was queued instead of replacing the old one right away";
EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "did not replan for the new goal";
}
TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
@@ -1044,7 +1163,8 @@ TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "the planning attempt for the old goal was not "
"cancelled";
}
TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
@@ -1063,8 +1183,10 @@ TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
fixture.stepOnce();
EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "the preempted leg was not reported "
"exactly once";
EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "the NEW leg got its outcome reported the "
"moment it was accepted";
}
TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
@@ -1108,7 +1230,120 @@ TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "the running behavior was not told to stop";
}
// ================================================================================================
// Guard "không đi mù" — dữ liệu quan sát của costmap quá hạn
// ================================================================================================
//
// move_base thế hệ 1 có đúng guard này (`move_base.cpp:2720`) và move_base2 trước đây KHÔNG có:
// costmap hết hạn nghĩa là robot đang tránh vật cản trên một bản đồ của quá khứ.
TEST(StaleCostmap, BlocksWheelsWhileControlling)
{
Fixture fixture;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
// Chạy tới khi đang bám plan và thực sự có lệnh khác 0.
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.loop_.step();
ASSERT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0) << "no command to block yet";
const std::size_t controller_calls_before = fixture.controller_.computeCount();
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0) << "sensor data is stale yet it kept "
"driving";
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0);
EXPECT_EQ(fixture.controller_.computeCount(), controller_calls_before)
<< "the controller must not compute a command on stale data";
EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling)
<< "the guard only blocks the wheels, it does not change state — exactly like the old "
"version";
}
TEST(StaleCostmap, ResumesWhenSensorDataBecomesCurrentAgain)
{
Fixture fixture;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
fixture.costmap_status_.setCurrent(true);
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
<< "sensors are fresh again yet the robot stays still";
}
TEST(StaleCostmap, DisabledByConfigLetsTheRobotDrive)
{
ControlLoopConfig config = baseConfig();
config.require_current_costmap = false;
Fixture fixture(config);
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
<< "the guard is disabled by config yet it still blocked";
}
TEST(StaleCostmap, NullPortMeansNoGuard)
{
// Đường đi của mọi test cổng-giả có sẵn: không ai bơm costmap_status thì lõi coi là còn hạn.
Fixture fixture;
fixture.deps_.costmap_status = nullptr;
std::string error;
ASSERT_TRUE(fixture.loop_.configure(baseConfig(), fixture.deps_, error)) << error;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false); // không ai hỏi nó cả
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0);
EXPECT_EQ(fixture.costmap_status_.queryCount(), 0u) << "the port was detached yet the core still "
"asked it";
}
int main(int argc, char** argv)