From a67fadfc484e306cce0110eb1e90feeb41e072be Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Thu, 24 Jul 2025 16:31:25 +0100 Subject: [PATCH 1/8] added custom XML parser for scenario definition --- aw_scenario_runner.py | 192 +++++++++++----------------- config.yaml | 17 ++- example_scenario.json | 93 ++++++++++---- srunner/tools/environment_parser.py | 155 ++++++++++++++++++++++ 4 files changed, 315 insertions(+), 142 deletions(-) create mode 100644 srunner/tools/environment_parser.py diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index e5e15ea..24560e1 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -7,7 +7,6 @@ import sys import time import logging -import argparse import datetime import yaml @@ -19,13 +18,12 @@ 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.log import LogUtil class AWScenarioRunner(object): - client_timeout = 10.0 - ego_vehicles = [] # world and scenario handlers @@ -39,41 +37,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 +85,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 @@ -127,7 +124,7 @@ def run_scenario(self, config) -> 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') + self.carla_client.load_world("Town01") ego_missing = True while ego_missing: @@ -152,17 +149,18 @@ def run_scenario(self, config) -> bool: 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)("") + 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 # ADD TRAFFIC MANAGER SEED TO CONFIG tm_port = int(self._args.traffic_port) # type: ignore @@ -203,9 +201,12 @@ def run_scenario(self, config) -> bool: return result def _load_route_scenario(self) -> None: - # take self.last_scenario_path + env_config = EnvironmentParser.parse_scenario_env( + os.path.join(self.results_manager.last_scenario, "scenario.xml") + ) + route_config = RouteParser.parse_routes_file( - self.last_scenario_path, self.route_id + self.results_manager.last_scenario, env_config.route_id ) # type: ignore return route_config[0] @@ -218,12 +219,19 @@ def run(self) -> None: # call the algorithm callback config = self._load_route_scenario() + + # setup CARLA settings + if self._carla_config["sync"]: + settings = self.carla_world.get_settings() + settings.synchonous_mode = True + settings.fixed_delta_seconds = self._carla_config["fixed_delta_seconds"] + self.carla_world.apply_settings(settings) + scenario_result = self.run_scenario(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 +242,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 +271,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..d718861 100644 --- a/config.yaml +++ b/config.yaml @@ -1,3 +1,18 @@ 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 +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: false # used for dev diff --git a/example_scenario.json b/example_scenario.json index 21f5875..86f7e75 100644 --- a/example_scenario.json +++ b/example_scenario.json @@ -132,34 +132,83 @@ { "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/srunner/tools/environment_parser.py b/srunner/tools/environment_parser.py new file mode 100644 index 0000000..8be494d --- /dev/null +++ b/srunner/tools/environment_parser.py @@ -0,0 +1,155 @@ +import xml.etree.ElementTree as ET +from xml.etree.ElementTree import Element + +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 + + +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 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 From 4f25e7a7443736a1b60f1ac45e3f450226150962 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 25 Jul 2025 11:10:57 +0100 Subject: [PATCH 2/8] started work on node to send bridge state and sensor state --- aw_scenario_runner.py | 73 +++++++++++++------ config.yaml | 3 +- example_scenario.json | 3 - .../autoagents/agent_state/autoware_state.py | 1 + srunner/autoagents/autoware_agent.py | 14 +++- .../autoagents/autoware_nodes/state_node.py | 3 + .../route_scenario_configuration.py | 12 ++- srunner/tools/route_parser.py | 29 ++------ 8 files changed, 77 insertions(+), 61 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 24560e1..4e63709 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -19,6 +19,10 @@ 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 @@ -120,27 +124,34 @@ 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 + + # setup world based on env config + # make sure to reload self.carla_world = self.carla_client.get_world() - self.carla_client.load_world("Town01") + self.carla_client.load_world(env_config.town) + # replace with spawning ego ego_missing = True while ego_missing: self.ego_vehicles = [] - for ego in config.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["name"]: + 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("Found ego") # update carla provider @@ -155,7 +166,11 @@ def run_scenario(self, config) -> bool: 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 + 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)) @@ -163,26 +178,27 @@ def run_scenario(self, config) -> bool: return False # 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") @@ -200,34 +216,43 @@ def run_scenario(self, config) -> bool: result = False return result - def _load_route_scenario(self) -> None: - env_config = EnvironmentParser.parse_scenario_env( + 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) + + # setup sensors + + return + + def _load_route_scenario( + self, env_config: EnvironmentConfig + ) -> RouteScenarioConfiguration: route_config = RouteParser.parse_routes_file( - self.results_manager.last_scenario, env_config.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() - - # setup CARLA settings - if self._carla_config["sync"]: - settings = self.carla_world.get_settings() - settings.synchonous_mode = True - settings.fixed_delta_seconds = self._carla_config["fixed_delta_seconds"] - self.carla_world.apply_settings(settings) + env_config = self._load_scenario_config() + route_config = self._load_route_scenario(env_config) - scenario_result = self.run_scenario(config) + scenario_result = self.run_scenario(route_config, env_config) return scenario_result def destroy(self) -> None: diff --git a/config.yaml b/config.yaml index d718861..f6bf1cb 100644 --- a/config.yaml +++ b/config.yaml @@ -7,6 +7,7 @@ carla: 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 @@ -15,4 +16,4 @@ scenario_runner: json: ./example_scenario.json debug: false route_id: 0 - in_docker: false # used for dev + in_docker: true # used for dev diff --git a/example_scenario.json b/example_scenario.json index 86f7e75..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": { @@ -147,7 +145,6 @@ "id": "rgb_front", "spawn_point": { "x": 0.7, - "y": 0.0, "z": 1.6, "roll": 0.0, 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..62ea5e7 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -7,22 +7,23 @@ from srunner.autoagents.agent_state import autoware_state +from srunner.tools.environment_parser import EnvironmentConfig + import threading import rclpy 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 @@ -45,6 +46,13 @@ 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 + while not self.autoware_state.bridge_ready: + return + 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..f1cd00c 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -10,6 +10,9 @@ class StateNode(Node): motion_state = "/api/motion/state" localize_state = "/api/localization/initialization_state" + ego_config = "/bridge/ego_vehicle/config" + bridge_state = "/bridge/state" + def __init__(self, autoware_state: autoware_state.AutowareState) -> None: super().__init__("state_node") self.autoware_state = autoware_state 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/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"): From 1ecb3d9ea71d3460b8b106662d56ab1e9ae434bf Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 25 Jul 2025 12:59:13 +0100 Subject: [PATCH 3/8] added code to spawn ego vehicle and sensors --- Dockerfile | 2 ++ aw_scenario_runner.py | 8 ++++-- srunner/tools/environment_parser.py | 44 +++++++++++++++++++++++++++++ 3 files changed, 51 insertions(+), 3 deletions(-) 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 4e63709..5dd5bcd 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -221,7 +221,7 @@ def _load_scenario_config(self) -> EnvironmentConfig: os.path.join(self.results_manager.last_scenario, "scenario.xml") ) - def _spawn_ego(self, env_config: EnvironmentConfig) -> None: + def _spawn_ego(self, world, env_config: EnvironmentConfig) -> None: ego = CarlaDataProvider.request_new_actor( model=env_config.ego_model, spawn_point=env_config.ego_spawn, @@ -229,9 +229,11 @@ def _spawn_ego(self, env_config: EnvironmentConfig) -> None: ) self.ego_vehicles.append(ego) + bp_library = world.get_blueprint_library() # setup sensors - - return + 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 diff --git a/srunner/tools/environment_parser.py b/srunner/tools/environment_parser.py index 8be494d..cf42927 100644 --- a/srunner/tools/environment_parser.py +++ b/srunner/tools/environment_parser.py @@ -1,6 +1,8 @@ import xml.etree.ElementTree as ET from xml.etree.ElementTree import Element +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + import carla import logging @@ -29,6 +31,18 @@ def __init__(self) -> None: 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) + class CameraRGB(DefaultSensor): """ @@ -58,6 +72,36 @@ def __init__(self) -> None: 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 From e6ee0d6af0fbd71091ae6f793f611142545c1844 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 25 Jul 2025 13:17:13 +0100 Subject: [PATCH 4/8] update --- aw_scenario_runner.py | 55 +++++++++++-------- srunner/autoagents/autoware_agent.py | 6 +- .../autoagents/autoware_nodes/state_node.py | 5 ++ 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 5dd5bcd..872141d 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -5,7 +5,6 @@ import importlib import signal import sys -import time import logging import datetime import yaml @@ -135,29 +134,32 @@ def run_scenario( self.carla_world = self.carla_client.get_world() self.carla_client.load_world(env_config.town) - # 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("Found ego") - # 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() if self._scenario_config["in_docker"]: @@ -165,7 +167,9 @@ def run_scenario( 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)("") + 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 @@ -177,6 +181,9 @@ def run_scenario( # 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._tm_config["port"]) # type: ignore CarlaDataProvider.set_traffic_manager_port(tm_port) @@ -221,7 +228,7 @@ def _load_scenario_config(self) -> EnvironmentConfig: os.path.join(self.results_manager.last_scenario, "scenario.xml") ) - def _spawn_ego(self, world, env_config: EnvironmentConfig) -> None: + def _spawn_ego(self, env_config: EnvironmentConfig) -> None: ego = CarlaDataProvider.request_new_actor( model=env_config.ego_model, spawn_point=env_config.ego_spawn, @@ -229,7 +236,7 @@ def _spawn_ego(self, world, env_config: EnvironmentConfig) -> None: ) self.ego_vehicles.append(ego) - bp_library = world.get_blueprint_library() + bp_library = self.carla_world.get_blueprint_library() # setup sensors for sensor in env_config.sensor_config: sensor._spawn(bp_library, ego) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 62ea5e7..a79f66e 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -11,6 +11,7 @@ import threading import rclpy +import time DEBUG_ENV = False @@ -29,8 +30,9 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: - 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) @@ -51,6 +53,8 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: # wait for it to return the correct message # hang until while not self.autoware_state.bridge_ready: + time.sleep(1) + return def set_route(self) -> None: diff --git a/srunner/autoagents/autoware_nodes/state_node.py b/srunner/autoagents/autoware_nodes/state_node.py index f1cd00c..6958bc8 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -4,6 +4,8 @@ from autoware_adapi_v1_msgs.msg import LocalizationInitializationState from srunner.autoagents.agent_state import autoware_state +from srunner.tools.environment_parser import DefaultSensor + class StateNode(Node): route_state = "/planning/mission_planning/state" @@ -55,3 +57,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 From c0263784701bbabff3e8da5a9b2987453ecf3816 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 25 Jul 2025 13:59:00 +0100 Subject: [PATCH 5/8] added new message types to docker container --- docker/autoware_msgs.tar | Bin 614400 -> 614400 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/docker/autoware_msgs.tar b/docker/autoware_msgs.tar index ed75dc0f344ebad50f1e4b6a8ee5c59f346dc409..79cd6e2c8603228671607c47514b9ecb392e2170 100644 GIT binary patch delta 24033 zcmb7MeP~r@n(w)D&U;QYW|L?&jcJVWqi!a|`*H5Qt__*ksB`Q7JzKHlg1J-aW=+I?Zxp^VN9tBO)Tno1|*srXaTSS*%^KUI~~Ez_;m z32$!iYU!-1daAXhdvohRtYNA%`o9voF>U=zeoVxpZgr#)DniRjEtV!48_@pRhw+=G=wcZ&<2t0$^tN`Rc1Q984d zPFz#srlX03InmmT7Yf>+s8SN5DLO9f5~eZ1A^Dn`Z-Ps+pi8ba&Uoq+$Z(213@7a$5PkDRWla33`W2VpFL~rUl3?gk)EL3MclGU>@Ic_P^>w0&zPWk zO5H>I6+sVyye}2Pmzlf%v0s>Vd1+|+m*FBLxAjU+yl^g` zKrvB)HC!c?vdFF>BZ#F0e)Ln47(+h;G@cZw^yZkbZ%uIMxQ4>+zddbW+N~9=DBZV9 zi4wQlNchUgjozk6CAi{Wk&M`N-@z4+CCz2jf2I7R^1H79zJc!Q*Dwr$nEIC$eN=R> zWt8|gobw4n?BM;}!Ex&zJgw3R!F~NUQ^va0Jw+Cg#f>#~i9xXys#o`|j6lDFt1#Ir z)suHcmcLVF>(*;(y53u6mAI*-KwUw(6}lt$Bw6PT7OQ>QTBrVStNWbpTLHl*VkrS5EBP#NZMvrkQt)xl&245CgUd*3t6Al7 za|aWwVQ>Y3*V3_*f?PUiW-v+}y?4D;iULi*@&?AvB*LwHO9Y3W?b;D-FR|)%<0fk^ z7h8JoCb9ruVgXzD2*~Jl?^X-3NKjt}9U<$$zF`%+$%Mo|but(sTlaK9-Eq`S=IZWd z=Qg4y>3`a3O;RaA^B~w|WeP{Lt3rjAjl{Ff)Kd>Qjk>j&Z2SO>1=Wao2oBb>(hga# z=Yz1H6!6{8LPOD~``#r>jmHGwY+m9=ETm?FIyHE2G@9tX<5rOkmQAC~U-5 zXeXjj#VDH;!9O54`aOE@2QYAS)&$Yy1asX?27VGoiYAwNZWx?m@RthU4{3rXCUNn(;?Utuh}gpY~mq(t1GSQAvyYb~8yTh?vv=xXb1Z+T5mKjyyp zPL)zo{3o5(TZYwi?$M@Vxb;y)N*}ghD4PCA(S*l-{r#W*^<)CoGk!fyT|V<)pZ3VF zx6R$QedFBUwsmaJbiSDBXy3eXAhtMxRMaJAQYl}*KCH^ikb$EH;J!0cg*WoewUw=m zxE0%(#=5Sz-)I}Ry{mI`$HuDgUu|w3TF~qM(R!z<65N;l#Q9$6+}yhHaFyHe=&6vP zKYFTIimF({1TcHavP;@pI;u9%ueYu1Y-?$Kd)(+zYH^f`pE$~*jhTn#1wNk zNmh@ms;VZ_9FzAW|Mp>n<^Mzfp;`U&(C^!=ML)@N^+P;I-Kk}WNuxZ+(Y^xDG)^(q zRDbF}9_RVLjycck?*>(UP3y*%?VCHczrH!vxP4Pwd;8|Lt*vz}uhqTzdUeA>8q8F^ z6#jw!*FRY^{EgdMI^XQt1W%IbZ2bw$OK)x7`u4V}UoWUyIreAzn*~nh@E_EokN)(> zXDphe?8x4-rt7kEVK-g()mRf8YRnlr2Eb?OieZ)O=jgs#z^MQ=hu7$u8`dJ7`G#tf z8{Cy!y1f<{~LIb+fC)qB4{+>ZM(K@9tn!6OuUZy}H*)8=k1xcwMb(BTX2 zZ7Rcu>44y-j zzXRoDOi*D5-~R|~o)n#p?=S&nI_4_32rvy#^kzmnLp zmSu&+Q1`8eC&9cx;5;`l;bHP5t?&kMEPR-sx;+oxL~cmZY1+cru~Yl8xS;s&V6aDZ z*&C5!on57#9GSAKBSk|+oe}Ex^9}4Gua}Tq!ETl?_kVu}3CEI2fhq3{g@o?TuE<2x zzJeiAu$w8P_SJp+u}lNSnp@)(J*79Yn%r%v{(Wy`vS|Gv*cZv14za0UJWcagbF1o| z_lsbjooOaK*v{H$YDrpgN9;^-SnLBS;lhTHNc@o3lPCM2D7M#?TNz4%7~pAM;t>>m!YS$U_~Xk zVvj}D_mRodz}wV3zpx*2wYu-9n&8mBmN|Tu{_}0SNdL`c@0Vzb=++TcrF%ZLsrM>z zL)Te$+>N`D6TEpT9el>%+(HJqg@pMP2|i~mx9PvIX{IFbyI+P1hts|Vjxm1}nCWc> zvr8eV)3r92Is^s2kE4RPGd3_xIW%pQ zxR7}cnI`-D1_zf>(o=6_!rX8q%}}0cDxoC~lEuZ&XtIbUMjk?$!;wO8dc4FTFCf^U z3YI$Ly29=*TEmH$xg}%zRW($#-1%GlLNP_v6;AO$_TZC)YOs>2Vd$@Wn;a@=&CNI4 zZ`BOdta28C7*G0E6R)B&WDTev&QQj)*1^S#3sxqBW?;?e-6+ULa=O~Wx?=m@>hSskrK+Z>eFs}OU*b*&3k(8N z3A!BG8e;!JrIcg`VTm)9CQeN{(@eTM;Gn5+$Nm{B3nxrYZo;JdIu(ZhzJSqw7Ibc} z5DUeEj$@EzDEE7gK*4FacXI*1$6#*$A7%BRWp9o?#?FgH{02601peo=Q>iOYIKK(A zCt9MWiJqzdIq%HSGfq0ocq>frJ?UU~R1(xqv0ZUAJq?+mMJ5PLXIN(3BOAoUj3orK zE#8zk>(Hh|E{UR(N6o`AYxoxB?WPry}&I#Bc8kn+M-|OM4uAQ;fCD1 z>L8g$TCg_3hrE|NWKwd-`+P`j9GWXF_%?@;tnvrn<%Ho2j3fAgF;PD$5PHPrYbDAD z6l0>kz_kbCa&+J#jU#x@0*hQ$Zd`J)N<*dun+9wUlihzLe<4Za%5$V73yDEx4igjX ze+Si!!6Pz(%be|!e@O|N5iErok^KS~mM~iw%m`Y!%W_9d(ejFE`LcqnBb|~| zJWX!aL%64_XqpUPP-RrDysTTAT^~+5DM{hi=h#BG*PN|>i5z-M{! zWGAIi9Y{Jm^BBt`_4#TXW$6-hJ);*>W1<0VhP2@MFgw3SMb#%)k1xQ+P?ZS0;6f(O z-QdO4;}8UtUZ3sx|;kR-Meg%}oD1spdqnEQXL zaI4U*6r=~M8O+XPjiT8h?;JB_QfAA7blb+kBU$I-c+61%8~bBr0fgiGRRo z4wEM!v~)sJGo1|MBsOHuP=NL&4scLWOQ{RdYHFv8&NTOxI$iR?f~(!-!pa%i?aJ1R z_TlZtoJOx+zc!4Ns_#DF61DAfYjy1o_r+Yi-Qm(eLy}7W&ZV@BClNbc>c*LuK*27? zKA_mKn+V4wmcECn*Y61{FcPR{S*_PK~YNl7DdKU2pGaZDiqj{B0T z>L7zz+oUeZUR0MmLw`8SE7s2+u>Sg;Do^9X19*zr@6xo7Mk$6R5Jd3m(5vsdlOY1m zOtWq&hHwJq9bqOekn-Lm`Abrzqj>?4xl2Ysr(y2Tz3(mwZ_7>(>7``8;r>!z__M2Y z^0GHK|CIGOxEF=P510eaw@#4pL?tfcB!j=ErJPd)mXz1QhYaTTD5uHGNOX3Fv7D?8 zLT71#!31X+%fU2&wIw9qe+bSom;=MGORpIvp?D+&!#$rT%27eHGq}KDUcI{L;sNU> z@5wWj;8OU}D&MWvLkNuJY2R=q(4!*@;&fagk zrO5q7Z!N!a!}pT9?}01IgS+t3Wa@*l6yZK@evH3^VX7?tfp6&t*kgZ7K1;He@)u@2 zG!qc#4~&S2e^11@tJc-K z)vw2LE3D%PUb$rnf8{HzjL$u$YQt7l%EO~{WDDjNH_xLSiPv)GdkEB$M-;g}alN+* zzrd4%*ea)pM(L?`H@N$%Lc^eS;6c;j9yaPx6O83^VGW zD-X_5maH4@$g@rjMTqprYx7hl)EPnP!{}kB!9s6buxdW^)e@?44c%_l)o%Sl!o+ zbUHZ@%#mZ^D)pjxMQS?lWng96f;B z$I~PjoT=yj)*E}}e zD&II@+6v7@v2fd&1{FM(=f=NqOLaOr|&Lr`mP%<~^*$n28!)`z_ sXR{0725Ji61~_cgK)j+40@mgaUdKiu=>bI<%ojOcAbVqY!Y?TPf5Jm_kN^Mx delta 23366 zcmcIseP~tJmiOG=^AVp7L5k+Zn51c%=fn{2*Ev@*)=B$X$LC~xj^j91M65XCvp%on zWke1lA{{A?gw(6|h);?mQlyB4G@L6%9*#%|A|j3y5fKq7A|fIpQu^erb&k1a@0DwQ z{bTY+5;*s_*V^lQuf5LQd*S`P7v3K%bfxHL8vml7@xRgkX*ZR1b6(!_^3zi3RMzvR zwYbIf2b+HNpMUEAtgoeIT3?|Mr2CDzZ@u~_TkQGZe-0%IznZXC;LkGVT6r%Af*Xnr z$$y);K+#V$nQjJ$TN>PIv{U(XdRj|rYUYQXLQ*9@V~&%~doHmLz)QOl$?uZ2pKEK( zY_!uEaEak}bPnp7gM13(V7Q@>P&d%Xa5tTYHZa_zQ*WJXrBWcctzfCNI~r{-<7P3^ zodril=jK{zl(eVv9r%}$%}LPamb@Fx?X!P7i>)^Ay5mmp>Tl8SZZdPA{q5{3b}jx; z-Ns_levSDxk=14$y@g1pLGV@`ysgpBdZ?PV>w@lM6T{+TEVq7LZaay-3obFdOQ*k^>4P@5 zN8kEhvay^8qW9_OF9@B_V;(ZRUuXOPZ9AI*gX2M+_CRA$+GhS~aswS6)IWZR%*)Gw z#(7vrm*!fTH0Vo5^?e?nQ#>-#Uxm@H3$xUCH$p)_k1`tSw7!qhRUQ=Tc{St&eg_pB zSr@ESuCy2DE?QhQ?q8(152g@WseSZ!svJfsbb=loyVcklt!}tND`dc!a8=j%H4aBq z(m~D^$_lZ8FoKFxz=!3F5)LZ*q?c>k1totM?I>F36{8ZnyD?dR|F_Xd6SE@I((?Y$`XuEk(dtrm2?mh4u84q*cWT^2c@LL(51#@ZG zrtLoP9~-dkmU@kOLGwm)QI!Z3_)lmt&WwN`MT^brGX7E3W*5c76h|?jC<#+Sni-Nt zE|bO5YnT&wFOsH*6&K+`f$t{v(;3hihwPSe%Od0ViZccNVuLEd>4_?pVAvywend=X z=-i75?PFm=OL@~dupA1P3HrSTRR-NgmCB$LZF9NU<_hLImjbz7DX6Yus<*8wRKwK* zuqM)N4vRD4T7h534CixLjtSQb{06oSbiW$~d{g8ZAol}SG7%(~8jFISEye=X#kQ~< z@~FDE3P!dOBN_0V8!LaiQ2q|$A?Kn|t55LIPx!Qp`sq#)^>(u`!45~b$0*EDMaXfE z3mq)ZwFPrdn>|s`^eD9Pv7LTi{SEwsw3loyhXsvrEYj?t@%coKivA;7!^>lFt2Aci zf_$GE-_zF#c?O)iRT!GI2Zf25t`%x0zfPqZF2e=8m!#fM@LRm{8ny zYOXOFk^GSH>)_WXjnxw+75FEN<|L@PXn|Au0;fmZPbVisaZ~kuE~w-}BHodd^VWWUK(VKUIn}a8cL5#n=WxkGUj7Fv=1{ zH!vpPmstRL7u3KN<83Q=`?0Zcq8S$bdlW?ZAdeXGOXK~ZvDNyAs&y9^aa@98i**DY zPmNC|vJ?1EStJ>-Zh9s}62ZaqdiX-XekR2kDgy<2z)K!spJKPS62;>NiTdy;Aav29QBp3{nuB+gYMKQ@J&RY zE*!u@vPIxqBl&?{|FBKq+Y@AbS&;pC3H<=qN#s*m)OWfPdN}0{QdCxA&i`)k?!v@! z)%h3F4&chPTL@qg?;w{34PbFXJ%Cd}pq!3|!u5&fpm|9`X`lrD$Fu?(|DqL^>MJZI z5oEybQ@BhhaXE{?MPtAUfnUWo#4RKwC9l?Opy(&(S{|P)5EEFRcrWODU@iQrrKW00 zx*=i*ROm*ro6T%b;0<25McBhE&Sq3#BD?8JGz1Mh63P{7;BQYfC&?kP1+<1vABTes z*bWQ(g*WVE*`cXymyq3VB2S%6FdFX>-%v_Dy3m*V6gPk4BKABjfhVYZDHt^mxJe4S%RpBe&;@$>mb^BMn6|PDa)~iblQxHP0 zzr&Gap-zSK!H%5{5y7P-IsK?Qb`kiaNp@VcEFVkiW#DDr09wXe5%8-FN2{=F0)9P7 zHgl`0&D;=xA85OHoZmTFm{Vp0qhg^Q9T0ub0VL!K@vLPgMq3<2>d9 zukbjjt?-yckap2v^+bs9DdV{%1Et+(8oucFahPzyQ2j!K2mVi_ffO34e-`kU#J%T& zQEJ50&7H@bs15iFKJ)NOVx zktch>)V)q4A4h4fauI{wVp5&D(bVcxu5>vq!7FUiSJ*@%NPFOvNw`^P{S2M7sZhma z5O;64n)(gJHfF-*TWh3T-7c8e!DfJ7HIG>x<&Hk{6YZ9ypLu|fO*;h-y9l3h!9pYS z1%5Z-Gg+|bKpoePU~q$DmKW?Xd$gVGWlrb@5|idW!O3Xc&;!ezSf~4iPQNsxJKP+2 zjy<4xN8k^zbAg5x4w~u|1|I^^`M1&*a}IaZSE;e(G#iAQ1&uOpqn|OAHhSQn;YIQ^ zK#ExQoBHSxk3%YtMSmUJ95{dz4hv0>5O-b{J(`39e=gE79d4lQh2M(+8jZ$+v#q5& zW>e5~(fpT*YN8<8oCE{l9rL~5y-Q~AL_Ez&Mll@Bxue2LFOvwfSukmaSA+9Y}uq>$ahgs8Ip^N*GIlAbT=#OH7hwKk<=kJlJx6Yn1K7*dspPPkNRfzQ#D*|l1 zHdsk3NZzuvh{IQj+`B|dg4>qXtGG?G;+Y(Jje5t@YrwmNr`Z`$gZC`8AS?P8sf6Sd zh%ryYBO4O&@W^EMpbv-{H;*>K9%{_cBn#oaEa+R|6CsSJq)lo)VAlN1(v9h7HU^aX zOG~d!qtJ>TsOoJBt(L8aR{T5_E#RmN1ioXFIe2Iaoo?&qG{Ytdq9s+WfY;evg5=Nz zW{L&gA^I8c%?TcbE6<)~tFv%{Ur#bfWzp$~2Ejuk@xZtHNC6Qx33v;W&!NZiR)KG0 z6@UZ6VY`6OBl0wi2Da}y1isUz6XC%`wV|9Y0Z7F5o`&2-BR6Y&TuA0o88NV0D9S8&=j=XR>KBeBR40(7Rm;@m76dhRlP)B zWy4K#UaNJD{m-f>*RcDc_o!=yB-Sw;y+v9t;2X#g(it>(Z4~%T?0N8kaI-MPt&I1; z=o4-e__!ZK2Xn*}_uDku(r0UJB18)Mc$l6r-OX4z;51VZQcQi^VU-d#HP^>`1nSeS2<&-0!yGkal*grR zYe9KI#@fsT==3<*XF`t&P)lZV0^KozT&22E2!xBRQqrb3V#br6^0d^Qu*>$x)6D0AF@6tlddWQ{z%E%N(SJi ztMG8lIzmQE_kfdn1ySiNcb>5RKufs!%0#2WNq8z2W4`i!kox#^{5W0RRLk?v@IG>6 zrIU#v@m!lL^49LtirQj;spRJ(5 z(f=B?>LQ1le5tXRkKE)|`=)MMn?+=Xn9LogBlz#D*7PWtOsbdg8t*6fdYVk#?G1~^ z@doYMV9tQ8wGan5`>~Xe6nw&dFKAoq^vsS1dF6Y!)Dh)mltU+5PT0Q*=B;xURbOw= z#?$)=yEEuKZ2wlR&<+zv8G6O_l_|aX#rUu!((_RkP$eWx0Y4_-$C-Q%Es?$!_!ANN zapQV3dFpd#%h{9mBCTn4ig}(CVt*zaKzJM*Da7ySJ} z4YOb?+2YM`66~ktPe<%onqWg(13vtp6D-8eg1&}z zk(}i>=k2Acd9#C{7j$>*bl~1|7JSu`tSZ*OuvpNu*#)ts88!57XC>&qS*qbB0Uu>c zMQ8fMF@eAQ3zQmO5%8<5Pt>HY3HWv99(~6;ydm&E5T1(Z@en_S^ll3LEvElEsZD{u zE%0|(dXZo(Hss36AZt3s>^DU<#=-6VlRE>x;9a2c8qMyPm-f{E> z@gdS@^qm0MmbBZBdWS9u;Om+OLA{ZgM@JKzoI*;q|-*GHm=Zb6Za>~Q9BWvjjE zJ4m(=y>yB$an*Ka2Azwnj;hwVK2q@biFyjrY!IT^7#TNKUWS_ld^5w*h`U9=w~}Jw za~3jGvrYTrr@-G4k;m#%s^PlIU471nnqK>v2N#`a+bQ<2i}Cbj8d$*f1%7uV19S#u zkHAMV0OtW?8SE1>_`;zpj5~I0KKITBN1)mlc{B5)b7zWP>7NX!D z_3#k|3^|$t&f+!4kkcNt4?D{D&;6~@X=w7dhwV7o(g=FhSBQ$xgNE?`~`*ibBB7X zFC4A6`hre-^x7DDWdBKQfDcE}a61xqpL0$9Qm?6Bk`7!Kl;KE?emsjKKiWaFYT7w@ zcaL|_CToGSRiXp$R*jZPEb5=ZhfPQ9hH0byM5~58Vn&VT5!}E_p+~J+!9ZON_5Y&- zBT~`NtWjq>UfmyLI3yt!(L(BIUu|p2bU9^X>VKdM9C*u_9W37Ew8ys)la1*$(yr+1 zO2G8kB&5<(L+Rn3Q!PET3P2QD=|vYTFtiK&JYtxx^TCp^L*P3Jk9QKg1U^Mpl&)N} zPEihI1bh*9k-;-f=m|LAndHGQC|3)RlWm#s%a5@AuyTXM0Gl From e3b3e8d78debdebd3d7ae0e3e569bf7617311007 Mon Sep 17 00:00:00 2001 From: Gwilym-Rutherford Date: Fri, 25 Jul 2025 14:01:36 +0100 Subject: [PATCH 6/8] Add bridge statae subscriber and sensor config publisher --- srunner/autoagents/autoware_agent.py | 17 ++++++++++++-- .../autoagents/autoware_nodes/state_node.py | 23 ++++++++++++++++++- srunner/tools/environment_parser.py | 2 ++ 3 files changed, 39 insertions(+), 3 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index a79f66e..5edc86f 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -9,6 +9,8 @@ from srunner.tools.environment_parser import EnvironmentConfig +from autoware_carla_interface.msg import EgoConfig, SensorConfig + import threading import rclpy import time @@ -52,10 +54,21 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: # 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) - - return + self.autoware_state.ego_config_publisher.publish(ego_config_msg) def set_route(self) -> None: # for every point in the plan diff --git a/srunner/autoagents/autoware_nodes/state_node.py b/srunner/autoagents/autoware_nodes/state_node.py index 6958bc8..8a57b15 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -3,6 +3,7 @@ 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 @@ -12,7 +13,7 @@ class StateNode(Node): motion_state = "/api/motion/state" localize_state = "/api/localization/initialization_state" - ego_config = "/bridge/ego_vehicle/config" + ego_config = "/bridge/ego_vehicle/config" # publish sensor config type: EgoConfig bridge_state = "/bridge/state" def __init__(self, autoware_state: autoware_state.AutowareState) -> None: @@ -31,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 diff --git a/srunner/tools/environment_parser.py b/srunner/tools/environment_parser.py index cf42927..6f470d9 100644 --- a/srunner/tools/environment_parser.py +++ b/srunner/tools/environment_parser.py @@ -43,6 +43,8 @@ def _spawn(self, bp_library, vehicle) -> None: CarlaDataProvider.get_world().spawn_actor(sensor_bp, self.spawn, vehicle) + def serealize(self): + return self.type, self.id class CameraRGB(DefaultSensor): """ From 48e8c8113280910e00c7aa9cc7c5f9e6616a39cc Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Sat, 26 Jul 2025 22:19:40 +0100 Subject: [PATCH 7/8] changes to requirements --- requirements.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 - From ab7f24254bbbeda3015d96032620864a011c93bd Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Sun, 27 Jul 2025 12:43:47 +0100 Subject: [PATCH 8/8] minor changes to autoware agent --- srunner/autoagents/autoware_agent.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 5edc86f..9edf6ec 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -54,18 +54,18 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: # 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)