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

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)