1317 lines
43 KiB
C++
1317 lines
43 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — test bảng chuyển state.
|
|
*
|
|
* Mỗi transition trong docs/STATE_MACHINE.md phải có ít nhất một test ở đây. Bảng đó là nguồn
|
|
* chuẩn; test này là thứ chứng minh code khớp với bảng.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <string>
|
|
|
|
#include <move_base2/core/state_machine.h>
|
|
|
|
using move_base2::ActionFeedback;
|
|
using move_base2::ControllerFeedback;
|
|
using move_base2::NavigationOutcome;
|
|
using move_base2::NavigationState;
|
|
using move_base2::PlannerFeedback;
|
|
using move_base2::RecoveryFeedback;
|
|
using move_base2::RecoveryOutputKind;
|
|
using move_base2::RecoveryTrigger;
|
|
using move_base2::StateMachine;
|
|
using move_base2::StateMachineConfig;
|
|
using move_base2::StateMachineInput;
|
|
using move_base2::StateMachineOutput;
|
|
using move_base2::VelocitySource;
|
|
|
|
namespace
|
|
{
|
|
|
|
/// @brief Cấu hình cơ sở cho test: ngưỡng ngắn để không phải tua thời gian dài.
|
|
StateMachineConfig baseConfig()
|
|
{
|
|
StateMachineConfig config;
|
|
config.planner_patience = 1.0; // [s]
|
|
config.controller_patience = 2.0; // [s]
|
|
config.oscillation_timeout = 0.0; // tắt trừ khi test bật riêng
|
|
config.oscillation_distance = 0.5; // [m]
|
|
config.max_planning_retries = -1;
|
|
config.recovery_behavior_count = 2;
|
|
config.recovery_enabled = true;
|
|
return config;
|
|
}
|
|
|
|
/**
|
|
* @class Driver
|
|
* @brief Vỏ mỏng quanh StateMachine để test đọc như kịch bản chứ không như lời gọi API.
|
|
*/
|
|
class Driver
|
|
{
|
|
public:
|
|
explicit Driver(const StateMachineConfig& config = baseConfig())
|
|
{
|
|
std::string error;
|
|
configured_ = machine_.configure(config, error);
|
|
EXPECT_TRUE(configured_) << error;
|
|
now_ = 1000.0;
|
|
}
|
|
|
|
/// @param seconds [s] Thời gian trôi trước cycle này.
|
|
Driver& advance(double seconds)
|
|
{
|
|
now_ += seconds;
|
|
return *this;
|
|
}
|
|
|
|
StateMachineOutput tick(StateMachineInput input)
|
|
{
|
|
input.now = robot::Time(now_);
|
|
last_ = machine_.update(input);
|
|
return last_;
|
|
}
|
|
|
|
/// @brief Cycle không có sự kiện gì.
|
|
StateMachineOutput idleTick()
|
|
{
|
|
return tick(StateMachineInput());
|
|
}
|
|
|
|
/// @brief Đưa máy từ kIdle tới kControlling: nhận yêu cầu rồi cấp một plan hợp lệ.
|
|
void driveToControlling()
|
|
{
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
ASSERT_EQ(tick(request).state, NavigationState::kPlanning);
|
|
|
|
StateMachineInput plan_ready;
|
|
plan_ready.planner = PlannerFeedback::kPlanReady;
|
|
ASSERT_EQ(tick(plan_ready).state, NavigationState::kControlling);
|
|
}
|
|
|
|
StateMachine& machine()
|
|
{
|
|
return machine_;
|
|
}
|
|
|
|
const StateMachineOutput& last() const
|
|
{
|
|
return last_;
|
|
}
|
|
|
|
double now() const
|
|
{
|
|
return now_;
|
|
}
|
|
|
|
private:
|
|
StateMachine machine_;
|
|
StateMachineOutput last_;
|
|
double now_ = 1000.0;
|
|
bool configured_ = false;
|
|
};
|
|
|
|
StateMachineInput planReady()
|
|
{
|
|
StateMachineInput input;
|
|
input.planner = PlannerFeedback::kPlanReady;
|
|
return input;
|
|
}
|
|
|
|
StateMachineInput plannerFailed()
|
|
{
|
|
StateMachineInput input;
|
|
input.planner = PlannerFeedback::kFailed;
|
|
return input;
|
|
}
|
|
|
|
StateMachineInput controller(ControllerFeedback feedback)
|
|
{
|
|
StateMachineInput input;
|
|
input.controller = feedback;
|
|
return input;
|
|
}
|
|
|
|
StateMachineInput recovery(RecoveryFeedback feedback)
|
|
{
|
|
StateMachineInput input;
|
|
input.recovery = feedback;
|
|
return input;
|
|
}
|
|
|
|
StateMachineInput actionFb(ActionFeedback feedback)
|
|
{
|
|
StateMachineInput input;
|
|
input.action = feedback;
|
|
return input;
|
|
}
|
|
|
|
/// @brief Yêu cầu mới với hình dạng D8: có goal hay không, và bao nhiêu action.
|
|
StateMachineInput requestWithActions(std::size_t action_count, bool has_goal = true)
|
|
{
|
|
StateMachineInput input;
|
|
input.has_pending_request = true;
|
|
input.pending_request_has_goal = has_goal;
|
|
input.pending_request_action_count = action_count;
|
|
return input;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
// ================================================================================================
|
|
// Cấu hình
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineConfig, RejectsOscillationTimeoutWithoutDistance)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.oscillation_timeout = 5.0;
|
|
config.oscillation_distance = 0.0;
|
|
|
|
std::string error;
|
|
EXPECT_FALSE(config.validate(error));
|
|
EXPECT_NE(error.find("oscillation_distance"), std::string::npos);
|
|
}
|
|
|
|
TEST(StateMachineConfig, RejectsNegativeOscillationDistance)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.oscillation_distance = -1.0;
|
|
|
|
std::string error;
|
|
EXPECT_FALSE(config.validate(error));
|
|
}
|
|
|
|
TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.recovery_behavior_count = 0;
|
|
config.recovery_enabled = true;
|
|
|
|
std::string error;
|
|
EXPECT_FALSE(config.validate(error)) << "this config would go ABORTED at runtime on the very "
|
|
"first failure";
|
|
}
|
|
|
|
TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.recovery_behavior_count = 0;
|
|
config.recovery_enabled = false;
|
|
|
|
std::string error;
|
|
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();
|
|
EXPECT_NE(text.find("planner_patience"), std::string::npos);
|
|
EXPECT_NE(text.find("controller_patience"), std::string::npos);
|
|
EXPECT_NE(text.find("oscillation_timeout"), std::string::npos);
|
|
EXPECT_NE(text.find("action_patience"), std::string::npos);
|
|
EXPECT_NE(text.find("max_planning_retries"), std::string::npos);
|
|
}
|
|
|
|
TEST(StateMachine, RefusesToRunBeforeConfigure)
|
|
{
|
|
StateMachine machine;
|
|
EXPECT_FALSE(machine.initialized());
|
|
|
|
StateMachineInput input;
|
|
input.has_pending_request = true;
|
|
const StateMachineOutput out = machine.update(input);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kIdle);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
EXPECT_FALSE(out.accept_request);
|
|
}
|
|
|
|
TEST(StateMachine, ConfigureFailsLoudlyOnBadConfig)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.oscillation_timeout = 5.0;
|
|
config.oscillation_distance = 0.0;
|
|
|
|
StateMachine machine;
|
|
std::string error;
|
|
EXPECT_FALSE(machine.configure(config, error));
|
|
EXPECT_FALSE(machine.initialized());
|
|
EXPECT_FALSE(error.empty());
|
|
}
|
|
|
|
// ================================================================================================
|
|
// IDLE
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineIdle, StaysIdleWithoutRequest)
|
|
{
|
|
Driver driver;
|
|
const StateMachineOutput out = driver.idleTick();
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kIdle);
|
|
EXPECT_FALSE(out.state_changed);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineIdle, RequestMovesToPlanningAndStartsPlanner)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput input;
|
|
input.has_pending_request = true;
|
|
|
|
const StateMachineOutput out = driver.tick(input);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kPlanning);
|
|
EXPECT_TRUE(out.state_changed);
|
|
EXPECT_TRUE(out.accept_request);
|
|
EXPECT_TRUE(out.start_planner);
|
|
EXPECT_TRUE(out.reset_oscillation_origin);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineIdle, CancelWithoutRequestIsIgnored)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput input;
|
|
input.cancel_requested = true;
|
|
|
|
const StateMachineOutput out = driver.tick(input);
|
|
EXPECT_EQ(out.state, NavigationState::kIdle);
|
|
EXPECT_FALSE(out.report_outcome);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// PLANNING
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachinePlanning, PlanReadyMovesToControllingAndAppliesPlan)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
const StateMachineOutput out = driver.tick(planReady());
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
EXPECT_TRUE(out.apply_plan);
|
|
EXPECT_TRUE(out.run_controller);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kController);
|
|
}
|
|
|
|
TEST(StateMachinePlanning, PlannerPatienceEscalatesToRecovery)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
driver.advance(0.5);
|
|
EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning);
|
|
|
|
driver.advance(0.6); // tổng 1.1 s > planner_patience = 1.0 s
|
|
const StateMachineOutput out = driver.tick(plannerFailed());
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
EXPECT_TRUE(out.start_recovery);
|
|
EXPECT_EQ(out.recovery_index, 0u);
|
|
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
|
|
EXPECT_TRUE(out.stop_planner);
|
|
}
|
|
|
|
TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.max_planning_retries = 1;
|
|
config.planner_patience = 100.0; // dài, để chắc chắn cái chặn là số lượt chứ không phải thời gian
|
|
Driver driver(config);
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning); // retries = 1
|
|
const StateMachineOutput out = driver.tick(plannerFailed()); // retries = 2 > 1
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
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
|
|
// thread riêng, tắt CẢ HAI cận (kiên nhẫn và số lượt) bị `validate()` từ chối — không còn gì phát
|
|
// hiện được planner treo. Test này kiểm đúng thứ nó vẫn luôn kiểm: đồng hồ kiên nhẫn không kêu.
|
|
// Cận số lượt đặt cao hơn hẳn số vòng lặp để nó không phải là thứ kết thúc test.
|
|
StateMachineConfig config = baseConfig();
|
|
config.planner_patience = 0.0;
|
|
config.max_planning_retries = 1000;
|
|
Driver driver(config);
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
for (int i = 0; i < 100; ++i)
|
|
{
|
|
driver.advance(1.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "round " << i;
|
|
}
|
|
}
|
|
|
|
TEST(StateMachinePlanning, CancelMovesToCancelling)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
StateMachineInput cancel;
|
|
cancel.cancel_requested = true;
|
|
const StateMachineOutput out = driver.tick(cancel);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kCancelling);
|
|
EXPECT_TRUE(out.stop_planner);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachinePlanning, PauseMovesToPausedAndResumeComesBack)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
StateMachineInput resume;
|
|
resume.resume_requested = true;
|
|
const StateMachineOutput out = driver.tick(resume);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kPlanning);
|
|
EXPECT_TRUE(out.start_planner);
|
|
}
|
|
|
|
TEST(StateMachinePlanning, NoRecoveryAvailableAbortsInsteadOfHanging)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.recovery_enabled = false;
|
|
config.recovery_behavior_count = 0;
|
|
Driver driver(config);
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
driver.advance(2.0);
|
|
const StateMachineOutput out = driver.tick(plannerFailed());
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kAborted);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// CONTROLLING
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineControlling, GoalReachedFinishesSucceeded)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kGoalReached));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kSucceeded);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kSucceeded);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineControlling, ValidCommandKeepsControlling)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
EXPECT_TRUE(out.run_controller);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kController);
|
|
}
|
|
|
|
TEST(StateMachineControlling, NoValidCommandGoesBackToPlanning)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kNoValidCommand));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kPlanning);
|
|
EXPECT_TRUE(out.start_planner);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop)
|
|
{
|
|
// Vòng lặp CONTROLLING -> PLANNING -> CONTROLLING không được làm mới đồng hồ controller_patience,
|
|
// nếu không thì controller hỏng vĩnh viễn sẽ không bao giờ dẫn tới recovery.
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
for (int i = 0; i < 20; ++i)
|
|
{
|
|
driver.advance(0.15);
|
|
const NavigationState state = driver.tick(controller(ControllerFeedback::kNoValidCommand)).state;
|
|
if (state == NavigationState::kRecovering)
|
|
{
|
|
EXPECT_EQ(driver.last().recovery_trigger, RecoveryTrigger::kControllingFailed);
|
|
SUCCEED();
|
|
return;
|
|
}
|
|
ASSERT_EQ(state, NavigationState::kPlanning) << "round " << i;
|
|
|
|
driver.advance(0.05);
|
|
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "round " << i;
|
|
}
|
|
|
|
FAIL() << "controller_patience never expires — the replanning loop kept refreshing the clock";
|
|
}
|
|
|
|
TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.oscillation_timeout = 1.0; // [s]
|
|
config.oscillation_distance = 0.5; // [m]
|
|
Driver driver(config);
|
|
driver.driveToControlling();
|
|
|
|
// Robot vẫn sinh được lệnh nhưng không đi đâu cả.
|
|
driver.advance(0.5);
|
|
ASSERT_EQ(driver.tick(controller(ControllerFeedback::kCommandValid)).state,
|
|
NavigationState::kControlling);
|
|
|
|
driver.advance(0.6); // tổng 1.1 s > oscillation_timeout
|
|
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
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();
|
|
config.oscillation_timeout = 1.0;
|
|
config.oscillation_distance = 0.5;
|
|
Driver driver(config);
|
|
driver.driveToControlling();
|
|
|
|
for (int i = 0; i < 20; ++i)
|
|
{
|
|
driver.advance(0.5);
|
|
StateMachineInput input = controller(ControllerFeedback::kCommandValid);
|
|
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) << "round " << i;
|
|
ASSERT_TRUE(out.reset_oscillation_origin) << "round " << i;
|
|
}
|
|
}
|
|
|
|
TEST(StateMachineControlling, NewPlanIsAppliedWithoutLeavingControlling)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput input = planReady();
|
|
input.controller = ControllerFeedback::kCommandValid;
|
|
const StateMachineOutput out = driver.tick(input);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
EXPECT_TRUE(out.apply_plan);
|
|
}
|
|
|
|
TEST(StateMachineControlling, CancelMovesToCancelling)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput cancel = controller(ControllerFeedback::kCommandValid);
|
|
cancel.cancel_requested = true;
|
|
|
|
const StateMachineOutput out = driver.tick(cancel);
|
|
EXPECT_EQ(out.state, NavigationState::kCancelling);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineControlling, PauseThenResumeReturnsToControlling)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
StateMachineInput resume;
|
|
resume.resume_requested = true;
|
|
const StateMachineOutput out = driver.tick(resume);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
EXPECT_TRUE(out.run_controller);
|
|
}
|
|
|
|
TEST(StateMachineControlling, LongPauseDoesNotBlowControllerPatience)
|
|
{
|
|
// Tạm dừng 60 giây rồi tiếp tục không được bị tính là "controller hỏng 60 giây".
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
driver.advance(60.0);
|
|
|
|
StateMachineInput resume;
|
|
resume.resume_requested = true;
|
|
ASSERT_EQ(driver.tick(resume).state, NavigationState::kControlling);
|
|
|
|
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
}
|
|
|
|
TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput blind = controller(ControllerFeedback::kCommandValid);
|
|
blind.pose_available = false;
|
|
|
|
const StateMachineOutput first = driver.tick(blind);
|
|
EXPECT_EQ(first.velocity_source, VelocitySource::kNone)
|
|
<< "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.
|
|
driver.advance(3.0);
|
|
const StateMachineOutput out = driver.tick(blind);
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kControllingFailed);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// RECOVERING
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineRecovering, TicksWhileRunningAndOwnsVelocity)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
|
|
const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kRunning));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
EXPECT_TRUE(out.tick_recovery);
|
|
EXPECT_FALSE(out.run_controller);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kRecovery);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, OneShotBehaviorDoesNotOwnVelocity)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
|
|
// Behavior đang chạy là loại không lái robot (đợi, xoá costmap).
|
|
StateMachineInput failed = plannerFailed();
|
|
failed.active_recovery_output = RecoveryOutputKind::kNone;
|
|
ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering);
|
|
|
|
// Ngay tại cycle KHỞI ĐỘNG recovery đã phải là kNone, không chờ tới tick sau: đổi nguồn vận tốc
|
|
// qua lại tốn một cycle zero mỗi lần, và ở đây không có vận tốc nào để trao quyền.
|
|
EXPECT_TRUE(driver.last().start_recovery);
|
|
EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone);
|
|
|
|
StateMachineInput running = recovery(RecoveryFeedback::kRunning);
|
|
running.active_recovery_output = RecoveryOutputKind::kNone;
|
|
const StateMachineOutput out = driver.tick(running);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
EXPECT_TRUE(out.tick_recovery);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, PathBehaviorDoesNotOwnVelocityEither)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
|
|
StateMachineInput failed = plannerFailed();
|
|
failed.active_recovery_output = RecoveryOutputKind::kPath;
|
|
ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering);
|
|
|
|
EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, SucceededGoesBackToPlanningWithNextIndex)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
ASSERT_EQ(driver.last().recovery_index, 0u);
|
|
|
|
const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kSucceeded));
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kPlanning);
|
|
EXPECT_TRUE(out.start_planner);
|
|
EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, FailedAlsoAdvancesToNextBehavior)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
|
|
EXPECT_EQ(driver.tick(recovery(RecoveryFeedback::kFailed)).state, NavigationState::kPlanning);
|
|
EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, ExhaustingAllBehaviorsAborts)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.recovery_behavior_count = 2;
|
|
Driver driver(config);
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
// Behavior 0
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
ASSERT_EQ(driver.last().recovery_index, 0u);
|
|
ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
|
|
|
|
// Behavior 1
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
ASSERT_EQ(driver.last().recovery_index, 1u);
|
|
ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
|
|
|
|
// Hết behavior
|
|
driver.advance(2.0);
|
|
const StateMachineOutput out = driver.tick(plannerFailed());
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kAborted);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, CancelMidRecoveryStopsBehaviorAndCancels)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
|
|
StateMachineInput cancel = recovery(RecoveryFeedback::kRunning);
|
|
cancel.cancel_requested = true;
|
|
const StateMachineOutput out = driver.tick(cancel);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kCancelling);
|
|
EXPECT_TRUE(out.cancel_recovery);
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
|
|
StateMachineInput pause = recovery(RecoveryFeedback::kRunning);
|
|
pause.pause_requested = true;
|
|
const StateMachineOutput paused = driver.tick(pause);
|
|
|
|
EXPECT_EQ(paused.state, NavigationState::kPaused);
|
|
EXPECT_TRUE(paused.cancel_recovery)
|
|
<< "keeping a behavior half-finished across a long pause is unsafe";
|
|
|
|
StateMachineInput resume;
|
|
resume.resume_requested = true;
|
|
EXPECT_EQ(driver.tick(resume).state, NavigationState::kPlanning);
|
|
}
|
|
|
|
TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
|
|
StateMachineInput blind = recovery(RecoveryFeedback::kRunning);
|
|
blind.pose_available = false;
|
|
const StateMachineOutput out = driver.tick(blind);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kRecovering);
|
|
EXPECT_TRUE(out.tick_recovery) << "still ticked so the behavior reports its own failure per its "
|
|
"contract";
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// PAUSED / CANCELLING / terminal
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachinePaused, CancelFromPausedGoesToCancelling)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
StateMachineInput cancel;
|
|
cancel.cancel_requested = true;
|
|
EXPECT_EQ(driver.tick(cancel).state, NavigationState::kCancelling);
|
|
}
|
|
|
|
TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
for (int i = 0; i < 50; ++i)
|
|
{
|
|
driver.advance(1.0);
|
|
ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "round " << i;
|
|
ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone);
|
|
}
|
|
}
|
|
|
|
TEST(StateMachineCancelling, WaitsForRobotToStopBeforeCancelled)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput cancel;
|
|
cancel.cancel_requested = true;
|
|
ASSERT_EQ(driver.tick(cancel).state, NavigationState::kCancelling);
|
|
|
|
StateMachineInput still_moving;
|
|
still_moving.robot_stopped = false;
|
|
EXPECT_EQ(driver.tick(still_moving).state, NavigationState::kCancelling);
|
|
EXPECT_FALSE(driver.last().report_outcome);
|
|
|
|
StateMachineInput stopped;
|
|
stopped.robot_stopped = true;
|
|
const StateMachineOutput out = driver.tick(stopped);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kCancelled);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled);
|
|
}
|
|
|
|
TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
const StateMachineOutput finish = driver.tick(controller(ControllerFeedback::kGoalReached));
|
|
ASSERT_TRUE(finish.report_outcome);
|
|
|
|
// Mọi cycle sau đó không được báo lại lần nữa.
|
|
for (int i = 0; i < 10; ++i)
|
|
{
|
|
const StateMachineOutput out = driver.idleTick();
|
|
ASSERT_FALSE(out.report_outcome) << "reported again at round " << i;
|
|
ASSERT_EQ(out.state, NavigationState::kIdle);
|
|
}
|
|
}
|
|
|
|
TEST(StateMachineTerminal, NewRequestAfterTerminalStartsFreshWithRecoveryIndexZero)
|
|
{
|
|
Driver driver;
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
driver.tick(request);
|
|
|
|
// Đốt hết behavior để ABORTED.
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
|
|
ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
|
|
driver.advance(2.0);
|
|
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kAborted);
|
|
|
|
// Yêu cầu mới phải bắt đầu lại từ behavior 0.
|
|
const StateMachineOutput out = driver.tick(request);
|
|
EXPECT_EQ(out.state, NavigationState::kPlanning);
|
|
EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u);
|
|
}
|
|
|
|
TEST(StateMachineTerminal, TerminalStateLastsExactlyOneCycle)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state,
|
|
NavigationState::kSucceeded);
|
|
EXPECT_EQ(driver.idleTick().state, NavigationState::kIdle);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// EXECUTING_ACTIONS (D8) — action chạy sau goal, hoặc thay goal khi yêu cầu chỉ-có-action
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely)
|
|
{
|
|
Driver driver;
|
|
|
|
const StateMachineOutput accepted = driver.tick(requestWithActions(1, /*has_goal=*/false));
|
|
EXPECT_EQ(accepted.state, NavigationState::kExecutingActions);
|
|
EXPECT_TRUE(accepted.accept_request);
|
|
EXPECT_TRUE(accepted.start_action);
|
|
EXPECT_EQ(accepted.action_index, 0u);
|
|
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,
|
|
NavigationState::kExecutingActions);
|
|
EXPECT_TRUE(driver.last().tick_action);
|
|
|
|
const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded));
|
|
EXPECT_EQ(done.state, NavigationState::kSucceeded);
|
|
EXPECT_TRUE(done.report_outcome);
|
|
EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded);
|
|
}
|
|
|
|
TEST(StateMachineActions, RequestWithoutGoalOrActionsAbortsDefensively)
|
|
{
|
|
Driver driver;
|
|
|
|
const StateMachineOutput out = driver.tick(requestWithActions(0, /*has_goal=*/false));
|
|
EXPECT_EQ(out.state, NavigationState::kAborted);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
|
|
EXPECT_FALSE(out.start_action);
|
|
}
|
|
|
|
TEST(StateMachineActions, ActionsRunInOrderAfterGoalReached)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(2)).state, NavigationState::kPlanning);
|
|
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling);
|
|
|
|
// Tới goal: chưa được SUCCEEDED — còn 2 action phải chạy, và chưa được báo kết quả.
|
|
const StateMachineOutput at_goal = driver.tick(controller(ControllerFeedback::kGoalReached));
|
|
ASSERT_EQ(at_goal.state, NavigationState::kExecutingActions);
|
|
EXPECT_FALSE(at_goal.report_outcome);
|
|
EXPECT_TRUE(at_goal.start_action);
|
|
EXPECT_EQ(at_goal.action_index, 0u);
|
|
|
|
// Action 0 xong -> khởi động action 1, vẫn ở kExecutingActions.
|
|
const StateMachineOutput next = driver.tick(actionFb(ActionFeedback::kSucceeded));
|
|
ASSERT_EQ(next.state, NavigationState::kExecutingActions);
|
|
EXPECT_TRUE(next.start_action);
|
|
EXPECT_EQ(next.action_index, 1u);
|
|
EXPECT_FALSE(next.report_outcome);
|
|
|
|
// Action cuối xong -> SUCCEEDED, báo kết quả đúng một lần.
|
|
const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded));
|
|
EXPECT_EQ(done.state, NavigationState::kSucceeded);
|
|
EXPECT_TRUE(done.report_outcome);
|
|
EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded);
|
|
}
|
|
|
|
TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(1)).state, NavigationState::kPlanning);
|
|
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling);
|
|
ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kFailed));
|
|
EXPECT_EQ(out.state, NavigationState::kAborted);
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
|
|
EXPECT_FALSE(out.start_recovery) << "recovery repairs navigation, it cannot rescue an action";
|
|
}
|
|
|
|
TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
StateMachineInput cancel;
|
|
cancel.cancel_requested = true;
|
|
const StateMachineOutput cancelling = driver.tick(cancel);
|
|
EXPECT_EQ(cancelling.state, NavigationState::kCancelling);
|
|
EXPECT_TRUE(cancelling.cancel_action);
|
|
|
|
StateMachineInput stopped;
|
|
stopped.robot_stopped = true;
|
|
const StateMachineOutput out = driver.tick(stopped);
|
|
EXPECT_EQ(out.state, NavigationState::kCancelled);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled);
|
|
}
|
|
|
|
TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
const StateMachineOutput paused = driver.tick(pause);
|
|
EXPECT_EQ(paused.state, NavigationState::kPaused);
|
|
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 keeps ticking the unfinished action";
|
|
EXPECT_FALSE(resumed.start_action) << "an action already in progress must not be started again";
|
|
}
|
|
|
|
TEST(StateMachineActions, ActionPatienceIsDisabledByDefault)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
// Action dài hợp lệ (ví dụ sạc pin): mặc định không có trần nào ở tầng navigation.
|
|
driver.advance(3600.0);
|
|
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
|
|
EXPECT_EQ(out.state, NavigationState::kExecutingActions);
|
|
EXPECT_TRUE(out.tick_action);
|
|
}
|
|
|
|
TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.action_patience = 1.0; // [s]
|
|
|
|
Driver driver(config);
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
driver.advance(0.5);
|
|
ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
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) << "the port must be told to stop the device safely before "
|
|
"finishing";
|
|
EXPECT_TRUE(out.report_outcome);
|
|
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
|
|
}
|
|
|
|
TEST(StateMachineActions, PauseRearmsActionPatienceClock)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.action_patience = 1.0; // [s]
|
|
|
|
Driver driver(config);
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
StateMachineInput pause;
|
|
pause.pause_requested = true;
|
|
ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);
|
|
|
|
// Dừng lâu hơn hẳn action_patience rồi mới resume — quãng dừng không được tính vào trần.
|
|
driver.advance(30.0);
|
|
StateMachineInput resume;
|
|
resume.resume_requested = true;
|
|
ASSERT_EQ(driver.tick(resume).state, NavigationState::kExecutingActions);
|
|
|
|
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) << "must not be wrongly ABORTED after "
|
|
"resume";
|
|
EXPECT_TRUE(out.tick_action);
|
|
}
|
|
|
|
TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero)
|
|
{
|
|
Driver driver;
|
|
ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
|
|
NavigationState::kExecutingActions);
|
|
|
|
StateMachineInput no_pose;
|
|
no_pose.pose_available = false;
|
|
const StateMachineOutput out = driver.tick(no_pose);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kExecutingActions);
|
|
EXPECT_TRUE(out.tick_action) << "operating a device in place needs no localization";
|
|
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
|
|
}
|
|
|
|
// ================================================================================================
|
|
// Bất biến áp cho mọi state
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously)
|
|
{
|
|
// Quét một chuỗi sự kiện dài, đủ để đi qua mọi state — kể cả kExecutingActions (D8) — và kiểm
|
|
// bất biến ở từng cycle.
|
|
Driver driver;
|
|
|
|
const StateMachineInput sequence[] = {
|
|
requestWithActions(1),
|
|
planReady(),
|
|
controller(ControllerFeedback::kCommandValid),
|
|
controller(ControllerFeedback::kNoValidCommand),
|
|
plannerFailed(),
|
|
plannerFailed(),
|
|
recovery(RecoveryFeedback::kRunning),
|
|
recovery(RecoveryFeedback::kSucceeded),
|
|
planReady(),
|
|
controller(ControllerFeedback::kGoalReached),
|
|
actionFb(ActionFeedback::kRunning),
|
|
actionFb(ActionFeedback::kSucceeded),
|
|
};
|
|
|
|
for (const auto& input : sequence)
|
|
{
|
|
driver.advance(0.6);
|
|
const StateMachineOutput out = driver.tick(input);
|
|
|
|
ASSERT_FALSE(out.run_controller && out.tick_recovery)
|
|
<< "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 running together with an "
|
|
"action";
|
|
ASSERT_FALSE(out.tick_recovery && out.tick_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) << " must be stopped";
|
|
}
|
|
if (out.velocity_source == VelocitySource::kController)
|
|
{
|
|
ASSERT_EQ(out.state, NavigationState::kControlling);
|
|
}
|
|
if (out.velocity_source == VelocitySource::kRecovery)
|
|
{
|
|
ASSERT_EQ(out.state, NavigationState::kRecovering);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(StateMachineInvariants, ResetReturnsToPristineState)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
driver.tick(controller(ControllerFeedback::kCommandValid));
|
|
|
|
driver.machine().reset();
|
|
|
|
EXPECT_EQ(driver.machine().state(), NavigationState::kIdle);
|
|
EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u);
|
|
EXPECT_EQ(driver.machine().planningRetries(), 0);
|
|
}
|
|
|
|
TEST(StateMachineInvariants, EveryStateHasANonEmptyName)
|
|
{
|
|
const NavigationState states[] = {
|
|
NavigationState::kIdle, NavigationState::kPlanning, NavigationState::kControlling,
|
|
NavigationState::kRecovering, NavigationState::kExecutingActions, NavigationState::kPaused,
|
|
NavigationState::kCancelling, NavigationState::kSucceeded, NavigationState::kAborted,
|
|
NavigationState::kCancelled,
|
|
};
|
|
|
|
for (const auto state : states)
|
|
{
|
|
const std::string name = move_base2::toString(state);
|
|
EXPECT_FALSE(name.empty());
|
|
EXPECT_NE(name, "UNKNOWN");
|
|
}
|
|
}
|
|
|
|
// ================================================================================================
|
|
// kBusy — planner bất đồng bộ
|
|
// ================================================================================================
|
|
|
|
TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning)
|
|
{
|
|
// kBusy phải hành xử đúng như kIdle ở mọi điểm quyết định: chờ tiếp, KHÔNG tính là lượt hỏng.
|
|
Driver driver;
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning);
|
|
|
|
StateMachineInput busy;
|
|
busy.planner = PlannerFeedback::kBusy;
|
|
|
|
for (int i = 1; i <= 5; ++i)
|
|
{
|
|
driver.advance(0.05);
|
|
EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning)
|
|
<< "left PLANNING at cycle " << i;
|
|
}
|
|
}
|
|
|
|
TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt)
|
|
{
|
|
StateMachineConfig config = baseConfig();
|
|
config.max_planning_retries = 2;
|
|
config.planner_patience = 100.0; // [s] loại đường kiên nhẫn để chỉ còn đếm lượt
|
|
|
|
Driver driver(config);
|
|
|
|
StateMachineInput request;
|
|
request.has_pending_request = true;
|
|
ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning);
|
|
|
|
StateMachineInput busy;
|
|
busy.planner = PlannerFeedback::kBusy;
|
|
|
|
for (int i = 1; i <= 10; ++i)
|
|
{
|
|
driver.advance(0.05);
|
|
driver.tick(busy);
|
|
}
|
|
|
|
EXPECT_EQ(driver.machine().state(), NavigationState::kPlanning)
|
|
<< "kBusy was counted as a failed planning attempt so max_planning_retries ran out";
|
|
}
|
|
|
|
TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
|
|
{
|
|
Driver driver;
|
|
driver.driveToControlling();
|
|
|
|
StateMachineInput busy;
|
|
busy.planner = PlannerFeedback::kBusy;
|
|
busy.controller = ControllerFeedback::kCommandValid;
|
|
|
|
driver.advance(0.05);
|
|
const StateMachineOutput out = driver.tick(busy);
|
|
|
|
EXPECT_EQ(out.state, NavigationState::kControlling);
|
|
EXPECT_FALSE(out.apply_plan) << "pushed a plan down to the controller while the planner had no "
|
|
"plan yet";
|
|
}
|
|
|
|
TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
|
|
{
|
|
// Từ khi planner chạy trên thread riêng, đây là thứ duy nhất phát hiện được plugin không bao giờ
|
|
// trả lời. Ở chế độ đồng bộ trước đây, planner treo làm treo luôn control loop — hỏng thì thấy ngay.
|
|
StateMachineConfig config = baseConfig();
|
|
config.planner_patience = 0.0;
|
|
config.max_planning_retries = -1;
|
|
|
|
std::string error;
|
|
EXPECT_FALSE(config.validate(error));
|
|
EXPECT_NE(error.find("hung planner"), std::string::npos) << error;
|
|
}
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|