295 lines
9.4 KiB
C++
295 lines
9.4 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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_
|