optimal
This commit is contained in:
294
test/move_base2_scenario_driver.h
Normal file
294
test/move_base2_scenario_driver.h
Normal file
@@ -0,0 +1,294 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — hiện thực ScenarioDriver để chạy kịch bản khai báo của nav_test_harness.
|
||||
*
|
||||
* Đặt trong test/ của move_base2 chứ không trong harness: harness không được phụ thuộc ngược vào
|
||||
* package tiêu thụ nào. Harness biết cách CHẠY và cách KIỂM; "chạy cái gì" do bên tiêu thụ hiện
|
||||
* thực — đây chính là chỗ đó.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
|
||||
#define MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
|
||||
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <nav_test_harness/scenario.h>
|
||||
#include <nav_test_harness/scenario_runner.h>
|
||||
|
||||
#include <move_base2/control_loop.h>
|
||||
|
||||
#include "fake_ports.h"
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
namespace testing
|
||||
{
|
||||
|
||||
/**
|
||||
* @class MoveBase2ScenarioDriver
|
||||
* @brief Chạy một kịch bản qua @ref ControlLoop với toàn bộ cổng giả.
|
||||
*
|
||||
* Kịch bản mô tả **hành vi của các cổng**, không mô tả thế giới vật lý: `planner_script` là chuỗi
|
||||
* kết quả `makePlan`, `controller_script` là chuỗi kết quả `computeVelocityCommands`. Nhờ vậy các
|
||||
* bất biến an toàn (mất TF -> dừng, NaN -> chặn, vượt trần -> clamp) kiểm được **xác định**, không
|
||||
* phụ thuộc vào việc dựng đúng một tình huống hình học.
|
||||
*
|
||||
* @note `obstacles` trong kịch bản **chưa được dùng** ở driver này: cổng recovery là fake theo kịch
|
||||
* bản nên không tra costmap. Kịch bản cần va chạm thật (`obstacle_behind_during_backup`) phải
|
||||
* chạy qua một driver khác nạp `recovery_core` thật kèm `FakeCollisionChecker` — thuộc phần
|
||||
* sau của Phase 5. Driver này **báo lỗi setup** nếu kịch bản khai obstacle, thay vì lặng lẽ
|
||||
* chạy một ca test không kiểm đúng thứ nó nói là đang kiểm.
|
||||
*/
|
||||
class MoveBase2ScenarioDriver final : public nav_test_harness::ScenarioDriver
|
||||
{
|
||||
public:
|
||||
bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
|
||||
{
|
||||
scenario_ = scenario;
|
||||
|
||||
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";
|
||||
return false;
|
||||
}
|
||||
|
||||
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 = "planner_script không hiểu: '" + item + "'";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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 if (item == "nan")
|
||||
{
|
||||
controller_script.push_back(ControllerScript::kNaN);
|
||||
}
|
||||
else if (item == "too_fast")
|
||||
{
|
||||
controller_script.push_back(ControllerScript::kTooFast);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = "controller_script không hiểu: '" + item + "'";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<RecoveryScript> recovery_script;
|
||||
for (const std::string& item : scenario.recovery_script)
|
||||
{
|
||||
if (item == "running")
|
||||
{
|
||||
recovery_script.push_back(RecoveryScript::kRunning);
|
||||
}
|
||||
else if (item == "succeeded")
|
||||
{
|
||||
recovery_script.push_back(RecoveryScript::kSucceeded);
|
||||
}
|
||||
else if (item == "failed")
|
||||
{
|
||||
recovery_script.push_back(RecoveryScript::kFailed);
|
||||
}
|
||||
else
|
||||
{
|
||||
error = "recovery_script không hiểu: '" + item + "'";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
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")
|
||||
{
|
||||
error = "events: action không hiểu: '" + event.action + "'";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
planner_.setScript(planner_script);
|
||||
controller_.setScript(controller_script);
|
||||
recovery_.setScript(recovery_script);
|
||||
|
||||
pose_.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 trừ khi kịch bản cần
|
||||
config.state_machine.oscillation_distance = 0.5; // [m]
|
||||
config.state_machine.max_planning_retries = -1;
|
||||
config.state_machine.recovery_behavior_count = 2;
|
||||
config.state_machine.recovery_enabled = true;
|
||||
|
||||
// Trần vận tốc lấy từ kỳ vọng của kịch bản: `expect_max_speed` vừa là điều được kiểm, vừa là
|
||||
// trần mà arbiter phải áp — nếu hai con số đó lệch nhau thì ca test không kiểm đúng thứ nó nói.
|
||||
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: kịch bản kiểm state và trần, 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_;
|
||||
deps_.planner = &planner_;
|
||||
deps_.controller = &controller_;
|
||||
deps_.recovery = &recovery_;
|
||||
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_)
|
||||
{
|
||||
// Ảnh chụp TRƯỚC khi cycle nào chạy. Không phải chi tiết kỹ thuật: nó là bằng chứng rằng lõi
|
||||
// chưa phát lệnh nào trước khi nhận yêu cầu — và không có nó thì `expect_no_cmd_in_states`
|
||||
// không kiểm được state ban đầu, vì state đó không bao giờ xuất hiện trong trace.
|
||||
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();
|
||||
|
||||
step.cycle = cycle_;
|
||||
step.state = toString(loop_.state());
|
||||
step.linear_x = loop_.lastCommand().linear.x; // [m/s]
|
||||
step.angular_z = loop_.lastCommand().angular.z; // [rad/s]
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
private:
|
||||
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_.setAvailable(false);
|
||||
}
|
||||
else if (event.action == "restore_pose")
|
||||
{
|
||||
pose_.setAvailable(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nav_test_harness::Scenario scenario_;
|
||||
|
||||
ControlLoop loop_;
|
||||
ControlLoopDeps deps_;
|
||||
FakeClockPort clock_;
|
||||
FakePosePort pose_;
|
||||
FakePlannerPort planner_;
|
||||
FakeControllerPort controller_;
|
||||
FakeRecoveryPort recovery_{ 2 };
|
||||
FakeMissionPort mission_;
|
||||
FakeActionPort action_;
|
||||
|
||||
std::size_t cycle_ = 0;
|
||||
bool started_ = false;
|
||||
};
|
||||
|
||||
} // namespace testing
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
|
||||
Reference in New Issue
Block a user