diff --git a/test_runner/random_test_runner/include/random_test_runner/file_interactions/junit_xml_reporter.hpp b/test_runner/random_test_runner/include/random_test_runner/file_interactions/junit_xml_reporter.hpp index 8e3722350d3..f34453d8d50 100644 --- a/test_runner/random_test_runner/include/random_test_runner/file_interactions/junit_xml_reporter.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/file_interactions/junit_xml_reporter.hpp @@ -40,6 +40,11 @@ class JunitXmlReporterTestCase void reportTimeout() { reportError("timeout", "Ego failed to reach goal within timeout"); } + void reportException(const std::string & type, const std::string & error_msg) + { + reportError(type, error_msg); + } + private: void reportError(const std::string & error_type, const std::string & message) { diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 5308eb50ca8..0f6be36379c 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -36,11 +36,13 @@ class TestExecutor ArchitectureType architecture_type, rclcpp::Logger logger); void initialize(); - void update(double current_time); + void update(); void deinitialize(); bool scenarioCompleted(); private: + void executeWithErrorHandling(std::function && func); + std::shared_ptr api_; TestDescription test_description_; const std::string ego_name_ = "ego"; diff --git a/test_runner/random_test_runner/launch/random_test.launch.py b/test_runner/random_test_runner/launch/random_test.launch.py index b8f5723be94..d9123fca05f 100644 --- a/test_runner/random_test_runner/launch/random_test.launch.py +++ b/test_runner/random_test_runner/launch/random_test.launch.py @@ -99,6 +99,8 @@ def __init__(self): {"default": "/tmp", "description": "Directory to which result.yaml and result.junit.xml files will be placed"}, + "initialize_duration": {"default": 35, "description": "How long test runner will wait for Autoware to initialize"}, + # test suite arguments # "test_name": {"default": "random_test", "description": "Test name. Used for descriptive purposes only"}, @@ -117,7 +119,7 @@ def __init__(self): "ego_goal_partial_randomization_distance value. If ego_goal_lanelet_id is set to -1, " "this value is ignored"}, "ego_goal_partial_randomization_distance": - {"default": 20.0, + {"default": 25.0, "description": "Distance from goal set by ego_goal_lanelet_id and ego_goal_s, within which goal " "pose will be randomized if ego_goal_partial_randomization is set to true"}, "npc_count": {"default": 10, "description": "Generated npc count"}, diff --git a/test_runner/random_test_runner/src/random_test_runner.cpp b/test_runner/random_test_runner/src/random_test_runner.cpp index 08ed92f14eb..742b4f8c160 100644 --- a/test_runner/random_test_runner/src/random_test_runner.cpp +++ b/test_runner/random_test_runner/src/random_test_runner.cpp @@ -65,7 +65,6 @@ RandomTestRunner::RandomTestRunner(const rclcpp::NodeOptions & option) traffic_simulator::Configuration configuration(map_path); configuration.simulator_host = test_control_parameters.simulator_host; - api_ = std::make_shared(this, configuration, 1.0, 20); auto lanelet_utils = std::make_shared(configuration.lanelet2_map_path()); TestSuiteParameters validated_params = validateParameters(test_suite_params, lanelet_utils); @@ -89,7 +88,7 @@ RandomTestRunner::RandomTestRunner(const rclcpp::NodeOptions & option) fmt::format("Generating test {}/{}", test_id + 1, test_case_parameters_vector.size()); RCLCPP_INFO_STREAM(get_logger(), message); test_executors_.emplace_back( - api_, + std::make_shared(this, configuration, 1.0, 20), TestRandomizer( get_logger(), validated_params, test_case_parameters_vector[test_id], lanelet_utils) .generate(), @@ -197,15 +196,8 @@ void RandomTestRunner::update() RCLCPP_INFO_STREAM(get_logger(), message); current_test_executor_->initialize(); } - if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) { - api_->startNpcLogic(); - } - if ( - api_->isEgoSpawned() && !api_->isNpcLogicStarted() && - api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { - api_->startNpcLogic(); - } - current_test_executor_->update(api_->getCurrentTime()); + + current_test_executor_->update(); } void RandomTestRunner::start() diff --git a/test_runner/random_test_runner/src/test_executor.cpp b/test_runner/random_test_runner/src/test_executor.cpp index 7963cfcd790..c2001a37b8c 100644 --- a/test_runner/random_test_runner/src/test_executor.cpp +++ b/test_runner/random_test_runner/src/test_executor.cpp @@ -65,102 +65,163 @@ TestExecutor::TestExecutor( void TestExecutor::initialize() { - 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)); - } + 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_).declare_parameter( + "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}); + // 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(); + 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); - } + 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); - } + 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 TestExecutor::update(double current_time) +void TestExecutor::update() { - 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(); + 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; } - 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 (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(); + 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; } - scenario_completed_ = true; } - } - api_->updateFrame(); + + api_->updateFrame(); + }); } void TestExecutor::deinitialize() { - std::string message = fmt::format("Deinitialize: {}", test_description_); - RCLCPP_INFO_STREAM(logger_, message); + 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); + if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { + api_->despawn(ego_name_); + } + for (const auto & npc : test_description_.npcs_descriptions) { + api_->despawn(npc.name); + } + }); +} + +void TestExecutor::executeWithErrorHandling(std::function && 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; } }