From e56b8a44d18a3257ef03d7d151b2f177e1be0872 Mon Sep 17 00:00:00 2001 From: Gustavo Silvera Date: Sun, 7 May 2023 12:59:01 -0400 Subject: [PATCH 1/4] add scenario runner patches and automatic sign placement based on route waypoints --- ScenarioRunner/carla_data_provider.py | 2 +- ScenarioRunner/route_parser.py | 363 +++++++++++++++ ScenarioRunner/route_scenario.py | 101 ++++- ScenarioRunner/run_experiment.py | 30 +- ScenarioRunner/scenario_manager.py | 261 +++++++++++ ScenarioRunner/scenario_runner.py | 623 ++++++++++++++++++++++++++ Scripts/Paths/ScenarioRunner.csv | 5 +- 7 files changed, 1347 insertions(+), 38 deletions(-) create mode 100644 ScenarioRunner/route_parser.py create mode 100644 ScenarioRunner/scenario_manager.py create mode 100755 ScenarioRunner/scenario_runner.py diff --git a/ScenarioRunner/carla_data_provider.py b/ScenarioRunner/carla_data_provider.py index a2b0a09..0611923 100644 --- a/ScenarioRunner/carla_data_provider.py +++ b/ScenarioRunner/carla_data_provider.py @@ -691,7 +691,7 @@ def request_new_batch_actors(model, amount, spawn_points, autopilot=False, print("The amount of spawn points is lower than the amount of vehicles spawned") break - if spawn_point and "dreyevr" not in blueprint.id: + if spawn_point: batch.append(SpawnActor(blueprint, spawn_point).then( SetAutopilot(FutureActor, autopilot, CarlaDataProvider._traffic_manager_port))) diff --git a/ScenarioRunner/route_parser.py b/ScenarioRunner/route_parser.py new file mode 100644 index 0000000..88850f9 --- /dev/null +++ b/ScenarioRunner/route_parser.py @@ -0,0 +1,363 @@ +#!/usr/bin/env python + +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Module used to parse all the route and scenario configuration parameters. +""" + +import json +import math +import xml.etree.ElementTree as ET + +import carla +from agents.navigation.local_planner import RoadOption +from srunner.scenarioconfigs.route_scenario_configuration import RouteScenarioConfiguration + +# TODO check this threshold, it could be a bit larger but not so large that we cluster scenarios. +TRIGGER_THRESHOLD = 2.0 # Threshold to say if a trigger position is new or repeated, works for matching positions +TRIGGER_ANGLE_THRESHOLD = 10 # Threshold to say if two angles can be considering matching when matching transforms. + + +class RouteParser(object): + + """ + Pure static class used to parse all the route and scenario configuration parameters. + """ + + @staticmethod + def parse_annotations_file(annotation_filename): + """ + Return the annotations of which positions where the scenarios are going to happen. + :param annotation_filename: the filename for the anotations file + :return: + """ + + with open(annotation_filename, 'r', encoding='utf-8') as f: + annotation_dict = json.loads(f.read()) + + final_dict = {} + + for town_dict in annotation_dict['available_scenarios']: + final_dict.update(town_dict) + + return final_dict # the file has a current maps name that is an one element vec + + @staticmethod + def parse_routes_file(route_filename, scenario_file, single_route=None): + """ + Returns a list of route elements. + :param route_filename: the path to a set of routes. + :param single_route: If set, only this route shall be returned + :return: List of dicts containing the waypoints, id and town of the routes + """ + + list_route_descriptions = [] + tree = ET.parse(route_filename) + for route in tree.iter("route"): + + route_id = route.attrib['id'] + if single_route and route_id != single_route: + continue + + new_config = RouteScenarioConfiguration() + new_config.town = route.attrib['town'] + new_config.name = "RouteScenario_{}".format(route_id) + new_config.weather = RouteParser.parse_weather(route) + new_config.scenario_file = scenario_file + new_config.scenario_number = single_route + + waypoint_list = [] # the list of waypoints that can be found on this route + for waypoint in route.iter('waypoint'): + waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']), + y=float(waypoint.attrib['y']), + z=float(waypoint.attrib['z']))) + + new_config.trajectory = waypoint_list + + list_route_descriptions.append(new_config) + + return list_route_descriptions + + @staticmethod + def parse_weather(route): + """ + Returns a carla.WeatherParameters with the corresponding weather for that route. If the route + has no weather attribute, the default one is triggered. + """ + + route_weather = route.find("weather") + if route_weather is None: + + weather = carla.WeatherParameters(sun_altitude_angle=70) + + else: + weather = carla.WeatherParameters() + for weather_attrib in route.iter("weather"): + + if 'cloudiness' in weather_attrib.attrib: + weather.cloudiness = float(weather_attrib.attrib['cloudiness']) + if 'precipitation' in weather_attrib.attrib: + weather.precipitation = float(weather_attrib.attrib['precipitation']) + if 'precipitation_deposits' in weather_attrib.attrib: + weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits']) + if 'wind_intensity' in weather_attrib.attrib: + weather.wind_intensity = float(weather_attrib.attrib['wind_intensity']) + if 'sun_azimuth_angle' in weather_attrib.attrib: + weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle']) + if 'sun_altitude_angle' in weather_attrib.attrib: + weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle']) + if 'wetness' in weather_attrib.attrib: + weather.wetness = float(weather_attrib.attrib['wetness']) + if 'fog_distance' in weather_attrib.attrib: + weather.fog_distance = float(weather_attrib.attrib['fog_distance']) + if 'fog_density' in weather_attrib.attrib: + weather.fog_density = float(weather_attrib.attrib['fog_density']) + + return weather + + @staticmethod + def parse_direction_signs_file(signs_filename, single_route=None): + """ + Return the annotations of where to place which signs to give human users + directions when driving in the simulator. + :param signs_filename: the filename for the sign placement data file + :param single_route: the route id (int) to specify which sign route + :return: dictionary of sign types and transforms + """ + import os + if not os.path.exists(signs_filename): + return None + with open(signs_filename, 'r') as f: + signs_dict = json.loads(f.read()) + + routes_dict = signs_dict['available_routes'][0] + + for route_name in routes_dict.keys(): + route_num = route_name.strip("Route") + if int(route_num) == int(single_route): + return routes_dict[route_name] + + return None + + @staticmethod + def check_trigger_position(new_trigger, existing_triggers): + """ + Check if this trigger position already exists or if it is a new one. + :param new_trigger: + :param existing_triggers: + :return: + """ + + for trigger_id in existing_triggers.keys(): + trigger = existing_triggers[trigger_id] + dx = trigger['x'] - new_trigger['x'] + dy = trigger['y'] - new_trigger['y'] + distance = math.sqrt(dx * dx + dy * dy) + + dyaw = (trigger['yaw'] - new_trigger['yaw']) % 360 + if distance < TRIGGER_THRESHOLD \ + and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)): + return trigger_id + + return None + + @staticmethod + def convert_waypoint_float(waypoint): + """ + Convert waypoint values to float + """ + waypoint['x'] = float(waypoint['x']) + waypoint['y'] = float(waypoint['y']) + waypoint['z'] = float(waypoint['z']) + waypoint['yaw'] = float(waypoint['yaw']) + + + @staticmethod + def convert_dict2transform(waypoint): + """ + Convert waypoint values to Carla.transform + """ + transform = carla.Transform() + transform.location.x = float(waypoint['x']) + transform.location.y = float(waypoint['y']) + transform.location.z = float(waypoint['z']) + transform.rotation.yaw = float(waypoint['yaw']) + return transform + + @staticmethod + def match_world_location_to_route(world_location, route_description): + """ + We match this location to a given route. + world_location: + route_description: + """ + def match_waypoints(waypoint1, wtransform): + """ + Check if waypoint1 and wtransform are similar + """ + dx = float(waypoint1['x']) - wtransform.location.x + dy = float(waypoint1['y']) - wtransform.location.y + dz = float(waypoint1['z']) - wtransform.location.z + dpos = math.sqrt(dx * dx + dy * dy + dz * dz) + + dyaw = (float(waypoint1['yaw']) - wtransform.rotation.yaw) % 360 + + return dpos < TRIGGER_THRESHOLD \ + and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)) + + match_position = 0 + # TODO this function can be optimized to run on Log(N) time + for route_waypoint in route_description: + if match_waypoints(world_location, route_waypoint[0]): + return match_position + match_position += 1 + + return None + + @staticmethod + def get_scenario_type(scenario, match_position, trajectory): + """ + Some scenarios have different types depending on the route. + :param scenario: the scenario name + :param match_position: the matching position for the scenarion + :param trajectory: the route trajectory the ego is following + :return: tag representing this subtype + + Also used to check which are not viable (Such as an scenario + that triggers when turning but the route doesnt') + WARNING: These tags are used at: + - VehicleTurningRoute + - SignalJunctionCrossingRoute + and changes to these tags will affect them + """ + + def check_this_waypoint(tuple_wp_turn): + """ + Decides whether or not the waypoint will define the scenario behavior + """ + if RoadOption.LANEFOLLOW == tuple_wp_turn[1]: + return False + elif RoadOption.CHANGELANELEFT == tuple_wp_turn[1]: + return False + elif RoadOption.CHANGELANERIGHT == tuple_wp_turn[1]: + return False + return True + + # Unused tag for the rest of scenarios, + # can't be None as they are still valid scenarios + subtype = 'valid' + + if scenario == 'Scenario4': + for tuple_wp_turn in trajectory[match_position:]: + if check_this_waypoint(tuple_wp_turn): + if RoadOption.LEFT == tuple_wp_turn[1]: + subtype = 'S4left' + elif RoadOption.RIGHT == tuple_wp_turn[1]: + subtype = 'S4right' + else: + subtype = None + break # Avoid checking all of them + subtype = None + + if scenario == 'Scenario7': + for tuple_wp_turn in trajectory[match_position:]: + if check_this_waypoint(tuple_wp_turn): + if RoadOption.LEFT == tuple_wp_turn[1]: + subtype = 'S7left' + elif RoadOption.RIGHT == tuple_wp_turn[1]: + subtype = 'S7right' + elif RoadOption.STRAIGHT == tuple_wp_turn[1]: + subtype = 'S7opposite' + else: + subtype = None + break # Avoid checking all of them + subtype = None + + if scenario == 'Scenario8': + for tuple_wp_turn in trajectory[match_position:]: + if check_this_waypoint(tuple_wp_turn): + if RoadOption.LEFT == tuple_wp_turn[1]: + subtype = 'S8left' + else: + subtype = None + break # Avoid checking all of them + subtype = None + + if scenario == 'Scenario9': + for tuple_wp_turn in trajectory[match_position:]: + if check_this_waypoint(tuple_wp_turn): + if RoadOption.RIGHT == tuple_wp_turn[1]: + subtype = 'S9right' + else: + subtype = None + break # Avoid checking all of them + subtype = None + + return subtype + + @staticmethod + def scan_route_for_scenarios(route_name, trajectory, world_annotations): + """ + Just returns a plain list of possible scenarios that can happen in this route by matching + the locations from the scenario into the route description + + :return: A list of scenario definitions with their correspondent parameters + """ + + # the triggers dictionaries: + existent_triggers = {} + # We have a table of IDs and trigger positions associated + possible_scenarios = {} + + # Keep track of the trigger ids being added + latest_trigger_id = 0 + + for town_name in world_annotations.keys(): + if town_name != route_name: + continue + + scenarios = world_annotations[town_name] + for scenario in scenarios: # For each existent scenario + if "scenario_type" not in scenario: + break + scenario_name = scenario["scenario_type"] + for event in scenario["available_event_configurations"]: + waypoint = event['transform'] # trigger point of this scenario + RouteParser.convert_waypoint_float(waypoint) + # We match trigger point to the route, now we need to check if the route affects + match_position = RouteParser.match_world_location_to_route( + waypoint, trajectory) + if match_position is not None: + # We match a location for this scenario, create a scenario object so this scenario + # can be instantiated later + + if 'other_actors' in event: + other_vehicles = event['other_actors'] + else: + other_vehicles = None + scenario_subtype = RouteParser.get_scenario_type(scenario_name, match_position, + trajectory) + if scenario_subtype is None: + continue + scenario_description = { + 'name': scenario_name, + 'other_actors': other_vehicles, + 'trigger_position': waypoint, + 'scenario_type': scenario_subtype, # some scenarios have route dependent configs + } + + trigger_id = RouteParser.check_trigger_position(waypoint, existent_triggers) + if trigger_id is None: + # This trigger does not exist create a new reference on existent triggers + existent_triggers.update({latest_trigger_id: waypoint}) + # Update a reference for this trigger on the possible scenarios + possible_scenarios.update({latest_trigger_id: []}) + trigger_id = latest_trigger_id + # Increment the latest trigger + latest_trigger_id += 1 + + possible_scenarios[trigger_id].append(scenario_description) + + return possible_scenarios, existent_triggers diff --git a/ScenarioRunner/route_scenario.py b/ScenarioRunner/route_scenario.py index 7771e33..6488f4c 100644 --- a/ScenarioRunner/route_scenario.py +++ b/ScenarioRunner/route_scenario.py @@ -48,8 +48,7 @@ RunningStopTest, ActorSpeedAboveThresholdTest) -# DReyeVR utils -from DReyeVR_utils import find_ego_vehicle +from examples.DReyeVR_utils import find_ego_vehicle SECONDS_GIVEN_PER_METERS = 0.4 @@ -206,6 +205,8 @@ def _update_route(self, world, config, debug_mode): if debug_mode: self._draw_waypoints(world, self.route, vertical_shift=1.0, persistency=50000.0) + self._setup_nav_signs(self.route) + def _initialize_ego_vehicle_DReyeVR(self, ego_vehicle): """ Set/Update the start position of the ego_vehicle (instead of _update_ego_vehicle below) @@ -276,6 +277,76 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=0.2, color=carla.Color(255, 0, 0), life_time=persistency) + def _get_valid_sign_transform(self, wp_transform): + # use the original waypoint location as long as it is on a sidewalk + _wp = CarlaDataProvider.get_map().get_waypoint(wp_transform.location, + project_to_road=False, + lane_type=carla.LaneType.Any) + if _wp is None: + return None + # find the first non-road waypoint so our drivers can read it (with a limit) + max_tries:int = 100 + while max_tries > 0 and _wp.lane_type not in [carla.LaneType.Sidewalk, carla.LaneType.Shoulder]: + max_tries -= 1 + right_wp = _wp.get_right_lane() + if right_wp is not None: + _wp = right_wp + else: + continue # skip this one + # carla transforms don't have a native assignment operator :/ + t = carla.Transform(_wp.transform.location, _wp.transform.rotation) + if max_tries == 0: # didn't find a sidewalk, so push it slightly right by the road lane width + push = t.get_right_vector() * _wp.lane_width * 1.1 # 10% more just to be safe + # carla locations don't have a native += operator + t.location = carla.Location(x=t.location.x + push.x, + y=t.location.y + push.y, + z=t.location.z + push.z) + t.location.z += 2.0 # go up slightly (for the height of the road sign) + t.rotation.yaw += 90.0 # rotate another 90 degrees + return t + + def _setup_nav_signs(self, waypoints: list): + """ + Draw the signs along the waypoints of a route automatically + """ + prev_sign_type = None # only request new actor if nav type changes + for i, w in enumerate(waypoints): + wp_transform, sign_type = w + if prev_sign_type is not None and prev_sign_type == sign_type: + continue # only spawn a sign when the waypoint type changes + prev_sign_type = sign_type + sign_transform = self._get_valid_sign_transform(wp_transform) + if sign_transform is None: + continue # invalid + # now we can finally go about spawning the sign in this location + DReyeVR_sign_type: str = "dreyevr_sign_straight" + if sign_type == RoadOption.LEFT: + DReyeVR_sign_type = "dreyevr_sign_left" + elif sign_type == RoadOption.RIGHT: + DReyeVR_sign_type = "dreyevr_sign_right" + elif sign_type == RoadOption.CHANGELANELEFT: + continue + elif sign_type == RoadOption.CHANGELANERIGHT: + continue + elif sign_type == RoadOption.STRAIGHT: + DReyeVR_sign_type = "dreyevr_sign_straight" + else: + continue + sign_type: str = f"static.prop.{DReyeVR_sign_type}" + print(f"Spawning ({sign_type}) sign {i} at {sign_transform}") + traffic_sign = CarlaDataProvider.request_new_actor(sign_type, + sign_transform, + rolename='navigation_sign') + # plot the final goal waypoint (at the end) + wp_transform_final = waypoints[-1][0] + goal_sign_transform = self._get_valid_sign_transform(wp_transform_final) + if goal_sign_transform is not None: + sign_type: str = f"static.prop.dreyevr_sign_goal" + print(f"Spawning ({sign_type}) sign {len(waypoints) - 1} at {goal_sign_transform}") + traffic_sign = CarlaDataProvider.request_new_actor(sign_type, + goal_sign_transform, + rolename='navigation_sign') + def _scenario_sampling(self, potential_scenarios_definitions, random_seed=0): """ The function used to sample the scenarios that are going to happen for this route. @@ -411,16 +482,16 @@ def _initialize_actors(self, config): # Create the background activity of the route town_amount = { - 'Town01': 120, - 'Town02': 100, - 'Town03': 120, - 'Town04': 200, - 'Town05': 120, - 'Town06': 150, - 'Town07': 110, - 'Town08': 180, - 'Town09': 300, - 'Town10': 120, + 'Town01': 0, + 'Town02': 0, + 'Town03': 0, + 'Town04': 0, + 'Town05': 0, + 'Town06': 0, + 'Town07': 0, + 'Town08': 0, + 'Town09': 0, + 'Town10': 0, } amount = town_amount[config.town] if config.town in town_amount else 0 @@ -441,12 +512,6 @@ def _initialize_actors(self, config): # Add all the actors of the specific scenarios to self.other_actors for scenario in self.list_scenarios: self.other_actors.extend(scenario.other_actors) - - for i, actor in enumerate(self.other_actors): - if actor.type_id == self.ego_vehicle.type_id: - self.other_actors.pop(i) # removes reference to any DReyeVR ego vehicles that might've been spawned - # NOTE: even if it thinks it spawned DReyeVR ego vehicles, it cant. Only one can be spawned by the - # carla ActorRegistry (See RegisterActor) def _create_behavior(self): """ diff --git a/ScenarioRunner/run_experiment.py b/ScenarioRunner/run_experiment.py index 5e64247..32f6a33 100755 --- a/ScenarioRunner/run_experiment.py +++ b/ScenarioRunner/run_experiment.py @@ -10,20 +10,15 @@ import argparse try: - sys.path.append( - glob.glob( - "../carla/dist/carla-*%d.%d-%s.egg" - % ( - sys.version_info.major, - sys.version_info.minor, - "win-amd64" if os.name == "nt" else "linux-x86_64", - ) - )[0] - ) + CARLA_ROOT: str = os.getenv("CARLA_ROOT") + egg_dir = os.path.join(CARLA_ROOT, "PythonAPI", "carla", "dist") + sys.path.append(glob.glob(os.path.join(egg_dir, f"carla-*.egg"))[0]) + sys.path.append(os.path.join(CARLA_ROOT, "PythonAPI", "examples")) except IndexError: - pass + print(f"Unable to find Carla PythonAPI file in {egg_dir}") import carla + from scenario_runner import ScenarioRunner recorder_file = None @@ -52,7 +47,8 @@ def run_schematic(argparser, scenario_runner_instance): # can be completely avoided if --visualize is False # NOTE: this import uses export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/examples - from schematic_mode import schematic_run + raise NotImplementedError + from dreyevr.examples.schematic_mode import schematic_run # for full definitions of these args see no_rendering_mode.py args = argparser.parse_known_args( @@ -77,19 +73,17 @@ def run_schematic(argparser, scenario_runner_instance): def start_recording(client, args, scenario_runner_instance): wait_until_SR_loaded(scenario_runner_instance) + time_str: str = datetime.now().strftime("%m_%d_%Y_%H_%M_%S") + filename: str = f"exp_{args.title}_{time_str}.rec" - now = datetime.now() - time_str = now.strftime("%m_%d_%Y_%H_%M_%S") - filename = "exp_%s_%s.rec" % (args.title, time_str) - - global recorder_file + global recorder_file # to "return" from this thread recorder_file = client.start_recorder(filename) print("Recording on file: %s" % recorder_file) def stop_recording(client): global recorder_file - print("Stopping recording, file saved to %s" % recorder_file) + print(f"Stopping recording, file saved to \"{recorder_file}\"") client.stop_recorder() diff --git a/ScenarioRunner/scenario_manager.py b/ScenarioRunner/scenario_manager.py new file mode 100644 index 0000000..2f07062 --- /dev/null +++ b/ScenarioRunner/scenario_manager.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python + +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +This module provides the ScenarioManager implementation. +It must not be modified and is for reference only! +""" + +from __future__ import print_function +import sys +import time + +import py_trees + +from srunner.autoagents.agent_wrapper import AgentWrapper +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.result_writer import ResultOutputProvider +from srunner.scenariomanager.timer import GameTime +from srunner.scenariomanager.watchdog import Watchdog + + +class ScenarioManager(object): + + """ + Basic scenario manager class. This class holds all functionality + required to start, and analyze a scenario. + + The user must not modify this class. + + To use the ScenarioManager: + 1. Create an object via manager = ScenarioManager() + 2. Load a scenario via manager.load_scenario() + 3. Trigger the execution of the scenario manager.run_scenario() + This function is designed to explicitly control start and end of + the scenario execution + 4. Trigger a result evaluation with manager.analyze_scenario() + 5. If needed, cleanup with manager.stop_scenario() + """ + + def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0): + """ + Setups up the parameters, which will be filled at load_scenario() + + """ + self.scenario = None + self.scenario_tree = None + self.scenario_class = None + self.ego_vehicles = None + self.other_actors = None + + self._debug_mode = debug_mode + self._agent = None + self._sync_mode = sync_mode + self._watchdog = None + self._timeout = timeout + + self._running = False + self._timestamp_last_run = 0.0 + self.scenario_duration_system = 0.0 + self.scenario_duration_game = 0.0 + self.start_system_time = None + self.end_system_time = None + + def _reset(self): + """ + Reset all parameters + """ + self._running = False + self._timestamp_last_run = 0.0 + self.scenario_duration_system = 0.0 + self.scenario_duration_game = 0.0 + self.start_system_time = None + self.end_system_time = None + GameTime.restart() + + def cleanup(self): + """ + This function triggers a proper termination of a scenario + """ + + if self._watchdog is not None: + self._watchdog.stop() + self._watchdog = None + + if self.scenario is not None: + self.scenario.terminate() + + if self._agent is not None: + self._agent.cleanup() + self._agent = None + + CarlaDataProvider.cleanup() + + def load_scenario(self, scenario, agent=None, route_id=None): + """ + Load a new scenario + """ + self._reset() + self._agent = AgentWrapper(agent) if agent else None + if self._agent is not None: + self._sync_mode = True + self.scenario_class = scenario + self.scenario = scenario.scenario + self.scenario_tree = self.scenario.scenario_tree + self.ego_vehicles = scenario.ego_vehicles + self.other_actors = scenario.other_actors + + # To print the scenario tree uncomment the next line + # py_trees.display.render_dot_tree(self.scenario_tree) + + if self._agent is not None: + self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode) + + self.route_id = route_id + self.load_DReyeVR_signs() + + def load_DReyeVR_signs(self): + # retrieve and place direction signs for route if they exist + from srunner.tools.route_parser import RouteParser + + import os + signs_file = os.path.join(os.getenv("SCENARIO_RUNNER_ROOT"), "srunner", "data", "all_routes_signs.json") + route_signs_list = RouteParser.parse_direction_signs_file(signs_file, self.route_id) + + if route_signs_list is None: + print(f"No route_signs_dict (json) for route {self.route_id} in \"{signs_file}\"") + return + + # place directional signs from json in world + assert len(route_signs_list) == 1 + route_signs_dict = route_signs_list[0] + print("Spawning DReyeVR signs for route", self.route_id) + for sign in route_signs_dict['sign_configurations']: + sign_type = sign['type'] + sign_waypoint = sign['transform'] + sign_transform = RouteParser.convert_dict2transform(sign_waypoint) + print(f"Loading \"{sign_type}\" @ {sign_transform}") + # TODO: keep track of these signs somewhere? + traffic_sign = CarlaDataProvider.request_new_actor(sign_type, sign_transform, rolename='navigation_sign') + + def run_scenario(self): + """ + Trigger the start of the scenario and wait for it to finish/fail + """ + print("ScenarioManager: Running scenario {}".format(self.scenario_tree.name)) + self.start_system_time = time.time() + start_game_time = GameTime.get_time() + + self._watchdog = Watchdog(float(self._timeout)) + self._watchdog.start() + self._running = True + + while self._running: + timestamp = None + world = CarlaDataProvider.get_world() + if world: + snapshot = world.get_snapshot() + if snapshot: + timestamp = snapshot.timestamp + if timestamp: + self._tick_scenario(timestamp) + + self.cleanup() + + self.end_system_time = time.time() + end_game_time = GameTime.get_time() + + self.scenario_duration_system = self.end_system_time - \ + self.start_system_time + self.scenario_duration_game = end_game_time - start_game_time + + if self.scenario_tree.status == py_trees.common.Status.FAILURE: + print("ScenarioManager: Terminated due to failure") + + def _tick_scenario(self, timestamp): + """ + Run next tick of scenario and the agent. + If running synchornously, it also handles the ticking of the world. + """ + + if self._timestamp_last_run < timestamp.elapsed_seconds and self._running: + self._timestamp_last_run = timestamp.elapsed_seconds + + self._watchdog.update() + + if self._debug_mode: + print("\n--------- Tick ---------\n") + + # Update game time and actor information + GameTime.on_carla_tick(timestamp) + CarlaDataProvider.on_carla_tick() + + if self._agent is not None: + ego_action = self._agent() # pylint: disable=not-callable + + if self._agent is not None: + self.ego_vehicles[0].apply_control(ego_action) + + # Tick scenario + self.scenario_tree.tick_once() + + if self._debug_mode: + print("\n") + py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) + sys.stdout.flush() + + if self.scenario_tree.status != py_trees.common.Status.RUNNING: + self._running = False + + if self._sync_mode and self._running and self._watchdog.get_status(): + CarlaDataProvider.get_world().tick() + + def get_running_status(self): + """ + returns: + bool: False if watchdog exception occured, True otherwise + """ + return self._watchdog.get_status() + + def stop_scenario(self): + """ + This function is used by the overall signal handler to terminate the scenario execution + """ + self._running = False + + def analyze_scenario(self, stdout, filename, junit, json): + """ + This function is intended to be called from outside and provide + the final statistics about the scenario (human-readable, in form of a junit + report, etc.) + """ + + failure = False + timeout = False + result = "SUCCESS" + + if self.scenario.test_criteria is None: + print("Nothing to analyze, this scenario has no criteria") + return True + + for criterion in self.scenario.get_criteria(): + if (not criterion.optional and + criterion.test_status != "SUCCESS" and + criterion.test_status != "ACCEPTABLE"): + failure = True + result = "FAILURE" + elif criterion.test_status == "ACCEPTABLE": + result = "ACCEPTABLE" + + if self.scenario.timeout_node.timeout and not failure: + timeout = True + result = "TIMEOUT" + + output = ResultOutputProvider(self, result, stdout, filename, junit, json) + output.write() + + return failure or timeout diff --git a/ScenarioRunner/scenario_runner.py b/ScenarioRunner/scenario_runner.py new file mode 100755 index 0000000..bbcd5a1 --- /dev/null +++ b/ScenarioRunner/scenario_runner.py @@ -0,0 +1,623 @@ +#!/usr/bin/env python + +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Welcome to CARLA scenario_runner + +This is the main script to be executed when running a scenario. +It loads the scenario configuration, loads the scenario and manager, +and finally triggers the scenario execution. +""" + +from __future__ import print_function + +import glob +import traceback +import argparse +from argparse import RawTextHelpFormatter +from datetime import datetime +from distutils.version import LooseVersion +import importlib +import inspect +import os +import signal +import sys +import time +import json +import pkg_resources + +sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI")) # for Carla stuff +sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI", "carla")) # for agents +sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI", "dreyevr")) # for DReyeVR stuff + +import carla + +from srunner.scenarioconfigs.openscenario_configuration import OpenScenarioConfiguration +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenario_manager import ScenarioManager +from srunner.scenarios.open_scenario import OpenScenario +from srunner.scenarios.route_scenario import RouteScenario +from srunner.tools.scenario_parser import ScenarioConfigurationParser +from srunner.tools.route_parser import RouteParser + +# Version of scenario_runner +VERSION = '0.9.13' + + +class ScenarioRunner(object): + + """ + This is the core scenario runner module. It is responsible for + running (and repeating) a single scenario or a list of scenarios. + + Usage: + scenario_runner = ScenarioRunner(args) + scenario_runner.run() + del scenario_runner + """ + + ego_vehicles = [] + + # Tunable parameters + client_timeout = 10.0 # in seconds + wait_for_world = 20.0 # in seconds + frame_rate = 20.0 # in Hz + + # CARLA world and scenario handlers + world = None + manager = None + + finished = False + + additional_scenario_module = None + + agent_instance = None + module_agent = None + + def __init__(self, args): + """ + Setup CARLA client and world + Setup ScenarioManager + """ + self._args = args + + if args.timeout: + self.client_timeout = float(args.timeout) + + # First of all, we need to create the client that will send the requests + # to the simulator. Here we'll assume the simulator is accepting + # requests in the localhost at port 2000. + self.client = carla.Client(args.host, int(args.port)) + self.client.set_timeout(self.client_timeout) + + dist = pkg_resources.get_distribution("carla") + if LooseVersion(dist.version) < LooseVersion('0.9.12'): + raise ImportError("CARLA version 0.9.12 or newer required. CARLA version found: {}".format(dist)) + + # Load agent if requested via command line args + # If something goes wrong an exception will be thrown by importlib (ok here) + if self._args.agent is not None: + module_name = os.path.basename(args.agent).split('.')[0] + sys.path.insert(0, os.path.dirname(args.agent)) + self.module_agent = importlib.import_module(module_name) + + # Create the ScenarioManager + self.manager = ScenarioManager(self._args.debug, self._args.sync, self._args.timeout) + + # Create signal handler for SIGINT + self._shutdown_requested = False + if sys.platform != 'win32': + signal.signal(signal.SIGHUP, self._signal_handler) + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + + self._start_wall_time = datetime.now() + + def destroy(self): + """ + Cleanup and delete actors, ScenarioManager and CARLA world + """ + + self._cleanup() + if self.manager is not None: + del self.manager + if self.world is not None: + del self.world + if self.client is not None: + del self.client + + def _signal_handler(self, signum, frame): + """ + Terminate scenario ticking when receiving a signal interrupt + """ + self._shutdown_requested = True + if self.manager: + self.manager.stop_scenario() + + def _get_scenario_class_or_fail(self, scenario): + """ + Get scenario class by scenario name + If scenario is not supported or not found, exit script + """ + + # Path of all scenario at "srunner/scenarios" folder + the path of the additional scenario argument + scenarios_list = glob.glob("{}/srunner/scenarios/*.py".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) + scenarios_list.append(self._args.additionalScenario) + + for scenario_file in scenarios_list: + + # Get their module + module_name = os.path.basename(scenario_file).split('.')[0] + sys.path.insert(0, os.path.dirname(scenario_file)) + scenario_module = importlib.import_module(module_name) + + # And their members of type class + for member in inspect.getmembers(scenario_module, inspect.isclass): + if scenario in member: + return member[1] + + # Remove unused Python paths + sys.path.pop(0) + + print("Scenario '{}' not supported ... Exiting".format(scenario)) + sys.exit(-1) + + def _cleanup(self): + """ + Remove and destroy all actors + """ + if self.finished: + return + + self.finished = True + + # Simulation still running and in synchronous mode? + if self.world is not None and self._args.sync: + try: + # Reset to asynchronous mode + settings = self.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + self.client.get_trafficmanager(int(self._args.trafficManagerPort)).set_synchronous_mode(False) + except RuntimeError: + sys.exit(-1) + + self.manager.cleanup() + + CarlaDataProvider.cleanup() + + for i, _ in enumerate(self.ego_vehicles): + if self.ego_vehicles[i]: + if not self._args.waitForEgo and self.ego_vehicles[i] is not None and self.ego_vehicles[i].is_alive: + print("Destroying ego vehicle {}".format(self.ego_vehicles[i].id)) + self.ego_vehicles[i].destroy() + self.ego_vehicles[i] = None + self.ego_vehicles = [] + + if self.agent_instance: + self.agent_instance.destroy() + self.agent_instance = None + + def _prepare_ego_vehicles(self, ego_vehicles): + """ + Spawn or update the ego vehicles + """ + + if not self._args.waitForEgo: + for vehicle in ego_vehicles: + self.ego_vehicles.append(CarlaDataProvider.request_new_actor(vehicle.model, + vehicle.transform, + vehicle.rolename, + color=vehicle.color, + actor_category=vehicle.category)) + else: + ego_vehicle_missing = True + while ego_vehicle_missing: + self.ego_vehicles = [] + ego_vehicle_missing = False + for ego_vehicle in ego_vehicles: + ego_vehicle_found = False + carla_vehicles = CarlaDataProvider.get_world().get_actors().filter('vehicle.*') + for carla_vehicle in carla_vehicles: + if carla_vehicle.attributes['role_name'] == ego_vehicle.rolename: + ego_vehicle_found = True + self.ego_vehicles.append(carla_vehicle) + break + if not ego_vehicle_found: + ego_vehicle_missing = True + break + + for i, _ in enumerate(self.ego_vehicles): + self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) + CarlaDataProvider.register_actor(self.ego_vehicles[i]) + + # sync state + if CarlaDataProvider.is_sync_mode(): + self.world.tick() + else: + self.world.wait_for_tick() + + def _analyze_scenario(self, config): + """ + Provide feedback about success/failure of a scenario + """ + + # Create the filename + current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) + junit_filename = None + json_filename = None + config_name = config.name + if self._args.outputDir != '': + config_name = os.path.join(self._args.outputDir, config_name) + + if self._args.junit: + junit_filename = config_name + current_time + ".xml" + if self._args.json: + json_filename = config_name + current_time + ".json" + filename = None + if self._args.file: + filename = config_name + current_time + ".txt" + + if not self.manager.analyze_scenario(self._args.output, filename, junit_filename, json_filename): + print("All scenario tests were passed successfully!") + else: + print("Not all scenario tests were successful") + if not (self._args.output or filename or junit_filename): + print("Please run with --output for further information") + + def _record_criteria(self, criteria, name): + """ + Filter the JSON serializable attributes of the criterias and + dumps them into a file. This will be used by the metrics manager, + in case the user wants specific information about the criterias. + """ + file_name = name[:-4] + ".json" + + # Filter the attributes that aren't JSON serializable + with open('temp.json', 'w', encoding='utf-8') as fp: + + criteria_dict = {} + for criterion in criteria: + + criterion_dict = criterion.__dict__ + criteria_dict[criterion.name] = {} + + for key in criterion_dict: + if key != "name": + try: + key_dict = {key: criterion_dict[key]} + json.dump(key_dict, fp, sort_keys=False, indent=4) + criteria_dict[criterion.name].update(key_dict) + except TypeError: + pass + + os.remove('temp.json') + + # Save the criteria dictionary into a .json file + with open(file_name, 'w', encoding='utf-8') as fp: + json.dump(criteria_dict, fp, sort_keys=False, indent=4) + + def _load_and_wait_for_world(self, town, ego_vehicles=None): + """ + Load a new CARLA world and provide data to CarlaDataProvider + """ + + if self._args.reloadWorld: + self.world = self.client.load_world(town) + else: + # if the world should not be reloaded, wait at least until all ego vehicles are ready + ego_vehicle_found = False + if self._args.waitForEgo: + while not ego_vehicle_found and not self._shutdown_requested: + vehicles = self.client.get_world().get_actors().filter('vehicle.*') + for ego_vehicle in ego_vehicles: + ego_vehicle_found = False + for vehicle in vehicles: + if vehicle.attributes['role_name'] == ego_vehicle.rolename: + ego_vehicle_found = True + break + if not ego_vehicle_found: + print("Not all ego vehicles ready. Waiting ... ") + time.sleep(1) + break + + self.world = self.client.get_world() + + if self._args.sync: + settings = self.world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1.0 / self.frame_rate + self.world.apply_settings(settings) + + CarlaDataProvider.set_client(self.client) + CarlaDataProvider.set_world(self.world) + + # Wait for the world to be ready + if CarlaDataProvider.is_sync_mode(): + self.world.tick() + else: + self.world.wait_for_tick() + + map_name = CarlaDataProvider.get_map().name.split('/')[-1] + if map_name not in (town, "OpenDriveMap"): + print("The CARLA server uses the wrong map: {}".format(map_name)) + print("This scenario requires to use map: {}".format(town)) + return False + + return True + + def _load_and_run_scenario(self, config): + """ + Load and run the scenario given by config + """ + result = False + if not self._load_and_wait_for_world(config.town, config.ego_vehicles): + self._cleanup() + return False + + if self._args.agent: + agent_class_name = self.module_agent.__name__.title().replace('_', '') + try: + self.agent_instance = getattr(self.module_agent, agent_class_name)(self._args.agentConfig) + config.agent = self.agent_instance + except Exception as e: # pylint: disable=broad-except + traceback.print_exc() + print("Could not setup required agent due to {}".format(e)) + self._cleanup() + return False + + CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort)) + tm = self.client.get_trafficmanager(int(self._args.trafficManagerPort)) + tm.set_random_device_seed(int(self._args.trafficManagerSeed)) + if self._args.sync: + tm.set_synchronous_mode(True) + + # Prepare scenario + print("Preparing scenario: " + config.name) + try: + self._prepare_ego_vehicles(config.ego_vehicles) + if self._args.openscenario: + scenario = OpenScenario(world=self.world, + ego_vehicles=self.ego_vehicles, + config=config, + config_file=self._args.openscenario, + timeout=100000) + elif self._args.route: + scenario = RouteScenario(world=self.world, + config=config, + debug_mode=self._args.debug) + else: + scenario_class = self._get_scenario_class_or_fail(config.type) + scenario = scenario_class(self.world, + self.ego_vehicles, + config, + self._args.randomize, + self._args.debug) + except Exception as exception: # pylint: disable=broad-except + print("The scenario cannot be loaded") + traceback.print_exc() + print(exception) + self._cleanup() + return False + + try: + if self._args.record: + recorder_name = "{}/{}/{}.log".format( + os.getenv('SCENARIO_RUNNER_ROOT', "./"), self._args.record, config.name) + self.client.start_recorder(recorder_name, True) + + # Load scenario and run it + self.manager.load_scenario(scenario, self.agent_instance, config.scenario_number) + self.manager.run_scenario() + + # Provide outputs if required + self._analyze_scenario(config) + + # Remove all actors, stop the recorder and save all criterias (if needed) + scenario.remove_all_actors() + if self._args.record: + self.client.stop_recorder() + self._record_criteria(self.manager.scenario.get_criteria(), recorder_name) + + result = True + + except Exception as e: # pylint: disable=broad-except + traceback.print_exc() + print(e) + result = False + + self._cleanup() + return result + + def _run_scenarios(self): + """ + Run conventional scenarios (e.g. implemented using the Python API of ScenarioRunner) + """ + result = False + + # Load the scenario configurations provided in the config file + scenario_configurations = ScenarioConfigurationParser.parse_scenario_configuration( + self._args.scenario, + self._args.configFile) + if not scenario_configurations: + print("Configuration for scenario {} cannot be found!".format(self._args.scenario)) + return result + + # Execute each configuration + for config in scenario_configurations: + for _ in range(self._args.repetitions): + self.finished = False + result = self._load_and_run_scenario(config) + + self._cleanup() + return result + + def _run_route(self): + """ + Run the route scenario + """ + result = False + + if self._args.route: + routes = self._args.route[0] + scenario_file = self._args.route[1] + single_route = None + if len(self._args.route) > 2: + single_route = self._args.route[2] + + # retrieve routes + route_configurations = RouteParser.parse_routes_file(routes, scenario_file, single_route) + + for config in route_configurations: + for _ in range(self._args.repetitions): + result = self._load_and_run_scenario(config) + + self._cleanup() + return result + + def _run_openscenario(self): + """ + Run a scenario based on OpenSCENARIO + """ + + # Load the scenario configurations provided in the config file + if not os.path.isfile(self._args.openscenario): + print("File does not exist") + self._cleanup() + return False + + openscenario_params = {} + if self._args.openscenarioparams is not None: + for entry in self._args.openscenarioparams.split(','): + [key, val] = [m.strip() for m in entry.split(':')] + openscenario_params[key] = val + config = OpenScenarioConfiguration(self._args.openscenario, self.client, openscenario_params) + + result = self._load_and_run_scenario(config) + self._cleanup() + return result + + def run(self): + """ + Run all scenarios according to provided commandline args + """ + result = True + if self._args.openscenario: + result = self._run_openscenario() + elif self._args.route: + result = self._run_route() + else: + result = self._run_scenarios() + + print("No more scenarios .... Exiting") + return result + + +def main(): + """ + main function + """ + description = ("CARLA Scenario Runner: Setup, Run and Evaluate scenarios using CARLA\n" + "Current version: " + VERSION) + + # pylint: disable=line-too-long + parser = argparse.ArgumentParser(description=description, + formatter_class=RawTextHelpFormatter) + parser.add_argument('-v', '--version', action='version', version='%(prog)s ' + VERSION) + parser.add_argument('--host', default='127.0.0.1', + help='IP of the host server (default: localhost)') + parser.add_argument('--port', default='2000', + help='TCP port to listen to (default: 2000)') + parser.add_argument('--timeout', default="10.0", + help='Set the CARLA client timeout value in seconds') + parser.add_argument('--trafficManagerPort', default='8000', + help='Port to use for the TrafficManager (default: 8000)') + parser.add_argument('--trafficManagerSeed', default='0', + help='Seed used by the TrafficManager (default: 0)') + parser.add_argument('--sync', action='store_true', + help='Forces the simulation to run synchronously') + parser.add_argument('--list', action="store_true", help='List all supported scenarios and exit') + + parser.add_argument( + '--scenario', help='Name of the scenario to be executed. Use the preposition \'group:\' to run all scenarios of one class, e.g. ControlLoss or FollowLeadingVehicle') + parser.add_argument('--openscenario', help='Provide an OpenSCENARIO definition') + parser.add_argument('--openscenarioparams', help='Overwrited for OpenSCENARIO ParameterDeclaration') + parser.add_argument( + '--route', help='Run a route as a scenario (input: (route_file,scenario_file,[route id]))', nargs='+', type=str) + + parser.add_argument( + '--agent', help="Agent used to execute the scenario. Currently only compatible with route-based scenarios.") + parser.add_argument('--agentConfig', type=str, help="Path to Agent's configuration file", default="") + + parser.add_argument('--output', action="store_true", help='Provide results on stdout') + parser.add_argument('--file', action="store_true", help='Write results into a txt file') + parser.add_argument('--junit', action="store_true", help='Write results into a junit file') + parser.add_argument('--json', action="store_true", help='Write results into a JSON file') + parser.add_argument('--outputDir', default='', help='Directory for output files (default: this directory)') + + parser.add_argument('--configFile', default='', help='Provide an additional scenario configuration file (*.xml)') + parser.add_argument('--additionalScenario', default='', help='Provide additional scenario implementations (*.py)') + + parser.add_argument('--debug', action="store_true", help='Run with debug output') + parser.add_argument('--reloadWorld', action="store_true", + help='Reload the CARLA world before starting a scenario (default=True)') + parser.add_argument('--record', type=str, default='', + help='Path were the files will be saved, relative to SCENARIO_RUNNER_ROOT.\nActivates the CARLA recording feature and saves to file all the criteria information.') + parser.add_argument('--randomize', action="store_true", help='Scenario parameters are randomized') + parser.add_argument('--repetitions', default=1, type=int, help='Number of scenario executions') + parser.add_argument('--waitForEgo', action="store_true", help='Connect the scenario to an existing ego vehicle') + + arguments = parser.parse_args() + # pylint: enable=line-too-long + + if arguments.list: + print("Currently the following scenarios are supported:") + print(*ScenarioConfigurationParser.get_list_of_scenarios(arguments.configFile), sep='\n') + return 1 + + if not arguments.scenario and not arguments.openscenario and not arguments.route: + print("Please specify either a scenario or use the route mode\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.route and (arguments.openscenario or arguments.scenario): + print("The route mode cannot be used together with a scenario (incl. OpenSCENARIO)'\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.agent and (arguments.openscenario or arguments.scenario): + print("Agents are currently only compatible with route scenarios'\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.openscenarioparams and not arguments.openscenario: + print("WARN: Ignoring --openscenarioparams when --openscenario is not specified") + + if arguments.route: + arguments.reloadWorld = True + + if arguments.agent: + arguments.sync = True + + scenario_runner = None + result = True + try: + scenario_runner = ScenarioRunner(arguments) + result = scenario_runner.run() + except Exception: # pylint: disable=broad-except + traceback.print_exc() + + finally: + if scenario_runner is not None: + scenario_runner.destroy() + del scenario_runner + return not result + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/Scripts/Paths/ScenarioRunner.csv b/Scripts/Paths/ScenarioRunner.csv index e0ef1f4..78cedec 100644 --- a/Scripts/Paths/ScenarioRunner.csv +++ b/Scripts/Paths/ScenarioRunner.csv @@ -1,3 +1,6 @@ ScenarioRunner/run_experiment.py,/ +ScenarioRunner/scenario_runner.py,/ ScenarioRunner/route_scenario.py,srunner/scenarios -ScenarioRunner/carla_data_provider.py,srunner/scenariomanager \ No newline at end of file +ScenarioRunner/route_parser.py,srunner/tools +ScenarioRunner/carla_data_provider.py,srunner/scenariomanager +ScenarioRunner/scenario_manager.py,srunner/scenariomanager \ No newline at end of file From 8b5fb306b4238116e12991da587a692e53c9dc8a Mon Sep 17 00:00:00 2001 From: Gustavo Silvera Date: Sun, 7 May 2023 21:13:06 -0400 Subject: [PATCH 2/4] incorporate pylint suggestions for code quality --- ScenarioRunner/carla_data_provider.py | 14 ++++- ScenarioRunner/route_parser.py | 11 +--- ScenarioRunner/route_scenario.py | 89 ++++++++++++++------------- ScenarioRunner/run_experiment.py | 2 +- ScenarioRunner/scenario_manager.py | 16 ++--- ScenarioRunner/scenario_runner.py | 16 ++--- 6 files changed, 77 insertions(+), 71 deletions(-) diff --git a/ScenarioRunner/carla_data_provider.py b/ScenarioRunner/carla_data_provider.py index 0611923..94dc494 100644 --- a/ScenarioRunner/carla_data_provider.py +++ b/ScenarioRunner/carla_data_provider.py @@ -222,6 +222,13 @@ def get_map(world=None): return CarlaDataProvider._map + @staticmethod + def get_random_seed(): + """ + @return true if syncronuous mode is used + """ + return CarlaDataProvider._rng + @staticmethod def is_sync_mode(): """ @@ -235,7 +242,8 @@ def find_weather_presets(): Get weather presets from CARLA """ rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + def name(x:str) -> str: + return ' '.join(m.group(0) for m in rgx.finditer(x)) presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] @@ -251,7 +259,7 @@ def prepare_map(): # Parse all traffic lights CarlaDataProvider._traffic_light_map.clear() for traffic_light in CarlaDataProvider._world.get_actors().filter('*traffic_light*'): - if traffic_light not in CarlaDataProvider._traffic_light_map.keys(): + if traffic_light not in list(CarlaDataProvider._traffic_light_map): CarlaDataProvider._traffic_light_map[traffic_light] = traffic_light.get_transform() else: raise KeyError( @@ -797,7 +805,7 @@ def cleanup(): for actor_id in CarlaDataProvider._carla_actor_pool.copy(): actor = CarlaDataProvider._carla_actor_pool[actor_id] # don't delete the DReyeVR ego vehicle bc it becomes awkward to continue playing - if actor is not None and actor.is_alive and actor.type_id != CarlaDataProvider.ego_DReyeVR: + if actor is not None and actor.is_alive and "dreyevr" not in actor.type_id: batch.append(DestroyActor(actor)) if CarlaDataProvider._client: diff --git a/ScenarioRunner/route_parser.py b/ScenarioRunner/route_parser.py index 88850f9..c553792 100644 --- a/ScenarioRunner/route_parser.py +++ b/ScenarioRunner/route_parser.py @@ -7,6 +7,7 @@ Module used to parse all the route and scenario configuration parameters. """ +import os import json import math import xml.etree.ElementTree as ET @@ -126,10 +127,9 @@ def parse_direction_signs_file(signs_filename, single_route=None): :param single_route: the route id (int) to specify which sign route :return: dictionary of sign types and transforms """ - import os if not os.path.exists(signs_filename): return None - with open(signs_filename, 'r') as f: + with open(signs_filename, 'r', encoding='UTF-8') as f: signs_dict = json.loads(f.read()) routes_dict = signs_dict['available_routes'][0] @@ -173,7 +173,6 @@ def convert_waypoint_float(waypoint): waypoint['z'] = float(waypoint['z']) waypoint['yaw'] = float(waypoint['yaw']) - @staticmethod def convert_dict2transform(waypoint): """ @@ -264,11 +263,7 @@ def check_this_waypoint(tuple_wp_turn): if scenario == 'Scenario7': for tuple_wp_turn in trajectory[match_position:]: if check_this_waypoint(tuple_wp_turn): - if RoadOption.LEFT == tuple_wp_turn[1]: - subtype = 'S7left' - elif RoadOption.RIGHT == tuple_wp_turn[1]: - subtype = 'S7right' - elif RoadOption.STRAIGHT == tuple_wp_turn[1]: + if RoadOption.STRAIGHT == tuple_wp_turn[1]: subtype = 'S7opposite' else: subtype = None diff --git a/ScenarioRunner/route_scenario.py b/ScenarioRunner/route_scenario.py index 6488f4c..0531619 100644 --- a/ScenarioRunner/route_scenario.py +++ b/ScenarioRunner/route_scenario.py @@ -14,7 +14,6 @@ import math import traceback import xml.etree.ElementTree as ET -from numpy import random import py_trees @@ -22,6 +21,9 @@ from agents.navigation.local_planner import RoadOption +# DReyeVR import +from examples.DReyeVR_utils import find_ego_vehicle + # pylint: disable=line-too-long from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData # pylint: enable=line-too-long @@ -33,12 +35,16 @@ from srunner.tools.py_trees_port import oneshot_behavior from srunner.scenarios.control_loss import ControlLoss -from srunner.scenarios.follow_leading_vehicle import FollowLeadingVehicle +from srunner.scenarios.follow_leading_vehicle import FollowLeadingVehicleRoute from srunner.scenarios.object_crash_vehicle import DynamicObjectCrossing from srunner.scenarios.object_crash_intersection import VehicleTurningRoute from srunner.scenarios.other_leading_vehicle import OtherLeadingVehicle from srunner.scenarios.maneuver_opposite_direction import ManeuverOppositeDirection -from srunner.scenarios.junction_crossing_route import SignalJunctionCrossingRoute, NoSignalJunctionCrossingRoute +from srunner.scenarios.junction_crossing_route import NoSignalJunctionCrossingRoute +from srunner.scenarios.signalized_junction_left_turn import SignalizedJunctionLeftTurn +from srunner.scenarios.signalized_junction_right_turn import SignalizedJunctionRightTurn +from srunner.scenarios.opposite_vehicle_taking_priority import OppositeVehicleRunningRedLight +from srunner.scenarios.background_activity import BackgroundActivity from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest, InRouteTest, @@ -48,20 +54,18 @@ RunningStopTest, ActorSpeedAboveThresholdTest) -from examples.DReyeVR_utils import find_ego_vehicle - SECONDS_GIVEN_PER_METERS = 0.4 NUMBER_CLASS_TRANSLATION = { "Scenario1": ControlLoss, - "Scenario2": FollowLeadingVehicle, + "Scenario2": FollowLeadingVehicleRoute, "Scenario3": DynamicObjectCrossing, "Scenario4": VehicleTurningRoute, "Scenario5": OtherLeadingVehicle, "Scenario6": ManeuverOppositeDirection, - "Scenario7": SignalJunctionCrossingRoute, - "Scenario8": SignalJunctionCrossingRoute, - "Scenario9": SignalJunctionCrossingRoute, + "Scenario7": OppositeVehicleRunningRedLight, + "Scenario8": SignalizedJunctionLeftTurn, + "Scenario9": SignalizedJunctionRightTurn, "Scenario10": NoSignalJunctionCrossingRoute } @@ -155,7 +159,7 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou self._update_route(world, config, debug_mode) - self.ego_vehicle = self._initialize_ego_vehicle_DReyeVR(find_ego_vehicle(world)) + self.ego_vehicle = self._initialize_ego_vehicle_dreyevr(find_ego_vehicle(world)) self.list_scenarios = self._build_scenario_instances(world, self.ego_vehicle, @@ -164,6 +168,9 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou timeout=self.timeout, debug_mode=debug_mode) + self.list_scenarios.append(BackgroundActivity( + world, ego_vehicle, self.config, self.route, timeout=self.timeout)) + super(RouteScenario, self).__init__(name=config.name, ego_vehicles=[self.ego_vehicle], config=config, @@ -185,7 +192,7 @@ def _update_route(self, world, config, debug_mode): world_annotations = RouteParser.parse_annotations_file(config.scenario_file) # prepare route's trajectory (interpolate and add the GPS route) - gps_route, route = interpolate_trajectory(world, config.trajectory) + gps_route, route = interpolate_trajectory(config.trajectory) potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(config.town, route, world_annotations) @@ -203,11 +210,11 @@ def _update_route(self, world, config, debug_mode): # Print route in debug mode if debug_mode: - self._draw_waypoints(world, self.route, vertical_shift=1.0, persistency=50000.0) + self._draw_waypoints(world, self.route, vertical_shift=0.1, persistency=50000.0) self._setup_nav_signs(self.route) - def _initialize_ego_vehicle_DReyeVR(self, ego_vehicle): + def _initialize_ego_vehicle_dreyevr(self, ego_vehicle): """ Set/Update the start position of the ego_vehicle (instead of _update_ego_vehicle below) """ @@ -216,7 +223,6 @@ def _initialize_ego_vehicle_DReyeVR(self, ego_vehicle): elevate_transform.location.z += 0.5 ego_vehicle.set_transform(elevate_transform) CarlaDataProvider.register_actor(ego_vehicle) - CarlaDataProvider.ego_DReyeVR = ego_vehicle.type_id return ego_vehicle def _update_ego_vehicle(self): @@ -255,7 +261,6 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): for w in waypoints: wp = w[0].location + carla.Location(z=vertical_shift) - size = 0.2 if w[1] == RoadOption.LEFT: # Yellow color = carla.Color(255, 255, 0) elif w[1] == RoadOption.RIGHT: # Cyan @@ -268,9 +273,8 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): color = carla.Color(128, 128, 128) else: # LANEFOLLOW color = carla.Color(0, 255, 0) # Green - size = 0.1 - world.debug.draw_point(wp, size=size, color=color, life_time=persistency) + world.debug.draw_point(wp, size=0.1, color=color, life_time=persistency) world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=0.2, color=carla.Color(0, 0, 255), life_time=persistency) @@ -280,72 +284,71 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): def _get_valid_sign_transform(self, wp_transform): # use the original waypoint location as long as it is on a sidewalk _wp = CarlaDataProvider.get_map().get_waypoint(wp_transform.location, - project_to_road=False, - lane_type=carla.LaneType.Any) + project_to_road=False, + lane_type=carla.LaneType.Any) if _wp is None: return None # find the first non-road waypoint so our drivers can read it (with a limit) - max_tries:int = 100 + max_tries: int = 100 while max_tries > 0 and _wp.lane_type not in [carla.LaneType.Sidewalk, carla.LaneType.Shoulder]: max_tries -= 1 right_wp = _wp.get_right_lane() if right_wp is not None: _wp = right_wp else: - continue # skip this one + continue # skip this one # carla transforms don't have a native assignment operator :/ t = carla.Transform(_wp.transform.location, _wp.transform.rotation) if max_tries == 0: # didn't find a sidewalk, so push it slightly right by the road lane width - push = t.get_right_vector() * _wp.lane_width * 1.1 # 10% more just to be safe + push = t.get_right_vector() * _wp.lane_width * 1.1 # 10% more just to be safe # carla locations don't have a native += operator t.location = carla.Location(x=t.location.x + push.x, y=t.location.y + push.y, z=t.location.z + push.z) - t.location.z += 2.0 # go up slightly (for the height of the road sign) - t.rotation.yaw += 90.0 # rotate another 90 degrees + t.location.z += 2.0 # go up slightly (for the height of the road sign) + t.rotation.yaw += 90.0 # rotate another 90 degrees return t def _setup_nav_signs(self, waypoints: list): """ Draw the signs along the waypoints of a route automatically """ - prev_sign_type = None # only request new actor if nav type changes + prev_sign_type = None # only request new actor if nav type changes for i, w in enumerate(waypoints): wp_transform, sign_type = w if prev_sign_type is not None and prev_sign_type == sign_type: - continue # only spawn a sign when the waypoint type changes + continue # only spawn a sign when the waypoint type changes prev_sign_type = sign_type sign_transform = self._get_valid_sign_transform(wp_transform) if sign_transform is None: - continue # invalid + continue # invalid # now we can finally go about spawning the sign in this location - DReyeVR_sign_type: str = "dreyevr_sign_straight" + dreyevr_sign_type: str = "dreyevr_sign_straight" if sign_type == RoadOption.LEFT: - DReyeVR_sign_type = "dreyevr_sign_left" + dreyevr_sign_type = "dreyevr_sign_left" elif sign_type == RoadOption.RIGHT: - DReyeVR_sign_type = "dreyevr_sign_right" + dreyevr_sign_type = "dreyevr_sign_right" elif sign_type == RoadOption.CHANGELANELEFT: continue elif sign_type == RoadOption.CHANGELANERIGHT: continue elif sign_type == RoadOption.STRAIGHT: - DReyeVR_sign_type = "dreyevr_sign_straight" + dreyevr_sign_type = "dreyevr_sign_straight" else: continue - sign_type: str = f"static.prop.{DReyeVR_sign_type}" + sign_type: str = f"static.prop.{dreyevr_sign_type}" print(f"Spawning ({sign_type}) sign {i} at {sign_transform}") - traffic_sign = CarlaDataProvider.request_new_actor(sign_type, - sign_transform, - rolename='navigation_sign') + CarlaDataProvider.request_new_actor(sign_type, sign_transform, + rolename='navigation_sign') # plot the final goal waypoint (at the end) wp_transform_final = waypoints[-1][0] goal_sign_transform = self._get_valid_sign_transform(wp_transform_final) if goal_sign_transform is not None: - sign_type: str = f"static.prop.dreyevr_sign_goal" + sign_type: str = "static.prop.dreyevr_sign_goal" print(f"Spawning ({sign_type}) sign {len(waypoints) - 1} at {goal_sign_transform}") - traffic_sign = CarlaDataProvider.request_new_actor(sign_type, - goal_sign_transform, - rolename='navigation_sign') + CarlaDataProvider.request_new_actor(sign_type, + goal_sign_transform, + rolename='navigation_sign') def _scenario_sampling(self, potential_scenarios_definitions, random_seed=0): """ @@ -353,7 +356,7 @@ def _scenario_sampling(self, potential_scenarios_definitions, random_seed=0): """ # fix the random seed for reproducibility - rng = random.RandomState(random_seed) + rng = CarlaDataProvider.get_random_seed() def position_sampled(scenario_choice, sampled_scenarios): """ @@ -479,7 +482,6 @@ def _initialize_actors(self, config): """ Set other_actors to the superset of all scenario actors """ - # Create the background activity of the route town_amount = { 'Town01': 0, @@ -504,7 +506,10 @@ def _initialize_actors(self, config): rolename='background') if new_actors is None: - raise Exception("Error: Unable to add the background activity, all spawn points were occupied") + raise RuntimeError("Error: Unable to add the background activity, all spawn points were occupied") + + if not hasattr(self, "other_actors"): + self.other_actors = [] for _actor in new_actors: self.other_actors.append(_actor) diff --git a/ScenarioRunner/run_experiment.py b/ScenarioRunner/run_experiment.py index 32f6a33..a8d143b 100755 --- a/ScenarioRunner/run_experiment.py +++ b/ScenarioRunner/run_experiment.py @@ -76,7 +76,7 @@ def start_recording(client, args, scenario_runner_instance): time_str: str = datetime.now().strftime("%m_%d_%Y_%H_%M_%S") filename: str = f"exp_{args.title}_{time_str}.rec" - global recorder_file # to "return" from this thread + global recorder_file # to "return" from this thread recorder_file = client.start_recorder(filename) print("Recording on file: %s" % recorder_file) diff --git a/ScenarioRunner/scenario_manager.py b/ScenarioRunner/scenario_manager.py index 2f07062..1973e62 100644 --- a/ScenarioRunner/scenario_manager.py +++ b/ScenarioRunner/scenario_manager.py @@ -13,7 +13,7 @@ from __future__ import print_function import sys import time - +import os import py_trees from srunner.autoagents.agent_wrapper import AgentWrapper @@ -21,6 +21,7 @@ from srunner.scenariomanager.result_writer import ResultOutputProvider from srunner.scenariomanager.timer import GameTime from srunner.scenariomanager.watchdog import Watchdog +from srunner.tools.route_parser import RouteParser class ScenarioManager(object): @@ -64,6 +65,7 @@ def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0): self.scenario_duration_game = 0.0 self.start_system_time = None self.end_system_time = None + self.route_id = None def _reset(self): """ @@ -116,13 +118,13 @@ def load_scenario(self, scenario, agent=None, route_id=None): self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode) self.route_id = route_id - self.load_DReyeVR_signs() + self.load_dreyevr_signs() - def load_DReyeVR_signs(self): - # retrieve and place direction signs for route if they exist - from srunner.tools.route_parser import RouteParser + def load_dreyevr_signs(self): + """ + retrieve and place direction signs for route if they exist + """ - import os signs_file = os.path.join(os.getenv("SCENARIO_RUNNER_ROOT"), "srunner", "data", "all_routes_signs.json") route_signs_list = RouteParser.parse_direction_signs_file(signs_file, self.route_id) @@ -140,7 +142,7 @@ def load_DReyeVR_signs(self): sign_transform = RouteParser.convert_dict2transform(sign_waypoint) print(f"Loading \"{sign_type}\" @ {sign_transform}") # TODO: keep track of these signs somewhere? - traffic_sign = CarlaDataProvider.request_new_actor(sign_type, sign_transform, rolename='navigation_sign') + CarlaDataProvider.request_new_actor(sign_type, sign_transform, rolename='navigation_sign') def run_scenario(self): """ diff --git a/ScenarioRunner/scenario_runner.py b/ScenarioRunner/scenario_runner.py index bbcd5a1..7e507d5 100755 --- a/ScenarioRunner/scenario_runner.py +++ b/ScenarioRunner/scenario_runner.py @@ -30,19 +30,15 @@ import json import pkg_resources -sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI")) # for Carla stuff -sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI", "carla")) # for agents -sys.path.insert(0, os.path.join(os.getenv("CARLA_ROOT"), "PythonAPI", "dreyevr")) # for DReyeVR stuff - import carla -from srunner.scenarioconfigs.openscenario_configuration import OpenScenarioConfiguration -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenario_manager import ScenarioManager -from srunner.scenarios.open_scenario import OpenScenario -from srunner.scenarios.route_scenario import RouteScenario -from srunner.tools.scenario_parser import ScenarioConfigurationParser from srunner.tools.route_parser import RouteParser +from srunner.tools.scenario_parser import ScenarioConfigurationParser +from srunner.scenarios.route_scenario import RouteScenario +from srunner.scenarios.open_scenario import OpenScenario +from srunner.scenariomanager.scenario_manager import ScenarioManager +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenarioconfigs.openscenario_configuration import OpenScenarioConfiguration # Version of scenario_runner VERSION = '0.9.13' From 0dc5973a3b26d917d5cb9b990b2a205da1f015b9 Mon Sep 17 00:00:00 2001 From: Gustavo Silvera Date: Mon, 8 May 2023 11:11:32 -0400 Subject: [PATCH 3/4] fix minor typos and improve PYTHONPATH recognition --- ScenarioRunner/route_scenario.py | 2 +- ScenarioRunner/run_experiment.py | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ScenarioRunner/route_scenario.py b/ScenarioRunner/route_scenario.py index 0531619..e427da5 100644 --- a/ScenarioRunner/route_scenario.py +++ b/ScenarioRunner/route_scenario.py @@ -169,7 +169,7 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou debug_mode=debug_mode) self.list_scenarios.append(BackgroundActivity( - world, ego_vehicle, self.config, self.route, timeout=self.timeout)) + world, self.ego_vehicle, self.config, self.route, timeout=self.timeout)) super(RouteScenario, self).__init__(name=config.name, ego_vehicles=[self.ego_vehicle], diff --git a/ScenarioRunner/run_experiment.py b/ScenarioRunner/run_experiment.py index a8d143b..43f5b7d 100755 --- a/ScenarioRunner/run_experiment.py +++ b/ScenarioRunner/run_experiment.py @@ -12,8 +12,10 @@ try: CARLA_ROOT: str = os.getenv("CARLA_ROOT") egg_dir = os.path.join(CARLA_ROOT, "PythonAPI", "carla", "dist") - sys.path.append(glob.glob(os.path.join(egg_dir, f"carla-*.egg"))[0]) - sys.path.append(os.path.join(CARLA_ROOT, "PythonAPI", "examples")) + sys.path.extend([glob.glob(os.path.join(egg_dir, f"carla-*.egg"))[0], + os.path.join(CARLA_ROOT, "PythonAPI"), + os.path.join(CARLA_ROOT, "PythonAPI", "carla"), + os.path.join(CARLA_ROOT, "PythonAPI", "examples")]) except IndexError: print(f"Unable to find Carla PythonAPI file in {egg_dir}") From fd593dc45b7b62c2487ecc5288c4958eb8441cf9 Mon Sep 17 00:00:00 2001 From: Gustavo Silvera Date: Wed, 24 May 2023 13:20:12 -0700 Subject: [PATCH 4/4] disabling shoulder as a valid sign placement lane --- ScenarioRunner/route_scenario.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ScenarioRunner/route_scenario.py b/ScenarioRunner/route_scenario.py index e427da5..021f46a 100644 --- a/ScenarioRunner/route_scenario.py +++ b/ScenarioRunner/route_scenario.py @@ -290,7 +290,11 @@ def _get_valid_sign_transform(self, wp_transform): return None # find the first non-road waypoint so our drivers can read it (with a limit) max_tries: int = 100 - while max_tries > 0 and _wp.lane_type not in [carla.LaneType.Sidewalk, carla.LaneType.Shoulder]: + valid_sign_lanes = [ + carla.LaneType.Sidewalk, + # carla.LaneType.Shoulder, # disable shoulder + ] + while max_tries > 0 and _wp.lane_type not in valid_sign_lanes: max_tries -= 1 right_wp = _wp.get_right_lane() if right_wp is not None: