diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 295f00b..6974d99 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -17,7 +17,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.tools.route_parser import RouteParser from srunner.tools.environment_parser import EnvironmentParser -from srunner.tools.environment_parser import EnvironmentConfig +from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig from srunner.scenarioconfigs.route_scenario_configuration import ( RouteScenarioConfiguration, ) @@ -191,7 +191,12 @@ def run_scenario( result = False return result - def run(self) -> bool: + def run(self) -> None: + """The Scenario loop. Read the scenario configuration from the parsed XML files, + configure the scenario in CARLA and execute. Use the results to and parse to the algorithm callback. + Repeats **iterations** times, as defined in config.yaml + + """ env_config = EnvironmentParser.parse_scenario_env( os.path.join(self.results_manager.last_scenario, "scenario.xml") ) @@ -199,8 +204,7 @@ def run(self) -> bool: self.results_manager.last_scenario, env_config )[self._scenario_config["route_id"]] - scenario_result = self.run_scenario(route_config, env_config) - return scenario_result + self.run_scenario(route_config, env_config) def destroy(self) -> None: """Deletes instances of all classes related to CARLA""" diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 8beb129..7bbee53 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -7,19 +7,18 @@ from srunner.autoagents.agent_state import autoware_state -from srunner.tools.environment_parser import EnvironmentConfig +from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig from autoware_carla_interface_msgs.msg import EgoConfig, SensorConfig import threading import rclpy import time +import logging +logger = logging.getLogger("scenario-runner") -DEBUG_ENV = False - -# uncomment if testing in scenario runner class AutowareAgent(AutonomousAgent): timestamp = None current_map = None @@ -66,7 +65,9 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: ego_config_msg.sensors.append(sensor_config_msg) # keep publishing ego_sensor config until the bridge is ready + # big performance diminishment here while not self.autoware_state.bridge_ready: + logger.info("Sending Sensor state to Agent...") time.sleep(5) self.state_node.ego_config_publisher.publish(ego_config_msg) @@ -81,10 +82,10 @@ def set_route(self) -> None: self.waypoints_world = self._global_plan_world_coord[:-1] # reinitialise localization - print("called localise") + logger.info("called localise") # self.autoware_node.request_localize() # None uses GNSS - print("called clear route") + logger.info("called clear route") # clear route # self.route_node.request_clear_route() @@ -108,7 +109,7 @@ def destroy(self) -> None: self._node_threads[thread].join() self._nodes[thread].destroy_node() except RuntimeError: - print("Cleaned up threads...") + logger.info("Cleaned up threads...") rclpy.shutdown() @@ -116,7 +117,7 @@ def run_step(self) -> None: """Tick method containing all logic based on autoware state""" self.counter += 1 if self.counter % 20 == 0: - print("1 second") + logger.info("1 second") if not self.agent_set_route: self.set_route() diff --git a/srunner/objects/ego_vehicle.py b/srunner/objects/ego_vehicle.py index a2fdece..7bce42f 100644 --- a/srunner/objects/ego_vehicle.py +++ b/srunner/objects/ego_vehicle.py @@ -62,3 +62,10 @@ def prepare_ego(self) -> None: self._actor.set_target_velocity(carla.Vector3D()) self._actor.set_target_angular_velocity(carla.Vector3D()) CarlaDataProvider.register_actor(self._actor, self._env.ego_spawn) + + def __del__(self) -> None: + """Clean up + Check if the actor exists in CARLA, delete if so + """ + if self._actor.is_alive(): + self._actor.destroy() diff --git a/srunner/objects/sensors.py b/srunner/objects/sensors.py new file mode 100644 index 0000000..e13195b --- /dev/null +++ b/srunner/objects/sensors.py @@ -0,0 +1,91 @@ +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + +import carla + + +class DefaultSensor(object): + """ + A class to encapsulate information about a basic sensor + """ + + def __init__(self) -> None: + self.type: str = "" + self.id: str = "" + self.spawn: carla.Transform | None = None + + def _spawn(self, bp_library, vehicle) -> None: + sensor_bp = bp_library.find(str(self.type)) + + ignored_params = ["serealize", "id", "spawn", "type"] + + sensor_params = [ + attr + for attr in dir(self) + if attr not in ignored_params or not attr.startswith("_") + ] + + for param in sensor_params: + sensor_bp.set_attribute(param, str(getattr(self, param))) + + CarlaDataProvider.get_world().spawn_actor(sensor_bp, self.spawn, vehicle) + CarlaDataProvider.get_world().wait_for_tick() + + def serealize(self): + return self.type, self.id + + +class CameraRGB(DefaultSensor): + """ + A class to hold additional information about a camera sensor + """ + + def __init__(self) -> None: + super().__init__() + self.image_size_x: int = 0 + self.image_size_y: int = 0 + self.fov: float = 0.0 + + +class LidarRayCast(DefaultSensor): + """ + A class to hold additional information about a Lidar sensor + """ + + def __init__(self) -> None: + super().__init__() + self.range: int = 0 + self.channels: int = 0 + self.points_per_second: int = 0 + self.upper_fov: float = 0.0 + self.lower_fov: float = 0.0 + self.rotation_frequency: int = 0 + + +class SensorGNSS(DefaultSensor): + """ + A class to hold additional information about GNSS + """ + + def __init__(self) -> None: + super().__init__() + self.noise_alt_stddev = 0.0 + self.noise_lat_stddev = 0.0 + self.noise_lon_stddev = 0.0 + self.noise_alt_bias = 0.0 + self.noise_lat_bias = 0.0 + self.noise_lon_bias = 0.0 + + +class SensorIMU(DefaultSensor): + """ + A class to hold additional information about IMU + """ + + def __init__(self) -> None: + super().__init__() + self.noise_accel_stddev_x = 0.0 + self.noise_accel_stddev_y = 0.0 + self.noise_accel_stddev_z = 0.0 + self.noise_gyro_stddev_x = 0.0 + self.noise_gyro_stddev_y = 0.0 + self.noise_gyro_stddev_z = 0.0 diff --git a/srunner/scenarioconfigs/environment_configuration.py b/srunner/scenarioconfigs/environment_configuration.py new file mode 100644 index 0000000..95ed619 --- /dev/null +++ b/srunner/scenarioconfigs/environment_configuration.py @@ -0,0 +1,17 @@ +from srunner.objects.sensors import DefaultSensor + +import carla + + +class EnvironmentConfig(object): + """ + Simple object to store information about the initial environment setup for the scenario loop + """ + + def __init__(self) -> None: + self.town: str = "" + self.ego_model: str = "" + self.ego_name: str = "" + self.ego_spawn: carla.Transform | None = None + self.sensor_config: list[DefaultSensor] = [] + self.route_id: int = 0 diff --git a/srunner/tools/environment_parser.py b/srunner/tools/environment_parser.py index e425128..72368c2 100644 --- a/srunner/tools/environment_parser.py +++ b/srunner/tools/environment_parser.py @@ -1,114 +1,13 @@ import xml.etree.ElementTree as ET from xml.etree.ElementTree import Element -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig +from srunner.objects.sensors import DefaultSensor, CameraRGB, LidarRayCast import carla import logging -class EnvironmentConfig(object): - """ - Simple object to store information about the initial environment setup for the scenario loop - """ - - def __init__(self) -> None: - self.town: str = "" - self.ego_model: str = "" - self.ego_name: str = "" - self.ego_spawn: carla.Transform | None = None - self.sensor_config: list[DefaultSensor] = [] - self.route_id: int = 0 - - -class DefaultSensor(object): - """ - A class to encapsulate information about a basic sensor - """ - - def __init__(self) -> None: - self.type: str = "" - self.id: str = "" - self.spawn: carla.Transform | None = None - - def _spawn(self, bp_library, vehicle) -> None: - sensor_bp = bp_library.find(str(self.type)) - - ignored_params = ["serealize", "id", "spawn", "type"] - - sensor_params = [ - attr - for attr in dir(self) - if attr not in ignored_params or not attr.startswith("_") - ] - - for param in sensor_params: - sensor_bp.set_attribute(param, str(getattr(self, param))) - - CarlaDataProvider.get_world().spawn_actor(sensor_bp, self.spawn, vehicle) - CarlaDataProvider.get_world().wait_for_tick() - - def serealize(self): - return self.type, self.id - - -class CameraRGB(DefaultSensor): - """ - A class to hold additional information about a camera sensor - """ - - def __init__(self) -> None: - super().__init__() - self.image_size_x: int = 0 - self.image_size_y: int = 0 - self.fov: float = 0.0 - - -class LidarRayCast(DefaultSensor): - """ - A class to hold additional information about a Lidar sensor - """ - - def __init__(self) -> None: - super().__init__() - self.range: int = 0 - self.channels: int = 0 - self.points_per_second: int = 0 - self.upper_fov: float = 0.0 - self.lower_fov: float = 0.0 - self.rotation_frequency: int = 0 - - -class SensorGNSS(DefaultSensor): - """ - A class to hold additional information about GNSS - """ - - def __init__(self) -> None: - super().__init__() - self.noise_alt_stddev = 0.0 - self.noise_lat_stddev = 0.0 - self.noise_lon_stddev = 0.0 - self.noise_alt_bias = 0.0 - self.noise_lat_bias = 0.0 - self.noise_lon_bias = 0.0 - - -class SensorIMU(DefaultSensor): - """ - A class to hold additional information about IMU - """ - - def __init__(self) -> None: - super().__init__() - self.noise_accel_stddev_x = 0.0 - self.noise_accel_stddev_y = 0.0 - self.noise_accel_stddev_z = 0.0 - self.noise_gyro_stddev_x = 0.0 - self.noise_gyro_stddev_y = 0.0 - self.noise_gyro_stddev_z = 0.0 - - class EnvironmentParser(object): """ Purely Static class for parsing Scenarion configuration files generated by the JSON parsers