diff --git a/Dockerfile b/Dockerfile index 04a6ecd..be919d3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -23,6 +23,8 @@ ENV AUTOWARE_MSG_PKG="/ros_workspace/install/setup.bash" ENV ROS_PKG="/opt/ros/${ROS_DISTRO}/setup.bash" # install pip requirements and carla +# NETWORKX has issues with collections.abc +# switch to different versions (python 3.9+) RUN python3 -m pip install -r requirements.txt && \ mv docker/PythonAPI.tar ./ && \ tar -xvf PythonAPI.tar && \ diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index e5e15ea..872141d 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -5,9 +5,7 @@ import importlib import signal import sys -import time import logging -import argparse import datetime import yaml @@ -19,13 +17,16 @@ from srunner.scenarios.route_scenario import RouteScenario 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.route_scenario_configuration import ( + RouteScenarioConfiguration, +) from srunner.tools.log import LogUtil class AWScenarioRunner(object): - client_timeout = 10.0 - ego_vehicles = [] # world and scenario handlers @@ -39,41 +40,38 @@ class AWScenarioRunner(object): wait_for_update = False finished = False - sync = False # CARLA-Bridge will do the ticking - aw_agent = None - def __init__(self, args: object) -> None: + def __init__(self, config: dict) -> None: """ Setup Scenario Manager and the Carla client """ - self._args = args + self._carla_config = config["carla"] + self._tm_config = config["traffic_manager"] + self._scenario_config = config["scenario_runner"] - if args.timeout: - self.client_timeout = float(args.timeout) - - self.carla_client = carla.Client(args.host, int(args.port)) - self.carla_client.set_timeout(self.client_timeout) + self.carla_client = carla.Client( + self._carla_config["host"], int(self._carla_config["port"]) + ) + self.carla_client.set_timeout(self._carla_config["timeout"]) # update the client CarlaDataProvider.set_client(self.carla_client) - if args.route_id: - self.route_id = str(args.route_id) - else: - self.route_id = "0" - # load autoware agent - autoware_agent_path = "srunner/autoagents/autoware_agent" - module_name = os.path.basename(autoware_agent_path).split(".")[0] - sys.path.insert(0, os.path.dirname(autoware_agent_path)) - self.module_aw_agent = importlib.import_module(module_name) + # only load if in docker environment + # debug + if self._scenario_config["in_docker"]: + autoware_agent_path = "srunner/autoagents/autoware_agent" + module_name = os.path.basename(autoware_agent_path).split(".")[0] + sys.path.insert(0, os.path.dirname(autoware_agent_path)) + self.module_aw_agent = importlib.import_module(module_name) # main class to execute scenarios self.scenario_manager = ScenarioManager( - args.debug, - self.sync, - self.client_timeout + self._scenario_config["debug"], + self._carla_config["sync"], + self._carla_config["timeout"], ) self.results_manager = ResultsManager() @@ -90,28 +88,30 @@ def __init__(self, args: object) -> None: # parse the JSON scenario file if self.scenario_decoder is None: self.scenario_decoder = XMLToFiles() - - scenario_name = os.path.split(os.path.splitext(args.json)[0])[1] - self._parse_json(args.json, scenario_name, 0) + + scenario_name = os.path.split( + os.path.splitext(self._scenario_config["json"])[0] + )[1] + self._parse_json(self._scenario_config["json"], scenario_name, "0") def _parse_json(self, json: str, scenario: str, iteration: str) -> None: """Parses a given JSON Scenario definition. Outputs two XML files used by scenario runner Args: json (str): filepath to JSON scenario definition - scenario (str): Name of the scenario + scenario (str): Name of the scenario iteration (str): ID of the scenario. Can be anything, but must be unique """ # create run directory if doesn't exist if not self.results_manager.results_path: - self.results_manager._create_run_folder() - - self.results_manager._create_scenario_folder( - scenario, iteration, self.results_path - ) - - self.scenario_decoder.parse_scenario(json, self.results_manager.last_scenario) - + self.results_manager.create_run_folder() + + self.results_manager.create_scenario_folder( + scenario, iteration, self.results_manager.results_path + ) + + self.scenario_decoder.parse_scenario(json, self.results_manager.last_scenario) + def _signal_handler(self, signum, frame) -> None: """ Handle shutdown signal, do cleanup @@ -123,68 +123,89 @@ def _signal_handler(self, signum, frame) -> None: if not self.scenario_manager.get_running_status(): raise RuntimeError("Scenario Timeout") - def run_scenario(self, config) -> bool: + def run_scenario( + self, route_config: RouteScenarioConfiguration, env_config: EnvironmentConfig + ) -> bool: # find the ego vehicle by name # only supports one ego - self.carla_world = self.carla_client.get_world() - self.carla_client.load_world('Town01') - - ego_missing = True - while ego_missing: - self.ego_vehicles = [] - for ego in config.ego_vehicles: - carla_vehicles = ( - self.carla_client.get_world().get_actors().filter("vehicle.*") - ) - for carla_vehicle in carla_vehicles: - if carla_vehicle.attributes["role_name"] == ego["name"]: - self.ego_vehicles.append(carla_vehicle) - ego_missing = False - break - print("Can't find ego, waiting...") - time.sleep(1) - print("Found ego") + # setup world based on env config + # make sure to reload + self.carla_world = self.carla_client.get_world() + self.carla_client.load_world(env_config.town) # update carla provider - CarlaDataProvider.set_client(self.carla_client) CarlaDataProvider.set_world(self.carla_world) + # replace with spawning ego + # ego_missing = True + # while ego_missing: + # self.ego_vehicles = [] + # for ego in route_config.ego_vehicles: + # carla_vehicles = ( + # self.carla_client.get_world().get_actors().filter("vehicle.*") + # ) + # + # for carla_vehicle in carla_vehicles: + # if carla_vehicle.attributes["role_name"] == ego: + # self.ego_vehicles.append(carla_vehicle) + # ego_missing = False + # break + # print("Can't find ego, waiting...") + # time.sleep(1) + # ego_missing = False + + print("Spawning ego...") + self._spawn_ego(env_config) + + print("Spawned ego...") + self.carla_world.wait_for_tick() - print("Loading Autoware agent") - agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") - try: - print(getattr(self.module_aw_agent, agent_class_name)) - self.aw_agent = getattr(self.module_aw_agent, agent_class_name)("") - config.agent = self.aw_agent - except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except - traceback.print_exc() - print("Could not setup required agent due to {}".format(e)) - # self._cleanup() - return False + if self._scenario_config["in_docker"]: + print("Loading Autoware agent") + agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") + try: + print(getattr(self.module_aw_agent, agent_class_name)) + self.aw_agent = getattr(self.module_aw_agent, agent_class_name)( + env_config + ) + route_config.agent = self.aw_agent + + # call the agent method to notify the bridge of the ego spawn + # only continue once state changes! + + except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except + traceback.print_exc() + print("Could not setup required agent due to {}".format(e)) + # self._cleanup() + return False + + # only set synchronous mode once bridge is ready + # tick synchronously until then # ADD TRAFFIC MANAGER SEED TO CONFIG - tm_port = int(self._args.traffic_port) # type: ignore + tm_port = int(self._tm_config["port"]) # type: ignore CarlaDataProvider.set_traffic_manager_port(tm_port) tm = self.carla_client.get_trafficmanager(tm_port) tm.set_random_device_seed(1) # ADD TO CONFIG - tm.set_synchronous_mode(True) + tm.set_synchronous_mode(self._tm_config["sync"]) print("Preparing ego...") # update ego position to one specified in route - ego_transform = config.ego_vehicles[0]["transform"] - self.ego_vehicles[0].set_transform(ego_transform) + self.ego_vehicles[0].set_transform(env_config.ego_spawn) self.ego_vehicles[0].set_target_velocity(carla.Vector3D()) self.ego_vehicles[0].set_target_angular_velocity(carla.Vector3D()) - CarlaDataProvider.register_actor(self.ego_vehicles[0], ego_transform) + CarlaDataProvider.register_actor(self.ego_vehicles[0], env_config.ego_spawn) print("Loading route...") try: scenario = RouteScenario( - world=self.carla_world, config=config, debug_mode=True + world=self.carla_world, + config=route_config, + debug_mode=self._carla_config["debug"], ) except Exception: print("Could not load Route Scenario") @@ -202,28 +223,49 @@ def run_scenario(self, config) -> bool: result = False return result - def _load_route_scenario(self) -> None: - # take self.last_scenario_path + def _load_scenario_config(self) -> EnvironmentConfig: + return EnvironmentParser.parse_scenario_env( + os.path.join(self.results_manager.last_scenario, "scenario.xml") + ) + + def _spawn_ego(self, env_config: EnvironmentConfig) -> None: + ego = CarlaDataProvider.request_new_actor( + model=env_config.ego_model, + spawn_point=env_config.ego_spawn, + rolename=env_config.ego_name, + ) + self.ego_vehicles.append(ego) + + bp_library = self.carla_world.get_blueprint_library() + # setup sensors + for sensor in env_config.sensor_config: + sensor._spawn(bp_library, ego) + print(f"Spawned sensor type {sensor.type}") + + def _load_route_scenario( + self, env_config: EnvironmentConfig + ) -> RouteScenarioConfiguration: route_config = RouteParser.parse_routes_file( - self.last_scenario_path, self.route_id + self.results_manager.last_scenario, env_config ) # type: ignore return route_config[0] - def run(self) -> None: + def run(self) -> bool: # load the route config # load the scenarion # run it # get the metrics # call the algorithm callback - config = self._load_route_scenario() - scenario_result = self.run_scenario(config) + env_config = self._load_scenario_config() + route_config = self._load_route_scenario(env_config) + + scenario_result = self.run_scenario(route_config, env_config) return scenario_result def destroy(self) -> None: - """Deletes instances of all classes related to CARLA - """ + """Deletes instances of all classes related to CARLA""" self._cleanup() if self.scenario_manager is not None: @@ -234,9 +276,7 @@ def destroy(self) -> None: del self.carla_world def _cleanup(self) -> None: - """Cleanup function. Removes instances of the CARLA client and WORLD, also destroys the Ego vehicle in CARLA. - - """ + """Cleanup function. Removes instances of the CARLA client and WORLD, also destroys the Ego vehicle in CARLA.""" # Simulation still running and in synchronous mode? if self.carla_world is not None: try: @@ -265,76 +305,24 @@ def _cleanup(self) -> None: def main(): - desc = """ - CARLA and Autoware Scenario Runner modified to only use a single scenario definition - """ - - # Avaliable arguments - # - # --port: CARLA port - # --host: CARLA host - # --alg: custom algorithm class to be imported. - # --json: JSON scenario definition - # --route-id: ROUTE id to use, specified in the JSON scenario definition - # --outputDir: Output directory to create the final results - # --no-record: does not record any metrics - # --timeout: client timeout - - arg_parser = argparse.ArgumentParser( - description=desc, formatter_class=argparse.RawTextHelpFormatter - ) - - arg_parser.add_argument("-p", "--port", default=2000, help="CARLA Port") - arg_parser.add_argument( - "--traffic-port", default=8000, help="CARLA Traffic Manager Port" - ) - arg_parser.add_argument("--host", default="127.0.0.1", help="CARLA Host") - arg_parser.add_argument( - "-a", - "--algorithm", - default=None, - help="The Algorithm to use when modifying scenarios", - ) - arg_parser.add_argument( - "-j", "--json", default=None, help="JSON Scenario description" - ) - arg_parser.add_argument( - "-r", "--route-id", default=0, help="Route to use from the SCENARIO definition" - ) - arg_parser.add_argument( - "-o", - "--outputDir", - default=None, - help="Specify a custom output directory for the results", - ) - arg_parser.add_argument( - "-n", "--no-record", default=None, help="Does not record a scenario replay" - ) - arg_parser.add_argument( - "-t", "--timeout", default=None, help="CARLA client timeout" - ) - arg_parser.add_argument( - "-d", "--debug", default=False, action="store_true", help="CARLA client timeout" - ) - - arguments = arg_parser.parse_args() - # configure logger - log_config = None - with open('config.yaml', 'r') as stream: - log_config = yaml.safe_load(stream) - - logger = logging.getLogger('scenario-runner') + config = None + with open("config.yaml", "r") as stream: + config = yaml.safe_load(stream) + + log_config = config["log"] + + logger = logging.getLogger("scenario-runner") logger.setLevel(logging.INFO) - - log_path = LogUtil.create_log_file(log_config['log']['path']) - logger.addHandler(logging.FileHandler(log_path, encoding='utf-8')) + + log_path = LogUtil.create_log_file(log_config["path"]) + logger.addHandler(logging.FileHandler(log_path, encoding="utf-8")) logger.addHandler(logging.StreamHandler(sys.stdout)) - + # reload world and sync must be present when running agent-based route scenarios scenario_runner = None try: - scenario_runner = AWScenarioRunner(arguments) + scenario_runner = AWScenarioRunner(config) results = scenario_runner.run() print(results) except Exception: # NOT GOOD PRACTICE PROBABLY CHANGE diff --git a/config.yaml b/config.yaml index c2fdbcb..f6bf1cb 100644 --- a/config.yaml +++ b/config.yaml @@ -1,3 +1,19 @@ log: path: 'logs/' - clear_old_logs: true + clear_old_logs: true +carla: + host: 127.0.0.1 + port: 2000 + timeout: 20 + sync: true + fixed_delta_seconds: 0.05 # update rate 1 / FPS + debug: false +traffic_manager: + active: false + sync: true # must be the same as carla sync + port: 8000 +scenario_runner: + json: ./example_scenario.json + debug: false + route_id: 0 + in_docker: true # used for dev diff --git a/docker/autoware_msgs.tar b/docker/autoware_msgs.tar index ed75dc0..79cd6e2 100644 Binary files a/docker/autoware_msgs.tar and b/docker/autoware_msgs.tar differ diff --git a/example_scenario.json b/example_scenario.json index 21f5875..018b743 100644 --- a/example_scenario.json +++ b/example_scenario.json @@ -3,7 +3,6 @@ { "route": { "id": 0, - "town": "Town01", "weathers": [ { "weather": { @@ -59,7 +58,6 @@ { "route": { "id": 1, - "town": "Town01", "weathers": [ { "weather": { @@ -132,34 +130,82 @@ { "scenario": { "town": "Town01", + "route-id": 0, "ego_vehicle": { "x": 1234, "y": 5.2, "z": 3123471, "yaw": -173, "model": "vehicle.toyota.prius", - "name": "ego_vehicle" - }, - "other_actor": { - "x": 1234, - "y": 5.2, - "z": 3123471, - "yaw": -173, - "model": "vehicle.toyota.prius" - }, - "route": { - "route-id": 0 - }, - "weather": { - "route_percentage": 0.0, - "cloudiness": 50.0, - "preciptation": 100.0, - "precipitation_deposits": 100.0, - "wetness": 100.0, - "wind_intensity": 100.0, - "sun_azimuth_angle": -1.0, - "sun_altitude_angle": 90.0, - "fog_density": 2.0 + "name": "ego_vehicle", + "sensor_configuration": [ + { + "sensor": { + "type": "sensor.camera.rgb", + "id": "rgb_front", + "spawn_point": { + "x": 0.7, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 90.0 + } + }, + { + "sensor": { + "type": "sensor.lidar.ray_cast", + "id": "top", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 3.1, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "range": 100, + "channels": 64, + "points_per_second": 300000, + "upper_fov": 10.0, + "lower_fov": -30.0, + "rotation_frequency": 20, + "noise_stddev": 0.0 + } + }, + { + "sensor": { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + } + }, + { + "sensor": { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + } + } + ] } } } diff --git a/requirements.txt b/requirements.txt index e9f8b0e..a5ccc9a 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,6 @@ py-trees==0.8.3 numpy<2; python_version >= '3.0' -networkx==2.2 +networkx Shapely==1.7.1 psutil xmlschema==1.0.18 @@ -13,4 +13,3 @@ six simple-watchdog-timer antlr4-python3-runtime==4.10 graphviz - diff --git a/srunner/autoagents/agent_state/autoware_state.py b/srunner/autoagents/agent_state/autoware_state.py index 39d4f68..dafb074 100644 --- a/srunner/autoagents/agent_state/autoware_state.py +++ b/srunner/autoagents/agent_state/autoware_state.py @@ -35,6 +35,7 @@ def reset_state(self) -> None: # internal message states self.sent_route: bool = False self.sent_engage: bool = False + self.bridge_ready: bool = False # ADS state self.motion_state: int = 0 diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index c325d04..9edf6ec 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -7,29 +7,34 @@ from srunner.autoagents.agent_state import autoware_state +from srunner.tools.environment_parser import EnvironmentConfig + +from autoware_carla_interface.msg import EgoConfig, SensorConfig + import threading import rclpy +import time DEBUG_ENV = False -# uncomment if testing in scenario runner - +# uncomment if testing in scenario runner class AutowareAgent(AutonomousAgent): timestamp = None current_map = None agent_set_route = False counter = 0 - def setup(self, path_to_conf_file: dict | None = None) -> None: + def setup(self, config: EnvironmentConfig | None = None) -> None: """Setup the Autoware Agent. - Initialise the state - Setup nodes Args: - _path_to_conf (dict | None): path to config, passed from AutonomousAgent + config (EnvironmentConfig | None): environment configuration file """ + rclpy.init(args=None) self.autoware_state = autoware_state.AutowareState("ego_vehicle", None) @@ -45,6 +50,26 @@ def setup(self, path_to_conf_file: dict | None = None) -> None: threading.Thread(target=rclpy.spin, args=(self.state_node)), ] + # check the bridge is ready + # publish sensor information to the bridge + # wait for it to return the correct message + # hang until + + ego_config_msg = EgoConfig() + ego_config_msg.ego_name = config.ego_name + ego_config_msg.ego_model = config.ego_model + ego_config_msg.sensors = [] + + for sensor_config in config.sensor_config: + sensor_config_msg = SensorConfig() + sensor_config_msg.sensor_type = sensor_config.type + sensor_config_msg.sensor_id = sensor_config.id + ego_config_msg.sensors.append(sensor_config_msg) + + while not self.autoware_state.bridge_ready: + time.sleep(1) + self.autoware_state.ego_config_publisher.publish(ego_config_msg) + def set_route(self) -> None: # for every point in the plan # convert to waypoint diff --git a/srunner/autoagents/autoware_nodes/state_node.py b/srunner/autoagents/autoware_nodes/state_node.py index 100885e..8a57b15 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -3,6 +3,9 @@ from autoware_adapi_v1_msgs.msg import MotionState from autoware_adapi_v1_msgs.msg import LocalizationInitializationState from srunner.autoagents.agent_state import autoware_state +from autoware_carla_interface.msg import EgoConfig, BridgeState + +from srunner.tools.environment_parser import DefaultSensor class StateNode(Node): @@ -10,6 +13,9 @@ class StateNode(Node): motion_state = "/api/motion/state" localize_state = "/api/localization/initialization_state" + ego_config = "/bridge/ego_vehicle/config" # publish sensor config type: EgoConfig + bridge_state = "/bridge/state" + def __init__(self, autoware_state: autoware_state.AutowareState) -> None: super().__init__("state_node") self.autoware_state = autoware_state @@ -26,6 +32,26 @@ def __init__(self, autoware_state: autoware_state.AutowareState) -> None: self.localize_state_cb, 10, ) + self.autoware_state_subscriber = self.create_subscription( + BridgeState, + self.bridge_state, + self.bridge_state_cb, + 10 + ) + + self.ego_config_publisher = self.create_publisher( + EgoConfig, + self.ego_config, + 10 + ) + + def bridge_state_cb(self, bridge_state_msg: BridgeState): + """Set the AutowareState attribute bridge_state + + Args: + bridge_state_msg (BridgeState): bridge state boolean value + """ + self.bridge_state = bridge_state_msg.bridge_ready def route_state_cb(self, route_state_msg: RouteState) -> None: """Set the AutowareState attribute route_state @@ -52,3 +78,6 @@ def localize_state_cb( localize_state_msg (LocalizationInitializationState): localization message received """ self.autoware_state.localize_state = localize_state_msg.state + + def publish_sensor_info(self, sensor_config: list[DefaultSensor]) -> None: + return diff --git a/srunner/scenarioconfigs/route_scenario_configuration.py b/srunner/scenarioconfigs/route_scenario_configuration.py index 6cdf82e..9b3aa03 100644 --- a/srunner/scenarioconfigs/route_scenario_configuration.py +++ b/srunner/scenarioconfigs/route_scenario_configuration.py @@ -16,7 +16,6 @@ class RouteConfiguration(object): - """ This class provides the basic configuration for a route """ @@ -31,17 +30,16 @@ def parse_xml(self, node): self.data = [] for waypoint in node.iter("waypoint"): - x = float(waypoint.attrib.get('x', 0)) - y = float(waypoint.attrib.get('y', 0)) - z = float(waypoint.attrib.get('z', 0)) - c = waypoint.attrib.get('connection', '') - connection = RoadOption[c.split('.')[1]] + x = float(waypoint.attrib.get("x", 0)) + y = float(waypoint.attrib.get("y", 0)) + z = float(waypoint.attrib.get("z", 0)) + c = waypoint.attrib.get("connection", "") + connection = RoadOption[c.split(".")[1]] self.data.append((carla.Location(x, y, z), connection)) class RouteScenarioConfiguration(ScenarioConfiguration): - """ Basic configuration of a RouteScenario """ diff --git a/srunner/tools/environment_parser.py b/srunner/tools/environment_parser.py new file mode 100644 index 0000000..6f470d9 --- /dev/null +++ b/srunner/tools/environment_parser.py @@ -0,0 +1,201 @@ +import xml.etree.ElementTree as ET +from xml.etree.ElementTree import Element + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + +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 = ["type", "id", "spawn"] + + sensor_params = [attr for attr in dir(self) if attr not in ignored_params] + + for param in sensor_params: + sensor_bp.set_attribute(param, getattr(self, param)) + + CarlaDataProvider.get_world().spawn_actor(sensor_bp, self.spawn, vehicle) + + 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_freq: int = 0 + self.noise_sttdev: float = 0.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 + """ + + logger = logging.getLogger("scenario-runner") + + @staticmethod + def parse_scenario_env(path: str) -> EnvironmentConfig: + config = EnvironmentConfig() + + tree = ET.parse(path) + + for scenario in tree.iter("scenario"): + config.town = scenario.attrib.get("town", "") + config.route_id + + for elem in scenario.iter(): + if elem.tag == "ego_vehicle": + EnvironmentParser.parse_ego_config(config, elem) + + print(config.sensor_config[0].fov) + return config + + @staticmethod + def parse_ego_config(config: EnvironmentConfig, elem: Element) -> None: + config.ego_model = elem.attrib.get("model", "") + config.ego_name = elem.attrib.get("name", "") + + # build the rotation of the ego - we are only concerned with yaw (Z) + ego_rotation = carla.Rotation(0.0, float(elem.attrib.get("yaw", 0.0)), 0.0) + + ego_location = carla.Location( + float(elem.attrib.get("x", 0.0)), + float(elem.attrib.get("y", 0.0)), + float(elem.attrib.get("z", 0.0)), + ) + + config.ego_spawn = carla.Transform(ego_location, ego_rotation) + + sensors = [] + + # parse the sensor configurations + for sensor in elem.iter("sensor"): + sensor_type = sensor.attrib.get("type", "") + if sensor_type == "": + EnvironmentParser.logger.error( + "Please include a sensor type, skipping sensor..." + ) + continue + + spawn = sensor.find("spawn_point") + + sensor_obj = DefaultSensor() + + if sensor_type == "sensor.camera.rgb": + sensor_obj = CameraRGB() + sensor_obj.image_size_x = sensor.attrib.get("image_size_x", 0) + sensor_obj.image_size_y = sensor.attrib.get("image_size_y", 0) + sensor_obj.fov = sensor.attrib.get("fov", 0.0) + + elif sensor_type == "sensor.lidar.ray_cast": + sensor_obj = LidarRayCast() + sensor_obj.channels = sensor.attrib.get("channels", 0) + sensor_obj.points_per_second = sensor.attrib.get("points_per_second", 0) + sensor_obj.upper_fov = sensor.attrib.get("upper_fov", 0.0) + sensor_obj.lower_fov = sensor.attrib.get("lower_fov", 0.0) + sensor_obj.rotation_freq = sensor.attrib.get("rotation_frequency", 0) + sensor_obj.noise_sttdev = sensor.attrib.get("noise_sttdev", 0.0) + + sensor_obj.type = sensor_type + sensor_obj.id = sensor.attrib.get("id", "") + + if spawn is not None: + sensor_obj.spawn = carla.Transform( + carla.Location( + float(spawn.attrib.get("x", 0.0)), + float(spawn.attrib.get("y", 0.0)), + float(spawn.attrib.get("z", 0.0)), + ), + carla.Rotation( + float(spawn.attrib.get("pitch", 0.0)), + float(spawn.attrib.get("yaw", 0.0)), + float(spawn.attrib.get("roll", 0.0)), + ), + ) + else: + EnvironmentParser.logger.error( + "Please include a spawn location for the sensor..." + ) + continue + + sensors.append(sensor_obj) + + config.sensor_config = sensors diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index 5255ed0..3b57179 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -19,6 +19,8 @@ ActorConfigurationData, ) +from srunner.tools.environment_parser import EnvironmentConfig + # Threshold to say if a scenarios trigger position is part of the route DIST_THRESHOLD = 2.0 ANGLE_THRESHOLD = 10 @@ -42,7 +44,7 @@ class RouteParser(object): """ @staticmethod - def parse_routes_file(config_path, single_route_id=""): + def parse_routes_file(config_path, env_config: EnvironmentConfig): """ Returns a list of route configuration elements. :param route_filename: the path to a set of routes. @@ -51,7 +53,7 @@ def parse_routes_file(config_path, single_route_id=""): """ route_filename = os.path.join(config_path, "route.xml") - scenario_config = os.path.join(config_path, "scenario.xml") + single_route_id = env_config.route_id route_configs = [] tree = ET.parse(route_filename) @@ -61,7 +63,7 @@ def parse_routes_file(config_path, single_route_id=""): continue route_config = RouteScenarioConfiguration() - route_config.town = route.attrib["town"] + route_config.town = env_config.town route_config.name = "RouteScenario_{}".format(route_id) route_config.weather = RouteParser.parse_weather(route) @@ -76,28 +78,9 @@ def parse_routes_file(config_path, single_route_id=""): ) ) route_config.keypoints = positions + route_config.ego_vehicles = [env_config.ego_name] scenario_configs = [] - # load the scenario configuration from the same directory - # TO DO -> implement defining other actors - scenario_tree = ET.parse(scenario_config) - - # get the name of the ego - _ego = scenario_tree.getroot().find("scenario").find("ego_vehicle") - - _ego_config = { - "name": _ego.attrib.get("name"), - "transform": carla.Transform( - carla.Location( - float(_ego.attrib.get("x")), - float(_ego.attrib.get("y")), - float(_ego.attrib.get("z")), - ), - carla.Rotation(0.0, float(_ego.attrib.get("yaw")), 0.0), - ), - } - - route_config.ego_vehicles = [_ego_config] # The list of ScenarioConfigurations that store the scenario's data for scenario in route.find("scenarios").iter("scenario"):