Skip to content

Commit

Permalink
TestExecutor tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Paweł Lech <[email protected]>
  • Loading branch information
@pawellech1
pawellech1 committed Dec 15, 2023
1 parent fce7b89 commit 5c60ef4
Show file tree
Hide file tree
Showing 8 changed files with 415 additions and 241 deletions.
1 change: 0 additions & 1 deletion test_runner/random_test_runner/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,6 @@ ament_auto_find_build_dependencies()
ament_auto_add_library(${PROJECT_NAME} SHARED
src/data_types.cpp
src/test_randomizer.cpp
src/test_executor.cpp
src/lanelet_utils.cpp
src/random_test_runner.cpp
)
Expand Down
4 changes: 2 additions & 2 deletions test_runner/random_test_runner/include/random_test_runner/random_test_runner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ class RandomTestRunner : public rclcpp::Node

std::random_device seed_randomization_device_;

std::vector<TestExecutor> test_executors_;
std::vector<TestExecutor>::iterator current_test_executor_;
std::vector<TestExecutor<traffic_simulator::API>> test_executors_;
std::vector<TestExecutor<traffic_simulator::API>>::iterator current_test_executor_;

JunitXmlReporter error_reporter_;

Expand Down
213 changes: 205 additions & 8 deletions test_runner/random_test_runner/include/random_test_runner/test_executor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,23 +27,220 @@
#include "random_test_runner/metrics/goal_reached_metric.hpp"
#include "traffic_simulator/api/api.hpp"

const double test_timeout = 60.0;

traffic_simulator_msgs::msg::VehicleParameters getVehicleParameters()
{
traffic_simulator_msgs::msg::VehicleParameters parameters;
parameters.name = "vehicle.volkswagen.t";
parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR;
parameters.performance.max_speed = 69.444;
parameters.performance.max_acceleration = 200;
parameters.performance.max_deceleration = 10.0;
parameters.bounding_box.center.x = 1.5;
parameters.bounding_box.center.y = 0.0;
parameters.bounding_box.center.z = 0.9;
parameters.bounding_box.dimensions.x = 4.5;
parameters.bounding_box.dimensions.y = 2.1;
parameters.bounding_box.dimensions.z = 1.8;
parameters.axles.front_axle.max_steering = 0.5;
parameters.axles.front_axle.wheel_diameter = 0.6;
parameters.axles.front_axle.track_width = 1.8;
parameters.axles.front_axle.position_x = 3.1;
parameters.axles.front_axle.position_z = 0.3;
parameters.axles.rear_axle.max_steering = 0.0;
parameters.axles.rear_axle.wheel_diameter = 0.6;
parameters.axles.rear_axle.track_width = 1.8;
parameters.axles.rear_axle.position_x = 0.0;
parameters.axles.rear_axle.position_z = 0.3;
return parameters;
}

template <typename traffic_simulator_api_type>
class TestExecutor
{
public:
TestExecutor(
std::shared_ptr<traffic_simulator::API> api, TestDescription description,
std::shared_ptr<traffic_simulator_api_type> api, TestDescription description,
JunitXmlReporterTestCase test_case_reporter, SimulatorType simulator_type,
ArchitectureType architecture_type, rclcpp::Logger logger);
ArchitectureType architecture_type, rclcpp::Logger logger)
: api_(std::move(api)),
test_description_(std::move(description)),
error_reporter_(std::move(test_case_reporter)),
simulator_type_(simulator_type),
architecture_type_(architecture_type),
logger_(logger)
{
}

void initialize()
{
executeWithErrorHandling([this]() {
std::string message = fmt::format("Test description: {}", test_description_);
RCLCPP_INFO_STREAM(logger_, message);
scenario_completed_ = false;

api_->updateFrame();

if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) {
api_->spawn(
ego_name_, api_->canonicalize(test_description_.ego_start_position),
getVehicleParameters(), traffic_simulator::VehicleBehavior::autoware());
api_->setEntityStatus(
ego_name_, api_->canonicalize(test_description_.ego_start_position),
traffic_simulator::helper::constructActionStatus());

if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) {
api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration(
traffic_simulator::helper::LidarType::VLP16, ego_name_,
stringFromArchitectureType(architecture_type_)));

double constexpr detection_update_duration = 0.1;
api_->attachDetectionSensor(
traffic_simulator::helper::constructDetectionSensorConfiguration(
ego_name_, stringFromArchitectureType(architecture_type_),
detection_update_duration));

api_->attachOccupancyGridSensor([&]() {
simulation_api_schema::OccupancyGridSensorConfiguration configuration;
configuration.set_architecture_type(stringFromArchitectureType(architecture_type_));
configuration.set_entity(ego_name_);
configuration.set_filter_by_range(true);
configuration.set_height(200);
configuration.set_range(300);
configuration.set_resolution(0.5);
configuration.set_update_duration(0.1);
configuration.set_width(200);
return configuration;
}());

api_->asFieldOperatorApplication(ego_name_).template declare_parameter<bool>(
"allow_goal_modification", true);
}

// XXX dirty hack: wait for autoware system to launch
// ugly but helps for now
std::this_thread::sleep_for(std::chrono::milliseconds{5000});

api_->requestAssignRoute(
ego_name_, std::vector({api_->canonicalize(test_description_.ego_goal_position)}));
api_->asFieldOperatorApplication(ego_name_).engage();

goal_reached_metric_.setGoal(test_description_.ego_goal_pose);
}

for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) {
const auto & npc_descr = test_description_.npcs_descriptions[i];
api_->spawn(
npc_descr.name, api_->canonicalize(npc_descr.start_position), getVehicleParameters());
api_->setEntityStatus(
npc_descr.name, api_->canonicalize(npc_descr.start_position),
traffic_simulator::helper::constructActionStatus(npc_descr.speed));
api_->requestSpeedChange(npc_descr.name, npc_descr.speed, true);
}
});
}

void update()
{
executeWithErrorHandling([this]() {
if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) {
api_->startNpcLogic();
}
if (
api_->isEgoSpawned() && !api_->isNpcLogicStarted() &&
api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) {
api_->startNpcLogic();
}

auto current_time = api_->getCurrentTime();

if (!std::isnan(current_time)) {
bool timeout_reached = current_time >= test_timeout;
if (timeout_reached) {
if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) {
if (!goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) {
RCLCPP_INFO(logger_, "Timeout reached");
error_reporter_.reportTimeout();
}
}
scenario_completed_ = true;

return;
}
}

if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) {
for (const auto & npc : test_description_.npcs_descriptions) {
if (api_->entityExists(npc.name) && api_->checkCollision(ego_name_, npc.name)) {
if (ego_collision_metric_.isThereEgosCollisionWith(npc.name, current_time)) {
std::string message =
fmt::format("New collision occurred between ego and {}", npc.name);
RCLCPP_INFO_STREAM(logger_, message);
error_reporter_.reportCollision(npc, current_time);
}
}
}

if (almost_standstill_metric_.isAlmostStandingStill(api_->getEntityStatus(ego_name_))) {
RCLCPP_INFO(logger_, "Standstill duration exceeded");
if (goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) {
RCLCPP_INFO(logger_, "Goal reached, standstill expected");
} else {
error_reporter_.reportStandStill();
}
scenario_completed_ = true;
}
}

api_->updateFrame();
});
}

void deinitialize()
{
executeWithErrorHandling([this]() {
std::string message = fmt::format("Deinitialize: {}", test_description_);
RCLCPP_INFO_STREAM(logger_, message);

if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) {
api_->despawn(ego_name_);
}
for (const auto & npc : test_description_.npcs_descriptions) {
api_->despawn(npc.name);
}
});
}

void initialize();
void update();
void deinitialize();
bool scenarioCompleted();
bool scenarioCompleted() { return scenario_completed_; }

private:
void executeWithErrorHandling(std::function<void()> && func);
void executeWithErrorHandling(std::function<void()> && func)
{
try {
func();
} catch (const common::AutowareError & error) {
error_reporter_.reportException("autoware error", error.what());
std::string message = fmt::format("common::AutowareError occurred: {}", error.what());
RCLCPP_ERROR_STREAM(logger_, message);
scenario_completed_ = true;
} catch (const common::scenario_simulator_exception::Error & error) {
error_reporter_.reportException("scenario simulator error", error.what());
std::string message =
fmt::format("common::scenario_simulator_exception::Error occurred: {}", error.what());
RCLCPP_ERROR_STREAM(logger_, message);
scenario_completed_ = true;
} catch (const std::runtime_error & error) {
std::string message = fmt::format("std::runtime_error occurred: {}", error.what());
RCLCPP_ERROR_STREAM(logger_, message);
scenario_completed_ = true;
} catch (...) {
RCLCPP_ERROR(logger_, "Unknown error occurred.");
scenario_completed_ = true;
}
}

std::shared_ptr<traffic_simulator::API> api_;
std::shared_ptr<traffic_simulator_api_type> api_;
TestDescription test_description_;
const std::string ego_name_ = "ego";

Expand Down
1 change: 1 addition & 0 deletions test_runner/random_test_runner/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<test_depend>ament_cmake_clang_format</test_depend>
<test_depend>ament_cmake_copyright</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_pep257</test_depend>
<test_depend>ament_cmake_xmllint</test_depend>
Expand Down
Loading

0 comments on commit 5c60ef4

Please sign in to comment.