diff --git a/.gitignore b/.gitignore index f84e11c..15d7629 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,5 @@ *.pyc -# ignore docker for now .ruff_cache -docker/id_rsa -id_rsa -PythonAPI srunner/autoagents/*.pyc srunner/autoagents/__pycache__/ srunner/scenariomanager/*.pyc @@ -13,7 +9,7 @@ srunner/scenarios/__pycache__/ srunner/utilities/*.pyc srunner/utilities/__pycache__/ results*.txt -tests +tests/ *.log .project .pydevproject @@ -21,8 +17,8 @@ tests *.svg *.dot .python-version -PythonAPI -results +PythonAPI/ +results/ srunner/osc2 srunner/osc2_dm srunner/osc2_stdlib @@ -30,5 +26,12 @@ metrics_manager.py docker-compose.yml jats paper -logs -typings +logs/ +typings/ +cawsr_workspace/ +tools/ +scenarios/ +algorithms/ +configs/ +results/ +Dockerfile_dev diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 0f7da7b..10aef04 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -2,6 +2,8 @@ Thank you for your interest in this project! We welcome contributions from the community. +To setup the development environment, please refer to the [docs/DEVELOPMENT.md](docs/DEVELOPMENT.md) file. + ## How to Contribute 1. **Fork the repository** on GitHub. 2. **Create a new branch** for your feature or bug fix. diff --git a/Dockerfile b/Dockerfile index 3c814b0..8d81804 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,6 +3,10 @@ FROM osrf/ros:humble-desktop RUN apt -y update && \ apt install --no-install-recommends -y libpng16-16 libtiff5 libjpeg8 build-essential curl wget git libxerces-c-dev python3-pip vim +# Create carla user with same UID/GID as the CARLA simulator container +RUN groupadd -g 1000 carla && \ + useradd -m -u 1000 -g carla -s /bin/bash carla + # clone repo COPY . /autoware_scenario_runner @@ -50,11 +54,19 @@ RUN mkdir /cyclonedds && \ mv /autoware_scenario_runner/docker/cyclonedds_local.xml /cyclonedds/ && \ mv /autoware_scenario_runner/docker/cyclonedds_distributed.xml /cyclonedds/ && \ rm -rf /autoware_scenario_runner/docker && \ - echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> ~/.bashrc && \ - echo "alias rossrc='source ${AUTOWARE_MSG_PKG} && source ${ROS_PKG} && echo Sourced'" >> ~/.bashrc && \ - source ~/.bashrc + echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /home/carla/.bashrc && \ + echo "alias rossrc='source ${AUTOWARE_MSG_PKG} && source ${ROS_PKG} && echo Sourced'" >> /home/carla/.bashrc ENV CARLA_API_ROOT="/autoware_scenario_runner/PythonAPI" ENV PYTHONPATH="${PYTHONPATH}:${CARLA_API_ROOT}/carla/agents:${CARLA_API_ROOT}/carla" +# Give carla user ownership of necessary directories +RUN chown -R carla:carla /autoware_scenario_runner && \ + chown -R carla:carla /ros_workspace && \ + chown -R carla:carla /cyclonedds && \ + usermod -aG docker carla + +# Switch to carla user +USER carla + ENTRYPOINT [ "/autoware_scenario_runner/entrypoint.sh" ] diff --git a/README.md b/README.md index 17af617..60a8ce0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) +For development and contributing docs see [docs/DEVELOPMENT.md](docs/DEVELOPMENT.md) + CAWSR: ScenarioRunner for CARLA with support for Autoware ======================== @@ -7,9 +9,9 @@ CAWSR (Carla Autoware Scenario Runner) is a scenario execution engine built for Prerequisites --------------------------- -Both CARLA and Autoware require a high-spec computer with a high-end Nvidia GPU. It is also possible to run a [**distributed**]() setup with multiple machines to help ease the workload, or run the entire stack locally. Currently, only Linux is supported (guide was written on Ubuntu 24.04). +Both CARLA and Autoware require a high-spec computer with a high-end Nvidia GPU. It is also possible to run a [**distributed**](#distributed) setup with multiple machines to help ease the workload, or run the entire stack locally. Currently, only Linux is supported (guide was written on Ubuntu 24.04). -Ensure the target machine(s) have the [Docker Engine]() and [Nvidia Container toolkit]() installed to enable gpu accelerated workflows in Docker. +Ensure the target machine(s) have the [Docker Engine](https://docs.docker.com/engine/install/) and [Nvidia Container toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) installed to enable gpu accelerated workflows in Docker. CAWSR Setup --------------------------- @@ -45,7 +47,8 @@ Running CAWSR CAWSR can both be ran `locally` or `distributed`. Due to the high-spec requirements, it is recommended to run distributed if you do not meet the following minimum specs: - At least **10GB** VRAM and a modern GPU (2080 ti or newer) - - We currently supports 20 and 30 series GPUs. Compatibility with the 40 series is current WIP. + - We currently supports 20 and 30 series GPUs. Compatibility with the 40 series is current WIP. + - At least **32GB** RAM - A modern Intel or AMD CPU with at least 8 cores. @@ -117,6 +120,11 @@ docker_compose.yml ``` All folders are mounted as Docker volumes into the CAWSR container, so any changes persist between host and container. +## Autoware Warmup + +Before using CAWSR, Autoware must build various `TensorRT` engines for inference. Engine builds are not cross-compatible across various GPU compute capabilities (see [here](https://developer.nvidia.com/cuda/gpus)), so they must be rebuilt per machine. This process usually only takes a few minutes on modern hardware, and starts after the initial launch of autoware. + + ## Configuring CAWSR CAWSR is designed to be highly configurable and supports easy swapping of config files. @@ -167,10 +175,10 @@ We use a custom implementation of a scenario definition in JSON. We have include Domain Model: ![Domain Model](./docs/resources/scenario_domain.png) -Contributing ------------- +Contributing & Development +-------------------------- -Please take a look at our [Contribution guidelines](). +Please take a look at our [Contribution guidelines](CONTRIBUTING.md). To setup a development container, follow our [development guidelines](docs/DEVELOPMENT.md) License ------- diff --git a/cawsr.py b/cawsr.py index eb952c1..c167885 100644 --- a/cawsr.py +++ b/cawsr.py @@ -18,6 +18,7 @@ import json import multiprocessing +import multiprocessing.queues import carla @@ -37,6 +38,7 @@ from srunner.scenarioconfigs.route_scenario_configuration import ( RouteScenarioConfiguration, ) +from srunner.tools.route_visualisation import visualise_route from srunner.tools import route_manipulation from srunner.objects.ego_vehicle import EgoVehicle from srunner.tools.log import LogUtil @@ -66,8 +68,12 @@ class AWScenarioRunner(object): aw_agent = None # autoware agent def __init__(self, cawsr_config: dict, carla_conf: CARLA) -> None: - """ - Setup Scenario Manager and the Carla client + """Initialises CAWSR, loading the Autoware agent module and setting up signal handlers for cleanup on shutdown. + Also initialises the ScenarioDefinitionManager for managing results directories and files. + + Args: + cawsr_config (dict): CAWSR configuration options loaded from YAML file + carla_conf (CARLA): CARLA-specific configuration options loaded from YAML file """ self.DEBUG = cawsr_config["debug"] self._conf = cawsr_config @@ -119,170 +125,275 @@ def run_scenario( current_definiton: Union[dict, None], algorithm: Union[Callable, None], ) -> None: - logger.info("Initialising Scenario Manager...") - self.scenario_manager = ScenarioManager( - self.DEBUG, - self._carla.SYNC, - self._carla.TIMEOUT, - ) - - logger.info("Starting the MetricsCollector thread...") - - MetricsCollector.init_state( - metrics_collected, - os.path.join(self.results_manager.last_scenario, "execution_time.txt"), - include=False, - ) - - logger.info("Connecting to client...") - self.carla_client = carla.Client(self._carla.HOST, self._carla.PORT) - self.carla_client.set_timeout(self._carla.TIMEOUT) - CarlaDataProvider.set_client(self.carla_client) - - logger.info("Fetching current world...") - - self.carla_world = self.carla_client.get_world() - logger.info("Updating map...") - self.carla_client.load_world(env_config.town) + """Starts the CAWSR scenario based on the provided configuration and seed + Handles connection to the Autoware agent and CARLA client, and runs the scenario manager until the scenario is complete or a shutdown signal is received. + Args: + route_config (RouteScenarioConfiguration): RouteSCenarioConfiguration object configuration + env_config (EnvironmentConfig): EnvironmentConfig object containing the environment configuration + scenario_name (str): Name of the scenario to run + seed (int): Random seed for reproducibility + algorithm_mode (bool): Whether to load algorithmic scenario generation loop, or just execute scenarios in sequence + result_ (_type_): Result object for storing scenario results + current_definiton (Union[dict, None]): Optional dict containing the current scenario definition, used for algorithmic scenario generation + algorithm (Union[Callable, None]): Optional Callable algorithm class, used for algorithmic scenario generation. See docs for expected interface. + """ + # Safe fallback — always defined so the finally block can always put something on the queue + result_dict = {"status": False, "driving_score": None} - logger.info("Updating world settings:") + try: + logger.info("Initialising Scenario Manager...") + self.scenario_manager = ScenarioManager( + self.DEBUG, + self._carla.SYNC, + self._carla.TIMEOUT, + ) - # tick asynchronously until then - settings = self.carla_world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = self._carla.FIXED_DELTA_SECONDS - self.carla_world.apply_settings(settings) + logger.info("Starting the MetricsCollector thread...") - logger.info(f"{settings.__str__()}") + MetricsCollector.init_state( + metrics_collected, + os.path.join(self.results_manager.last_scenario, "execution_time.txt"), + include=False, + ) - logger.info("Restarting GameTime...") - GameTime.restart() + logger.info("Connecting to client...") + self.carla_client = carla.Client(self._carla.HOST, self._carla.PORT) + self.carla_client.set_timeout(self._carla.TIMEOUT) + CarlaDataProvider.set_client(self.carla_client) + + logger.info("Fetching current world...") + + self.carla_world = self.carla_client.get_world() + logger.info("Updating map...") + self.carla_client.load_world(env_config.town) + + logger.info("Updating world settings:") + + # tick asynchronously until then + settings = self.carla_world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = self._carla.FIXED_DELTA_SECONDS + self.carla_world.apply_settings(settings) + + logger.info(f"{settings.__str__()}") + + logger.info("Restarting GameTime...") + GameTime.restart() + + # update the world + CarlaDataProvider.set_world(self.carla_world) + + # attempt to spawn the ego 3 times for redundancy + max_attempts = 3 + for attempt in range(max_attempts): + try: + logger.info("Spawning ego...") + ego = EgoVehicle(env_config) + actor = ego.spawn() + self.ego_vehicles.append(actor) + logger.info(f"Spawned ego with id: {actor.id}") + break # exit loop if successful + except Exception as e: + logger.error(f"Failed to spawn ego on attempt {attempt + 1}: {e}") + if attempt == max_attempts - 1: + logger.error( + "Failed to spawn ego: Terminating scenario. This is most likely an issue with CARLA" + ) + raise + time.sleep(2) + + # client must tick to spawn actors + self._tick_carla() - # update the world - CarlaDataProvider.set_world(self.carla_world) + logger.info("Initialising Autoware...") + agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") + try: + logger.info(getattr(self.module_aw_agent, agent_class_name)) + self.aw_agent = getattr(self.module_aw_agent, agent_class_name)( + env_config + ) # agent init function + route_config.agent = self.aw_agent + except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except + logger.error("Could not setup required agent due to {}".format(e)) + self._cleanup() + return # result_dict fallback will be put by finally + + logger.info("Loading route...") + + gps_route, route = route_manipulation.interpolate_trajectory( + route_config.keypoints + ) + route_config.agent.set_global_plan(gps_route, route) # set agent route - logger.info("Spawning ego...") - ego = EgoVehicle(env_config) - actor = ego.spawn() - self.ego_vehicles.append(actor) - logger.info(f"Spawned ego with id: {actor.id}") + # visualise the route in CARLA and set location to first waypoint + visualise_route([waypoint[0] for waypoint in route]) + ego.prepare_ego(route[0][0]) - # client must tick to spawn actors - self._tick_carla() + # allow the agent X ticks to initialize sensors and set the route + # uses the same tick pattern as the scenario_manager main loop + logger.info("Initialising agent route...") + budget = self._conf["initialisation_budget"] + max_route_attempts = 3 + route_ready = False - logger.info("Initialising Autoware...") - agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") - try: - logger.info(getattr(self.module_aw_agent, agent_class_name)) - self.aw_agent = getattr(self.module_aw_agent, agent_class_name)( - env_config - ) # agent init function - route_config.agent = self.aw_agent - except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except - logger.error("Could not setup required agent due to {}".format(e)) - self._cleanup() - result = False - return - - logger.info("Loading route...") - - # TO DO - # interpolate route at a larger distance (i.e 5m) to reduce waypoints - # remove segment sampling from awagent.set_route - gps_route, route = route_manipulation.interpolate_trajectory( - route_config.keypoints - ) - route_config.agent.set_global_plan(gps_route, route) # set agent route + for attempt in range(1, max_route_attempts + 1): + if attempt > 1: + logger.info( + f"Retrying agent route setup ({attempt}/{max_route_attempts})" + ) + self.aw_agent.agent_set_route = False + self.aw_agent.sent_route = False + self.aw_agent.autoware_state.sent_route = False + self.aw_agent.route_node.request_clear_route() + + for tick in range(1, budget + 1): + _tick_carla_start = time.perf_counter_ns() / 1e6 + world = CarlaDataProvider.get_world() + if world: + world.tick() + for _ in range(5): + time.sleep(0) + MetricsCollector.update_key( + "carla_time", (time.perf_counter_ns() / 1e6) - _tick_carla_start + ) + timestamp = None + if world: + snapshot = world.get_snapshot() + if snapshot: + timestamp = snapshot.timestamp + if timestamp: + self.aw_agent._carla_timestamp = timestamp + GameTime.on_carla_tick(timestamp) + CarlaDataProvider.on_carla_tick() + self.aw_agent.run_step() + if self.aw_agent.autoware_state.route_set(): + route_ready = True + break + + if route_ready: + break + + logger.info(f"Agent failed to initialise route on attempt {attempt}") + + if not route_ready: + logger.error( + "Agent failed to initialise route after 3 attempts; skipping scenario." + ) + result_dict["status"] = False + result_dict["driving_score"] = None - ego.prepare_ego(route[0][0]) # set location to first waypoint + if algorithm_mode: + algorithm._update_generator(seed) # type: ignore + definition = algorithm._scenario_callback( # type: ignore + current_definiton, 0.0 + ) + result_dict["definition"] = definition + + MetricsCollector.reset() + return # finally handles the put + + logger.info("Loading Traffic Manager...") + tm_port = int(self._carla.TRAFFIC_MANAGER.PORT) # type: ignore + CarlaDataProvider.set_traffic_manager_port(tm_port) + tm = self.carla_client.get_trafficmanager(tm_port) + + tm.set_random_device_seed(int(self._carla.TRAFFIC_MANAGER.SEED)) + tm.set_synchronous_mode(self._carla.SYNC) + + try: + scenario = RouteScenario( + world=self.carla_world, + config=route_config, + debug_mode=self.DEBUG, + timeout=self._conf.get("route_timeout", None), + ego_vehicle=ego._actor, + route=route, + ) + except Exception: + logger.error("Could not load Route Scenario") + traceback.print_exc() + raise # scenario is unbound; propagate to outer except so finally puts the fallback result_dict - # allow the agent X ticks to initialize sensors and set the route - logger.info("Initialising agent route...") - budget = self._conf["initialisation_budget"] - status = False - for tick in range(1, budget + 1): - status = self.aw_agent.run_step_init() # type: ignore - self._tick_carla() - if not status: - logger.info("Agent failed to initialise route") - else: - logger.info("Successfully initialised agent; route set.") + logger.info("Starting scenario...") - logger.info("Loading Traffic Manager...") - tm_port = int(self._carla.TRAFFIC_MANAGER.PORT) # type: ignore - CarlaDataProvider.set_traffic_manager_port(tm_port) - tm = self.carla_client.get_trafficmanager(tm_port) + self.aw_agent.scenario_loaded = True - tm.set_random_device_seed(int(self._carla.TRAFFIC_MANAGER.SEED)) - tm.set_synchronous_mode(self._carla.SYNC) + try: + self.carla_client.start_recorder( + "/home/carla/recordings/recording.log", True + ) + self.scenario_manager.load_scenario( + scenario, self.aw_agent, follow_ego=True + ) - try: - scenario = RouteScenario( - world=self.carla_world, - config=route_config, - debug_mode=self.DEBUG, - ego_vehicle=ego._actor, - route=route, - ) - except Exception: - logger.info("Could not load Route Scenario") - traceback.print_exc() + self.scenario_manager.run_scenario() + result = True + except Exception: + traceback.print_exc() + logger.info( + "Could not load scenario. Please check if the agent class is loading correctly." + ) + result = False + finally: + self.carla_client.stop_recorder() - logger.info("Starting scenario...") + # stop the MetricsCollector thread + MetricsCollector.reset() - try: - self.carla_client.start_recorder("/home/carla/recording.log", True) - self.scenario_manager.load_scenario( - scenario, self.aw_agent, follow_ego=True + criteria = self._output_criteria( + self.scenario_manager.scenario.get_criteria(), # type: ignore + f"{self.results_manager.last_scenario}/{scenario_name}.json", ) + logger.info("Calculating driving score...") + driving_score = self._calculate_driving_score(criteria) + result_dict["driving_score"] = driving_score + result_dict["status"] = result + logger.info("Processed driving score...") + + if algorithm_mode: + algorithm._update_generator(seed) # type: ignore + try: + definition = algorithm._scenario_callback( # type: ignore + current_definiton, result_dict["driving_score"] + ) + result_dict["definition"] = definition + except Exception: + logger.error( + "Something went wrong while processing algorithm callback; is CARLA alive?" + ) - self.scenario_manager.run_scenario() - result = True except Exception: + logger.error("Unhandled exception in run_scenario:") traceback.print_exc() - logger.info( - "Could not load scenario. Please check if the agent class is loading correctly." - ) - result = False - - self.carla_client.stop_recorder() - # stop the MetricsCollector thread - MetricsCollector.reset() - # analyse the scenario - criteria = self._output_criteria( - self.scenario_manager.scenario.get_criteria(), # type: ignore - f"{self.results_manager.last_scenario}/{scenario_name}.json", - ) - logger.info("Calculating driving score...") - driving_score = self._calculate_driving_score(criteria) + finally: + try: + result_.get_nowait() + logger.info("Clearing process pipe...") + except multiprocessing.queues.Empty: + logger.info("Process pipe is empty") - result_dict = result_.get() - result_dict["status"] = result - result_dict["driving_score"] = driving_score - - # read the scenario definition - if algorithm_mode: - algorithm._update_generator(seed) # type: ignore - - definition = algorithm._scenario_callback( # type: ignore - current_definiton, driving_score - ) - result_dict["definition"] = definition - - result_.put(result_dict) + result_.put(result_dict) + result_.close() + result_.join_thread() + logger.info("Stopping process...") + os._exit(0) def _tick_carla(self) -> None: - timestamp = None + """Advances CARLA 1 tick into the future""" world = CarlaDataProvider.get_world() if world: + world.tick() + for _ in range(5): + time.sleep(0) + snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp - if timestamp: - CarlaDataProvider.get_world().tick() - GameTime.on_carla_tick(timestamp) - CarlaDataProvider.on_carla_tick() + if self.aw_agent is not None: + self.aw_agent._carla_timestamp = timestamp + GameTime.on_carla_tick(timestamp) + CarlaDataProvider.on_carla_tick() def _load_alg(self) -> type[BasicAlgorithm]: """Load an algorithm instance from mounted docker volume algorithms/ @@ -304,16 +415,14 @@ def _load_alg(self) -> type[BasicAlgorithm]: ) def run_algorithm(self) -> None: - """Executes CAWSR in algorithm mode""" + """Executes CAWSR in algorithm mode, loading a custom algorithm interface for scenario generation and optimisation. + Scenarios are generated iteratively based on the previous scenario's performance, and the initial scenario definition provided. + """ - # load the algorithm and scenario defintion + # load the algorithm and scenario definitions optimisation_algorithm = self._load_alg() scenario = pathlib.Path(self._conf["algorithm"]["initial_definition"]) - # add some code here - # if scenario = null (initial definition not given) - # run the algorithm to generate a new, random scenario - logger.info(str(scenario.absolute())) json_definition = self._load_scenario( scenario_name=scenario.stem, @@ -327,6 +436,7 @@ def run_algorithm(self) -> None: for i in range(0, runs): logger.info(f"Running scenario {i + 1}/{runs}") + CARLAManager._set_recording_dir(self.results_manager.recording_path) logger.info("Starting CARLA container....") CARLAManager.restart_carla() time.sleep(10) # allow CARLA to load @@ -338,9 +448,9 @@ def run_algorithm(self) -> None: self.results_manager.last_scenario, env_config )[ 0 - ] # route id. Multiple routes currently aren't supported, so use first route -> fix to use config + ] # route id. Multiple routes currently aren't supported, so use first route - json_definition = self._cawsr_process( + result = self._cawsr_process( route_config=route_config, env_config=env_config, scenario_name=scenario.stem, @@ -351,28 +461,24 @@ def run_algorithm(self) -> None: ) self.results_manager.parse_json( - json_definition, # type: ignore + result["definition"], # type: ignore scenario.stem, str(i), save_def=True, ) def run_benchmark(self) -> None: - """Executes CAWSR in benchmark mode. Scenarios are loaded from the target directory""" target_dir = pathlib.Path(self._conf["benchmark"]["scenarios"]) scenarios = [scenario for scenario in target_dir.glob("*.json")] runs = len(scenarios) for i in range(0, runs): - logger.info(f"Running scenario {i + 1}/{runs}") - - logger.info("Starting CARLA container....") - CARLAManager.restart_carla() - time.sleep(10) # allow CARLA to load + retry_attempts = 2 + attempts = 0 if self._conf["benchmark"]["random_sampling"]: scenario = random.choice(scenarios) - scenarios.remove(scenario) # each scenario can only be picked once + scenarios.remove(scenario) else: scenario = scenarios[i] @@ -383,22 +489,44 @@ def run_benchmark(self) -> None: save_def=True, ) - env_config = EnvironmentParser.parse_scenario_env( - self.results_manager.fetch_scenario_xml() - ) - route_config = RouteParser.parse_routes_file( - self.results_manager.last_scenario, env_config - )[ - 0 - ] # route id. Multiple routes currently aren't supported, so use first route + while attempts < retry_attempts: + logger.info( + f"Running scenario {i + 1}/{runs} (attempt {attempts + 1}/{retry_attempts})" + ) - self._cawsr_process( - route_config=route_config, - env_config=env_config, - scenario_name=scenario.stem, - run=i + 1, - algorithm_mode=False, - ) + CARLAManager._set_recording_dir(self.results_manager.recording_path) + logger.info("Starting CARLA container....") + CARLAManager.restart_carla() + + # Wait for CARLA to be ready, with a configurable timeout + carla_startup_wait = self._conf.get("carla_startup_wait", 20) + logger.info(f"Waiting {carla_startup_wait}s for CARLA to be ready...") + time.sleep(carla_startup_wait) + + env_config = EnvironmentParser.parse_scenario_env( + self.results_manager.fetch_scenario_xml() + ) + route_config = RouteParser.parse_routes_file( + self.results_manager.last_scenario, env_config + )[0] + + result = self._cawsr_process( + route_config=route_config, + env_config=env_config, + scenario_name=scenario.stem, + run=i + 1, + algorithm_mode=False, + ) + + if not result["status"] and result["driving_score"] is None: + attempts += 1 + if attempts < retry_attempts: + logger.warning( + f"Scenario {scenario.stem} failed with no result " + f"(attempt {attempts}/{retry_attempts}), retrying..." + ) + else: + break def _cawsr_process( self, @@ -409,9 +537,24 @@ def _cawsr_process( algorithm_mode: bool = False, current_definiton: Union[dict, None] = None, algorithm: Union[Callable, None] = None, - ) -> Optional[dict]: + ) -> dict: + """Spawns a CAWSR process to execute a given, configured scenario. + Handles setup, execution, and teardown of the scenario, and returns the scenario definition if in algorithm mode for use in the next iteration of the algorithm. + + Args: + route_config (RouteScenarioConfiguration): Scenario configuration object containing the route and agent information + env_config (EnvironmentConfig): Scenario configuration object containing the environment information, such as weather and town + scenario_name (str): Name of the scenario, used for results management and logging + run (int): Current run number, used for logging and results management + algorithm_mode (bool, optional): Whether to run in algorithm mode. Defaults to False. + current_definiton (Union[dict, None], optional): The current scenario definition. Defaults to None. + algorithm (Union[Callable, None], optional): The algorithm to use. Defaults to None. + + Returns: + Optional[dict]: The updated scenario definition if in algorithm mode, otherwise None. + """ scenario_result = multiprocessing.Queue() - scenario_result.put({"status": False, "criteria": {}}) + scenario_result.put({"status": None, "driving_score": None, "criteria": {}}) if algorithm_mode: seed = self._rng.integers( @@ -420,8 +563,9 @@ def _cawsr_process( else: seed = 0 + env_config.iteration = run scenario_process = multiprocessing.Process( - target=self.run_scenario, # need to catch connection exception + target=self.run_scenario, args=( route_config, env_config, @@ -433,37 +577,47 @@ def _cawsr_process( algorithm, ), ) - # run scenario until finished + logger.info(f"Starting {scenario_name} process.") scenario_process.start() scenario_process.join() - # fetch result and clean up - result = scenario_result.get() - if scenario_process.is_alive(): + logger.error("Scenario process timed out, killing...") scenario_process.kill() - self.results_manager.cleanup_xml() + scenario_process.join() - # copy over the recording from CARLA container - time.sleep(1) # ensure file is written - CARLAManager.fetch_file( - "/home/carla/recording.log", - self.results_manager.last_scenario, - ) + logger.info("Extracting results...") + # Use get_nowait() so we never block if the subprocess died without putting + # a result on the queue (e.g. a CARLA crash before run_scenario's finally ran) + try: + result = scenario_result.get_nowait() + logger.info(f"Subprocess returned: {result}") + except multiprocessing.queues.Empty: + logger.error( + "Scenario process died without returning a result, using fallback." + ) + result = {"status": False, "driving_score": None} + + # if the process exits (CARLA crash for example), these are none # fetch execution status of the scenario (failure or success) - status = result["status"] - driving_score = result["driving_score"] + status = result.get("status", None) + driving_score = result.get("driving_score", None) logger.info(f"Scenario iteration {run} achieved a score of {driving_score}") logger.info( f"Scenario iteration {run} ended with status {'SUCCESS' if status else 'FAILURE'}" ) - # return new scenario definition if in alg mode - if algorithm_mode: - return result["definition"] + # only cleanup if scenario finished successfully - driving score has been set + if result["driving_score"] is not None: + logger.info("Cleaning up scenario artifacts...") + self.results_manager.cleanup_xml() + + time.sleep(1) + + return result def _load_scenario( self, @@ -473,8 +627,20 @@ def _load_scenario( save_def: bool = True, return_def: bool = False, ) -> Optional[dict]: + """Loads a given scenario from the JSON definition file + + Args: + scenario_name (str): Name of the scenario, used for results management and logging + path (str): Path to the JSON definition file + run (int): Current run number, used for logging and results management + save_def (bool, optional): Whether to save the scenario definition. Defaults to True. + return_def (bool, optional): Whether to return the scenario definition. Defaults to False. + + Returns: + Optional[dict]: The loaded scenario definition if `return_def` is True, otherwise None. + """ try: - with open(path, "r", encoding="UTF-8") as raw_json: # type: ignore + with open(path, "r", encoding="UTF-8") as raw_json: json_definition = json.loads(raw_json.read()) except FileNotFoundError: logger.info("JSON definition does not exist, exiting...") @@ -507,6 +673,16 @@ def run(self) -> None: def _output_criteria( self, criteria, file_name: str, save_file: bool = True ) -> dict: + """Filter the criteria.json generated by scenario-runner to calculate driving score + + Args: + criteria (_type_): The criteria generated by scenario-runner, containing all the information about the scenario execution, including the criteria results and their attributes + file_name (str): The name of the file to save the filtered criteria to, used for results management and logging + save_file (bool, optional): Whether to save the filtered criteria to a file. Defaults to True. + + Returns: + dict: The filtered criteria dictionary + """ # Filter the attributes that aren't JSON serializable with open("temp.json", "w", encoding="utf-8") as fp: criteria_dict = {} @@ -525,13 +701,21 @@ def _output_criteria( os.remove("temp.json") - # Save the criteria dictionary into a .json file + # save the criteria dictionary into a .json file if save_file: with open(file_name, "w", encoding="utf-8") as fp: json.dump(criteria_dict, fp, sort_keys=False, indent=4) return criteria_dict def _calculate_driving_score(self, criteria: dict) -> float: + """Returns driving score based on a set of provided infractions, and the criteria log generated by scenario runner. + + Args: + criteria (dict): The criteria dictionary generated by scenario runner, containing the results of all criteria and their attributes. See `_output_criteria` for details on how this is generated. + + Returns: + float: The calculated driving score + """ infractions_dict = { "OutsideRouteLanesTest": 0.3, "CollisionTest": 1.0, @@ -572,16 +756,22 @@ def destroy(self) -> None: def _cleanup(self) -> None: """Cleanup function. Removes instances of the CARLA client and WORLD, also destroys the Ego vehicle in CARLA.""" + CARLAManager.stop_carla() + try: - CarlaDataProvider.get_client().get_trafficmanager( - int(self._carla.TRAFFIC_MANAGER.PORT) - ).set_synchronous_mode(False) + client = CarlaDataProvider.get_client() + if client is not None: + client.get_trafficmanager( + int(self._carla.TRAFFIC_MANAGER.PORT) + ).set_synchronous_mode(False) except RuntimeError: sys.exit(-1) - self.scenario_manager.cleanup() + if self.scenario_manager is not None: + self.scenario_manager.cleanup() - CarlaDataProvider.cleanup() + if CarlaDataProvider is not None: + CarlaDataProvider.cleanup() for i, _ in enumerate(self.ego_vehicles): if self.ego_vehicles[i]: @@ -594,7 +784,7 @@ def _cleanup(self) -> None: self.ego_vehicles = [] if self.aw_agent: - self.aw_agent.destroy() + self.aw_agent.destroy(cleanup=True) self.aw_agent = None @@ -651,7 +841,7 @@ def main(): scenario_runner = AWScenarioRunner(config, carla_config) results = scenario_runner.run() logger.info(results) - except Exception: # NOT GOOD PRACTICE PROBABLY CHANGE + except Exception: # catch any anonymous exceptions to ensure cleanup is always called, as CARLA can leave zombie processes traceback.print_exc() finally: if scenario_runner is not None: diff --git a/config.yaml b/config.yaml index 0d06a3d..114f382 100644 --- a/config.yaml +++ b/config.yaml @@ -12,6 +12,7 @@ cawsr: #active: true # part of scenario definition initialisation_budget: 500 # ticks + route_timeout: 300 # seconds agent: srunner/autoagents/autoware_agent mode: 'benchmark' # benchmark or algorithm diff --git a/dev/Dockerfile b/dev/Dockerfile new file mode 100644 index 0000000..45eec4a --- /dev/null +++ b/dev/Dockerfile @@ -0,0 +1,80 @@ +FROM osrf/ros:humble-desktop + +RUN apt -y update && \ + apt install --no-install-recommends -y libpng16-16 libtiff5 libjpeg8 build-essential curl wget git libxerces-c-dev python3-pip vim + +# Create carla user with same UID/GID as the CARLA simulator container +RUN groupadd -g 1000 carla && \ + useradd -m -u 1000 -g carla -s /bin/bash carla && \ + mkdir /autoware_scenario_runner && \ + mkdir /container_tools + +# clone repo +COPY ./docker /autoware_scenario_runner/docker +COPY ./requirements.txt /autoware_scenario_runner/requirements.txt +COPY ./entrypoint.sh /container_tools/entrypoint.sh + +WORKDIR /autoware_scenario_runner + +SHELL [ "/bin/bash", "-c" ] + +RUN mkdir /ros_workspace/ && \ + cd /ros_workspace/ && \ + mv /autoware_scenario_runner/docker/autoware_msgs.tar.xz /ros_workspace/ && \ + tar -xvf /ros_workspace/autoware_msgs.tar.xz && \ + rm -rf /ros_workspace/autoware_msgs.tar.xz && \ + source /opt/ros/humble/setup.bash && \ + apt-get update && \ + apt-get install -y ros-humble-rmw-cyclonedds-cpp ros-humble-tf-transformations && \ + rosdep install -i --from-path /ros_workspace/autoware_msgs/src --rosdistro humble -y && cd autoware_msgs && \ + colcon build + +ENV AUTOWARE_MSG_PKG="/ros_workspace/autoware_msgs/install/setup.bash" +ENV ROS_PKG="/opt/ros/${ROS_DISTRO}/setup.bash" + +# install docker inside container +RUN apt-get update -y && \ + apt-get install ca-certificates curl -y && \ + install -m 0755 -d /etc/apt/keyrings && \ + curl -fsSL https://download.docker.com/linux/ubuntu/gpg -o /etc/apt/keyrings/docker.asc && \ + chmod a+r /etc/apt/keyrings/docker.asc && \ + echo \ + "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.asc] https://download.docker.com/linux/ubuntu \ + $(. /etc/os-release && echo "${UBUNTU_CODENAME:-$VERSION_CODENAME}") stable" | \ + tee /etc/apt/sources.list.d/docker.list > /dev/null && \ + apt-get update -y && \ + apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin -y + +# install pip packages and setup docker entrypoint +RUN python3 -m pip install -r requirements.txt && \ + mkdir -p /carla_api && \ + mv /autoware_scenario_runner/docker/PythonAPI.tar /carla_api/ && \ + tar -xvf /carla_api/PythonAPI.tar -C /carla_api && \ + rm -rf /carla_api/PythonAPI.tar && \ + cd /container_tools && \ + chmod +x /container_tools/entrypoint.sh && \ + mkdir logs + +# update CYCLONE DDS Config for ROS +RUN mkdir /cyclonedds && \ + mv /autoware_scenario_runner/docker/cyclonedds_local.xml /cyclonedds/ && \ + mv /autoware_scenario_runner/docker/cyclonedds_distributed.xml /cyclonedds/ && \ + rm -rf /autoware_scenario_runner/docker && \ + echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /home/carla/.bashrc && \ + echo "alias rossrc='source ${AUTOWARE_MSG_PKG} && source ${ROS_PKG} && echo Sourced'" >> /home/carla/.bashrc + +ENV CARLA_API_ROOT="/carla_api/PythonAPI" +ENV PYTHONPATH="${PYTHONPATH}:${CARLA_API_ROOT}/carla/agents:${CARLA_API_ROOT}/carla" + +# Give carla user ownership of necessary directories +RUN chown -R carla:carla /autoware_scenario_runner && \ + chown -R carla:carla /carla_api && \ + chown -R carla:carla /container_tools && \ + chown -R carla:carla /ros_workspace && \ + chown -R carla:carla /cyclonedds && \ + usermod -aG docker carla + +# Switch to carla user +USER carla + +ENTRYPOINT [ "/container_tools/entrypoint.sh" ] diff --git a/dev/pull_workspace.sh b/dev/pull_workspace.sh new file mode 100755 index 0000000..52b2615 --- /dev/null +++ b/dev/pull_workspace.sh @@ -0,0 +1,4 @@ +#!/bin/bash +# clone into current directory +git clone https://github.com/Intelligent-Testing-Lab/cawsr_workspace.git +find ./cawsr_workspace -maxdepth 1 ! -name ".env" ! -name ".git" ! -name ".gitignore" ! -name "requirements.txt" ! -name "README.md" ! -name "docker-compose.yml" -exec mv {} . \; diff --git a/docker/cyclonedds.xml b/docker/cyclonedds.xml new file mode 100644 index 0000000..e69de29 diff --git a/docs/DEVELOPMENT.md b/docs/DEVELOPMENT.md new file mode 100644 index 0000000..8a3fdda --- /dev/null +++ b/docs/DEVELOPMENT.md @@ -0,0 +1,29 @@ +# Development +## Setup + +Firstly, ensure you have the dependencies installed, and successfuly set up CAWSR following the [README.md](../README.md). +Next, move `dev/Dockerfile` to the root directory and build the development image: +```bash +docker build -f Dockerfile -t cawsr_dev . +``` + +This will create a docker image tagged `cawsr_dev` that you can use for development. + +## Workspace +To pull the cawsr workspace into the root of the repository, use the following script: +```bash +chmod +x dev/pull_workspace.sh +./dev/pull_workspace.sh +``` +This will be mounted into the dev container to allow you to configure CAWSR as usual. See [README.md](../README.md) for more information on configuring CAWSR. + +## Running CAWSR +The `dev/docker-compose.yml` file includes the services needed to launch the development container. Ensure the file is in the root of the repository and run: +```bash +docker compose -f dev/docker-compose.yml up cawsr autoware-latest +``` + +Development follows a similar pattern to the operation of CAWSR - simply start the autoware container and CAWSR. The `cawsr_dev` image mounts root of the repository into the container, so any changes you make are applied in CAWSR. If you need to configure CAWSR, either edit `dev/.env` to change the runtime configuration, or `configs/` to configure CAWSR. + +## Contributing +See [CONTRIBUTING.md](../CONTRIBUTING.md) diff --git a/engine.sh b/engine.sh new file mode 100644 index 0000000..6684a9e --- /dev/null +++ b/engine.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +# start the autoware container with docker compose +docker compose -f docker-compose-autoware.yaml up -d + +# enter the container and start autoware +docker exec -it autoware bash -c "source /opt/ros/humble/setup.bash && source /workspace/install/setup.bash && ros2 topic pub -1 /autoware/restart autoware_cawsr_msgs/msg/AutowareRestart "{carla_map: 'Town01', ego_name: 'ego_vehicle'}"" diff --git a/paper.bib b/paper.bib index 0eff17b..99716a4 100644 --- a/paper.bib +++ b/paper.bib @@ -154,3 +154,11 @@ @INPROCEEDINGS{9294422 pages={1-6}, keywords={Sensors;Autonomous vehicles;Sensor systems;Engines;Bridges;Three-dimensional displays;Vehicle dynamics}, doi={10.1109/ITSC45102.2020.9294422}} + + +@inproceedings{awcl_26, + title = {Empirical mapping of technical bottlenecks in autonomous driving: A cross-platform evaluation of simulated and real-world performance}, + author = {Kaljavesi, Gemb and Majstorovic, Domagoj and Clement, Maxime and Diermeyer, Frank}, + year = {2026}, + howpublished = {Preprint} +} diff --git a/paper.md b/paper.md index ab46dfb..81553d1 100644 --- a/paper.md +++ b/paper.md @@ -34,7 +34,7 @@ bibliography: paper.bib # Summary -`CAWSR` (**C**ARLA-**A**uto**W**are-**S**cenario **R**unner) facilitates the simulation-based testing of the open-source autonomous driving system, Autoware, within CARLA, the state-of-the-art open-source driving simulator. Building on existing tools, this project introduces a research-oriented testing framework for the execution of complex driving scenarios, as well as supporting implementation of a wide range of verification strategies. +`CAWSR` (**C**ARLA-**A**uto**W**are-**S**cenario **R**unner) facilitates the simulation-based testing of the open-source autonomous driving system, Autoware, within CARLA, the state-of-the-art open-source driving simulator. Building on existing tools, this project introduces a research-oriented testing framework for the execution of complex driving scenarios, as well as supporting the implementation of a wide range of verification strategies. # Statement of Need @@ -53,8 +53,6 @@ This gap has created a significant bottleneck for the research community. Previously, researchers developing scenario generation algorithms mainly relied on combining Apollo with the LGSVL simulator [@9294422]. However, LGSVL is now outdated, with official support ending in January 2022. This leaves many researchers without a suitable industry-grade "subject" for evaluating their algorithms. -While recent tools like PCLA [@tehrani2025pcla] attempt to simplify deploying Autoware (and other ADS implementations) into CARLA, they focus primarily on simplifying the ADS implementations and abstracting the setup process across different CARLA versions. -They lack the deep integration required between the agent and simulator to execute complex, route-based scenarios. `CAWSR` aims to bridge this gap by enabling the evaluation of Autoware in complex driving scenarios within CARLA. By building on the established CARLA platform, this work provides a modern replacement for the outdated Apollo/LGSVL workflow. @@ -67,10 +65,23 @@ This facilitates a wide range of verification strategies based on common metrics Lastly, it is worth noting that simulators can often introduce unintended nondeterminism, which leads to inconsistent test results [@9793395; @osikowicz2025empirically]. Therefore, `CAWSR` is designed to minimise such nondeterminism throughout the evaluation pipeline. +# State of the Field -# Tool Overview +Simulation-based testing of Autonomous Vehicles spans several tools, but CAWSR occupies a unique niche at the intersection of industry-grade ADS integration, scenario-based testing, and programmatic scenario generation. -`CAWSR` is a fully synchronous testing framework that directly integrates the CARLA simulator, Scenario Runner (as the scenario executor), and Autoware (as the System Under Test) to facilitate autonomous driving testing research. The tool is distributed as a containerized deployment using Docker and currently supports two modes of operation: +**Simulators**: CARLA [@carla_sim] and SR [@carla_scenario_runner_2025] play a key role as the de-facto standard for open-source ADS research. However, their driving agent architecture inherently assumes a black-box model (for a good reason to encapsulate the ADS details), lacking the necessary mechanism to manage the complex massage passing of a ROS2-based system like Autoware. + +**Autoware Integrations**: The CARLA-Autoware Bridge [@carlaautowarebridge] provides low-level data translation between CARLA and ROS2, serving as a vital dependency for our work. However, it does not provide any scenario execution capabilities. PCLA [@tehrani2025pcla] simplifies the deployment of multiple pretrained ML-based driving agents across multiple CARLA versions, but abstracts away the deep simulator-agent integration required for modular ADS like Autoware. + +Recent concurrent developments, such as the autoware-carla leaderboard [@awcl_26], have also recognised this gap, introducing support for executing predefined Leaderboard scenarios with Autoware in CARLA. While these efforts confirm the urgent need, they optimise mainly for static benchmark execution. CAWSR diverges in its fundamental design by exposing a programmatic interface tailored for algorithmic, iterative scenario generation, which is a strict prerequisite for systematic ADS testing research. + +**Build vs. Contribute**: The state of the field presents a clear rationale for CAWSR as a new tool rather than a contribution to an existing project. Extending SR directly to support Autoware would require fundamentally rewriting its core agent model, breaking backwards compatibility for ML-based users. Therefore, building CAWSR as a standalone orchestration layer that leverages SR's underlying behavior trees, while maintaining a specialised ROS2 agent interface, presents the most viable scholarly contribution. + + +# Software Design + + +`CAWSR` is a fully synchronous testing framework that directly integrates the CARLA simulator, Scenario Runner (as the scenario executor), and Autoware (as the System Under Test) to facilitate autonomous driving testing research. The tool is distributed as a containerized deployment using Docker to manage complex dependencies and simplify the setup process. Currently, two modes of operation are supported: 1. *Scenario Generation Mode:* Enables the dynamic generation and execution of scenarios (e.g. iterative scenario generation) provided by a user-defined algorithm. This is particularly useful for assessing the performance of new simulation-based ADS testing techniques. 2. *Benchmark Mode:* Allows the execution of a predefined set of scenario definitions provided by the user. This is useful for standardised evaluations and comparisons between different driving agents. @@ -85,7 +96,8 @@ The evaluation pipeline is engineered to be fully synchronous, minimising uninte - JSON Parser: Translates the *scenario_definition* (see \autoref{fig:scenario_domain}) into a Behavior Tree (BT). It utilises Scenario Runner's *Atomic Behaviours* and *Atomic Conditions* as modular primitives to define discrete actions (e.g., spawning pedestrians) and logic triggers. -- ScenarioManager: Orchestrates the simulation loop by evaluating the BT to update actor states and triggering CARLA simulation ticks. Execution terminates based on CARLA Leaderboard criteria [@carla_leaderboard], as summarised in \autoref{tab:termination_criteria}. Post-execution, the module calculates the Driving Score (DS) according to the official leaderboard metrics. + +- ScenarioManager [@carla_scenario_runner_2025]: Orchestrates the simulation loop by evaluating the BT to update actor states and triggering CARLA simulation ticks. Execution terminates based on CARLA Leaderboard criteria [@carla_leaderboard], as summarised in \autoref{tab:termination_criteria}. Post-execution, the module calculates the Driving Score (DS) according to the official leaderboard metrics. - Agent and CarlaBridge: The Agent manages the ROS2 connection to Autoware. At each timestep, the CarlaBridge [@carlaautowarebridge] transforms CARLA snapshots and sensor data into the Autoware coordinate system. Autoware processes these inputs to issue control commands, which the Agent then applies to the ego vehicle. @@ -101,11 +113,20 @@ This model is based on the format introduced by Scenario Runner, facilitating su ![Scenario definition domain model.\label{fig:scenario_domain}](./docs/resources/scenario_domain.pdf) -# Conclusion +# Research Impact + +`CAWSR` provides a significant research impact by bridging the gap between the widely-used CARLA simulator and the industry-grade Autoware ADS. +It addresses a critical bottleneck in the ADS testing research community by offering a modern replacement for the outdated Apollo/LGSVL workflow, which has lacked official support since 2022. + +It enhances research reproducibility by implementing a fully synchronous evaluation pipeline that minimises unintended nondeterminism in simulation-based testing. +To ensure community readiness and ease of use, it is distributed as a Docker container. + +Furthermore, by adopting CARLA Leaderboard metrics, `CAWSR` enables researchers to directly compare Autoware with other state-of-the-art driving agents in the CARLA environment. +It provides an essential foundation for the systematic evaluation of various testing approaches for ADS. -To summarise, `CAWSR` provides ADS testing research community an easy to use Autoware evaluation pipeline. -We hope that this work can facilitate the evaluation of new testing approaches on a state of the art driving system. +# AI Usage Disclosure +Generative AI tools were used in this work solely to support high-level research concepts and structural ideas. All software implementation, including the source code, architecture, and deployment scripts, was authored entirely by the researchers without AI assistance. # Acknowledgements diff --git a/paper.pdf b/paper.pdf index e22b0df..2576214 100644 Binary files a/paper.pdf and b/paper.pdf differ diff --git a/srunner/autoagents/agent_wrapper.py b/srunner/autoagents/agent_wrapper.py index 2a9bf2f..851e441 100644 --- a/srunner/autoagents/agent_wrapper.py +++ b/srunner/autoagents/agent_wrapper.py @@ -26,13 +26,13 @@ def __init__(self, agent): """ self._agent = agent - def __call__( - self, - ): + def __call__(self, timestamp=None): """ - Pass the call directly to the agent + Pass the call directly to the agent. + If timestamp is provided, forward it to propagate the CARLA + snapshot through the chain so the agent does not re-acquire it. """ - return self._agent() + return self._agent(timestamp) def setup_sensors(self, vehicle, debug_mode=False): """ diff --git a/srunner/autoagents/autonomous_agent.py b/srunner/autoagents/autonomous_agent.py index 7980efd..2eac8d5 100644 --- a/srunner/autoagents/autonomous_agent.py +++ b/srunner/autoagents/autonomous_agent.py @@ -22,6 +22,7 @@ def __init__(self, path_to_conf_file): # current global plans to reach a destination self._global_plan = None self._global_plan_world_coord = None + self._carla_timestamp = None # this data structure will contain all sensor data # self.sensor_interface = SensorInterface() @@ -72,18 +73,15 @@ def destroy(self): """ pass - def __call__(self): + def __call__(self, timestamp=None): """ Execute the agent call, e.g. agent() Returns the next vehicle controls - """ - # no need for any of this, handled by autoware - # input_data = self.sensor_interface.get_data() - # timestamp = GameTime.get_time() - # wallclock = GameTime.get_wallclocktime() - # print('======[Agent] Wallclock_time = {} / Sim_time = {}'.format(wallclock, timestamp)) - # control.manual_gear_shift = False + If timestamp (carla.Timestamp) is provided, store it so that + subclasses can use the same CARLA snapshot without re-acquiring it. + """ + self._carla_timestamp = timestamp self.run_step() diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 5692b91..2100cf9 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -17,6 +17,8 @@ from srunner.autoagents.autoware_carla_interface.carla_autoware import ( InitializeInterface, ) +from srunner.scenariomanager.timer import GameTime +from srunner.tools.CARLA_manager import CARLAManager import threading import rclpy @@ -31,6 +33,7 @@ class AutowareAgent(AutonomousAgent): timestamp = None agent_set_route = False + scenario_loaded = False counter = 0 last_tick = time.perf_counter_ns() @@ -46,6 +49,7 @@ def setup(self, config: EnvironmentConfig) -> None: rclpy.init(args=None) self.config = config + self.tick_delta = CARLAManager.FIXED_DELTA_SECONDS # initialise autoware state object self.autoware_state = autoware_state.AutowareState("ego_vehicle", None) @@ -56,6 +60,27 @@ def setup(self, config: EnvironmentConfig) -> None: self.carla_interface = InitializeInterface(self.config, self._node) self.state_node = state_node.StateNode(self.autoware_state, self._node_state) + + try: + # run state node and cawsr bridge in separate executors + self._executors = [ + rclpy.executors.SingleThreadedExecutor(), + rclpy.executors.SingleThreadedExecutor(), + ] + + self._executors[0].add_node(self._node) + self._executors[1].add_node(self._node_state) + + self._executor_threads = [ + threading.Thread(target=self._executors[0].spin, daemon=True), + threading.Thread(target=self._executors[1].spin, daemon=True), + ] + except rclpy.executors.ExternalShutdownException: + logger.info("Node Executor shutdown externally...") + + for thread in self._executor_threads: + thread.start() + self.state_node.reset_autoware(self.config.town, self.config.ego_name) self.route_node = route_node.RouteNode(self.autoware_state, self._node_state) @@ -63,25 +88,7 @@ def setup(self, config: EnvironmentConfig) -> None: self.autoware_state, self._node_state ) - # run state note and cawsr bridge in separate executors - self._executors = [ - rclpy.executors.SingleThreadedExecutor(), - rclpy.executors.SingleThreadedExecutor(), - ] - - self._executors[0].add_node(self._node) - self._executors[1].add_node(self._node_state) - - self._executor_threads = [ - threading.Thread(target=self._executors[0].spin, daemon=True), - threading.Thread(target=self._executors[1].spin, daemon=True), - ] - - for thread in self._executor_threads: - thread.start() - self.sent_route = False - self.initialised = False self.carla_interface.load_world() self.carla_interface.run_bridge() @@ -92,7 +99,6 @@ def set_route(self) -> None: self.goal_pose_world = self._global_plan_world_coord[-1] self.waypoints_world = self._global_plan_world_coord[:-1] - # autoware cannot handle many waypoints, becomes unreliable n_waypoints = len(self.waypoints_world) segment_size = int(n_waypoints / 3) @@ -115,10 +121,17 @@ def _convert_to_waypoint(self, point): node=self._node, ) - def destroy(self) -> None: + def cleanup(self) -> None: + self.destroy(cleanup=True) + + def destroy(self, cleanup=False) -> None: """Cleanup""" logger.info("Sending shutdown signal to autoware...") - self.state_node.reset_autoware(self.config.town, self.config.ego_name) + if not cleanup: + self.state_node.reset_autoware(self.config.town, self.config.ego_name) + else: + self.state_node.shutdown_autoware() + logger.info("Waiting for shutdown. Starting Node cleanup") time.sleep(1) # sleep for 1 second for sanity try: @@ -130,17 +143,17 @@ def destroy(self) -> None: except RuntimeError: logger.info("Failed to clean up executor thread...") - def cleanup(self) -> None: - self.destroy() - - def run_step_init(self) -> bool: - """Route Initialisation loop + def run_step(self) -> None: + """Tick method containing all logic based on autoware state""" + self.counter += 1 + if self.counter % int(1 / self.tick_delta) == 0: + logger.info( + f"Ticked 1 second game-time, Current time: {GameTime.get_time():.2f}s, Current tick: {self.counter}. Ratio of 1s:{time.perf_counter_ns() - self.last_tick}s (Sim vs Wall)" + ) - Ticks CARLA and Autoware, allowing the agent to localise and plan the route. - Operates on a fixed tick budget to ensure determinism. If the agent goes over the budget, it is treated as a failure. - """ + self.last_tick = time.perf_counter_ns() - self.carla_interface.tick_bridge() + self.carla_interface.tick_bridge(self._carla_timestamp) if not self.agent_set_route: self.set_route() @@ -163,29 +176,9 @@ def run_step_init(self) -> bool: self.route_node.publish_route(goal_pose, waypoints) self.sent_route = True - # check if the current route is set and we are able to send engage - if self.autoware_state.route_set() and not self.autoware_state.sent_engage: - return True - - return False - - def run_step(self) -> None: - """Tick method containing all logic based on autoware state""" - self.counter += 1 - if self.counter % 20 == 0: - logger.info( - f"Ticked 1 second game-time, actual tick is {(time.perf_counter_ns() - self.last_tick) / 1e6}ms" - ) - self.last_tick = time.perf_counter_ns() - - # if not self.initialised: - # self.initialised = self.run_step_init() - # - # if self.initialised: - # logger.info("Set agent route!") - - # check if the current route is set and we can publish engage - if self.autoware_state.route_set() and not self.autoware_state.sent_engage: + if ( + self.scenario_loaded + and self.autoware_state.route_set() + and not self.autoware_state.sent_engage + ): self.autoware_node.publish_engage(True) - - self.carla_interface.tick_bridge() diff --git a/srunner/autoagents/autoware_carla_interface/carla_autoware.py b/srunner/autoagents/autoware_carla_interface/carla_autoware.py index ce6b070..aefd310 100644 --- a/srunner/autoagents/autoware_carla_interface/carla_autoware.py +++ b/srunner/autoagents/autoware_carla_interface/carla_autoware.py @@ -34,17 +34,15 @@ def __init__(self): self.sensor = None self.ego_actor = None self.running = False - self.timestamp_last_run = 0.0 self.timeout = 20.0 def _stop_loop(self): self.running = False def _tick_sensor(self, timestamp): - if self.timestamp_last_run < timestamp.elapsed_seconds and self.running: - self.timestamp_last_run = timestamp.elapsed_seconds + if self.running: try: - ego_action = self.sensor() + ego_action = self.sensor(timestamp) except SensorReceivedNoData as e: raise RuntimeError(e) self.ego_actor.apply_control(ego_action) @@ -52,7 +50,7 @@ def _tick_sensor(self, timestamp): class InitializeInterface(object): def __init__(self, config: EnvironmentConfig, node): - self.interface = carla_ros2_interface(node) + self.interface = carla_ros2_interface(node, config) self.world = None self.sensor_wrapper = None self.ego_actor = None @@ -76,13 +74,13 @@ def run_bridge(self): self.bridge_loop.start_game_time = GameTime.get_time() self.bridge_loop.running = True - def tick_bridge(self): - timestamp = None - world = CarlaDataProvider.get_world() - if world: - snapshot = world.get_snapshot() - if snapshot: - timestamp = snapshot.timestamp + def tick_bridge(self, timestamp=None): + if timestamp is None: + world = CarlaDataProvider.get_world() + if world: + snapshot = world.get_snapshot() + if snapshot: + timestamp = snapshot.timestamp if timestamp: self.prev_tick_wall_time = time.time() self.bridge_loop._tick_sensor(timestamp) diff --git a/srunner/autoagents/autoware_carla_interface/carla_ros.py b/srunner/autoagents/autoware_carla_interface/carla_ros.py index 398f759..c60c0f5 100644 --- a/srunner/autoagents/autoware_carla_interface/carla_ros.py +++ b/srunner/autoagents/autoware_carla_interface/carla_ros.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License.sr/bin/env python -import json +from __future__ import annotations + import math +import queue # pylint: disable=import-error from autoware_vehicle_msgs.msg import ControlModeReport @@ -26,8 +28,6 @@ from geometry_msgs.msg import Pose from geometry_msgs.msg import PoseWithCovarianceStamped import numpy -import datetime -import pathlib from rosgraph_msgs.msg import Clock from sensor_msgs.msg import CameraInfo from sensor_msgs.msg import Image @@ -40,6 +40,7 @@ from transforms3d.euler import euler2quat from srunner.scenariomanager.timer import GameTime +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.autoagents.autoware_carla_interface.modules.carla_utils import ( carla_location_to_ros_point, ) @@ -53,15 +54,20 @@ from srunner.autoagents.autoware_carla_interface.modules.carla_wrapper import ( SensorInterface, ) +from srunner.tools.CARLA_manager import CARLAManager +from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig + +import rclpy class carla_ros2_interface(object): - def __init__(self, node): + def __init__(self, node: rclpy.node.Node, config: EnvironmentConfig): self.sensor_interface = SensorInterface() + self.config = config self.prev_timestamp = None self.prev_steer_output = 0.0 self.tau = 0.2 - self.timestamp = None + self.timestamp = 0.0 self.ego_actor = None self.physics_control = None self.channels = 0 @@ -69,6 +75,7 @@ def __init__(self, node): self.id_to_camera_info_map = {} self.cv_bridge = CvBridge() self.first_ = True + self._pending_initialpose = None self.pub_lidar = {} self.sensor_frequencies = { "top": 11, @@ -77,12 +84,14 @@ def __init__(self, node): "camera": 11, "imu": 50, "status": 50, - "pose": 2, + "pose": 20, } + self.publish_prev_times = { - sensor: datetime.datetime.now() for sensor in self.sensor_frequencies + sensor: -1000.0 for sensor in self.sensor_frequencies } + self.delta = CARLAManager.FIXED_DELTA_SECONDS # delta in seconds self.ros2_node = node self.clock_publisher = self.ros2_node.create_publisher(Clock, "/clock", 10) @@ -90,11 +99,9 @@ def __init__(self, node): obj_clock.clock = Time(sec=int(0)) self.clock_publisher.publish(obj_clock) - # load sensor config and create publishers - sensors_config = pathlib.Path( - "srunner/autoagents/autoware_carla_interface/objects/sensors.json" - ) - self.sensors = json.load(open(sensors_config.absolute())) + self.sensors = { + "sensors": [sensor.sensor_dict() for sensor in self.config.sensor_config] + } self.sub_control = self.ros2_node.create_subscription( ActuationCommandStamped, @@ -107,7 +114,9 @@ def __init__(self, node): PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1 ) - self.current_control = carla.VehicleControl() + self._control_queue = queue.Queue(1) + self._control_queue.put_nowait((0, carla.VehicleControl())) + self._last_control = carla.VehicleControl() self.pub_pose_with_cov = self.ros2_node.create_publisher( PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1 @@ -137,6 +146,15 @@ def __init__(self, node): self.pub_camera_info = self.ros2_node.create_publisher( CameraInfo, "/sensing/camera/traffic_light/camera_info", 1 ) + + self.pub_camera_yolo_info = self.ros2_node.create_publisher( + CameraInfo, "/sensing/camera/camera4/camera_info", 1 + ) + + self.pub_camera_yolo = self.ros2_node.create_publisher( + Image, "/sensing/camera/camera4/image_raw", 1 + ) + elif sensor["type"] == "sensor.lidar.ray_cast": if sensor["id"] in self.sensor_frequencies: self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( @@ -158,36 +176,47 @@ def __init__(self, node): ) pass - def __call__(self): - input_data = self.sensor_interface.get_data() - timestamp = GameTime.get_time() + def __call__(self, timestamp=None): + if timestamp is None: + timestamp = ( + CarlaDataProvider.get_world().get_snapshot().timestamp.elapsed_seconds + ) + elif hasattr(timestamp, "elapsed_seconds"): + timestamp = timestamp.elapsed_seconds + + input_data = self.sensor_interface.get_data(expected_time=timestamp) control = self.run_step(input_data, timestamp) return control def checkFrequency(self, sensor): - time_delta = ( - datetime.datetime.now() - self.publish_prev_times[sensor] - ).microseconds / 1000000.0 + time_delta = GameTime.get_time() - self.publish_prev_times[sensor] + if time_delta <= 0.0: + return False if 1.0 / time_delta >= self.sensor_frequencies[sensor]: return True return False - def get_msg_header(self, frame_id): + def get_msg_header(self, frame_id, timestamp=None): """Obtain and modify ROS message header.""" header = Header() header.frame_id = frame_id - seconds = int(self.timestamp) # type: ignore - nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) # type: ignore + ts = timestamp if timestamp is not None else self.timestamp + seconds = int(ts) + nanoseconds = int((ts - int(ts)) * 1000000000.0) header.stamp = Time(sec=seconds, nanosec=nanoseconds) return header - def lidar(self, carla_lidar_measurement, id_): + def lidar(self, carla_lidar_measurement, id_, timestamp=None): """Transform the received lidar measurement into a ROS point cloud message.""" + if id_ not in self.publish_prev_times or id_ not in self.pub_lidar: + return if self.checkFrequency(id_): return - self.publish_prev_times[id_] = datetime.datetime.now() + self.publish_prev_times[id_] = GameTime.get_time() - header = self.get_msg_header(frame_id="velodyne_top_changed") + header = self.get_msg_header( + frame_id="velodyne_top_changed", timestamp=timestamp + ) fields = [ PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), @@ -249,18 +278,15 @@ def initialpose_callback(self, data): pose = data.pose.pose pose.position.z += 2.0 carla_pose_transform = ros_pose_to_carla_transform(pose) - if self.ego_actor is not None: - self.ego_actor.set_transform(carla_pose_transform) - else: - print("Can't find Ego Vehicle") + self._pending_initialpose = carla_pose_transform - def pose(self): + def pose(self, timestamp=None): """Transform odometry data to Pose and publish Pose with Covariance message.""" if self.checkFrequency("pose"): return - self.publish_prev_times["pose"] = datetime.datetime.now() + self.publish_prev_times["pose"] = GameTime.get_time() - header = self.get_msg_header(frame_id="map") + header = self.get_msg_header(frame_id="map", timestamp=timestamp) out_pose_with_cov = PoseWithCovarianceStamped() pose_carla = Pose() pose_carla.position = carla_location_to_ros_point( @@ -327,7 +353,7 @@ def _build_camera_info(self, camera_actor): camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] self._camera_info = camera_info - def camera(self, carla_camera_data): + def camera(self, carla_camera_data, timestamp=None): """Transform the received carla camera data into a ROS image and info message and publish.""" while self.first_: self._camera_info_ = self._build_camera_info(carla_camera_data) @@ -335,7 +361,7 @@ def camera(self, carla_camera_data): if self.checkFrequency("camera"): return - self.publish_prev_times["camera"] = datetime.datetime.now() + self.publish_prev_times["camera"] = GameTime.get_time() image_data_array = numpy.ndarray( shape=(carla_camera_data.height, carla_camera_data.width, 4), @@ -345,21 +371,34 @@ def camera(self, carla_camera_data): # cspell:ignore interp bgra img_msg = self.cv_bridge.cv2_to_imgmsg(image_data_array, encoding="bgra8") img_msg.header = self.get_msg_header( - frame_id="traffic_light_left_camera/camera_optical_link" + frame_id="traffic_light_left_camera/camera_optical_link", + timestamp=timestamp, ) cam_info = self._camera_info cam_info.header = img_msg.header self.pub_camera_info.publish(cam_info) self.pub_camera.publish(img_msg) - def imu(self, carla_imu_measurement): + # build another message to publish for yolo + img_msg.header = self.get_msg_header( + frame_id="camera4/camera_optical_link", timestamp=timestamp + ) + + cam_info = self._camera_info + cam_info.header = img_msg.header + self.pub_camera_yolo_info.publish(cam_info) + self.pub_camera_yolo.publish(img_msg) + + def imu(self, carla_imu_measurement, timestamp=None): """Transform a received imu measurement into a ROS Imu message and publish Imu message.""" if self.checkFrequency("imu"): return - self.publish_prev_times["imu"] = datetime.datetime.now() + self.publish_prev_times["imu"] = GameTime.get_time() imu_msg = Imu() - imu_msg.header = self.get_msg_header(frame_id="tamagawa/imu_link_changed") + imu_msg.header = self.get_msg_header( + frame_id="tamagawa/imu_link_changed", timestamp=timestamp + ) imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z @@ -382,17 +421,19 @@ def imu(self, carla_imu_measurement): def first_order_steering(self, steer_input): """First order steering model.""" - steer_output = 0.0 + timestamp = self.timestamp if self.prev_timestamp is None: - self.prev_timestamp = self.timestamp + self.prev_timestamp = timestamp - dt = self.timestamp - self.prev_timestamp # type: ignore - if dt > 0.0: - steer_output = self.prev_steer_output + ( - steer_input - self.prev_steer_output - ) * (dt / (self.tau + dt)) + dt = timestamp - self.prev_timestamp # type: ignore + if dt <= 0.0: + return self.prev_steer_output + + steer_output = self.prev_steer_output + ( + steer_input - self.prev_steer_output + ) * (dt / (self.tau + dt)) self.prev_steer_output = steer_output - self.prev_timestamp = self.timestamp + self.prev_timestamp = timestamp return steer_output def control_callback(self, in_cmd): @@ -409,14 +450,19 @@ def control_callback(self, in_cmd): self.first_order_steering(-in_cmd.actuation.steer_cmd) * max_steer_ratio ) out_cmd.brake = in_cmd.actuation.brake_cmd - self.current_control = out_cmd + + try: + self._control_queue.put_nowait((self.timestamp, out_cmd)) + self._last_control = out_cmd + except queue.Full: + pass def ego_status(self): """Publish ego vehicle status.""" if self.checkFrequency("status"): return - self.publish_prev_times["status"] = datetime.datetime.now() + self.publish_prev_times["status"] = GameTime.get_time() # convert velocity from cartesian to ego frame trans_mat = numpy.array(self.ego_actor.get_transform().get_matrix()).reshape( @@ -474,6 +520,13 @@ def ego_status(self): def run_step(self, input_data, timestamp): self.timestamp = timestamp + if self._pending_initialpose is not None: + if self.ego_actor is not None: + self.ego_actor.set_transform(self._pending_initialpose) + else: + print("Can't find Ego Vehicle") + self._pending_initialpose = None + seconds = int(self.timestamp) nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) obj_clock = Clock() @@ -483,19 +536,28 @@ def run_step(self, input_data, timestamp): # publish data of all sensors for key, data in input_data.items(): sensor_type = self.id_to_sensor_type_map[key] + sensor_timestamp = data[0] + sensor_data = data[1] if sensor_type == "sensor.camera.rgb": - self.camera(data[1]) + self.camera(sensor_data, timestamp=sensor_timestamp) elif sensor_type == "sensor.other.gnss": - self.pose() + self.pose(timestamp=sensor_timestamp) elif sensor_type == "sensor.lidar.ray_cast": - self.lidar(data[1], key) + self.lidar(sensor_data, key, timestamp=sensor_timestamp) elif sensor_type == "sensor.other.imu": - self.imu(data[1]) + self.imu(sensor_data, timestamp=sensor_timestamp) else: - self.ros2_node.get_logger().info("No Publisher for [{key}] Sensor") + self.ros2_node.get_logger().info(f"No Publisher for [{key}] Sensor") self.ego_status() - return self.current_control + + try: + _, control = self._control_queue.get_nowait() + self._last_control = control + except queue.Empty: + control = self._last_control + + return control def shutdown(self): self.ros2_node.destroy_node() diff --git a/srunner/autoagents/autoware_carla_interface/modules/carla_wrapper.py b/srunner/autoagents/autoware_carla_interface/modules/carla_wrapper.py index 0fa4276..bd34440 100644 --- a/srunner/autoagents/autoware_carla_interface/modules/carla_wrapper.py +++ b/srunner/autoagents/autoware_carla_interface/modules/carla_wrapper.py @@ -15,6 +15,7 @@ from __future__ import print_function import logging +import time from queue import Empty from queue import Queue @@ -22,6 +23,9 @@ import numpy as np # type: ignore from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.tools.CARLA_manager import CARLAManager + +logger = logging.getLogger("scenario-runner") # Sensor Wrapper for Agent @@ -34,6 +38,10 @@ def __init__(self, data, frame): self.data = data self.frame = frame + @property + def timestamp(self): + return self.frame * CARLAManager.FIXED_DELTA_SECONDS + class CallBack(object): def __init__(self, tag, sensor, data_provider): @@ -54,27 +62,27 @@ def __call__(self, data): elif isinstance(data, GenericMeasurement): self._parse_pseudo_sensor(data, self._tag) else: - logging.error("No callback method for this sensor.") + logger.error("No callback method for this sensor.") # Parsing CARLA physical Sensors def _parse_image_cb(self, image, tag): - self._data_provider.update_sensor(tag, image, image.frame) + self._data_provider.update_sensor(tag, image, image.timestamp) def _parse_lidar_cb(self, lidar_data, tag): - self._data_provider.update_sensor(tag, lidar_data, lidar_data.frame) + self._data_provider.update_sensor(tag, lidar_data, lidar_data.timestamp) def _parse_imu_cb(self, imu_data, tag): - self._data_provider.update_sensor(tag, imu_data, imu_data.frame) + self._data_provider.update_sensor(tag, imu_data, imu_data.timestamp) def _parse_gnss_cb(self, gnss_data, tag): array = np.array( [gnss_data.latitude, gnss_data.longitude, gnss_data.altitude], dtype=np.float64, ) - self._data_provider.update_sensor(tag, array, gnss_data.frame) + self._data_provider.update_sensor(tag, array, gnss_data.timestamp) def _parse_pseudo_sensor(self, package, tag): - self._data_provider.update_sensor(tag, package.data, package.frame) + self._data_provider.update_sensor(tag, package.data, package.timestamp) class SensorInterface(object): @@ -83,6 +91,7 @@ def __init__(self): self._new_data_buffers = Queue() self._queue_timeout = 10 self.tag = "" + self._last_data = {} def register_sensor(self, tag, sensor): self.tag = tag @@ -95,20 +104,74 @@ def update_sensor(self, tag, data, timestamp): if tag not in self._sensors_objects: raise ValueError(f"Sensor with tag [{tag}] has not been created") + if hasattr(timestamp, "elapsed_seconds"): + timestamp = timestamp.elapsed_seconds + self._new_data_buffers.put((tag, timestamp, data)) - def get_data(self): - try: - data_dict = {} - while len(data_dict.keys()) < len(self._sensors_objects.keys()): - sensor_data = self._new_data_buffers.get(True, self._queue_timeout) - data_dict[sensor_data[0]] = (sensor_data[1], sensor_data[2]) - except Empty: - raise SensorReceivedNoData( - f"Sensor with tag [{self.tag}] took too long to send its data" + def get_data(self, expected_time=None): + # Drain all available entries, updating the per-sensor cache with the + # most recent frame seen for each sensor. After the drain the cache + # holds the freshest data for every registered tag. + # + # If expected_time (elapsed_seconds) is provided, the method ensures at + # least one sensor has data from >= expected_time before returning, so + # callers know callbacks for the current frame have started delivering. + epsilon = 1e-6 + + # Phase 1 – non-blocking update of the persistent cache + while True: + try: + tag, sensor_timestamp, data = self._new_data_buffers.get(False) + self._last_data[tag] = (sensor_timestamp, data) + except Empty: + break + + # Phase 2 – if an expected frame time is given and no sensor has fresh + # data yet, briefly block until at least one callback from the current + # frame arrives (proving callbacks are active), then drain again. + if expected_time is not None: + has_fresh = any( + ts >= expected_time - epsilon for ts, _ in self._last_data.values() ) - - return data_dict + if not has_fresh and self._sensors_objects: + try: + tag, sensor_timestamp, data = self._new_data_buffers.get( + True, 0.001 + ) + self._last_data[tag] = (sensor_timestamp, data) + except Empty: + pass + # Drain anything else that arrived while we waited + while True: + try: + tag, sensor_timestamp, data = self._new_data_buffers.get(False) + self._last_data[tag] = (sensor_timestamp, data) + except Empty: + break + + # Phase 3 – blocking wait for any sensor that has NEVER produced data + missing = set(self._sensors_objects.keys()) - set(self._last_data.keys()) + if missing: + deadline = time.monotonic() + self._queue_timeout + while missing: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + raise SensorReceivedNoData( + f"Sensor(s) {missing} failed to produce initial data" + ) + try: + tag, sensor_timestamp, data = self._new_data_buffers.get( + True, remaining + ) + self._last_data[tag] = (sensor_timestamp, data) + missing.discard(tag) + except Empty: + raise SensorReceivedNoData( + f"Sensor(s) {missing} failed to produce initial data" + ) + + return dict(self._last_data) # Sensor Wrapper @@ -121,8 +184,8 @@ class SensorWrapper(object): def __init__(self, agent): self._agent = agent - def __call__(self): - return self._agent() # type: ignore + def __call__(self, timestamp=None): + return self._agent(timestamp) # type: ignore def setup_sensors(self, vehicle, debug_mode=False): """Create and attach the sensor defined in objects.json.""" diff --git a/srunner/autoagents/autoware_carla_interface/objects/sensors.json b/srunner/autoagents/autoware_carla_interface/objects/sensors.json index 7103118..e4fc2b6 100644 --- a/srunner/autoagents/autoware_carla_interface/objects/sensors.json +++ b/srunner/autoagents/autoware_carla_interface/objects/sensors.json @@ -13,7 +13,8 @@ }, "image_size_x": 1920, "image_size_y": 1080, - "fov": 90.0 + "fov": 90.0, + "publish_frequency": 11.0 }, { "type": "sensor.lidar.ray_cast", @@ -27,12 +28,13 @@ "yaw": 0.0 }, "range": 100, - "channels": 64, - "points_per_second": 300000, + "channels": 128, + "points_per_second": 1200000, "upper_fov": 10.0, "lower_fov": -30.0, "rotation_frequency": 20, - "noise_stddev": 0.0 + "noise_stddev": 0.0, + "publish_frequency": 11.0 }, { "type": "sensor.other.gnss", @@ -44,7 +46,8 @@ "roll": 0.0, "pitch": 0.0, "yaw": 0.0 - } + }, + "publish_frequency": 2.0 }, { "type": "sensor.other.imu", @@ -56,7 +59,8 @@ "roll": 0.0, "pitch": 0.0, "yaw": 0.0 - } + }, + "publish_frequency": 50.0 } ] } diff --git a/srunner/autoagents/autoware_nodes/autoware_node.py b/srunner/autoagents/autoware_nodes/autoware_node.py index 6b7e172..f008fb3 100644 --- a/srunner/autoagents/autoware_nodes/autoware_node.py +++ b/srunner/autoagents/autoware_nodes/autoware_node.py @@ -7,7 +7,6 @@ from typing import Optional -from rclpy.node import Node from autoware_vehicle_msgs.msg import Engage from autoware_adapi_v1_msgs.srv import InitializeLocalization from autoware_adapi_v1_msgs.srv._initialize_localization import ( @@ -26,15 +25,16 @@ logger.propagate = False -class AutowareNode(): +class AutowareNode: engage_topic = "/autoware/engage" localize_service = "/api/localization/initialize" def __init__(self, autoware_state_instance: autoware_state.AutowareState, node): - self.node = node - self.engage_publisher = self.node.create_publisher(Engage, self.engage_topic, 10) + self.engage_publisher = self.node.create_publisher( + Engage, self.engage_topic, 10 + ) self.localize_client = self.node.create_client( InitializeLocalization, self.localize_service diff --git a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py index 6066633..68f9bf2 100644 --- a/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py +++ b/srunner/autoagents/autoware_nodes/autoware_types/waypoint.py @@ -7,7 +7,6 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from visualization_msgs.msg import Marker from tf_transformations import quaternion_from_euler from geometry_msgs.msg import Pose from geometry_msgs.msg import Point @@ -69,7 +68,6 @@ def autoware_from_world_coords(self) -> Pose: return pose - def __str__(self) -> str: if self.pose: return f"{self.pose}" diff --git a/srunner/autoagents/autoware_nodes/route_node.py b/srunner/autoagents/autoware_nodes/route_node.py index ad7bde6..46fbe66 100644 --- a/srunner/autoagents/autoware_nodes/route_node.py +++ b/srunner/autoagents/autoware_nodes/route_node.py @@ -5,7 +5,6 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . -from rclpy.node import Node from geometry_msgs.msg import Pose # Import the service types directly @@ -21,7 +20,8 @@ logger = logging.getLogger("scenario-runner") logger.propagate = False -class RouteNode(): + +class RouteNode: last_goal = None last_waypoints = [] @@ -46,26 +46,22 @@ def __init__(self, autoware_state, node) -> None: ClearRoute, self.clear_route_service_name ) - self.goal_publisher = self.node.create_publisher(PoseStamped, self.goal_topic, 10) + self.goal_publisher = self.node.create_publisher( + PoseStamped, self.goal_topic, 10 + ) self.checkpoint_publisher = self.node.create_publisher( PoseStamped, self.checkpoint_topic, 10 ) # Good practice: Wait for the service server to be available before trying to call it - logger.info( - f"Waiting for '{self.set_route_points_service_name}' service..." - ) + logger.info(f"Waiting for '{self.set_route_points_service_name}' service...") while not self.set_route_client.wait_for_service(timeout_sec=1.0): logger.info( f"Service '{self.set_route_points_service_name}' not available, waiting again..." ) - logger.info( - f"Service '{self.set_route_points_service_name}' available." - ) + logger.info(f"Service '{self.set_route_points_service_name}' available.") - logger.info( - f"Waiting for '{self.clear_route_service_name}' service..." - ) + logger.info(f"Waiting for '{self.clear_route_service_name}' service...") while not self.clear_route_client.wait_for_service(timeout_sec=1.0): logger.info( f"Service '{self.clear_route_service_name}' not available, waiting again..." @@ -129,8 +125,6 @@ def clear_route_response_callback(self, future): if response.status.success: logger.info("Route cleared successfully!") else: - logger.warn( - f"Failed to clear route: {response.status.message}" - ) + logger.warn(f"Failed to clear route: {response.status.message}") except Exception as e: logger.error(f"Service call failed: {e}") diff --git a/srunner/autoagents/autoware_nodes/state_node.py b/srunner/autoagents/autoware_nodes/state_node.py index aeb0c8a..5814255 100644 --- a/srunner/autoagents/autoware_nodes/state_node.py +++ b/srunner/autoagents/autoware_nodes/state_node.py @@ -5,22 +5,20 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . -from rclpy.node import Node from tier4_planning_msgs.msg import RouteState from autoware_adapi_v1_msgs.msg import MotionState from autoware_adapi_v1_msgs.msg import LocalizationInitializationState from srunner.autoagents.agent_state import autoware_state import logging -from autoware_cawsr_msgs.msg import AutowareRestart -import logging +from autoware_cawsr_msgs.msg import AutowareRestart, AutowareShutdown logger = logging.getLogger("scenario-runner") logger.propagate = False -class StateNode(): +class StateNode: route_state = "/planning/mission_planning/state" motion_state = "/api/motion/state" localize_state = "/api/localization/initialization_state" @@ -48,6 +46,10 @@ def __init__(self, autoware_state: autoware_state.AutowareState, node) -> None: AutowareRestart, self.reset_topic, 10 ) + self.shutdown_autoware_pub = self.node.create_publisher( + AutowareShutdown, "/autoware/shutdown", 10 + ) + def route_state_cb(self, route_state_msg: RouteState) -> None: """Set the AutowareState attribute route_state @@ -86,3 +88,13 @@ def reset_autoware(self, carla_map: str, ego_name: str): msg.ego_name = str(ego_name) self.restart_autoware_pub.publish(msg) + + def shutdown_autoware(self): + """Publishes an empty message to shutdown autoware.""" + + # fill with dummy data, since this topic only handles shutting autoware down + msg = AutowareShutdown() + msg.carla_map = "Town01" + msg.ego_name = "ego_vehicle" + + self.shutdown_autoware_pub.publish(msg) diff --git a/srunner/objects/sensors.py b/srunner/objects/sensors.py index 5334872..e4aba56 100644 --- a/srunner/objects/sensors.py +++ b/srunner/objects/sensors.py @@ -1,5 +1,3 @@ -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider - import carla @@ -12,23 +10,40 @@ def __init__(self) -> None: self.type: str = "" self.id: str = "" self.spawn: carla.Transform | None = None - - def _spawn(self, bp_library, vehicle) -> None: - sensor_bp = bp_library.find(str(self.type)) - - ignored_params = ["serealize", "id", "_spawn", "type", "spawn"] - - sensor_params = [attr for attr in dir(self) if attr not in ignored_params] + self.publish_frequency: int = 0 + + def _spawn_to_dict(self) -> dict: + if self.spawn is None: + return {} + return { + "x": self.spawn.location.x, + "y": self.spawn.location.y, + "z": self.spawn.location.z, + "pitch": self.spawn.rotation.pitch, + "roll": self.spawn.rotation.roll, + "yaw": self.spawn.rotation.yaw, + } + + def sensor_dict(self) -> dict: + """Converts all sensor attributes in a dictionary + + Returns: + dict: Dictionary for sensor params in the format expected by autoware_carla_interface + """ + sensor_config = {} + + ignored_attr = ["sensor_dict", "_spawn_to_dict"] + sensor_params = [attr for attr in dir(self) if attr not in ignored_attr] sensor_params = filter(lambda x: not x.startswith("__"), sensor_params) - for param in sensor_params: - sensor_bp.set_attribute(param, str(getattr(self, param))) - - CarlaDataProvider.get_world().spawn_actor(sensor_bp, self.spawn, vehicle) - CarlaDataProvider.get_world().tick() + for sensor_param in sensor_params: + if sensor_param.lower() == "spawn": + sensor_config["spawn_point"] = self._spawn_to_dict() + continue + sensor_config[sensor_param] = getattr(self, sensor_param) - def serealize(self): - return self.type, self.id + print(sensor_config) + return sensor_config class CameraRGB(DefaultSensor): diff --git a/srunner/scenarioconfigs/environment_configuration.py b/srunner/scenarioconfigs/environment_configuration.py index 28a3548..74c0495 100644 --- a/srunner/scenarioconfigs/environment_configuration.py +++ b/srunner/scenarioconfigs/environment_configuration.py @@ -26,3 +26,4 @@ def __init__(self) -> None: self.ego_spawn: carla.Transform | None = None self.sensor_config: list[DefaultSensor] = [] self.route_id: int = 0 + self.iteration: int = 0 diff --git a/srunner/scenarioconfigs/scenario_configuration.py b/srunner/scenarioconfigs/scenario_configuration.py index a6dc6aa..60d6277 100644 --- a/srunner/scenarioconfigs/scenario_configuration.py +++ b/srunner/scenarioconfigs/scenario_configuration.py @@ -13,13 +13,22 @@ class ActorConfigurationData(object): - """ This is a configuration base class to hold model and transform attributes """ - def __init__(self, model, transform, rolename='other', speed=0, autopilot=False, - random=False, color=None, category="car", args=None): + def __init__( + self, + model, + transform, + rolename="other", + speed=0, + autopilot=False, + random=False, + color=None, + category="car", + args=None, + ): self.model = model self.rolename = rolename self.transform = transform @@ -36,30 +45,34 @@ def parse_from_node(node, rolename): static method to initialize an ActorConfigurationData from a given ET tree """ - model = node.attrib.get('model', 'vehicle.*') + model = node.attrib.get("model", "vehicle.*") - pos_x = float(node.attrib.get('x', 0)) - pos_y = float(node.attrib.get('y', 0)) - pos_z = float(node.attrib.get('z', 0)) - yaw = float(node.attrib.get('yaw', 0)) + pos_x = float(node.attrib.get("x", 0)) + pos_y = float(node.attrib.get("y", 0)) + pos_z = float(node.attrib.get("z", 0)) + yaw = float(node.attrib.get("yaw", 0)) - transform = carla.Transform(carla.Location(x=pos_x, y=pos_y, z=pos_z), carla.Rotation(yaw=yaw)) + transform = carla.Transform( + carla.Location(x=pos_x, y=pos_y, z=pos_z), carla.Rotation(yaw=yaw) + ) - rolename = node.attrib.get('rolename', rolename) + rolename = node.attrib.get("rolename", rolename) - speed = node.attrib.get('speed', 0) + speed = node.attrib.get("speed", 0) autopilot = False - if 'autopilot' in node.keys(): + if "autopilot" in node.keys(): autopilot = True random_location = False - if 'random_location' in node.keys(): + if "random_location" in node.keys(): random_location = True - color = node.attrib.get('color', None) + color = node.attrib.get("color", None) - return ActorConfigurationData(model, transform, rolename, speed, autopilot, random_location, color) + return ActorConfigurationData( + model, transform, rolename, speed, autopilot, random_location, color + ) @staticmethod def parse_from_dict(actor_dict, rolename): @@ -67,25 +80,30 @@ def parse_from_dict(actor_dict, rolename): static method to initialize an ActorConfigurationData from a given ET tree """ - model = actor_dict['model'] if 'model' in actor_dict else 'vehicle.*' + model = actor_dict["model"] if "model" in actor_dict else "vehicle.*" - pos_x = float(actor_dict['x']) if 'x' in actor_dict else 0 - pos_y = float(actor_dict['y']) if 'y' in actor_dict else 0 - pos_z = float(actor_dict['z']) if 'z' in actor_dict else 0 - yaw = float(actor_dict['yaw']) if 'yaw' in actor_dict else 0 - transform = carla.Transform(carla.Location(x=pos_x, y=pos_y, z=pos_z), carla.Rotation(yaw=yaw)) + pos_x = float(actor_dict["x"]) if "x" in actor_dict else 0 + pos_y = float(actor_dict["y"]) if "y" in actor_dict else 0 + pos_z = float(actor_dict["z"]) if "z" in actor_dict else 0 + yaw = float(actor_dict["yaw"]) if "yaw" in actor_dict else 0 + transform = carla.Transform( + carla.Location(x=pos_x, y=pos_y, z=pos_z), carla.Rotation(yaw=yaw) + ) - rolename = actor_dict['rolename'] if 'rolename' in actor_dict else rolename - speed = actor_dict['speed'] if 'speed' in actor_dict else 0 - autopilot = actor_dict['autopilot'] if 'autopilot' in actor_dict else False - random_location = actor_dict['random_location'] if 'random_location' in actor_dict else False - color = actor_dict['color'] if 'color' in actor_dict else None + rolename = actor_dict["rolename"] if "rolename" in actor_dict else rolename + speed = actor_dict["speed"] if "speed" in actor_dict else 0 + autopilot = actor_dict["autopilot"] if "autopilot" in actor_dict else False + random_location = ( + actor_dict["random_location"] if "random_location" in actor_dict else False + ) + color = actor_dict["color"] if "color" in actor_dict else None - return ActorConfigurationData(model, transform, rolename, speed, autopilot, random_location, color) + return ActorConfigurationData( + model, transform, rolename, speed, autopilot, random_location, color + ) class ScenarioConfiguration(object): - """ This class provides a basic scenario configuration incl.: - configurations for all actors diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/srunner/scenariomanager/actorcontrols/actor_control.py index 333618d..e246d64 100644 --- a/srunner/scenariomanager/actorcontrols/actor_control.py +++ b/srunner/scenariomanager/actorcontrols/actor_control.py @@ -26,7 +26,6 @@ class ActorControl(object): - """ This class provides a wrapper (access mechanism) for user-defined actor controls. The controllers are loaded via importlib. Therefore, the module name of the controller @@ -63,7 +62,6 @@ class ActorControl(object): _last_lane_offset_command = None def __init__(self, actor, control_py_module, args, scenario_file_path): - # use importlib to import the control module if not control_py_module: if isinstance(actor, carla.Walker): @@ -77,16 +75,20 @@ def __init__(self, actor, control_py_module, args, scenario_file_path): if scenario_file_path: sys.path.append(scenario_file_path) if ".py" in control_py_module: - module_name = os.path.basename(control_py_module).split('.')[0] + module_name = os.path.basename(control_py_module).split(".")[0] sys.path.append(os.path.dirname(control_py_module)) module_control = importlib.import_module(module_name) - control_class_name = module_control.__name__.title().replace('_', '') + control_class_name = module_control.__name__.title().replace("_", "") else: sys.path.append(os.path.dirname(__file__)) module_control = importlib.import_module(control_py_module) - control_class_name = control_py_module.split('.')[-1].title().replace('_', '') + control_class_name = ( + control_py_module.split(".")[-1].title().replace("_", "") + ) - self.control_instance = getattr(module_control, control_class_name)(actor, args) + self.control_instance = getattr(module_control, control_class_name)( + actor, args + ) def reset(self): """ diff --git a/srunner/scenariomanager/actorcontrols/basic_control.py b/srunner/scenariomanager/actorcontrols/basic_control.py index fa59fa5..d5e4ff3 100644 --- a/srunner/scenariomanager/actorcontrols/basic_control.py +++ b/srunner/scenariomanager/actorcontrols/basic_control.py @@ -15,7 +15,6 @@ class BasicControl(object): - """ This class is the base class for user-defined actor controllers All user-defined agents must be derived from this class. @@ -106,7 +105,8 @@ def reset(self): """ raise NotImplementedError( "This function must be re-implemented by the user-defined actor control." - "If this error becomes visible the class hierarchy is somehow broken") + "If this error becomes visible the class hierarchy is somehow broken" + ) def run_step(self): """ @@ -115,4 +115,5 @@ def run_step(self): """ raise NotImplementedError( "This function must be re-implemented by the user-defined actor control." - "If this error becomes visible the class hierarchy is somehow broken") + "If this error becomes visible the class hierarchy is somehow broken" + ) diff --git a/srunner/scenariomanager/actorcontrols/carla_autopilot.py b/srunner/scenariomanager/actorcontrols/carla_autopilot.py index 5b03011..2d5527c 100644 --- a/srunner/scenariomanager/actorcontrols/carla_autopilot.py +++ b/srunner/scenariomanager/actorcontrols/carla_autopilot.py @@ -14,12 +14,10 @@ - No lateral maneuvers can be enforced """ - from srunner.scenariomanager.actorcontrols.basic_control import BasicControl class CarlaAutoPilotControl(BasicControl): - """ Controller class for vehicles derived from BasicControl. diff --git a/srunner/scenariomanager/actorcontrols/external_control.py b/srunner/scenariomanager/actorcontrols/external_control.py index 694f2ff..30ef008 100644 --- a/srunner/scenariomanager/actorcontrols/external_control.py +++ b/srunner/scenariomanager/actorcontrols/external_control.py @@ -17,7 +17,6 @@ class ExternalControl(BasicControl): - """ Actor control class for actors, with externally implemented longitudinal and lateral controlers (e.g. Autoware). diff --git a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py index f6caebd..944b7ef 100644 --- a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py @@ -20,7 +20,6 @@ class NpcVehicleControl(BasicControl): - """ Controller class for vehicles derived from BasicControl. @@ -30,15 +29,18 @@ class NpcVehicleControl(BasicControl): actor (carla.Actor): Vehicle actor that should be controlled. """ - _args = {'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05} + _args = {"K_P": 1.0, "K_D": 0.01, "K_I": 0.0, "dt": 0.05} def __init__(self, actor, args=None): super(NpcVehicleControl, self).__init__(actor) self._local_planner = LocalPlanner( # pylint: disable=undefined-variable - self._actor, opt_dict={ - 'target_speed': self._target_speed * 3.6, - 'lateral_control_dict': self._args}) + self._actor, + opt_dict={ + "target_speed": self._target_speed * 3.6, + "lateral_control_dict": self._args, + }, + ) if self._waypoints: self._update_plan() @@ -52,7 +54,8 @@ def _update_plan(self): plan = [] for transform in self._waypoints: waypoint = CarlaDataProvider.get_map().get_waypoint( - transform.location, project_to_road=True, lane_type=carla.LaneType.Any) + transform.location, project_to_road=True, lane_type=carla.LaneType.Any + ) plan.append((waypoint, RoadOption.LANEFOLLOW)) self._local_planner.set_global_plan(plan) @@ -60,7 +63,7 @@ def _update_offset(self): """ Update the plan (waypoint list) of the LocalPlanner """ - self._local_planner._vehicle_controller._lat_controller._offset = self._offset # pylint: disable=protected-access + self._local_planner._vehicle_controller._lat_controller._offset = self._offset # pylint: disable=protected-access def reset(self): """ @@ -115,10 +118,11 @@ def run_step(self): self._actor.apply_control(control) - current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) + current_speed = math.sqrt( + self._actor.get_velocity().x ** 2 + self._actor.get_velocity().y ** 2 + ) if self._init_speed: - # If _init_speed is set, and the PID controller is not yet up to the point to take over, # we manually set the vehicle to drive with the correct velocity if abs(target_speed - current_speed) > 3: @@ -128,7 +132,9 @@ def run_step(self): self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0)) # Change Brake light state - if (current_speed > target_speed or target_speed < 0.2) and not self._brake_lights_active: + if ( + current_speed > target_speed or target_speed < 0.2 + ) and not self._brake_lights_active: light_state = self._actor.get_light_state() light_state |= carla.VehicleLightState.Brake self._actor.set_light_state(carla.VehicleLightState(light_state)) diff --git a/srunner/scenariomanager/actorcontrols/pedestrian_control.py b/srunner/scenariomanager/actorcontrols/pedestrian_control.py index cd7554b..9c5db1b 100644 --- a/srunner/scenariomanager/actorcontrols/pedestrian_control.py +++ b/srunner/scenariomanager/actorcontrols/pedestrian_control.py @@ -17,7 +17,6 @@ class PedestrianControl(BasicControl): - """ Controller class for pedestrians derived from BasicControl. @@ -27,7 +26,9 @@ class PedestrianControl(BasicControl): def __init__(self, actor, args=None): if not isinstance(actor, carla.Walker): - raise RuntimeError("PedestrianControl: The to be controlled actor is not a pedestrian") + raise RuntimeError( + "PedestrianControl: The to be controlled actor is not a pedestrian" + ) super(PedestrianControl, self).__init__(actor) @@ -73,5 +74,7 @@ def run_step(self): if not self._waypoints: self._reached_goal = True else: - control.direction = self._actor.get_transform().rotation.get_forward_vector() + control.direction = ( + self._actor.get_transform().rotation.get_forward_vector() + ) self._actor.apply_control(control) diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 5f1d58d..b001437 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -26,7 +26,6 @@ class SimpleVehicleControl(BasicControl): - """ Controller class for vehicles derived from BasicControl. @@ -92,45 +91,64 @@ def __init__(self, actor, args=None): self._last_update = None self._consider_traffic_lights = False self._consider_obstacles = False - self._proximity_threshold = float('inf') + self._proximity_threshold = float("inf") self._waypoint_reached_threshold = 4.0 self._max_deceleration = None self._max_acceleration = None self._obstacle_sensor = None - self._obstacle_distance = float('inf') + self._obstacle_distance = float("inf") self._obstacle_actor = None self._visualizer = None self._brake_lights_active = False - if args and 'consider_obstacles' in args and strtobool(args['consider_obstacles']): - self._consider_obstacles = strtobool(args['consider_obstacles']) - bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.other.obstacle') - bp.set_attribute('distance', '250') - if args and 'proximity_threshold' in args: - self._proximity_threshold = float(args['proximity_threshold']) - bp.set_attribute('distance', str(max(float(args['proximity_threshold']), 250))) - bp.set_attribute('hit_radius', '1') - bp.set_attribute('only_dynamics', 'True') + if ( + args + and "consider_obstacles" in args + and strtobool(args["consider_obstacles"]) + ): + self._consider_obstacles = strtobool(args["consider_obstacles"]) + bp = ( + CarlaDataProvider.get_world() + .get_blueprint_library() + .find("sensor.other.obstacle") + ) + bp.set_attribute("distance", "250") + if args and "proximity_threshold" in args: + self._proximity_threshold = float(args["proximity_threshold"]) + bp.set_attribute( + "distance", str(max(float(args["proximity_threshold"]), 250)) + ) + bp.set_attribute("hit_radius", "1") + bp.set_attribute("only_dynamics", "True") self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor( - bp, carla.Transform(carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) + bp, + carla.Transform( + carla.Location(x=self._actor.bounding_box.extent.x, z=1.0) + ), + attach_to=self._actor, + ) self._obstacle_sensor.listen(lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda - if args and 'consider_trafficlights' in args and strtobool(args['consider_trafficlights']): - self._consider_traffic_lights = strtobool(args['consider_trafficlights']) + if ( + args + and "consider_trafficlights" in args + and strtobool(args["consider_trafficlights"]) + ): + self._consider_traffic_lights = strtobool(args["consider_trafficlights"]) - if args and 'waypoint_reached_threshold' in args: - self._waypoint_reached_threshold = float(args['waypoint_reached_threshold']) + if args and "waypoint_reached_threshold" in args: + self._waypoint_reached_threshold = float(args["waypoint_reached_threshold"]) - if args and 'max_deceleration' in args: - self._max_deceleration = float(args['max_deceleration']) + if args and "max_deceleration" in args: + self._max_deceleration = float(args["max_deceleration"]) - if args and 'max_acceleration' in args: - self._max_acceleration = float(args['max_acceleration']) + if args and "max_acceleration" in args: + self._max_acceleration = float(args["max_acceleration"]) - if args and 'attach_camera' in args and strtobool(args['attach_camera']): + if args and "attach_camera" in args and strtobool(args["attach_camera"]): self._visualizer = Visualizer(self._actor) def _on_obstacle(self, event): @@ -190,9 +208,13 @@ def run_step(self): map_wp = None if not self._generated_waypoint_list: - map_wp = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor)) + map_wp = CarlaDataProvider.get_map().get_waypoint( + CarlaDataProvider.get_location(self._actor) + ) else: - map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location) + map_wp = CarlaDataProvider.get_map().get_waypoint( + self._generated_waypoint_list[-1].location + ) while len(self._generated_waypoint_list) < 50: map_wps = map_wp.next(2.0) if map_wps: @@ -202,25 +224,38 @@ def run_step(self): break # Remove all waypoints that are too close to the vehicle - while (self._generated_waypoint_list and - self._generated_waypoint_list[0].location.distance(self._actor.get_location()) < 0.5): + while ( + self._generated_waypoint_list + and self._generated_waypoint_list[0].location.distance( + self._actor.get_location() + ) + < 0.5 + ): self._generated_waypoint_list = self._generated_waypoint_list[1:] - direction_norm = self._set_new_velocity(self._offset_waypoint(self._generated_waypoint_list[0])) + direction_norm = self._set_new_velocity( + self._offset_waypoint(self._generated_waypoint_list[0]) + ) if direction_norm < 2.0: self._generated_waypoint_list = self._generated_waypoint_list[1:] else: # When changing from "free" driving without pre-defined waypoints to a defined route with waypoints # it may happen that the first few waypoints are too close to the ego vehicle for obtaining a # reasonable control command. Therefore, we drop these waypoints first. - while self._waypoints and self._waypoints[0].location.distance(self._actor.get_location()) < 0.5: + while ( + self._waypoints + and self._waypoints[0].location.distance(self._actor.get_location()) + < 0.5 + ): self._waypoints = self._waypoints[1:] self._reached_goal = False if not self._waypoints: self._reached_goal = True else: - direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0])) + direction_norm = self._set_new_velocity( + self._offset_waypoint(self._waypoints[0]) + ) if direction_norm < self._waypoint_reached_threshold: self._waypoints = self._waypoints[1:] if not self._waypoints: @@ -241,8 +276,9 @@ def _offset_waypoint(self, transform): offset_location = transform.location else: right_vector = transform.get_right_vector() - offset_location = transform.location + carla.Location(x=self._offset*right_vector.x, - y=self._offset*right_vector.y) + offset_location = transform.location + carla.Location( + x=self._offset * right_vector.x, y=self._offset * right_vector.y + ) return offset_location @@ -272,7 +308,9 @@ def _set_new_velocity(self, next_location): if not self._last_update: self._last_update = current_time - current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) + current_speed = math.sqrt( + self._actor.get_velocity().x ** 2 + self._actor.get_velocity().y ** 2 + ) if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity @@ -280,16 +318,26 @@ def _set_new_velocity(self, next_location): distance = max(self._obstacle_distance, 0) if distance > 0: current_speed_other = math.sqrt( - self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) + self._obstacle_actor.get_velocity().x ** 2 + + self._obstacle_actor.get_velocity().y ** 2 + ) if current_speed_other < current_speed: - acceleration = -0.5 * (current_speed - current_speed_other)**2 / distance - target_speed = max(acceleration * (current_time - self._last_update) + current_speed, 0) + acceleration = ( + -0.5 * (current_speed - current_speed_other) ** 2 / distance + ) + target_speed = max( + acceleration * (current_time - self._last_update) + + current_speed, + 0, + ) else: target_speed = 0 if self._consider_traffic_lights: - if (self._actor.is_at_traffic_light() and - self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): + if ( + self._actor.is_at_traffic_light() + and self._actor.get_traffic_light_state() == carla.TrafficLightState.Red + ): target_speed = 0 if target_speed < current_speed: @@ -299,8 +347,11 @@ def _set_new_velocity(self, next_location): light_state |= carla.VehicleLightState.Brake self._actor.set_light_state(carla.VehicleLightState(light_state)) if self._max_deceleration is not None: - target_speed = max(target_speed, current_speed - (current_time - - self._last_update) * self._max_deceleration) + target_speed = max( + target_speed, + current_speed + - (current_time - self._last_update) * self._max_deceleration, + ) else: if self._brake_lights_active: self._brake_lights_active = False @@ -308,8 +359,11 @@ def _set_new_velocity(self, next_location): light_state &= ~carla.VehicleLightState.Brake self._actor.set_light_state(carla.VehicleLightState(light_state)) if self._max_acceleration is not None: - tmp_speed = min(target_speed, current_speed + (current_time - - self._last_update) * self._max_acceleration) + tmp_speed = min( + target_speed, + current_speed + + (current_time - self._last_update) * self._max_acceleration, + ) # If the tmp_speed is < 0.5 the vehicle may not properly accelerate. # Therefore, we bump the speed to 0.5 m/s if target_speed allows. target_speed = max(tmp_speed, min(0.5, target_speed)) @@ -330,7 +384,11 @@ def _set_new_velocity(self, next_location): if self._waypoints: delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw else: - new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw + new_yaw = ( + CarlaDataProvider.get_map() + .get_waypoint(next_location) + .transform.rotation.yaw + ) delta_yaw = new_yaw - current_yaw if math.fabs(delta_yaw) > 360: diff --git a/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py b/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py index 62f6426..e2bb12b 100644 --- a/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py +++ b/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py @@ -17,7 +17,6 @@ class VehicleLongitudinalControl(BasicControl): - """ Controller class for vehicles derived from BasicControl. diff --git a/srunner/scenariomanager/actorcontrols/visualizer.py b/srunner/scenariomanager/actorcontrols/visualizer.py index 5c0c670..38ad999 100644 --- a/srunner/scenariomanager/actorcontrols/visualizer.py +++ b/srunner/scenariomanager/actorcontrols/visualizer.py @@ -24,7 +24,6 @@ class Visualizer(object): - """ Visualizer class for actor controllers. @@ -60,25 +59,41 @@ def __init__(self, actor): self._camera_bird = None self._camera_actor = None - bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.camera.rgb') - bp.set_attribute('image_size_x', '1000') - bp.set_attribute('image_size_y', '400') - self._camera_bird = CarlaDataProvider.get_world().spawn_actor(bp, carla.Transform( - carla.Location(x=20.0, z=50.0), carla.Rotation(pitch=-90, yaw=-90)), attach_to=self._actor) - self._camera_bird.listen(lambda image: self._on_camera_update( - image, birdseye=True)) # pylint: disable=unnecessary-lambda - - bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.camera.rgb') - bp.set_attribute('image_size_x', '1000') - bp.set_attribute('image_size_y', '400') - self._camera_actor = CarlaDataProvider.get_world().spawn_actor(bp, carla.Transform( - carla.Location(x=2.3, z=1.0)), attach_to=self._actor) - self._camera_actor.listen(lambda image: self._on_camera_update( - image, birdseye=False)) # pylint: disable=unnecessary-lambda + bp = ( + CarlaDataProvider.get_world() + .get_blueprint_library() + .find("sensor.camera.rgb") + ) + bp.set_attribute("image_size_x", "1000") + bp.set_attribute("image_size_y", "400") + self._camera_bird = CarlaDataProvider.get_world().spawn_actor( + bp, + carla.Transform( + carla.Location(x=20.0, z=50.0), carla.Rotation(pitch=-90, yaw=-90) + ), + attach_to=self._actor, + ) + self._camera_bird.listen( + lambda image: self._on_camera_update(image, birdseye=True) + ) # pylint: disable=unnecessary-lambda + + bp = ( + CarlaDataProvider.get_world() + .get_blueprint_library() + .find("sensor.camera.rgb") + ) + bp.set_attribute("image_size_x", "1000") + bp.set_attribute("image_size_y", "400") + self._camera_actor = CarlaDataProvider.get_world().spawn_actor( + bp, carla.Transform(carla.Location(x=2.3, z=1.0)), attach_to=self._actor + ) + self._camera_actor.listen( + lambda image: self._on_camera_update(image, birdseye=False) + ) # pylint: disable=unnecessary-lambda if self._video_writer: - fourcc = cv2.VideoWriter_fourcc(*'mp4v') - self._video = cv2.VideoWriter('recorded_video.avi', fourcc, 13, (1000, 800)) + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + self._video = cv2.VideoWriter("recorded_video.avi", fourcc, 13, (1000, 800)) def reset(self): """ @@ -116,13 +131,24 @@ def render(self): if self._cv_image_actor is not None and self._cv_image_bird is not None: im_v = cv2.vconcat([self._cv_image_actor, self._cv_image_bird]) cv2.circle(im_v, (900, 300), 80, (170, 170, 170), -1) - text = str(int(round((self._actor.get_velocity().x * 3.6))))+" kph" - - speed = np.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) - - text = str(int(round((speed * 3.6))))+" kph" - text = ' '*(7-len(text)) + text - im_v = cv2.putText(im_v, text, (830, 310), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2, cv2.LINE_AA) + text = str(int(round((self._actor.get_velocity().x * 3.6)))) + " kph" + + speed = np.sqrt( + self._actor.get_velocity().x ** 2 + self._actor.get_velocity().y ** 2 + ) + + text = str(int(round((speed * 3.6)))) + " kph" + text = " " * (7 - len(text)) + text + im_v = cv2.putText( + im_v, + text, + (830, 310), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (0, 0, 0), + 2, + cv2.LINE_AA, + ) cv2.imshow("", im_v) cv2.waitKey(1) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 83996fb..612d8ac 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -26,13 +26,12 @@ def calculate_velocity(actor): """ Method to calculate the velocity of a actor """ - velocity_squared = actor.get_velocity().x**2 - velocity_squared += actor.get_velocity().y**2 + velocity_squared = actor.get_velocity().x ** 2 + velocity_squared += actor.get_velocity().y ** 2 return math.sqrt(velocity_squared) class CarlaDataProvider(object): # pylint: disable=too-many-public-methods - """ This class provides access to various data of all registered actors It buffers the data and updates it on every CARLA tick @@ -88,12 +87,18 @@ def register_actor(actor, transform=None): with CarlaDataProvider._lock: if actor in CarlaDataProvider._actor_velocity_map: raise KeyError( - "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)) + "Vehicle '{}' already registered. Cannot register twice!".format( + actor.id + ) + ) else: CarlaDataProvider._actor_velocity_map[actor] = 0.0 if actor in CarlaDataProvider._actor_location_map: raise KeyError( - "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)) + "Vehicle '{}' already registered. Cannot register twice!".format( + actor.id + ) + ) elif transform: CarlaDataProvider._actor_location_map[actor] = transform.location else: @@ -101,7 +106,10 @@ def register_actor(actor, transform=None): if actor in CarlaDataProvider._actor_transform_map: raise KeyError( - "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)) + "Vehicle '{}' already registered. Cannot register twice!".format( + actor.id + ) + ) else: CarlaDataProvider._actor_transform_map[actor] = transform @@ -138,7 +146,9 @@ def on_carla_tick(): with CarlaDataProvider._lock: for actor in CarlaDataProvider._actor_velocity_map: if actor is not None and actor.is_alive: - CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity(actor) + CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity( + actor + ) for actor in CarlaDataProvider._actor_location_map: if actor is not None and actor.is_alive: @@ -146,7 +156,9 @@ def on_carla_tick(): for actor in CarlaDataProvider._actor_transform_map: if actor is not None and actor.is_alive: - CarlaDataProvider._actor_transform_map[actor] = actor.get_transform() + CarlaDataProvider._actor_transform_map[actor] = ( + actor.get_transform() + ) world = CarlaDataProvider._world if world is None: @@ -159,13 +171,14 @@ def get_velocity(actor): """ returns the absolute velocity for the given actor """ - for key in CarlaDataProvider._actor_velocity_map: - if key.id == actor.id: - return CarlaDataProvider._actor_velocity_map[key] + with CarlaDataProvider._lock: + for key in CarlaDataProvider._actor_velocity_map: + if key.id == actor.id: + return CarlaDataProvider._actor_velocity_map[key] # We are intentionally not throwing here # This may cause exception loops in py_trees - print('{}.get_velocity: {} not found!' .format(__name__, actor)) + print("{}.get_velocity: {} not found!".format(__name__, actor)) return 0.0 @staticmethod @@ -173,13 +186,14 @@ def get_location(actor): """ returns the location for the given actor """ - for key in CarlaDataProvider._actor_location_map: - if key.id == actor.id: - return CarlaDataProvider._actor_location_map[key] + with CarlaDataProvider._lock: + for key in CarlaDataProvider._actor_location_map: + if key.id == actor.id: + return CarlaDataProvider._actor_location_map[key] # We are intentionally not throwing here # This may cause exception loops in py_trees - print('{}.get_location: {} not found!' .format(__name__, actor)) + print("{}.get_location: {} not found!".format(__name__, actor)) return None @staticmethod @@ -187,18 +201,18 @@ def get_transform(actor): """ returns the transform for the given actor """ - for key in CarlaDataProvider._actor_transform_map: - if key.id == actor.id: - # The velocity location information is the entire behavior tree updated every tick - # The ego vehicle is created before the behavior tree tick, so exception handling needs to be added - if CarlaDataProvider._actor_transform_map[key] is None: - return actor.get_transform() - return CarlaDataProvider._actor_transform_map[key] + with CarlaDataProvider._lock: + for key in CarlaDataProvider._actor_transform_map: + if key.id == actor.id: + cached = CarlaDataProvider._actor_transform_map[key] + if cached is not None: + return cached + break + else: + print("{}.get_transform: {} not found!".format(__name__, actor)) + return None - # We are intentionally not throwing here - # This may cause exception loops in py_trees - print('{}.get_transform: {} not found!' .format(__name__, actor)) - return None + return actor.get_transform() @staticmethod def set_client(client): @@ -242,7 +256,7 @@ def get_map(world=None): if CarlaDataProvider._map is None: if world is None: if CarlaDataProvider._world is None: - raise ValueError("class member \'world'\' not initialized yet") + raise ValueError("class member 'world'' not initialized yet") else: CarlaDataProvider._map = CarlaDataProvider._world.get_map() else: @@ -303,9 +317,9 @@ 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)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + 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)) + 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] @staticmethod @@ -319,22 +333,31 @@ def prepare_map(): # Parse all traffic lights CarlaDataProvider._traffic_light_map.clear() - for traffic_light in CarlaDataProvider._world.get_actors().filter('*traffic_light*'): + for traffic_light in CarlaDataProvider._world.get_actors().filter( + "*traffic_light*" + ): if traffic_light not in list(CarlaDataProvider._traffic_light_map): - CarlaDataProvider._traffic_light_map[traffic_light] = traffic_light.get_transform() + CarlaDataProvider._traffic_light_map[traffic_light] = ( + traffic_light.get_transform() + ) else: raise KeyError( - "Traffic light '{}' already registered. Cannot register twice!".format(traffic_light.id)) + "Traffic light '{}' already registered. Cannot register twice!".format( + traffic_light.id + ) + ) @staticmethod def annotate_trafficlight_in_group(traffic_light): """ Get dictionary with traffic light group info for a given traffic light """ - dict_annotations = {'ref': [], 'opposite': [], 'left': [], 'right': []} + dict_annotations = {"ref": [], "opposite": [], "left": [], "right": []} # Get the waypoints - ref_location = CarlaDataProvider.get_trafficlight_trigger_location(traffic_light) + ref_location = CarlaDataProvider.get_trafficlight_trigger_location( + traffic_light + ) ref_waypoint = CarlaDataProvider.get_map().get_waypoint(ref_location) ref_yaw = ref_waypoint.transform.rotation.yaw @@ -342,11 +365,15 @@ def annotate_trafficlight_in_group(traffic_light): for target_tl in group_tl: if traffic_light.id == target_tl.id: - dict_annotations['ref'].append(target_tl) + dict_annotations["ref"].append(target_tl) else: # Get the angle between yaws - target_location = CarlaDataProvider.get_trafficlight_trigger_location(target_tl) - target_waypoint = CarlaDataProvider.get_map().get_waypoint(target_location) + target_location = CarlaDataProvider.get_trafficlight_trigger_location( + target_tl + ) + target_waypoint = CarlaDataProvider.get_map().get_waypoint( + target_location + ) target_yaw = target_waypoint.transform.rotation.yaw diff = (target_yaw - ref_yaw) % 360 @@ -354,25 +381,32 @@ def annotate_trafficlight_in_group(traffic_light): if diff > 330: continue elif diff > 225: - dict_annotations['right'].append(target_tl) + dict_annotations["right"].append(target_tl) elif diff > 135.0: - dict_annotations['opposite'].append(target_tl) + dict_annotations["opposite"].append(target_tl) elif diff > 30: - dict_annotations['left'].append(target_tl) + dict_annotations["left"].append(target_tl) return dict_annotations @staticmethod - def get_trafficlight_trigger_location(traffic_light): # pylint: disable=invalid-name + def get_trafficlight_trigger_location(traffic_light): # pylint: disable=invalid-name """ Calculates the yaw of the waypoint that represents the trigger volume of the traffic light """ + def rotate_point(point, angle): """ rotate a given point by a given angle """ - x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y - y_ = math.sin(math.radians(angle)) * point.x - math.cos(math.radians(angle)) * point.y + x_ = ( + math.cos(math.radians(angle)) * point.x + - math.sin(math.radians(angle)) * point.y + ) + y_ = ( + math.sin(math.radians(angle)) * point.x + - math.cos(math.radians(angle)) * point.y + ) return carla.Vector3D(x_, y_, point.z) @@ -387,7 +421,9 @@ def rotate_point(point, angle): return carla.Location(point_location.x, point_location.y, point_location.z) @staticmethod - def update_light_states(ego_light, annotations, states, freeze=False, timeout=1000000000): + def update_light_states( + ego_light, annotations, states, freeze=False, timeout=1000000000 + ): """ Update traffic light states """ @@ -395,7 +431,7 @@ def update_light_states(ego_light, annotations, states, freeze=False, timeout=10 for state in states: relevant_lights = [] - if state == 'ego': + if state == "ego": relevant_lights = [ego_light] else: relevant_lights = annotations[state] @@ -404,11 +440,15 @@ def update_light_states(ego_light, annotations, states, freeze=False, timeout=10 prev_green_time = light.get_green_time() prev_red_time = light.get_red_time() prev_yellow_time = light.get_yellow_time() - reset_params.append({'light': light, - 'state': prev_state, - 'green_time': prev_green_time, - 'red_time': prev_red_time, - 'yellow_time': prev_yellow_time}) + reset_params.append( + { + "light": light, + "state": prev_state, + "green_time": prev_green_time, + "red_time": prev_red_time, + "yellow_time": prev_yellow_time, + } + ) light.set_state(states[state]) if freeze: @@ -424,10 +464,10 @@ def reset_lights(reset_params): Reset traffic lights """ for param in reset_params: - param['light'].set_state(param['state']) - param['light'].set_green_time(param['green_time']) - param['light'].set_red_time(param['red_time']) - param['light'].set_yellow_time(param['yellow_time']) + param["light"].set_state(param["state"]) + param["light"].set_green_time(param["green_time"]) + param["light"].set_red_time(param["red_time"]) + param["light"].set_yellow_time(param["yellow_time"]) @staticmethod def get_next_traffic_light(actor, use_cached_location=True): @@ -455,10 +495,12 @@ def get_next_traffic_light(actor, use_cached_location=True): distance_to_relevant_traffic_light = float("inf") for traffic_light in CarlaDataProvider._traffic_light_map: - if hasattr(traffic_light, 'trigger_volume'): + if hasattr(traffic_light, "trigger_volume"): tl_t = CarlaDataProvider._traffic_light_map[traffic_light] transformed_tv = tl_t.transform(traffic_light.trigger_volume.location) - distance = carla.Location(transformed_tv).distance(list_of_waypoints[-1].transform.location) + distance = carla.Location(transformed_tv).distance( + list_of_waypoints[-1].transform.location + ) if distance < distance_to_relevant_traffic_light: relevant_traffic_light = traffic_light @@ -471,7 +513,9 @@ def generate_spawn_points(): """ Generate spawn points for the current map """ - spawn_points = list(CarlaDataProvider.get_map(CarlaDataProvider._world).get_spawn_points()) + spawn_points = list( + CarlaDataProvider.get_map(CarlaDataProvider._world).get_spawn_points() + ) CarlaDataProvider._rng.shuffle(spawn_points) CarlaDataProvider._spawn_points = spawn_points CarlaDataProvider._spawn_index = 0 @@ -507,7 +551,6 @@ def get_road_lanes(wp): lane_id_set = set() pre_left = wp while wp and wp.lane_type == carla.LaneType.Driving: - if wp.lane_id in lane_id_set: break lane_id_set.add(wp.lane_id) @@ -522,7 +565,6 @@ def get_road_lanes(wp): lane_id_set.clear() wp = pre_left while wp and wp.lane_type == carla.LaneType.Driving: - if wp.lane_id in lane_id_set: break lane_id_set.add(wp.lane_id) @@ -550,7 +592,9 @@ def get_waypoint_by_laneid(lane_num: int): else: pos = CarlaDataProvider._spawn_points[CarlaDataProvider._spawn_index] # pylint: disable=unsubscriptable-object CarlaDataProvider._spawn_index += 1 - wp = CarlaDataProvider.get_map().get_waypoint(pos.location, project_to_road=True, lane_type=carla.LaneType.Driving) + wp = CarlaDataProvider.get_map().get_waypoint( + pos.location, project_to_road=True, lane_type=carla.LaneType.Driving + ) road_lanes = CarlaDataProvider.get_road_lanes(wp) @@ -561,10 +605,17 @@ def get_waypoint_by_laneid(lane_num: int): return road_lanes[lane - 1] @staticmethod - def create_blueprint(model, rolename='scenario', color=None, actor_category="car", attribute_filter=None): + def create_blueprint( + model, + rolename="scenario", + color=None, + actor_category="car", + attribute_filter=None, + ): """ Function to setup the blueprint of an actor given its model and other relevant parameters """ + def check_attribute_value(blueprint, name, value): """ Checks if the blueprint has that attribute with that value @@ -585,17 +636,17 @@ def check_attribute_value(blueprint, name, value): return False _actor_blueprint_categories = { - 'car': 'vehicle.tesla.model3', - 'van': 'vehicle.volkswagen.t2', - 'truck': 'vehicle.carlamotors.carlacola', - 'trailer': '', - 'semitrailer': '', - 'bus': 'vehicle.volkswagen.t2', - 'motorbike': 'vehicle.kawasaki.ninja', - 'bicycle': 'vehicle.diamondback.century', - 'train': '', - 'tram': '', - 'pedestrian': 'walker.pedestrian.0001', + "car": "vehicle.tesla.model3", + "van": "vehicle.volkswagen.t2", + "truck": "vehicle.carlamotors.carlacola", + "trailer": "", + "semitrailer": "", + "bus": "vehicle.volkswagen.t2", + "motorbike": "vehicle.kawasaki.ninja", + "bicycle": "vehicle.diamondback.century", + "train": "", + "tram": "", + "pedestrian": "walker.pedestrian.0001", } # Set the model @@ -603,46 +654,63 @@ def check_attribute_value(blueprint, name, value): blueprints = CarlaDataProvider._blueprint_library.filter(model) if attribute_filter is not None: for key, value in attribute_filter.items(): - blueprints = [x for x in blueprints if check_attribute_value(x, key, value)] + blueprints = [ + x for x in blueprints if check_attribute_value(x, key, value) + ] blueprint = CarlaDataProvider._rng.choice(blueprints) except ValueError: # The model is not part of the blueprint library. Let's take a default one for the given category bp_filter = "vehicle.*" new_model = _actor_blueprint_categories[actor_category] - if new_model != '': + if new_model != "": bp_filter = new_model - print("WARNING: Actor model {} not available. Using instead {}".format(model, new_model)) - blueprint = CarlaDataProvider._rng.choice(CarlaDataProvider._blueprint_library.filter(bp_filter)) + print( + "WARNING: Actor model {} not available. Using instead {}".format( + model, new_model + ) + ) + blueprint = CarlaDataProvider._rng.choice( + CarlaDataProvider._blueprint_library.filter(bp_filter) + ) # Set the color if color: - if not blueprint.has_attribute('color'): + if not blueprint.has_attribute("color"): print( "WARNING: Cannot set Color ({}) for actor {} due to missing blueprint attribute".format( - color, blueprint.id)) + color, blueprint.id + ) + ) else: - default_color_rgba = blueprint.get_attribute('color').as_color() - default_color = '({}, {}, {})'.format(default_color_rgba.r, default_color_rgba.g, default_color_rgba.b) + default_color_rgba = blueprint.get_attribute("color").as_color() + default_color = "({}, {}, {})".format( + default_color_rgba.r, default_color_rgba.g, default_color_rgba.b + ) try: - blueprint.set_attribute('color', color) + blueprint.set_attribute("color", color) except ValueError: # Color can't be set for this vehicle - print("WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format( - color, blueprint.id, default_color)) - blueprint.set_attribute('color', default_color) + print( + "WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format( + color, blueprint.id, default_color + ) + ) + blueprint.set_attribute("color", default_color) else: - if blueprint.has_attribute('color') and rolename != 'hero': - color = CarlaDataProvider._rng.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) + if blueprint.has_attribute("color") and rolename != "hero": + color = CarlaDataProvider._rng.choice( + blueprint.get_attribute("color").recommended_values + ) + blueprint.set_attribute("color", color) # Make pedestrians mortal - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'false') + if blueprint.has_attribute("is_invincible"): + blueprint.set_attribute("is_invincible", "false") # Set the rolename - if blueprint.has_attribute('role_name'): - blueprint.set_attribute('role_name', rolename) + if blueprint.has_attribute("role_name"): + blueprint.set_attribute("role_name", rolename) return blueprint @@ -656,9 +724,11 @@ def handle_actor_batch(batch, tick=True): actors = [] if CarlaDataProvider._client: - responses = CarlaDataProvider._client.apply_batch_sync(batch, sync_mode and tick) + responses = CarlaDataProvider._client.apply_batch_sync( + batch, sync_mode and tick + ) else: - raise ValueError("class member \'client'\' not initialized yet") + raise ValueError("class member 'client'' not initialized yet") # Wait (or not) for the actors to be spawned properly before we do anything if not tick: @@ -679,23 +749,35 @@ def handle_actor_batch(batch, tick=True): return actors @staticmethod - def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, - random_location=False, color=None, actor_category="car", - attribute_filter=None, tick=True): + def request_new_actor( + model, + spawn_point, + rolename="scenario", + autopilot=False, + random_location=False, + color=None, + actor_category="car", + attribute_filter=None, + tick=True, + ): """ This method tries to create a new actor, returning it if successful (None otherwise). """ - blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category, attribute_filter) + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, color, actor_category, attribute_filter + ) if random_location: actor = None while not actor: - spawn_point = CarlaDataProvider._rng.choice(CarlaDataProvider._spawn_points) + spawn_point = CarlaDataProvider._rng.choice( + CarlaDataProvider._spawn_points + ) actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point) else: # For non prop models, slightly lift the actor to avoid collisions with the ground - z_offset = 0.2 if 'prop' not in model else 0 + z_offset = 0.2 if "prop" not in model else 0 # DO NOT USE spawn_point directly, as this will modify spawn_point permanently _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation) @@ -705,7 +787,11 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point) if actor is None: - print("WARNING: Cannot spawn actor {} at position {}".format(model, spawn_point.location)) + print( + "WARNING: Cannot spawn actor {} at position {}".format( + model, spawn_point.location + ) + ) return None # De/activate the autopilot of the actor if it belongs to vehicle @@ -742,11 +828,11 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): - actor_list: list of ActorConfigurationData """ - SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name - PhysicsCommand = carla.command.SetSimulatePhysics # pylint: disable=invalid-name - FutureActor = carla.command.FutureActor # pylint: disable=invalid-name - ApplyTransform = carla.command.ApplyTransform # pylint: disable=invalid-name - SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + PhysicsCommand = carla.command.SetSimulatePhysics # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + ApplyTransform = carla.command.ApplyTransform # pylint: disable=invalid-name + SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name SetVehicleLightState = carla.command.SetVehicleLightState # pylint: disable=invalid-name batch = [] @@ -755,21 +841,29 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): CarlaDataProvider.generate_spawn_points() for actor in actor_list: - # Get the blueprint blueprint = CarlaDataProvider.create_blueprint( - actor.model, actor.rolename, actor.color, actor.category, attribute_filter) + actor.model, + actor.rolename, + actor.color, + actor.category, + attribute_filter, + ) # Get the spawn point transform = actor.transform if not transform: continue if actor.random_location: - if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + if CarlaDataProvider._spawn_index >= len( + CarlaDataProvider._spawn_points + ): print("No more spawn points to use") break else: - _spawn_point = CarlaDataProvider._spawn_points[CarlaDataProvider._spawn_index] # pylint: disable=unsubscriptable-object + _spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object CarlaDataProvider._spawn_index += 1 else: @@ -777,11 +871,11 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): _spawn_point.rotation = transform.rotation _spawn_point.location.x = transform.location.x _spawn_point.location.y = transform.location.y - if blueprint.has_tag('walker'): + if blueprint.has_tag("walker"): # On imported OpenDRIVE maps, spawning of pedestrians can fail. # By increasing the z-value the chances of success are increased. map_name = CarlaDataProvider._map.name.split("/")[-1] - if not map_name.startswith('OpenDrive'): + if not map_name.startswith("OpenDrive"): _spawn_point.location.z = transform.location.z + 0.2 else: _spawn_point.location.z = transform.location.z + 0.8 @@ -790,14 +884,32 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): # Get the command command = SpawnActor(blueprint, _spawn_point) - command.then(SetAutopilot(FutureActor, actor.autopilot, CarlaDataProvider._traffic_manager_port)) - - if actor.args is not None and 'physics' in actor.args and actor.args['physics'] == "off": - command.then(ApplyTransform(FutureActor, _spawn_point)).then(PhysicsCommand(FutureActor, False)) - elif actor.category == 'misc': + command.then( + SetAutopilot( + FutureActor, + actor.autopilot, + CarlaDataProvider._traffic_manager_port, + ) + ) + + if ( + actor.args is not None + and "physics" in actor.args + and actor.args["physics"] == "off" + ): + command.then(ApplyTransform(FutureActor, _spawn_point)).then( + PhysicsCommand(FutureActor, False) + ) + elif actor.category == "misc": command.then(PhysicsCommand(FutureActor, True)) - if actor.args is not None and 'lights' in actor.args and actor.args['lights'] == "on": - command.then(SetVehicleLightState(FutureActor, carla.VehicleLightState.All)) + if ( + actor.args is not None + and "lights" in actor.args + and actor.args["lights"] == "on" + ): + command.then( + SetVehicleLightState(FutureActor, carla.VehicleLightState.All) + ) batch.append(command) @@ -815,9 +927,16 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): return actors @staticmethod - def request_new_batch_actors(model, amount, spawn_points, autopilot=False, - random_location=False, rolename='scenario', - attribute_filter=None, tick=True): + def request_new_batch_actors( + model, + amount, + spawn_points, + autopilot=False, + random_location=False, + rolename="scenario", + attribute_filter=None, + tick=True, + ): """ Simplified version of "request_new_actors". This method also create several actors in batch. @@ -828,9 +947,9 @@ def request_new_batch_actors(model, amount, spawn_points, autopilot=False, while others are randomized (color) """ - SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name - FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name CarlaDataProvider.generate_spawn_points() @@ -838,25 +957,44 @@ def request_new_batch_actors(model, amount, spawn_points, autopilot=False, for i in range(amount): # Get vehicle by model - blueprint = CarlaDataProvider.create_blueprint(model, rolename, attribute_filter=attribute_filter) + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, attribute_filter=attribute_filter + ) if random_location: - if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): - print("No more spawn points to use. Spawned {} actors out of {}".format(i + 1, amount)) + if CarlaDataProvider._spawn_index >= len( + CarlaDataProvider._spawn_points + ): + print( + "No more spawn points to use. Spawned {} actors out of {}".format( + i + 1, amount + ) + ) break else: - spawn_point = CarlaDataProvider._spawn_points[CarlaDataProvider._spawn_index] # pylint: disable=unsubscriptable-object + spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object CarlaDataProvider._spawn_index += 1 else: try: spawn_point = spawn_points[i] except IndexError: - print("The amount of spawn points is lower than the amount of vehicles spawned") + print( + "The amount of spawn points is lower than the amount of vehicles spawned" + ) break if spawn_point: - batch.append(SpawnActor(blueprint, spawn_point).then( - SetAutopilot(FutureActor, autopilot, CarlaDataProvider._traffic_manager_port))) + batch.append( + SpawnActor(blueprint, spawn_point).then( + SetAutopilot( + FutureActor, + autopilot, + CarlaDataProvider._traffic_manager_port, + ) + ) + ) actors = CarlaDataProvider.handle_actor_batch(batch, tick) for actor in actors: @@ -892,7 +1030,10 @@ def get_hero_actor(): Get the actor object of the hero actor if it exists, returns none otherwise. """ for actor_id in CarlaDataProvider._carla_actor_pool: - if CarlaDataProvider._carla_actor_pool[actor_id].attributes['role_name'] == 'hero': + if ( + CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] + == "hero" + ): return CarlaDataProvider._carla_actor_pool[actor_id] return None @@ -910,9 +1051,11 @@ def get_actor_by_id(actor_id): @staticmethod def get_actor_by_name(role_name: str): - for actor_id in CarlaDataProvider._carla_actor_pool: - if CarlaDataProvider._carla_actor_pool[actor_id].attributes['role_name'] == role_name: + if ( + CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] + == role_name + ): return CarlaDataProvider._carla_actor_pool[actor_id] print(f"Non-existing actor name {role_name}") return None @@ -936,12 +1079,19 @@ def remove_actors_in_surrounding(location, distance): provided location """ for actor_id in CarlaDataProvider._carla_actor_pool.copy(): - if CarlaDataProvider._carla_actor_pool[actor_id].get_location().distance(location) < distance: + if ( + CarlaDataProvider._carla_actor_pool[actor_id] + .get_location() + .distance(location) + < distance + ): CarlaDataProvider._carla_actor_pool[actor_id].destroy() CarlaDataProvider._carla_actor_pool.pop(actor_id) # Remove all keys with None values - CarlaDataProvider._carla_actor_pool = dict({k: v for k, v in CarlaDataProvider._carla_actor_pool.items() if v}) + CarlaDataProvider._carla_actor_pool = dict( + {k: v for k, v in CarlaDataProvider._carla_actor_pool.items() if v} + ) @staticmethod def get_traffic_manager_port(): @@ -962,7 +1112,7 @@ def cleanup(): """ Cleanup and remove all entries from all dictionaries """ - DestroyActor = carla.command.DestroyActor # pylint: disable=invalid-name + DestroyActor = carla.command.DestroyActor # pylint: disable=invalid-name batch = [] for actor_id in CarlaDataProvider._carla_actor_pool.copy(): diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py index 316def2..18efbf6 100644 --- a/srunner/scenariomanager/lights_sim.py +++ b/srunner/scenariomanager/lights_sim.py @@ -18,11 +18,11 @@ class RouteLightsBehavior(py_trees.behaviour.Behaviour): - """ Behavior responsible for turning the street lights on and off depending on the weather conditions. Only those around the ego vehicle will be turned on, regardless of weather conditions """ + SUN_ALTITUDE_THRESHOLD_1 = 15 SUN_ALTITUDE_THRESHOLD_2 = 165 @@ -33,7 +33,9 @@ class RouteLightsBehavior(py_trees.behaviour.Behaviour): # In cases where more than one weather conditition is active, decrease the thresholds COMBINED_THRESHOLD = 10 - def __init__(self, ego_vehicle, radius=50, radius_increase=15, name="LightsBehavior"): + def __init__( + self, ego_vehicle, radius=50, radius_increase=15, name="LightsBehavior" + ): """ Setup parameters """ @@ -44,7 +46,9 @@ def __init__(self, ego_vehicle, radius=50, radius_increase=15, name="LightsBehav self._world = CarlaDataProvider.get_world() self._light_manager = self._world.get_lightmanager() self._light_manager.set_day_night_cycle(False) - self._vehicle_lights = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam + self._vehicle_lights = ( + carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam + ) self._prev_night_mode = False @@ -70,7 +74,9 @@ def update(self): def _get_night_mode(self, weather): """Check wheather or not the street lights need to be turned on""" altitude_dist = weather.sun_altitude_angle - self.SUN_ALTITUDE_THRESHOLD_1 - altitude_dist = min(altitude_dist, self.SUN_ALTITUDE_THRESHOLD_2 - weather.sun_altitude_angle) + altitude_dist = min( + altitude_dist, self.SUN_ALTITUDE_THRESHOLD_2 - weather.sun_altitude_angle + ) cloudiness_dist = self.CLOUDINESS_THRESHOLD - weather.cloudiness fog_density_dist = self.FOG_THRESHOLD - weather.fog_density @@ -110,8 +116,10 @@ def _turn_close_lights_on(self, location): self._light_manager.turn_off(off_lights) # Vehicles - all_vehicles = CarlaDataProvider.get_all_actors().filter('*vehicle.*') - scenario_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'scenario'] + all_vehicles = CarlaDataProvider.get_all_actors().filter("*vehicle.*") + scenario_vehicles = [ + v for v in all_vehicles if v.attributes["role_name"] == "scenario" + ] for vehicle in scenario_vehicles: if vehicle.get_location().distance(location) > radius: @@ -135,8 +143,10 @@ def _turn_all_lights_off(self): self._light_manager.turn_off(off_lights) # Vehicles - all_vehicles = CarlaDataProvider.get_all_actors().filter('*vehicle.*') - scenario_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'scenario'] + all_vehicles = CarlaDataProvider.get_all_actors().filter("*vehicle.*") + scenario_vehicles = [ + v for v in all_vehicles if v.attributes["role_name"] == "scenario" + ] for vehicle in scenario_vehicles: lights = vehicle.get_light_state() @@ -150,4 +160,4 @@ def _turn_all_lights_off(self): def terminate(self, new_status): self._light_manager.set_day_night_cycle(True) - return super().terminate(new_status) \ No newline at end of file + return super().terminate(new_status) diff --git a/srunner/scenariomanager/result_writer.py b/srunner/scenariomanager/result_writer.py index 7f358f7..5af5648 100644 --- a/srunner/scenariomanager/result_writer.py +++ b/srunner/scenariomanager/result_writer.py @@ -18,13 +18,14 @@ class ResultOutputProvider(object): - """ This module contains the _result gatherer and write for CARLA scenarios. It shall be used from the ScenarioManager only. """ - def __init__(self, data, result, stdout=True, filename=None, junitfile=None, jsonfile=None): + def __init__( + self, data, result, stdout=True, filename=None, junitfile=None, jsonfile=None + ): """ Setup all parameters - _data contains all scenario-related information @@ -41,10 +42,12 @@ def __init__(self, data, result, stdout=True, filename=None, junitfile=None, jso self._junit = junitfile self._json = jsonfile - self._start_time = time.strftime('%Y-%m-%d %H:%M:%S', - time.localtime(self._data.start_system_time)) - self._end_time = time.strftime('%Y-%m-%d %H:%M:%S', - time.localtime(self._data.end_system_time)) + self._start_time = time.strftime( + "%Y-%m-%d %H:%M:%S", time.localtime(self._data.start_system_time) + ) + self._end_time = time.strftime( + "%Y-%m-%d %H:%M:%S", time.localtime(self._data.end_system_time) + ) def write(self): """ @@ -57,7 +60,7 @@ def write(self): output = self.create_output_text() if self._filename is not None: - with open(self._filename, 'w', encoding='utf-8') as fd: + with open(self._filename, "w", encoding="utf-8") as fd: fd.write(output) if self._stdout: print(output) @@ -68,7 +71,8 @@ def create_output_text(self): """ output = "\n" output += " ======= Results of Scenario: {} ---- {} =======\n".format( - self._data.scenario_tree.name, self._result) + self._data.scenario_tree.name, self._result + ) end_line_length = len(output) - 3 output += "\n" @@ -88,7 +92,9 @@ def create_output_text(self): system_time = round(self._data.scenario_duration_system, 2) game_time = round(self._data.scenario_duration_game, 2) - ratio = round(self._data.scenario_duration_game / self._data.scenario_duration_system, 3) + ratio = round( + self._data.scenario_duration_game / self._data.scenario_duration_system, 3 + ) list_statistics = [["Start Time", "{}".format(self._start_time)]] list_statistics.extend([["End Time", "{}".format(self._end_time)]]) @@ -96,12 +102,12 @@ def create_output_text(self): list_statistics.extend([["Game Time", "{}s".format(game_time)]]) list_statistics.extend([["Ratio (Game / System)", "{}".format(ratio)]]) - output += tabulate(list_statistics, tablefmt='fancy_grid') + output += tabulate(list_statistics, tablefmt="fancy_grid") output += "\n\n" # Criteria part output += " > Criteria Information\n" - header = ['Actor', 'Criterion', 'Result', 'Actual Value', 'Success Value'] + header = ["Actor", "Criterion", "Result", "Actual Value", "Success Value"] list_statistics = [header] for criterion in self._data.scenario.get_criteria(): @@ -113,22 +119,37 @@ def create_output_text(self): actor = "{} (id={})".format(criterion.actor.type_id[8:], criterion.actor.id) - list_statistics.extend([[ - actor, name, criterion.test_status, criterion.actual_value, criterion.success_value]]) + list_statistics.extend( + [ + [ + actor, + name, + criterion.test_status, + criterion.actual_value, + criterion.success_value, + ] + ] + ) # Timeout actor = "" criteria = "Timeout (Req.)" - result = "SUCCESS" if self._data.scenario_duration_game < self._data.scenario.timeout else "FAILURE" + result = ( + "SUCCESS" + if self._data.scenario_duration_game < self._data.scenario.timeout + else "FAILURE" + ) actual_value = round(self._data.scenario_duration_game, 2) expected_value = round(self._data.scenario.timeout, 2) - list_statistics.extend([[actor, criteria, result, actual_value, expected_value]]) + list_statistics.extend( + [[actor, criteria, result, actual_value, expected_value]] + ) # Global and final output message - list_statistics.extend([['', 'GLOBAL RESULT', self._result, '', '']]) + list_statistics.extend([["", "GLOBAL RESULT", self._result, "", ""]]) - output += tabulate(list_statistics, tablefmt='fancy_grid') + output += tabulate(list_statistics, tablefmt="fancy_grid") output += "\n" output += " " + "=" * end_line_length + "\n" @@ -181,7 +202,7 @@ def result_dict(name, actor, optional, expected, actual, success): criterion.optional, criterion.success_value, criterion.actual_value, - criterion.test_status in ["SUCCESS", "ACCEPTABLE"] + criterion.test_status in ["SUCCESS", "ACCEPTABLE"], ) ) @@ -197,10 +218,10 @@ def result_dict(name, actor, optional, expected, actual, success): result_object = { "scenario": self._data.scenario_tree.name, "success": self._result in ["SUCCESS", "ACCEPTABLE"], - "criteria": json_list + "criteria": json_list, } - with open(self._json, "w", encoding='utf-8') as fp: + with open(self._json, "w", encoding="utf-8") as fp: json.dump(result_object, fp, indent=4) def _write_to_junit(self): @@ -219,67 +240,92 @@ def _write_to_junit(self): if self._data.scenario_duration_game >= self._data.scenario.timeout: failure_count += 1 - with open(self._junit, "w", encoding='utf-8') as junit_file: - - junit_file.write("\n") - - test_suites_string = ("\n" % - (test_count, - failure_count, - self._start_time, - self._data.scenario_duration_system)) + with open(self._junit, "w", encoding="utf-8") as junit_file: + junit_file.write('\n') + + test_suites_string = ( + '\n' + % ( + test_count, + failure_count, + self._start_time, + self._data.scenario_duration_system, + ) + ) junit_file.write(test_suites_string) - test_suite_string = (" \n" % - (self._data.scenario_tree.name, - test_count, - failure_count, - self._data.scenario_duration_system)) + test_suite_string = ( + ' \n' + % ( + self._data.scenario_tree.name, + test_count, + failure_count, + self._data.scenario_duration_system, + ) + ) junit_file.write(test_suite_string) for criterion in self._data.scenario.get_criteria(): - testcase_name = criterion.name + "_" + \ - criterion.actor.type_id[8:] + "_" + str(criterion.actor.id) - result_string = (" \n".format( - testcase_name, self._data.scenario_tree.name)) + testcase_name = ( + criterion.name + + "_" + + criterion.actor.type_id[8:] + + "_" + + str(criterion.actor.id) + ) + result_string = ( + ' \n'.format( + testcase_name, self._data.scenario_tree.name + ) + ) if criterion.test_status != "SUCCESS": - result_string += " \n".format( - criterion.name, criterion.actual_value) + criterion.name, criterion.actual_value + ) else: result_string += " Exact Value: {} = {}\n".format( - criterion.name, criterion.actual_value) + criterion.name, criterion.actual_value + ) result_string += " \n" junit_file.write(result_string) # Handle timeout separately - result_string = (" \n".format( - self._data.scenario_duration_system, - self._data.scenario_tree.name)) + result_string = ( + ' \n'.format( + self._data.scenario_duration_system, self._data.scenario_tree.name + ) + ) if self._data.scenario_duration_game >= self._data.scenario.timeout: - result_string += " \n".format( - "Duration", self._data.scenario_duration_game) + "Duration", self._data.scenario_duration_game + ) else: result_string += " Exact Value: {} = {}\n".format( - "Duration", self._data.scenario_duration_game) + "Duration", self._data.scenario_duration_game + ) result_string += " \n" junit_file.write(result_string) diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index bb1c7dc..643437e 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -112,6 +112,8 @@ def load_scenario(self, scenario, agent=None, follow_ego=False): self.world_cam = CarlaDataProvider.get_world().get_spectator() self._camera_offset = carla.Location(x=0, y=0, z=50) self._camera_pitch = -90.0 # degrees + self._smooth_cam_loc = None + self._smooth_cam_yaw = None def run_scenario(self): """ @@ -127,8 +129,23 @@ def run_scenario(self): while self._running: _tick_start = time.perf_counter_ns() / 1e6 - timestamp = None + + # Advance CARLA to the next frame *before* agent processing + # so sensor callbacks deliver during the tick and data is + # immediately available for the agent on this same iteration. + _tick_carla_start = time.perf_counter_ns() / 1e6 world = CarlaDataProvider.get_world() + if world: + world.tick() + # Yield the GIL several times so CARLA internal threads + # can push sensor callbacks into the data buffers. + for _ in range(5): + time.sleep(0) + MetricsCollector.update_key( + "carla_time", (time.perf_counter_ns() / 1e6) - _tick_carla_start + ) + + timestamp = None if world: snapshot = world.get_snapshot() if snapshot: @@ -171,24 +188,19 @@ def _tick_scenario(self, timestamp): """ if self._timestamp_last_run < timestamp.elapsed_seconds and self._running: self._timestamp_last_run = timestamp.elapsed_seconds + start_tick = time.perf_counter_ns() self._watchdog.update() if self._debug_mode: print("\n--------- Tick ---------\n") - if self._agent is not None: - self._agent() # pylint: disable=not-callable - - _tick_carla_start = time.perf_counter_ns() / 1e6 if self._sync_mode and self._watchdog.get_status(): - CarlaDataProvider.get_world().tick() GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() - MetricsCollector.update_key( - "carla_time", (time.perf_counter_ns() / 1e6) - _tick_carla_start - ) + if self._agent is not None: + self._agent(timestamp) # pylint: disable=not-callable # Tick scenario _scenario_tick_start = time.perf_counter_ns() / 1e6 @@ -207,12 +219,25 @@ def _tick_scenario(self, timestamp): def _tick_spectator_cam(self, ego: carla.Actor) -> None: """Ticks the spectator camera for the chosen ego""" vehicle_transform = ego.get_transform() - delta_spec_loc = vehicle_transform.location + self._camera_offset + target_loc = vehicle_transform.location + self._camera_offset + target_yaw = vehicle_transform.rotation.yaw + + smoothing = 0.1 + + if self._smooth_cam_loc is None: + self._smooth_cam_loc = target_loc + self._smooth_cam_yaw = target_yaw + else: + self._smooth_cam_loc.x += (target_loc.x - self._smooth_cam_loc.x) * smoothing + self._smooth_cam_loc.y += (target_loc.y - self._smooth_cam_loc.y) * smoothing + self._smooth_cam_loc.z += (target_loc.z - self._smooth_cam_loc.z) * smoothing + dyaw = (target_yaw - self._smooth_cam_yaw + 180.0) % 360.0 - 180.0 + self._smooth_cam_yaw = (self._smooth_cam_yaw + dyaw * smoothing) % 360.0 delta_spec_trans = carla.Transform( - delta_spec_loc, + self._smooth_cam_loc, carla.Rotation( - pitch=self._camera_pitch, yaw=vehicle_transform.rotation.yaw, roll=0 + pitch=self._camera_pitch, yaw=self._smooth_cam_yaw, roll=0 ), ) self.world_cam.set_transform(delta_spec_trans) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 85970a7..dd2e0fa 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -80,7 +80,7 @@ def get_actor_control(actor): Method to return the type of control to the actor. """ control = actor.get_control() - actor_type = actor.type_id.split('.')[0] + actor_type = actor.type_id.split(".")[0] if not isinstance(actor, carla.Walker): control.steering = 0 else: @@ -90,7 +90,6 @@ def get_actor_control(actor): class AtomicBehavior(py_trees.behaviour.Behaviour): - """ Base class for all atomic behaviors used to setup a scenario @@ -123,10 +122,15 @@ def initialise(self): """ if self._actor is not None: try: - check_attr = operator.attrgetter("running_WF_actor_{}".format(self._actor.id)) + check_attr = operator.attrgetter( + "running_WF_actor_{}".format(self._actor.id) + ) terminate_wf = copy.copy(check_attr(py_trees.blackboard.Blackboard())) py_trees.blackboard.Blackboard().set( - "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True) + "terminate_WF_actor_{}".format(self._actor.id), + terminate_wf, + overwrite=True, + ) except AttributeError: # It is ok to continue, if the Blackboard variable does not exist pass @@ -136,11 +140,13 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ - self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.terminate()[%s->%s]" + % (self.__class__.__name__, self.status, new_status) + ) class RunScript(AtomicBehavior): - """ This is an atomic behavior to start execution of an additional script. @@ -173,7 +179,7 @@ def update(self): Start script """ path = None - script_components = self._script.split(' ') + script_components = self._script.split(" ") if len(script_components) > 1: path = script_components[1] @@ -186,7 +192,9 @@ def update(self): subprocess.Popen(self._script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -212,9 +220,9 @@ def update(self): """ old_value = CarlaDataProvider.get_osc_global_param_value(self._parameter_ref) - if self._rule == '+': + if self._rule == "+": new_value = self._value + float(old_value) - elif self._rule == '*': + elif self._rule == "*": new_value = self._value * float(old_value) else: new_value = self._value @@ -222,12 +230,13 @@ def update(self): CarlaDataProvider.update_osc_global_params({self._parameter_ref: new_value}) new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class ChangeWeather(AtomicBehavior): - """ Atomic to write a new weather configuration into the blackboard. Used in combination with OSCWeatherBehavior() to have a continuous weather simulation. @@ -257,12 +266,13 @@ def update(self): returns: py_trees.common.Status.SUCCESS """ - py_trees.blackboard.Blackboard().set("CarlaWeather", self._weather, overwrite=True) + py_trees.blackboard.Blackboard().set( + "CarlaWeather", self._weather, overwrite=True + ) return py_trees.common.Status.SUCCESS class ChangeRoadFriction(AtomicBehavior): - """ Atomic to update the road friction in CARLA. @@ -292,15 +302,21 @@ def update(self): py_trees.common.Status.SUCCESS """ - for actor in CarlaDataProvider.get_all_actors().filter('static.trigger.friction'): + for actor in CarlaDataProvider.get_all_actors().filter( + "static.trigger.friction" + ): actor.destroy() - friction_bp = CarlaDataProvider.get_world().get_blueprint_library().find('static.trigger.friction') + friction_bp = ( + CarlaDataProvider.get_world() + .get_blueprint_library() + .find("static.trigger.friction") + ) extent = carla.Location(1000000.0, 1000000.0, 1000000.0) - friction_bp.set_attribute('friction', str(self._friction)) - friction_bp.set_attribute('extent_x', str(extent.x)) - friction_bp.set_attribute('extent_y', str(extent.y)) - friction_bp.set_attribute('extent_z', str(extent.z)) + friction_bp.set_attribute("friction", str(self._friction)) + friction_bp.set_attribute("extent_x", str(extent.x)) + friction_bp.set_attribute("extent_y", str(extent.y)) + friction_bp.set_attribute("extent_z", str(extent.z)) # Spawn Trigger Friction transform = carla.Transform() @@ -311,7 +327,6 @@ def update(self): class ChangeActorControl(AtomicBehavior): - """ Atomic to change the longitudinal/lateral control logic for an actor. The (actor, controller) pair is stored inside the Blackboard. @@ -331,14 +346,25 @@ class ChangeActorControl(AtomicBehavior): _actor_control (ActorControl): Instance of the actor control. """ - def __init__(self, actor, control_py_module, args, scenario_file_path=None, name="ChangeActorControl"): + def __init__( + self, + actor, + control_py_module, + args, + scenario_file_path=None, + name="ChangeActorControl", + ): """ Setup actor controller. """ super(ChangeActorControl, self).__init__(name, actor) - self._actor_control = ActorControl(actor, control_py_module=control_py_module, - args=args, scenario_file_path=scenario_file_path) + self._actor_control = ActorControl( + actor, + control_py_module=control_py_module, + args=args, + scenario_file_path=scenario_file_path, + ) def update(self): """ @@ -362,13 +388,14 @@ def update(self): actor_dict[self._actor.id].reset() actor_dict[self._actor.id] = self._actor_control - py_trees.blackboard.Blackboard().set("ActorsWithController", actor_dict, overwrite=True) + py_trees.blackboard.Blackboard().set( + "ActorsWithController", actor_dict, overwrite=True + ) return py_trees.common.Status.SUCCESS class UpdateAllActorControls(AtomicBehavior): - """ Atomic to update (run one control loop step) all actor controls. @@ -408,7 +435,6 @@ def update(self): class ChangeActorTargetSpeed(AtomicBehavior): - """ Atomic to change the target speed for an actor controller. @@ -458,9 +484,19 @@ class ChangeActorTargetSpeed(AtomicBehavior): Defaults to False. """ - def __init__(self, actor, target_speed, init_speed=False, - duration=None, distance=None, relative_actor=None, - value=None, value_type=None, continuous=False, name="ChangeActorTargetSpeed"): + def __init__( + self, + actor, + target_speed, + init_speed=False, + duration=None, + distance=None, + relative_actor=None, + value=None, + value_type=None, + continuous=False, + name="ChangeActorTargetSpeed", + ): """ Setup parameters """ @@ -495,7 +531,7 @@ def initialise(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() @@ -505,14 +541,16 @@ def initialise(self): relative_velocity = CarlaDataProvider.get_velocity(self._relative_actor) # get target velocity - if self._value_type == 'delta': + if self._value_type == "delta": self._target_speed = relative_velocity + self._value - elif self._value_type == 'factor': + elif self._value_type == "factor": self._target_speed = relative_velocity * self._value else: - print('self._value_type must be delta or factor') + print("self._value_type must be delta or factor") - actor_dict[self._actor.id].update_target_speed(self._target_speed, start_time=self._start_time) + actor_dict[self._actor.id].update_target_speed( + self._target_speed, start_time=self._start_time + ) if self._init_speed: actor_dict[self._actor.id].set_init_speed() @@ -535,10 +573,13 @@ def update(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE - if actor_dict[self._actor.id].get_last_longitudinal_command() != self._start_time: + if ( + actor_dict[self._actor.id].get_last_longitudinal_command() + != self._start_time + ): return py_trees.common.Status.SUCCESS new_status = py_trees.common.Status.RUNNING @@ -547,19 +588,27 @@ def update(self): relative_velocity = CarlaDataProvider.get_velocity(self._relative_actor) # get target velocity - if self._value_type == 'delta': - actor_dict[self._actor.id].update_target_speed(relative_velocity + self._value) - elif self._value_type == 'factor': - actor_dict[self._actor.id].update_target_speed(relative_velocity * self._value) + if self._value_type == "delta": + actor_dict[self._actor.id].update_target_speed( + relative_velocity + self._value + ) + elif self._value_type == "factor": + actor_dict[self._actor.id].update_target_speed( + relative_velocity * self._value + ) else: - print('self._value_type must be delta or factor') + print("self._value_type must be delta or factor") # check duration and driven_distance if not self._continuous: - if (self._duration is not None) and (GameTime.get_time() - self._start_time > self._duration): + if (self._duration is not None) and ( + GameTime.get_time() - self._start_time > self._duration + ): new_status = py_trees.common.Status.SUCCESS - driven_distance = CarlaDataProvider.get_location(self._actor).distance(self._start_location) + driven_distance = CarlaDataProvider.get_location(self._actor).distance( + self._start_location + ) if (self._distance is not None) and (driven_distance > self._distance): new_status = py_trees.common.Status.SUCCESS @@ -570,7 +619,6 @@ def update(self): class SyncArrivalOSC(AtomicBehavior): - """ Atomic to make two actors arrive at their corresponding places at the same time @@ -592,8 +640,17 @@ class SyncArrivalOSC(AtomicBehavior): DISTANCE_THRESHOLD = 1 - def __init__(self, actor, master_actor, actor_target, master_target, final_speed, - relative_to_master=False, relative_type='', name="SyncArrivalOSC"): + def __init__( + self, + actor, + master_actor, + actor_target, + master_target, + final_speed, + relative_to_master=False, + relative_type="", + name="SyncArrivalOSC", + ): """ Setup required parameters """ @@ -627,28 +684,33 @@ def initialise(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() # Get the distance of the actor to its endpoint distance = calculate_distance( - CarlaDataProvider.get_location(self._actor), self._actor_target.location) + CarlaDataProvider.get_location(self._actor), self._actor_target.location + ) # Get the time to arrival of the reference to its endpoint distance_reference = calculate_distance( - CarlaDataProvider.get_location(self._master_actor), self._master_target.location) + CarlaDataProvider.get_location(self._master_actor), + self._master_target.location, + ) velocity_reference = CarlaDataProvider.get_velocity(self._master_actor) if velocity_reference > 0: time_reference = distance_reference / velocity_reference else: - time_reference = float('inf') + time_reference = float("inf") # Get the required velocity of the actor desired_velocity = distance / time_reference - actor_dict[self._actor.id].update_target_speed(desired_velocity, start_time=self._start_time) + actor_dict[self._actor.id].update_target_speed( + desired_velocity, start_time=self._start_time + ) def update(self): """ @@ -662,36 +724,46 @@ def update(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE - if actor_dict[self._actor.id].get_last_longitudinal_command() != self._start_time: + if ( + actor_dict[self._actor.id].get_last_longitudinal_command() + != self._start_time + ): return py_trees.common.Status.SUCCESS new_status = py_trees.common.Status.RUNNING # Get the distance of the actor to its endpoint distance = calculate_distance( - CarlaDataProvider.get_location(self._actor), self._actor_target.location) + CarlaDataProvider.get_location(self._actor), self._actor_target.location + ) if distance < self.DISTANCE_THRESHOLD: - return py_trees.common.Status.SUCCESS # Behaviour ends when the actor reaches its endpoint + return ( + py_trees.common.Status.SUCCESS + ) # Behaviour ends when the actor reaches its endpoint # Get the time to arrival of the reference to its endpoint distance_reference = calculate_distance( - CarlaDataProvider.get_location(self._master_actor), self._master_target.location) + CarlaDataProvider.get_location(self._master_actor), + self._master_target.location, + ) velocity_reference = CarlaDataProvider.get_velocity(self._master_actor) if velocity_reference > 0: time_reference = distance_reference / velocity_reference else: - time_reference = float('inf') + time_reference = float("inf") # Get the required velocity of the actor desired_velocity = distance / time_reference actor_dict[self._actor.id].update_target_speed(desired_velocity) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -708,12 +780,11 @@ def terminate(self, new_status): pass if actor_dict and self._actor.id in actor_dict: - if self._relative_to_master: master_speed = CarlaDataProvider.get_velocity(self._master_actor) - if self._relative_type == 'delta': + if self._relative_type == "delta": final_speed = master_speed + self._final_speed - elif self._relative_type == 'factor': + elif self._relative_type == "factor": final_speed = master_speed * self._final_speed else: print("'relative_type' must be delta or factor") @@ -728,7 +799,6 @@ def terminate(self, new_status): class ChangeActorWaypoints(AtomicBehavior): - """ Atomic to change the waypoints for an actor controller. @@ -782,15 +852,17 @@ def initialise(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() # Transforming OSC waypoints to Carla waypoints carla_route_elements = [] - for (osc_point, routing_option) in self._waypoints: - carla_transforms = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(osc_point) + for osc_point, routing_option in self._waypoints: + carla_transforms = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( + osc_point + ) carla_route_elements.append((carla_transforms, routing_option)) # Obtain final route, considering the routing option @@ -816,23 +888,40 @@ def initialise(self): try: interpolated_trace = grp.trace_route(waypoint, waypoint_next) except networkx.NetworkXNoPath: - print("WARNING: No route from {} to {} - Using direct path instead".format(waypoint, waypoint_next)) + print( + "WARNING: No route from {} to {} - Using direct path instead".format( + waypoint, waypoint_next + ) + ) route.append(carla_route_elements[i][0]) continue for wp_tuple in interpolated_trace: # The router sometimes produces points that go backward, or are almost identical # We have to filter these, to avoid problems - if route and wp_tuple[0].transform.location.distance(route[-1].location) > 1.0: - new_heading_vec = wp_tuple[0].transform.location - route[-1].location + if ( + route + and wp_tuple[0].transform.location.distance(route[-1].location) + > 1.0 + ): + new_heading_vec = ( + wp_tuple[0].transform.location - route[-1].location + ) new_heading = np.arctan2(new_heading_vec.y, new_heading_vec.x) if len(route) > 1: last_heading_vec = route[-1].location - route[-2].location else: - last_heading_vec = route[-1].location - ego_next_wp.transform.location - last_heading = np.arctan2(last_heading_vec.y, last_heading_vec.x) + last_heading_vec = ( + route[-1].location - ego_next_wp.transform.location + ) + last_heading = np.arctan2( + last_heading_vec.y, last_heading_vec.x + ) heading_delta = math.fabs(new_heading - last_heading) - if math.fabs(heading_delta) < 0.5 or math.fabs(heading_delta) > 5.5: + if ( + math.fabs(heading_delta) < 0.5 + or math.fabs(heading_delta) > 5.5 + ): route.append(wp_tuple[0].transform) elif not route: route.append(wp_tuple[0].transform) @@ -857,7 +946,7 @@ def update(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE actor = actor_dict[self._actor.id] @@ -874,20 +963,24 @@ def update(self): if current_waypoint_idx >= len(self._times): return py_trees.common.Status.SUCCESS remaining_time = self._times[current_waypoint_idx] - current_relative_time - self._update_speed(actor, self._waypoints[current_waypoint_idx], remaining_time) + self._update_speed( + actor, self._waypoints[current_waypoint_idx], remaining_time + ) return py_trees.common.Status.RUNNING def _update_speed(self, actor, target_waypoint, remaining_time): target_location = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( - target_waypoint[0]).location - remaining_dist = calculate_distance(CarlaDataProvider.get_location(self._actor), target_location) + target_waypoint[0] + ).location + remaining_dist = calculate_distance( + CarlaDataProvider.get_location(self._actor), target_location + ) target_speed = remaining_dist / max(remaining_time, 0.001) actor.update_target_speed(target_speed) class ChangeActorWaypointsToReachPosition(ChangeActorWaypoints): - """ Atomic to change the waypoints for an actor controller in order to reach a given position. @@ -947,7 +1040,6 @@ def initialise(self): class ChangeActorLateralMotion(AtomicBehavior): - """ Atomic to change the waypoints for an actor controller. @@ -988,8 +1080,15 @@ class ChangeActorLateralMotion(AtomicBehavior): Defaults to None. """ - def __init__(self, actor, direction='left', distance_lane_change=25, distance_other_lane=100, - lane_changes=1, name="ChangeActorLateralMotion"): + def __init__( + self, + actor, + direction="left", + distance_lane_change=25, + distance_other_lane=100, + lane_changes=1, + name="ChangeActorLateralMotion", + ): """ Setup parameters """ @@ -1025,24 +1124,34 @@ def initialise(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() # get start position - position_actor = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor)) + position_actor = CarlaDataProvider.get_map().get_waypoint( + CarlaDataProvider.get_location(self._actor) + ) # calculate plan with scenario_helper function self._plan, self._target_lane_id = generate_target_waypoint_list_multilane( - position_actor, self._direction, self._distance_same_lane, - self._distance_other_lane, self._distance_lane_change, check=False, lane_changes=self._lane_changes) + position_actor, + self._direction, + self._distance_same_lane, + self._distance_other_lane, + self._distance_lane_change, + check=False, + lane_changes=self._lane_changes, + ) if self._plan: for elem in self._plan: self._waypoints.append(elem[0].transform) - actor_dict[self._actor.id].update_waypoints(self._waypoints, start_time=self._start_time) + actor_dict[self._actor.id].update_waypoints( + self._waypoints, start_time=self._start_time + ) super(ChangeActorLateralMotion, self).initialise() @@ -1063,7 +1172,7 @@ def update(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE if not self._plan: @@ -1075,12 +1184,16 @@ def update(self): new_status = py_trees.common.Status.RUNNING - current_position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location()) + current_position_actor = CarlaDataProvider.get_map().get_waypoint( + self._actor.get_location() + ) current_lane_id = current_position_actor.lane_id if current_lane_id == self._target_lane_id: # driving on new lane - distance = current_position_actor.transform.location.distance(self._pos_before_lane_change) + distance = current_position_actor.transform.location.distance( + self._pos_before_lane_change + ) if distance > self._distance_other_lane: # long enough distance on new lane --> SUCCESS @@ -1096,7 +1209,9 @@ def update(self): else: break - actor_dict[self._actor.id].update_waypoints(new_waypoints, start_time=self._start_time) + actor_dict[self._actor.id].update_waypoints( + new_waypoints, start_time=self._start_time + ) else: self._pos_before_lane_change = current_position_actor.transform.location @@ -1105,7 +1220,6 @@ def update(self): class ChangeActorLaneOffset(AtomicBehavior): - """ OpenSCENARIO atomic. Atomic to change the offset of the controller. @@ -1139,7 +1253,14 @@ class ChangeActorLaneOffset(AtomicBehavior): OFFSET_THRESHOLD = 0.1 - def __init__(self, actor, offset, relative_actor=None, continuous=True, name="ChangeActorWaypoints"): + def __init__( + self, + actor, + offset, + relative_actor=None, + continuous=True, + name="ChangeActorWaypoints", + ): """ Setup parameters """ @@ -1171,12 +1292,14 @@ def initialise(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() - actor_dict[self._actor.id].update_offset(self._offset, start_time=self._start_time) + actor_dict[self._actor.id].update_offset( + self._offset, start_time=self._start_time + ) super(ChangeActorLaneOffset, self).initialise() @@ -1196,10 +1319,13 @@ def update(self): except AttributeError: pass - if not actor_dict or not self._actor.id in actor_dict: + if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE - if actor_dict[self._actor.id].get_last_lane_offset_command() != self._start_time: + if ( + actor_dict[self._actor.id].get_last_lane_offset_command() + != self._start_time + ): # Differentiate between lane offset and other lateral commands self._overwritten = True return py_trees.common.Status.SUCCESS @@ -1310,8 +1436,16 @@ class ChangeLateralDistance(AtomicBehavior): OFFSET_THRESHOLD = 0.3 - def __init__(self, actor, offset, relative_actor=None, freespace=False, - continuous=True, name="ChangeActorWaypoints", event_name=None): + def __init__( + self, + actor, + offset, + relative_actor=None, + freespace=False, + continuous=True, + name="ChangeActorWaypoints", + event_name=None, + ): """ Setup parameters """ @@ -1327,9 +1461,15 @@ def __init__(self, actor, offset, relative_actor=None, freespace=False, self._map = CarlaDataProvider.get_map() if freespace: if self._offset > 0: - self._offset += self._relative_actor.bounding_box.extent.y + self._actor.bounding_box.extent.y + self._offset += ( + self._relative_actor.bounding_box.extent.y + + self._actor.bounding_box.extent.y + ) else: - self._offset -= self._relative_actor.bounding_box.extent.y + self._actor.bounding_box.extent.y + self._offset -= ( + self._relative_actor.bounding_box.extent.y + + self._actor.bounding_box.extent.y + ) self._actor_name = actor.attributes.get("role_name") self._event_name = event_name if relative_actor: @@ -1357,7 +1497,9 @@ def initialise(self): self._start_time = GameTime.get_time() - actor_dict[self._actor.id].update_offset(self._offset, start_time=self._start_time) + actor_dict[self._actor.id].update_offset( + self._offset, start_time=self._start_time + ) super(ChangeLateralDistance, self).initialise() @@ -1380,7 +1522,10 @@ def update(self): if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE - if actor_dict[self._actor.id].get_last_lane_offset_command() != self._start_time: + if ( + actor_dict[self._actor.id].get_last_lane_offset_command() + != self._start_time + ): # Differentiate between lane offset and other lateral commands self._overwritten = True # last_lane_offset_command_time = actor_dict[self._actor.id].get_last_lane_offset_command() @@ -1458,7 +1603,6 @@ def terminate(self, new_status): class ActorTransformSetterToOSCPosition(AtomicBehavior): - """ OpenSCENARIO atomic This class contains an atomic behavior to set the transform of an OpenSCENARIO actor. @@ -1477,7 +1621,13 @@ class ActorTransformSetterToOSCPosition(AtomicBehavior): Therefore: calculate_distance(actor, transform) < 1 meter """ - def __init__(self, actor, osc_position, physics=True, name="ActorTransformSetterToOSCPosition"): + def __init__( + self, + actor, + osc_position, + physics=True, + name="ActorTransformSetterToOSCPosition", + ): """ Setup parameters """ @@ -1487,7 +1637,6 @@ def __init__(self, actor, osc_position, physics=True, name="ActorTransformSetter self._osc_transform = None def initialise(self): - super(ActorTransformSetterToOSCPosition, self).initialise() if self._actor.is_alive: @@ -1502,13 +1651,17 @@ def update(self): # calculate transform with method in openscenario_parser.py self._osc_transform = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( - self._osc_position) + self._osc_position + ) self._actor.set_transform(self._osc_transform) if not self._actor.is_alive: new_status = py_trees.common.Status.FAILURE - if calculate_distance(self._actor.get_location(), self._osc_transform.location) < 1.0: + if ( + calculate_distance(self._actor.get_location(), self._osc_transform.location) + < 1.0 + ): if self._physics: self._actor.set_simulate_physics(enabled=True) new_status = py_trees.common.Status.SUCCESS @@ -1517,7 +1670,6 @@ def update(self): class AccelerateToVelocity(AtomicBehavior): - """ This class contains an atomic acceleration behavior. The controlled traffic participant will accelerate with _throttle_value_ until reaching @@ -1544,9 +1696,11 @@ def __init__(self, actor, throttle_value, target_velocity, name="Acceleration"): def initialise(self): # In case of walkers, we have to extract the current heading - if self._type == 'walker': + if self._type == "walker": self._control.speed = self._target_velocity - self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() + self._control.direction = CarlaDataProvider.get_transform( + self._actor + ).get_forward_vector() super(AccelerateToVelocity, self).initialise() @@ -1556,7 +1710,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._type == 'vehicle': + if self._type == "vehicle": if CarlaDataProvider.get_velocity(self._actor) < self._target_velocity: self._control.throttle = self._throttle_value else: @@ -1564,13 +1718,14 @@ def update(self): self._control.throttle = 0 self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class UniformAcceleration(AtomicBehavior): - """ This class contains an atomic acceleration behavior. The controlled traffic participant will accelerate with _throttle_value_ until reaching @@ -1585,9 +1740,18 @@ class UniformAcceleration(AtomicBehavior): The behavior will terminate, if the actor's velocity is at least target_velocity """ + OFFSET_THRESHOLD = 0.1 - def __init__(self, actor, start_velocity, target_velocity, acceleration, start_time, name="Acceleration"): + def __init__( + self, + actor, + start_velocity, + target_velocity, + acceleration, + start_time, + name="Acceleration", + ): """ Setup parameters including acceleration value (via throttle_value), start_velocity, target velocity and duration @@ -1599,14 +1763,18 @@ def __init__(self, actor, start_velocity, target_velocity, acceleration, start_t self._acceleration = acceleration self._start_time = start_time self._target_velocity = target_velocity - print(f"actor_type:{self._type}, start_speed:{self._start_velocity}, " - f"acceleration:{self._acceleration},target_speed:{self._target_velocity},start_time:{self._start_time}") + print( + f"actor_type:{self._type}, start_speed:{self._start_velocity}, " + f"acceleration:{self._acceleration},target_speed:{self._target_velocity},start_time:{self._start_time}" + ) def initialise(self): # In case of walkers, we have to extract the current heading - if self._type == 'walker': + if self._type == "walker": self._control.speed = self._start_velocity - self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() + self._control.direction = CarlaDataProvider.get_transform( + self._actor + ).get_forward_vector() super(UniformAcceleration, self).initialise() @@ -1618,15 +1786,19 @@ def update(self): time_now = GameTime.get_time() time_variation = time_now - self._start_time - speed_variation = CarlaDataProvider.get_velocity(self._actor) - self._start_velocity - if self._type == 'vehicle': + speed_variation = ( + CarlaDataProvider.get_velocity(self._actor) - self._start_velocity + ) + if self._type == "vehicle": curr_speed = CarlaDataProvider.get_velocity(self._actor) if abs(self._target_velocity - curr_speed) < self.OFFSET_THRESHOLD: self._control.throttle = 0 self._control.brake = 0 new_status = py_trees.common.Status.SUCCESS - print(f"time_variation:{time_variation},speed_variation:{speed_variation}," - f" current_speed:{CarlaDataProvider.get_velocity(self._actor)}") + print( + f"time_variation:{time_variation},speed_variation:{speed_variation}," + f" current_speed:{CarlaDataProvider.get_velocity(self._actor)}" + ) if speed_variation / time_variation < self._acceleration: self._control.throttle = 1 self._control.brake = 0 @@ -1635,13 +1807,14 @@ def update(self): self._control.brake = 1 self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class ChangeTargetSpeed(AtomicBehavior): - """ Important parameters: - actor: CARLA actor to execute the behavior @@ -1649,6 +1822,7 @@ class ChangeTargetSpeed(AtomicBehavior): The behavior will terminate, if the actor's velocity is at least target_velocity """ + OFFSET_THRESHOLD = 0.7 def __init__(self, actor, target_velocity, name="ChangeTargetSpeed"): @@ -1663,9 +1837,11 @@ def __init__(self, actor, target_velocity, name="ChangeTargetSpeed"): def initialise(self): # In case of walkers, we have to extract the current heading - if self._type == 'walker': + if self._type == "walker": self._control.speed = self._target_velocity - self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() + self._control.direction = CarlaDataProvider.get_transform( + self._actor + ).get_forward_vector() super(ChangeTargetSpeed, self).initialise() @@ -1675,34 +1851,37 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._type == 'vehicle': + if self._type == "vehicle": # curr_speed = CarlaDataProvider.get_velocity(self._actor) - curr_speed = calculate_velocity(self._actor)*3.6 + curr_speed = calculate_velocity(self._actor) * 3.6 if abs(self._target_velocity - curr_speed) < self.OFFSET_THRESHOLD: self._control.throttle = 0 self._control.brake = 0 new_status = py_trees.common.Status.SUCCESS - print(f'finish change speed!! current speed={curr_speed} km/h') + print(f"finish change speed!! current speed={curr_speed} km/h") else: if curr_speed < self._target_velocity: # 加速 self._control.throttle = 1 self._control.brake = 0 - print(f'current speed={curr_speed} km/h, target speed={self._target_velocity} km/h, accelerate!!! ') + print( + f"current speed={curr_speed} km/h, target speed={self._target_velocity} km/h, accelerate!!! " + ) else: # 减速 self._control.throttle = 0 self._control.brake = 1 - print('decelerate!!!') + print("decelerate!!!") self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class DecelerateToVelocity(AtomicBehavior): - """ This class contains an atomic acceleration behavior. The controlled traffic participant will accelerate with _throttle_value_ until reaching @@ -1729,9 +1908,11 @@ def __init__(self, actor, brake_value, target_velocity, name="Deceleration"): def initialise(self): # In case of walkers, we have to extract the current heading - if self._type == 'walker': + if self._type == "walker": self._control.speed = self._target_velocity - self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() + self._control.direction = CarlaDataProvider.get_transform( + self._actor + ).get_forward_vector() super(DecelerateToVelocity, self).initialise() @@ -1741,7 +1922,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._type == 'vehicle': + if self._type == "vehicle": speed = CarlaDataProvider.get_velocity(self._actor) if speed > self._target_velocity: self._control.brake = self._brake_value @@ -1752,13 +1933,14 @@ def update(self): self._control.brake = 0 self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class AccelerateToCatchUp(AtomicBehavior): - """ This class contains an atomic acceleration behavior. The car will accelerate until it is faster than another car, in order to catch up distance. @@ -1777,8 +1959,16 @@ class AccelerateToCatchUp(AtomicBehavior): then the behaviour ends with a failure. """ - def __init__(self, actor, other_actor, throttle_value=1, delta_velocity=10, trigger_distance=5, - max_distance=500, name="AccelerateToCatchUp"): + def __init__( + self, + actor, + other_actor, + throttle_value=1, + delta_velocity=10, + trigger_distance=5, + max_distance=500, + name="AccelerateToCatchUp", + ): """ Setup parameters The target_speet is calculated on the fly. @@ -1796,23 +1986,26 @@ def __init__(self, actor, other_actor, throttle_value=1, delta_velocity=10, trig self._initial_actor_pos = None def initialise(self): - # get initial actor position self._initial_actor_pos = CarlaDataProvider.get_location(self._actor) super(AccelerateToCatchUp, self).initialise() def update(self): - # get actor speed actor_speed = CarlaDataProvider.get_velocity(self._actor) - target_speed = CarlaDataProvider.get_velocity(self._other_actor) + self._delta_velocity + target_speed = ( + CarlaDataProvider.get_velocity(self._other_actor) + self._delta_velocity + ) # distance between actors distance = CarlaDataProvider.get_location(self._actor).distance( - CarlaDataProvider.get_location(self._other_actor)) + CarlaDataProvider.get_location(self._other_actor) + ) # driven distance of actor - driven_distance = CarlaDataProvider.get_location(self._actor).distance(self._initial_actor_pos) + driven_distance = CarlaDataProvider.get_location(self._actor).distance( + self._initial_actor_pos + ) if actor_speed < target_speed: # set throttle to throttle_value to accelerate @@ -1837,7 +2030,6 @@ def update(self): class KeepVelocity(AtomicBehavior): - """ This class contains an atomic behavior to keep the provided velocity. The controlled traffic participant will accelerate as fast as possible @@ -1855,8 +2047,15 @@ class KeepVelocity(AtomicBehavior): Alternatively, a parallel termination behavior has to be used. """ - def __init__(self, actor, target_velocity, force_speed=False, - duration=float("inf"), distance=float("inf"), name="KeepVelocity"): + def __init__( + self, + actor, + target_velocity, + force_speed=False, + duration=float("inf"), + distance=float("inf"), + name="KeepVelocity", + ): """ Setup parameters including acceleration value (via throttle_value) and target velocity @@ -1882,10 +2081,12 @@ def initialise(self): self._start_time = GameTime.get_time() # In case of walkers, we have to extract the current heading - if self._type == 'walker': + if self._type == "walker": self._control.speed = self._target_velocity - self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() - elif self._type == 'vehicle': + self._control.direction = CarlaDataProvider.get_transform( + self._actor + ).get_forward_vector() + elif self._type == "vehicle": self._control.hand_brake = False self._actor.apply_control(self._control) @@ -1899,7 +2100,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._type == 'vehicle': + if self._type == "vehicle": if not self._forced_speed: if CarlaDataProvider.get_velocity(self._actor) < self._target_velocity: self._control.throttle = 1.0 @@ -1907,9 +2108,16 @@ def update(self): self._control.throttle = 0.0 self._actor.apply_control(self._control) else: - yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw * (math.pi / 180) - self._actor.set_target_velocity(carla.Vector3D( - math.cos(yaw) * self._target_velocity, math.sin(yaw) * self._target_velocity, 0)) + yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw * ( + math.pi / 180 + ) + self._actor.set_target_velocity( + carla.Vector3D( + math.cos(yaw) * self._target_velocity, + math.sin(yaw) * self._target_velocity, + 0, + ) + ) # Add a throttle. Useless speed-wise, but makes the bicycle riders pedal. self._actor.apply_control(carla.VehicleControl(throttle=1.0)) @@ -1924,7 +2132,9 @@ def update(self): if GameTime.get_time() - self._start_time > self._duration: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -1934,9 +2144,9 @@ def terminate(self, new_status): to avoid further acceleration. """ try: - if self._type == 'vehicle': + if self._type == "vehicle": self._control.throttle = 0.0 - elif self._type == 'walker': + elif self._type == "walker": self._control.speed = 0.0 if self._actor is not None and self._actor.is_alive: self._actor.apply_control(self._control) @@ -1946,7 +2156,6 @@ def terminate(self, new_status): class ChangeAutoPilot(AtomicBehavior): - """ This class contains an atomic behavior to disable/enable the use of the autopilot. @@ -1969,14 +2178,17 @@ def __init__(self, actor, activate, parameters=None, name="ChangeAutoPilot"): self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._activate = activate self._tm = CarlaDataProvider.get_client().get_trafficmanager( - CarlaDataProvider.get_traffic_manager_port()) + CarlaDataProvider.get_traffic_manager_port() + ) self._parameters = parameters def update(self): """ De/activate autopilot """ - self._actor.set_autopilot(self._activate, CarlaDataProvider.get_traffic_manager_port()) + self._actor.set_autopilot( + self._activate, CarlaDataProvider.get_traffic_manager_port() + ) if self._parameters is not None: if "auto_lane_change" in self._parameters: @@ -1988,7 +2200,9 @@ def update(self): max_road_speed = self._actor.get_speed_limit() if max_road_speed is not None: percentage = (max_road_speed - max_speed) / max_road_speed * 100.0 - self._tm.vehicle_percentage_speed_difference(self._actor, percentage) + self._tm.vehicle_percentage_speed_difference( + self._actor, percentage + ) else: print("ChangeAutopilot: Unable to find the vehicle's speed limit") @@ -2006,12 +2220,13 @@ def update(self): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class StopVehicle(AtomicBehavior): - """ This class contains an atomic stopping behavior. The controlled traffic participant will decelerate with _bake_value_ until reaching a full stop. @@ -2030,7 +2245,7 @@ def __init__(self, actor, brake_value, name="Stopping"): super(StopVehicle, self).__init__(name, actor) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._control, self._type = get_actor_control(actor) - if self._type == 'walker': + if self._type == "walker": self._control.speed = 0 self._brake_value = brake_value @@ -2040,7 +2255,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._type == 'vehicle': + if self._type == "vehicle": if CarlaDataProvider.get_velocity(self._actor) > EPSILON: self._control.brake = self._brake_value else: @@ -2051,13 +2266,14 @@ def update(self): self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class SyncArrival(AtomicBehavior): - """ This class contains an atomic behavior to set velocity of actor so that it reaches location at the same time as @@ -2073,7 +2289,9 @@ class SyncArrival(AtomicBehavior): certain distance, etc. """ - def __init__(self, actor, actor_reference, target_location, gain=1, name="SyncArrival"): + def __init__( + self, actor, actor_reference, target_location, gain=1, name="SyncArrival" + ): """ Setup required parameters """ @@ -2092,18 +2310,20 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - distance_reference = calculate_distance(CarlaDataProvider.get_location(self._actor_reference), - self._target_location) - distance = calculate_distance(CarlaDataProvider.get_location(self._actor), - self._target_location) + distance_reference = calculate_distance( + CarlaDataProvider.get_location(self._actor_reference), self._target_location + ) + distance = calculate_distance( + CarlaDataProvider.get_location(self._actor), self._target_location + ) velocity_reference = CarlaDataProvider.get_velocity(self._actor_reference) - time_reference = float('inf') + time_reference = float("inf") if velocity_reference > 0: time_reference = distance_reference / velocity_reference velocity_current = CarlaDataProvider.get_velocity(self._actor) - time_current = float('inf') + time_current = float("inf") if velocity_current > 0: time_current = distance / velocity_current @@ -2117,7 +2337,9 @@ def update(self): self._control.brake = min([abs(control_value), 1]) self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -2133,7 +2355,6 @@ def terminate(self, new_status): class SyncArrivalWithAgent(AtomicBehavior): - """ Atomic to make two actors arrive at their corresponding places at the same time. This uses a controller and presuposes that the actor can reach its destination by following the lane. @@ -2151,8 +2372,15 @@ class SyncArrivalWithAgent(AtomicBehavior): Defaults to 'SyncArrivalWithAgent'. """ - def __init__(self, actor, reference_actor, actor_target, reference_target, end_dist=1, - name="SyncArrivalWithAgent"): + def __init__( + self, + actor, + reference_actor, + actor_target, + reference_target, + end_dist=1, + name="SyncArrivalWithAgent", + ): """ Setup required parameters """ @@ -2171,7 +2399,8 @@ def initialise(self): self._agent = ConstantVelocityAgent( self._actor, map_inst=CarlaDataProvider.get_map(), - grp_inst=CarlaDataProvider.get_global_route_planner()) + grp_inst=CarlaDataProvider.get_global_route_planner(), + ) def update(self): """ @@ -2182,24 +2411,29 @@ def update(self): # Get the distance of the actor to its endpoint distance = calculate_distance( - CarlaDataProvider.get_location(self._actor), self._actor_target.location) + CarlaDataProvider.get_location(self._actor), self._actor_target.location + ) # Check if the reference actor has passed its target if distance < self._end_dist: ref_dir = self._reference_target.get_forward_vector() - ref_veh = self._reference_target.location - self._reference_actor.get_location() + ref_veh = ( + self._reference_target.location - self._reference_actor.get_location() + ) if ref_veh.dot(ref_dir) > 0: return py_trees.common.Status.SUCCESS # Get the time to arrival of the reference to its endpoint distance_reference = calculate_distance( - CarlaDataProvider.get_location(self._reference_actor), self._reference_target.location) + CarlaDataProvider.get_location(self._reference_actor), + self._reference_target.location, + ) velocity_reference = CarlaDataProvider.get_velocity(self._reference_actor) if velocity_reference > 0: time_reference = distance_reference / velocity_reference else: - time_reference = float('inf') + time_reference = float("inf") # Get the required velocity of the actor desired_velocity = distance / time_reference @@ -2207,7 +2441,9 @@ def update(self): self._agent.set_target_speed(3.6 * desired_velocity) self._actor.apply_control(self._agent.run_step()) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -2218,7 +2454,6 @@ def terminate(self, new_status): class CutIn(AtomicBehavior): - """ Atomic to make an actor lane change using a Python API agent, cutting in front of another one @@ -2236,9 +2471,17 @@ class CutIn(AtomicBehavior): Defaults to 'CutIn'. """ - def __init__(self, actor, reference_actor, direction, speed_perc=100, - same_lane_time=0, other_lane_time=0, change_time=2, - name="CutIn"): + def __init__( + self, + actor, + reference_actor, + direction, + speed_perc=100, + same_lane_time=0, + other_lane_time=0, + change_time=2, + name="CutIn", + ): """ Setup required parameters """ @@ -2262,8 +2505,14 @@ def initialise(self): self._actor, 3.6 * speed * self._speed_perc / 100, map_inst=CarlaDataProvider.get_map(), - grp_inst=CarlaDataProvider.get_global_route_planner()) - self._agent.lane_change(self._direction, self._same_lane_time, self._other_lane_time, self._change_time) + grp_inst=CarlaDataProvider.get_global_route_planner(), + ) + self._agent.lane_change( + self._direction, + self._same_lane_time, + self._other_lane_time, + self._change_time, + ) def update(self): """ @@ -2276,12 +2525,13 @@ def update(self): self._actor.apply_control(self._agent.run_step()) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class AddNoiseToVehicle(AtomicBehavior): - """ This class contains an atomic jitter behavior. To add noise to steer as well as throttle of the vehicle. @@ -2313,14 +2563,15 @@ def update(self): self._control.throttle = self._throttle_value new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) self._actor.apply_control(self._control) return new_status class AddNoiseToRouteEgo(AtomicBehavior): - """ This class contains an atomic jitter behavior. To add noise to steer as well as throttle of the vehicle. @@ -2333,7 +2584,15 @@ class AddNoiseToRouteEgo(AtomicBehavior): The behavior terminates after setting the new actor controls """ - def __init__(self, actor, throttle_mean, throttle_std, steer_mean, steer_std, name="AddNoiseToVehicle"): + def __init__( + self, + actor, + throttle_mean, + throttle_std, + steer_mean, + steer_std, + name="AddNoiseToVehicle", + ): """ Setup actor , maximum steer value and throttle value """ @@ -2353,7 +2612,9 @@ def update(self): control = py_trees.blackboard.Blackboard().get("AV_control") if not control: - print("WARNING: Couldn't add noise to the ego because the control couldn't be found") + print( + "WARNING: Couldn't add noise to the ego because the control couldn't be found" + ) return new_status throttle_noise = random.normal(self._throttle_mean, self._throttle_std) @@ -2362,14 +2623,15 @@ def update(self): steer_noise = random.normal(self._steer_mean, self._steer_std) control.steer = max(0, min(1, control.steer + steer_noise)) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) self._actor.apply_control(control) return new_status class ChangeNoiseParameters(AtomicBehavior): - """ This class contains an atomic jitter behavior. To add noise to steer as well as throttle of the vehicle. @@ -2379,8 +2641,16 @@ class ChangeNoiseParameters(AtomicBehavior): The behavior terminates after one iteration """ - def __init__(self, new_steer_noise, new_throttle_noise, - noise_mean, noise_std, dynamic_mean_for_steer, dynamic_mean_for_throttle, name="ChangeJittering"): + def __init__( + self, + new_steer_noise, + new_throttle_noise, + noise_mean, + noise_std, + dynamic_mean_for_steer, + dynamic_mean_for_throttle, + name="ChangeJittering", + ): """ Setup actor , maximum steer value and throttle value """ @@ -2400,11 +2670,17 @@ def update(self): Change the noise parameters from the structure copy that it receives. """ - self._new_steer_noise[0] = min(0, -(self._noise_to_apply - self._dynamic_mean_for_steer)) - self._new_throttle_noise[0] = min(self._noise_to_apply + self._dynamic_mean_for_throttle, 1) + self._new_steer_noise[0] = min( + 0, -(self._noise_to_apply - self._dynamic_mean_for_steer) + ) + self._new_throttle_noise[0] = min( + self._noise_to_apply + self._dynamic_mean_for_throttle, 1 + ) new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -2420,7 +2696,15 @@ class BasicAgentBehavior(AtomicBehavior): The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, target_location=None, plan=None, target_speed=20, opt_dict=None, name="BasicAgentBehavior"): + def __init__( + self, + actor, + target_location=None, + plan=None, + target_speed=20, + opt_dict=None, + name="BasicAgentBehavior", + ): """ Setup actor and maximum steer value """ @@ -2439,12 +2723,19 @@ def __init__(self, actor, target_location=None, plan=None, target_speed=20, opt_ def initialise(self): """Initialises the agent""" - self._agent = BasicAgent(self._actor, self._target_speed, opt_dict=self._opt_dict, - map_inst=CarlaDataProvider.get_map(), grp_inst=CarlaDataProvider.get_global_route_planner()) + self._agent = BasicAgent( + self._actor, + self._target_speed, + opt_dict=self._opt_dict, + map_inst=CarlaDataProvider.get_map(), + grp_inst=CarlaDataProvider.get_global_route_planner(), + ) if self._plan: self._agent.set_global_plan(self._plan) elif self._target_location: - init_wp = self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)) + init_wp = self._map.get_waypoint( + CarlaDataProvider.get_location(self._actor) + ) end_wp = self._map.get_waypoint(self._target_location) self._plan = self._agent.trace_route(init_wp, end_wp) self._agent.set_global_plan(self._plan) @@ -2457,7 +2748,9 @@ def update(self): self._control = self._agent.run_step() self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -2469,7 +2762,6 @@ def terminate(self, new_status): class ConstantVelocityAgentBehavior(AtomicBehavior): - """ This class contains an atomic behavior, which uses the constant_velocity_agent from CARLA to control the actor until @@ -2483,8 +2775,14 @@ class ConstantVelocityAgentBehavior(AtomicBehavior): The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, target_location, target_speed=None, - opt_dict=None, name="ConstantVelocityAgentBehavior"): + def __init__( + self, + actor, + target_location, + target_speed=None, + opt_dict=None, + name="ConstantVelocityAgentBehavior", + ): """ Set up actor and local planner """ @@ -2503,11 +2801,16 @@ def __init__(self, actor, target_location, target_speed=None, def initialise(self): """Initialises the agent""" self._agent = ConstantVelocityAgent( - self._actor, self._target_speed * 3.6, opt_dict=self._opt_dict, - map_inst=CarlaDataProvider.get_map(), grp_inst=CarlaDataProvider.get_global_route_planner()) + self._actor, + self._target_speed * 3.6, + opt_dict=self._opt_dict, + map_inst=CarlaDataProvider.get_map(), + grp_inst=CarlaDataProvider.get_global_route_planner(), + ) self._plan = self._agent.trace_route( self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), - self._map.get_waypoint(self._target_location)) + self._map.get_waypoint(self._target_location), + ) self._agent.set_global_plan(self._plan) def update(self): @@ -2520,7 +2823,9 @@ def update(self): self._control = self._agent.run_step() self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -2533,8 +2838,8 @@ def terminate(self, new_status): self._agent.destroy_sensor() super(ConstantVelocityAgentBehavior, self).terminate(new_status) -class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): +class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): """ This class contains an atomic behavior, which uses the constant_velocity_agent from CARLA to control the actor until @@ -2542,17 +2847,24 @@ class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): Important parameters: - actor: CARLA actor to execute the behavior. - reference_actor: Reference CARLA actor to get target speed. - - speed_increment: Float value (m/s). + - speed_increment: Float value (m/s). How much the actor will be faster then the reference_actor. - target_location: Is the desired target location (carla.location), - the actor should move to. + the actor should move to. If it's None, the actor will follow the lane and never stop. - plan: List of [carla.Waypoint, RoadOption] to pass to the controller. The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, reference_actor, target_location=None, speed_increment=10, - opt_dict=None, name="AdaptiveConstantVelocityAgentBehavior"): + def __init__( + self, + actor, + reference_actor, + target_location=None, + speed_increment=10, + opt_dict=None, + name="AdaptiveConstantVelocityAgentBehavior", + ): """ Set up actor and local planner """ @@ -2573,13 +2885,19 @@ def initialise(self): # Get target speed target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 - self._agent = ConstantVelocityAgent(self._actor, target_speed, opt_dict=self._opt_dict, - map_inst=self._map, grp_inst=self._grp) + self._agent = ConstantVelocityAgent( + self._actor, + target_speed, + opt_dict=self._opt_dict, + map_inst=self._map, + grp_inst=self._grp, + ) if self._target_location is not None: self._plan = self._agent.trace_route( self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), - self._map.get_waypoint(self._target_location)) + self._map.get_waypoint(self._target_location), + ) self._agent.set_global_plan(self._plan) def update(self): @@ -2594,7 +2912,9 @@ def update(self): self._control = self._agent.run_step() self._actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -2607,8 +2927,8 @@ def terminate(self, new_status): self._agent.destroy_sensor() super().terminate(new_status) -class Idle(AtomicBehavior): +class Idle(AtomicBehavior): """ This class contains an idle behavior scenario @@ -2646,8 +2966,8 @@ def update(self): return new_status -class WaitForever(AtomicBehavior): +class WaitForever(AtomicBehavior): """ This class contains a behavior that just waits forever. Useful to stop some behavior sequences from stopping unwated parts of the behavior tree @@ -2670,7 +2990,6 @@ def update(self): class WaypointFollower(AtomicBehavior): - """ This is an atomic behavior to follow waypoints while maintaining a given speed. If no plan is provided, the actor will follow its foward waypoints indefinetely. @@ -2711,8 +3030,15 @@ class WaypointFollower(AtomicBehavior): following behavior must terminate the WaypointFollower. """ - def __init__(self, actor, target_speed=None, plan=None, blackboard_queue_name=None, - avoid_collision=False, name="FollowWaypoints"): + def __init__( + self, + actor, + target_speed=None, + plan=None, + blackboard_queue_name=None, + avoid_collision=False, + name="FollowWaypoints", + ): """ Set up actor and local planner """ @@ -2726,7 +3052,7 @@ def __init__(self, actor, target_speed=None, plan=None, blackboard_queue_name=No self._blackboard_queue_name = blackboard_queue_name if blackboard_queue_name is not None: self._queue = Blackboard().get(blackboard_queue_name) - self._args_lateral_dict = {'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05} + self._args_lateral_dict = {"K_P": 1.0, "K_D": 0.01, "K_I": 0.0, "dt": 0.05} self._avoid_collision = avoid_collision self._unique_id = 0 @@ -2742,17 +3068,25 @@ def initialise(self): self._unique_id = int(round(time.time() * 1e9)) try: # check whether WF for this actor is already running and add new WF to running_WF list - check_attr = operator.attrgetter("running_WF_actor_{}".format(self._actor.id)) + check_attr = operator.attrgetter( + "running_WF_actor_{}".format(self._actor.id) + ) running = check_attr(py_trees.blackboard.Blackboard()) active_wf = copy.copy(running) active_wf.append(self._unique_id) py_trees.blackboard.Blackboard().set( - "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True) + "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True + ) except AttributeError: # no WF is active for this actor - py_trees.blackboard.Blackboard().set("terminate_WF_actor_{}".format(self._actor.id), [], overwrite=True) py_trees.blackboard.Blackboard().set( - "running_WF_actor_{}".format(self._actor.id), [self._unique_id], overwrite=True) + "terminate_WF_actor_{}".format(self._actor.id), [], overwrite=True + ) + py_trees.blackboard.Blackboard().set( + "running_WF_actor_{}".format(self._actor.id), + [self._unique_id], + overwrite=True, + ) for actor in self._actor_dict: self._apply_local_planner(actor) @@ -2774,21 +3108,26 @@ def _apply_local_planner(self, actor): if isinstance(self._plan[0], carla.Location): self._actor_dict[actor] = self._plan else: - self._actor_dict[actor] = [element[0].transform.location for element in self._plan] + self._actor_dict[actor] = [ + element[0].transform.location for element in self._plan + ] else: local_planner = LocalPlanner( # pylint: disable=undefined-variable - actor, opt_dict={ - 'target_speed': self._target_speed * 3.6, - 'lateral_control_dict': self._args_lateral_dict, - 'max_throttle': 1.0}) + actor, + opt_dict={ + "target_speed": self._target_speed * 3.6, + "lateral_control_dict": self._args_lateral_dict, + "max_throttle": 1.0, + }, + ) if self._plan is not None: if isinstance(self._plan[0], carla.Location): plan = [] for location in self._plan: - waypoint = CarlaDataProvider.get_map().get_waypoint(location, - project_to_road=True, - lane_type=carla.LaneType.Any) + waypoint = CarlaDataProvider.get_map().get_waypoint( + location, project_to_road=True, lane_type=carla.LaneType.Any + ) plan.append((waypoint, RoadOption.LANEFOLLOW)) local_planner.set_global_plan(plan) else: @@ -2817,9 +3156,13 @@ def update(self): active_wf.remove(self._unique_id) py_trees.blackboard.Blackboard().set( - "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True) + "terminate_WF_actor_{}".format(self._actor.id), + terminate_wf, + overwrite=True, + ) py_trees.blackboard.Blackboard().set( - "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True) + "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True + ) new_status = py_trees.common.Status.SUCCESS return new_status @@ -2864,7 +3207,9 @@ def update(self): else: control = actor.get_control() control.speed = self._target_speed - control.direction = CarlaDataProvider.get_transform(actor).rotation.get_forward_vector() + control.direction = CarlaDataProvider.get_transform( + actor + ).rotation.get_forward_vector() actor.apply_control(control) if success: @@ -2892,7 +3237,6 @@ def terminate(self, new_status): class LaneChange(WaypointFollower): - """ This class inherits from the class WaypointFollower. @@ -2919,9 +3263,17 @@ class LaneChange(WaypointFollower): A parallel termination behavior has to be used. """ - def __init__(self, actor, speed=10, direction='left', distance_same_lane=5, distance_other_lane=100, - distance_lane_change=25, lane_changes=1, name='LaneChange'): - + def __init__( + self, + actor, + speed=10, + direction="left", + distance_same_lane=5, + distance_other_lane=100, + distance_lane_change=25, + lane_changes=1, + name="LaneChange", + ): self._direction = direction self._distance_same_lane = distance_same_lane self._distance_other_lane = distance_other_lane @@ -2936,30 +3288,40 @@ def __init__(self, actor, speed=10, direction='left', distance_same_lane=5, dist super(LaneChange, self).__init__(actor, target_speed=speed, name=name) def initialise(self): - # get start position - position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location()) + position_actor = CarlaDataProvider.get_map().get_waypoint( + self._actor.get_location() + ) # calculate plan with scenario_helper function self._plan, self._target_lane_id = generate_target_waypoint_list_multilane( - position_actor, self._direction, self._distance_same_lane, - self._distance_other_lane, self._distance_lane_change, check=True, lane_changes=self._lane_changes) + position_actor, + self._direction, + self._distance_same_lane, + self._distance_other_lane, + self._distance_lane_change, + check=True, + lane_changes=self._lane_changes, + ) super(LaneChange, self).initialise() def update(self): - if not self._plan: print("{} couldn't perform the expected lane change".format(self._actor)) return py_trees.common.Status.FAILURE status = super(LaneChange, self).update() - current_position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location()) + current_position_actor = CarlaDataProvider.get_map().get_waypoint( + self._actor.get_location() + ) current_lane_id = current_position_actor.lane_id if current_lane_id == self._target_lane_id: # driving on new lane - distance = current_position_actor.transform.location.distance(self._pos_before_lane_change) + distance = current_position_actor.transform.location.distance( + self._pos_before_lane_change + ) if distance > self._distance_other_lane: # long enough distance on new lane --> SUCCESS @@ -2971,14 +3333,12 @@ def update(self): class SetInitSpeed(AtomicBehavior): - """ This class contains an atomic behavior to set the init_speed of an actor, succeding immeditely after initializing """ - def __init__(self, actor, init_speed=10, name='SetInitSpeed'): - + def __init__(self, actor, init_speed=10, name="SetInitSpeed"): self._init_speed = init_speed self._terminate = None self._actor = actor @@ -3006,7 +3366,6 @@ def update(self): class HandBrakeVehicle(AtomicBehavior): - """ This class contains an atomic hand brake behavior. To set the hand brake value of the vehicle. @@ -3033,20 +3392,21 @@ def update(self): Set handbrake """ new_status = py_trees.common.Status.SUCCESS - if self._type == 'vehicle': + if self._type == "vehicle": self._control.hand_brake = self._hand_brake_value self._vehicle.apply_control(self._control) else: self._hand_brake_value = None - self.logger.debug("%s.update()[%s->%s]" % - (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" + % (self.__class__.__name__, self.status, new_status) + ) self._vehicle.apply_control(self._control) return new_status class ActorDestroy(AtomicBehavior): - """ This class contains an actor destroy behavior. Given an actor this behavior will delete it. @@ -3075,7 +3435,6 @@ def update(self): class ActorTransformSetter(AtomicBehavior): - """ This class contains an atomic behavior to set the transform of an actor. @@ -3119,7 +3478,10 @@ def update(self): if not self._actor.is_alive: new_status = py_trees.common.Status.FAILURE - if calculate_distance(self._actor.get_location(), self._transform.location) < 1.0: + if ( + calculate_distance(self._actor.get_location(), self._transform.location) + < 1.0 + ): if self._physics is not None: self._actor.set_simulate_physics(self._physics) new_status = py_trees.common.Status.SUCCESS @@ -3128,7 +3490,6 @@ def update(self): class BatchActorTransformSetter(AtomicBehavior): - """ This class contains an atomic behavior to set the transform of an actor. @@ -3140,7 +3501,9 @@ class BatchActorTransformSetter(AtomicBehavior): The behavior terminates immediately """ - def __init__(self, actor_transform_list, physics=True, name="BatchActorTransformSetter"): + def __init__( + self, actor_transform_list, physics=True, name="BatchActorTransformSetter" + ): """ Init """ @@ -3165,7 +3528,6 @@ def update(self): class TrafficLightStateSetter(AtomicBehavior): - """ This class contains an atomic behavior to set the state of a given traffic light @@ -3214,8 +3576,15 @@ class TrafficLightControllerSetter(AtomicBehavior): """ - def __init__(self, traffic_signal_id, state, duration, delay=None, ref_id=None, - name="TrafficLightControllerSetter"): + def __init__( + self, + traffic_signal_id, + state, + duration, + delay=None, + ref_id=None, + name="TrafficLightControllerSetter", + ): """ Init """ @@ -3233,7 +3602,9 @@ def __init__(self, traffic_signal_id, state, duration, delay=None, ref_id=None, def initialise(self): self._start_time = GameTime.get_time() - self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id(self.actor_id) + self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id( + self.actor_id + ) if self._actor is None: return py_trees.common.Status.FAILURE @@ -3245,10 +3616,10 @@ def initialise(self): else: return py_trees.common.Status.FAILURE self._previous_traffic_light_info[self._actor] = { - 'state': self._actor.get_state(), - 'green_time': self._actor.get_green_time(), - 'red_time': self._actor.get_red_time(), - 'yellow_time': self._actor.get_yellow_time() + "state": self._actor.get_state(), + "green_time": self._actor.get_green_time(), + "red_time": self._actor.get_red_time(), + "yellow_time": self._actor.get_yellow_time(), } self._actor.set_state(self._state) self._actor.set_green_time(self.duration_time) @@ -3271,16 +3642,23 @@ def update(self): def terminate(self, new_status): """Reset all traffic lights back to their previous states""" if self._previous_traffic_light_info: - self._actor.set_state(self._previous_traffic_light_info[self._actor]['state']) - self._actor.set_green_time(self._previous_traffic_light_info[self._actor]['green_time']) - self._actor.set_red_time(self._previous_traffic_light_info[self._actor]['red_time']) - self._actor.set_yellow_time(self._previous_traffic_light_info[self._actor]['yellow_time']) + self._actor.set_state( + self._previous_traffic_light_info[self._actor]["state"] + ) + self._actor.set_green_time( + self._previous_traffic_light_info[self._actor]["green_time"] + ) + self._actor.set_red_time( + self._previous_traffic_light_info[self._actor]["red_time"] + ) + self._actor.set_yellow_time( + self._previous_traffic_light_info[self._actor]["yellow_time"] + ) super(TrafficLightControllerSetter, self).terminate(new_status) class ActorSource(AtomicBehavior): - """ Implementation for a behavior that will indefinitely create actors at a given transform if no other actor exists in a given radius @@ -3296,8 +3674,15 @@ class ActorSource(AtomicBehavior): A parallel termination behavior has to be used. """ - def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name, - actor_limit=7, name="ActorSource"): + def __init__( + self, + actor_type_list, + transform, + threshold, + blackboard_queue_name, + actor_limit=7, + name="ActorSource", + ): """ Setup class members """ @@ -3315,13 +3700,21 @@ def update(self): if self._actor_limit > 0: world_actors = CarlaDataProvider.get_all_actors() spawn_point_blocked = False - if (self._last_blocking_actor and - self._spawn_point.location.distance(self._last_blocking_actor.get_location()) < self._threshold): + if ( + self._last_blocking_actor + and self._spawn_point.location.distance( + self._last_blocking_actor.get_location() + ) + < self._threshold + ): spawn_point_blocked = True if not spawn_point_blocked: for actor in world_actors: - if self._spawn_point.location.distance(actor.get_location()) < self._threshold: + if ( + self._spawn_point.location.distance(actor.get_location()) + < self._threshold + ): spawn_point_blocked = True self._last_blocking_actor = actor break @@ -3329,16 +3722,16 @@ def update(self): if not spawn_point_blocked: try: new_actor = CarlaDataProvider.request_new_actor( - random.choice(self._actor_types), self._spawn_point) + random.choice(self._actor_types), self._spawn_point + ) self._actor_limit -= 1 self._queue.put(new_actor) - except: # pylint: disable=bare-except + except: # pylint: disable=bare-except print("ActorSource unable to spawn actor") return new_status class ActorSink(AtomicBehavior): - """ Implementation for a behavior that will indefinitely destroy actors that wander near a given location within a specified threshold. @@ -3361,7 +3754,9 @@ def __init__(self, sink_location, threshold, name="ActorSink"): def update(self): new_status = py_trees.common.Status.RUNNING - CarlaDataProvider.remove_actors_in_surrounding(self._sink_location, self._threshold) + CarlaDataProvider.remove_actors_in_surrounding( + self._sink_location, self._threshold + ) return new_status @@ -3380,17 +3775,30 @@ class ActorFlow(AtomicBehavior): - initial_actors: Populates all the flow trajectory at the start """ - def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, - actor_speed=20 / 3.6, initial_actors=False, initial_junction=False, name="ActorFlow"): + def __init__( + self, + source_wp, + sink_wp, + spawn_dist_interval, + sink_dist=2, + actor_speed=20 / 3.6, + initial_actors=False, + initial_junction=False, + name="ActorFlow", + ): """ Setup class members """ super().__init__(name) self._rng = CarlaDataProvider.get_random_seed() self._world = CarlaDataProvider.get_world() - self._tm = CarlaDataProvider.get_client().get_trafficmanager(CarlaDataProvider.get_traffic_manager_port()) + self._tm = CarlaDataProvider.get_client().get_trafficmanager( + CarlaDataProvider.get_traffic_manager_port() + ) - self._collision_bp = self._world.get_blueprint_library().find('sensor.other.collision') + self._collision_bp = self._world.get_blueprint_library().find( + "sensor.other.collision" + ) self._is_constant_velocity_active = True self._source_wp = source_wp @@ -3409,7 +3817,11 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._max_spawn_dist = spawn_dist_interval[1] self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - self._attribute_filter = {'base_type': 'car', 'has_lights': True, 'special_type': ''} + self._attribute_filter = { + "base_type": "car", + "has_lights": True, + "special_type": "", + } self._actor_list = [] self._collision_sensor_list = [] @@ -3429,12 +3841,17 @@ def initialise(self): continue self._spawn_actor(wp.transform) ref_loc = wp.transform.location - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._spawn_dist = self._rng.uniform( + self._min_spawn_dist, self._max_spawn_dist + ) def _spawn_actor(self, transform): actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', transform, rolename='scenario', - attribute_filter=self._attribute_filter, tick=False + "vehicle.*", + transform, + rolename="scenario", + attribute_filter=self._attribute_filter, + tick=False, ) if actor is None: return py_trees.common.Status.RUNNING @@ -3450,9 +3867,13 @@ def _spawn_actor(self, transform): sensor = None if self._is_constant_velocity_active: self._tm.ignore_vehicles_percentage(actor, 100) - actor.enable_constant_velocity(carla.Vector3D(self._speed, 0, 0)) # For when physics are active + actor.enable_constant_velocity( + carla.Vector3D(self._speed, 0, 0) + ) # For when physics are active - sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) + sensor = self._world.spawn_actor( + self._collision_bp, carla.Transform(), attach_to=actor + ) sensor.listen(lambda _: self.stop_constant_velocity()) self._tm.ignore_lights_percentage(actor, 100) @@ -3463,7 +3884,9 @@ def _spawn_actor(self, transform): def update(self): """Controls the created actors and creaes / removes other when needed""" # Control the vehicles, removing them when needed - for actor, sensor in zip(list(self._actor_list), list(self._collision_sensor_list)): + for actor, sensor in zip( + list(self._actor_list), list(self._collision_sensor_list) + ): location = CarlaDataProvider.get_location(actor) if not location: continue @@ -3481,7 +3904,9 @@ def update(self): distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_list[-1]) - distance = self._source_location.distance(actor_location) if actor_location else 0 + distance = ( + self._source_location.distance(actor_location) if actor_location else 0 + ) if distance > self._spawn_dist: self._spawn_actor(self._source_transform) @@ -3518,8 +3943,8 @@ def terminate(self, new_status): # Patched by removing its movement actor.disable_constant_velocity() actor.set_autopilot(False, CarlaDataProvider.get_traffic_manager_port()) - actor.set_target_velocity(carla.Vector3D(0,0,0)) - actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0)) try: actor.destroy() except RuntimeError: @@ -3540,15 +3965,25 @@ class OppositeActorFlow(AtomicBehavior): - offset: offset from the center lane of the actors """ - def __init__(self, reference_wp, reference_actor, spawn_dist_interval, - time_distance=1.5, base_distance=30, sink_dist=2, name="OppositeActorFlow"): + def __init__( + self, + reference_wp, + reference_actor, + spawn_dist_interval, + time_distance=1.5, + base_distance=30, + sink_dist=2, + name="OppositeActorFlow", + ): """ Setup class members """ super().__init__(name) self._rng = CarlaDataProvider.get_random_seed() self._world = CarlaDataProvider.get_world() - self._tm = CarlaDataProvider.get_client().get_trafficmanager(CarlaDataProvider.get_traffic_manager_port()) + self._tm = CarlaDataProvider.get_client().get_trafficmanager( + CarlaDataProvider.get_traffic_manager_port() + ) self._reference_wp = reference_wp self._reference_actor = reference_actor @@ -3560,10 +3995,14 @@ def __init__(self, reference_wp, reference_actor, spawn_dist_interval, self._sink_dist = sink_dist - self._attribute_filter = {'base_type': 'car', 'has_lights': True, 'special_type': ''} + self._attribute_filter = { + "base_type": "car", + "has_lights": True, + "special_type": "", + } # Opposite direction needs earlier vehicle detection - self._opt_dict = {'base_vehicle_threshold': 10, 'detection_speed_ratio': 1.6} + self._opt_dict = {"base_vehicle_threshold": 10, "detection_speed_ratio": 1.6} self._actor_list = [] self._grp = CarlaDataProvider.get_global_route_planner() @@ -3599,11 +4038,15 @@ def _move_waypoint_backwards(self, wp, distance): def initialise(self): """Get the actor flow source and sink, depending on the reference actor speed""" - self._speed = self._reference_actor.get_speed_limit() # Km / h + self._speed = self._reference_actor.get_speed_limit() # Km / h self._flow_distance = self._time_distance * self._speed + self._base_distance - self._sink_wp = self._move_waypoint_forward(self._reference_wp, self._flow_distance) - self._source_wp = self._move_waypoint_backwards(self._reference_wp, self._flow_distance) + self._sink_wp = self._move_waypoint_forward( + self._reference_wp, self._flow_distance + ) + self._source_wp = self._move_waypoint_backwards( + self._reference_wp, self._flow_distance + ) self._source_transform = self._source_wp.transform self._source_location = self._source_transform.location @@ -3615,13 +4058,18 @@ def initialise(self): def _spawn_actor(self): actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter=self._attribute_filter, tick=False + "vehicle.*", + self._source_transform, + rolename="scenario", + attribute_filter=self._attribute_filter, + tick=False, ) if actor is None: return py_trees.common.Status.RUNNING - controller = BasicAgent(actor, self._speed, self._opt_dict, self._map, self._grp) + controller = BasicAgent( + actor, self._speed, self._opt_dict, self._map, self._grp + ) controller.set_global_plan(self._route) self._actor_list.append([actor, controller]) @@ -3647,7 +4095,9 @@ def update(self): distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_list[-1][0]) - distance = self._source_location.distance(actor_location) if actor_location else 0 + distance = ( + self._source_location.distance(actor_location) if actor_location else 0 + ) if distance > self._spawn_dist: self._spawn_actor() @@ -3667,8 +4117,8 @@ def terminate(self, new_status): # Patched by removing its movement actor.disable_constant_velocity() actor.set_autopilot(False, CarlaDataProvider.get_traffic_manager_port()) - actor.set_target_velocity(carla.Vector3D(0,0,0)) - actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0)) try: actor.destroy() except RuntimeError: @@ -3689,18 +4139,28 @@ class InvadingActorFlow(AtomicBehavior): - offset: offset from the center lane of the actors """ - def __init__(self, source_wp, sink_wp, reference_actor, spawn_dist, - sink_dist=2, offset=0, name="OppositeActorFlow"): + def __init__( + self, + source_wp, + sink_wp, + reference_actor, + spawn_dist, + sink_dist=2, + offset=0, + name="OppositeActorFlow", + ): """ Setup class members """ super().__init__(name) self._world = CarlaDataProvider.get_world() - self._tm = CarlaDataProvider.get_client().get_trafficmanager(CarlaDataProvider.get_traffic_manager_port()) + self._tm = CarlaDataProvider.get_client().get_trafficmanager( + CarlaDataProvider.get_traffic_manager_port() + ) self._reference_actor = reference_actor - self._source_wp = source_wp + self._source_wp = source_wp self._source_transform = self._source_wp.transform self._source_location = self._source_transform.location @@ -3711,13 +4171,21 @@ def __init__(self, source_wp, sink_wp, reference_actor, spawn_dist, self._sink_dist = sink_dist - self._attribute_filter = {'base_type': 'car', 'has_lights': True, 'special_type': ''} + self._attribute_filter = { + "base_type": "car", + "has_lights": True, + "special_type": "", + } self._actor_list = [] # Opposite direction needs earlier vehicle detection - self._opt_dict = {'base_vehicle_threshold': 10, 'detection_speed_ratio': 2, 'distance_ratio': 0.2} - self._opt_dict['offset'] = offset + self._opt_dict = { + "base_vehicle_threshold": 10, + "detection_speed_ratio": 2, + "distance_ratio": 0.2, + } + self._opt_dict["offset"] = offset self._grp = CarlaDataProvider.get_global_route_planner() self._map = CarlaDataProvider.get_map() @@ -3732,13 +4200,18 @@ def initialise(self): def _spawn_actor(self): actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter=self._attribute_filter, tick=False + "vehicle.*", + self._source_transform, + rolename="scenario", + attribute_filter=self._attribute_filter, + tick=False, ) if actor is None: return py_trees.common.Status.RUNNING - controller = BasicAgent(actor, self._speed, self._opt_dict, self._map, self._grp) + controller = BasicAgent( + actor, self._speed, self._opt_dict, self._map, self._grp + ) controller.set_global_plan(self._route) self._actor_list.append([actor, controller]) @@ -3762,7 +4235,9 @@ def update(self): distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_list[-1][0]) - distance = self._source_location.distance(actor_location) if actor_location else 0 + distance = ( + self._source_location.distance(actor_location) if actor_location else 0 + ) if distance > self._spawn_dist: self._spawn_actor() @@ -3782,8 +4257,8 @@ def terminate(self, new_status): # Patched by removing its movement actor.disable_constant_velocity() actor.set_autopilot(False, CarlaDataProvider.get_traffic_manager_port()) - actor.set_target_velocity(carla.Vector3D(0,0,0)) - actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0)) try: actor.destroy() except RuntimeError: @@ -3804,8 +4279,15 @@ class BicycleFlow(AtomicBehavior): - initial_actors (bool): Boolean to initialy populate all the flow with bicycles """ - def __init__(self, plan, spawn_dist_interval, sink_dist=2, - actor_speed=20 / 3.6, initial_actors=False, name="BicycleFlow"): + def __init__( + self, + plan, + spawn_dist_interval, + sink_dist=2, + actor_speed=20 / 3.6, + initial_actors=False, + name="BicycleFlow", + ): """ Setup class members """ @@ -3843,7 +4325,9 @@ def initialise(self): continue self._spawn_actor(wp.transform) ref_loc = wp.transform.location - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._spawn_dist = self._rng.uniform( + self._min_spawn_dist, self._max_spawn_dist + ) def _spawn_actor(self, transform): """Spawn the actor""" @@ -3864,19 +4348,29 @@ def _spawn_actor(self, transform): return actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', transform, rolename='scenario no lights', - attribute_filter={'base_type': 'bicycle'}, tick=False + "vehicle.*", + transform, + rolename="scenario no lights", + attribute_filter={"base_type": "bicycle"}, + tick=False, ) if actor is None: return - controller = BasicAgent(actor, 3.6 * self._speed, opt_dict=self._opt_dict, - map_inst=CarlaDataProvider.get_map(), grp_inst=CarlaDataProvider.get_global_route_planner()) + controller = BasicAgent( + actor, + 3.6 * self._speed, + opt_dict=self._opt_dict, + map_inst=CarlaDataProvider.get_map(), + grp_inst=CarlaDataProvider.get_global_route_planner(), + ) controller.set_global_plan(plan) initial_vec = plan[0][0].transform.get_forward_vector() actor.set_target_velocity(self._speed * initial_vec) - actor.apply_control(carla.VehicleControl(throttle=1, gear=1, manual_gear_shift=True)) + actor.apply_control( + carla.VehicleControl(throttle=1, gear=1, manual_gear_shift=True) + ) self._actor_data.append([actor, controller]) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) @@ -3925,8 +4419,8 @@ def terminate(self, new_status): # Patched by removing its movement actor.disable_constant_velocity() actor.set_autopilot(False, CarlaDataProvider.get_traffic_manager_port()) - actor.set_target_velocity(carla.Vector3D(0,0,0)) - actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0)) try: actor.destroy() except RuntimeError: @@ -3934,7 +4428,6 @@ def terminate(self, new_status): class OpenVehicleDoor(AtomicBehavior): - """ Implementation for a behavior that will open the door of a vehicle, then close it after a while. @@ -3965,7 +4458,9 @@ def update(self): Keep running until termination condition is satisfied """ new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -3996,10 +4491,10 @@ def initialise(self): for tl in self._traffic_lights_dict: elapsed_time = tl.get_elapsed_time() self._previous_traffic_light_info[tl] = { - 'state': tl.get_state(), - 'green_time': tl.get_green_time(), - 'red_time': tl.get_red_time(), - 'yellow_time': tl.get_yellow_time() + "state": tl.get_state(), + "green_time": tl.get_green_time(), + "red_time": tl.get_red_time(), + "yellow_time": tl.get_yellow_time(), } tl.set_state(self._traffic_lights_dict[tl]) tl.set_green_time(self._duration + elapsed_time) @@ -4017,14 +4512,13 @@ def terminate(self, new_status): """Reset all traffic lights back to their previous states""" if self._previous_traffic_light_info: for tl in self._traffic_lights_dict: - tl.set_state(self._previous_traffic_light_info[tl]['state']) - tl.set_green_time(self._previous_traffic_light_info[tl]['green_time']) - tl.set_red_time(self._previous_traffic_light_info[tl]['red_time']) - tl.set_yellow_time(self._previous_traffic_light_info[tl]['yellow_time']) + tl.set_state(self._previous_traffic_light_info[tl]["state"]) + tl.set_green_time(self._previous_traffic_light_info[tl]["green_time"]) + tl.set_red_time(self._previous_traffic_light_info[tl]["red_time"]) + tl.set_yellow_time(self._previous_traffic_light_info[tl]["yellow_time"]) class StartRecorder(AtomicBehavior): - """ Atomic that starts the CARLA recorder. Only one can be active at a time, and if this isn't the case, the recorder will @@ -4052,7 +4546,6 @@ def update(self): class StopRecorder(AtomicBehavior): - """ Atomic that stops the CARLA recorder. @@ -4073,7 +4566,6 @@ def update(self): class TrafficLightManipulator(AtomicBehavior): - """ Atomic behavior that manipulates traffic lights around the ego_vehicle to trigger scenarios 7 to 10. This is done by setting 2 of the traffic light at the intersection to green (with some complex precomputation @@ -4095,36 +4587,88 @@ class TrafficLightManipulator(AtomicBehavior): RESET_TIME = 6 # Time waited before resetting all the junction (seconds) # Experimental values - TRIGGER_DISTANCE = 10 # Distance that makes all vehicles in the lane enter the junction (meters) + TRIGGER_DISTANCE = ( + 10 # Distance that makes all vehicles in the lane enter the junction (meters) + ) DIST_TO_WAITING_TIME = 0.04 # Used to wait longer at larger intersections (s/m) - INT_CONF_OPP1 = {'ego': RED, 'ref': RED, 'left': RED, 'right': RED, 'opposite': GREEN} - INT_CONF_OPP2 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': RED, 'opposite': GREEN} - INT_CONF_LFT1 = {'ego': RED, 'ref': RED, 'left': GREEN, 'right': RED, 'opposite': RED} - INT_CONF_LFT2 = {'ego': GREEN, 'ref': GREEN, 'left': GREEN, 'right': RED, 'opposite': RED} - INT_CONF_RGT1 = {'ego': RED, 'ref': RED, 'left': RED, 'right': GREEN, 'opposite': RED} - INT_CONF_RGT2 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': GREEN, 'opposite': RED} + INT_CONF_OPP1 = { + "ego": RED, + "ref": RED, + "left": RED, + "right": RED, + "opposite": GREEN, + } + INT_CONF_OPP2 = { + "ego": GREEN, + "ref": GREEN, + "left": RED, + "right": RED, + "opposite": GREEN, + } + INT_CONF_LFT1 = { + "ego": RED, + "ref": RED, + "left": GREEN, + "right": RED, + "opposite": RED, + } + INT_CONF_LFT2 = { + "ego": GREEN, + "ref": GREEN, + "left": GREEN, + "right": RED, + "opposite": RED, + } + INT_CONF_RGT1 = { + "ego": RED, + "ref": RED, + "left": RED, + "right": GREEN, + "opposite": RED, + } + INT_CONF_RGT2 = { + "ego": GREEN, + "ref": GREEN, + "left": RED, + "right": GREEN, + "opposite": RED, + } - INT_CONF_REF1 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': RED, 'opposite': RED} - INT_CONF_REF2 = {'ego': YELLOW, 'ref': YELLOW, 'left': RED, 'right': RED, 'opposite': RED} + INT_CONF_REF1 = { + "ego": GREEN, + "ref": GREEN, + "left": RED, + "right": RED, + "opposite": RED, + } + INT_CONF_REF2 = { + "ego": YELLOW, + "ref": YELLOW, + "left": RED, + "right": RED, + "opposite": RED, + } # Depending on the scenario, IN ORDER OF IMPORTANCE, the traffic light changed # The list has to contain only items of the INT_CONF SUBTYPE_CONFIG_TRANSLATION = { - 'S7left': ['left', 'opposite', 'right'], - 'S7right': ['left', 'opposite'], - 'S7opposite': ['right', 'left', 'opposite'], - 'S8left': ['opposite'], - 'S9right': ['left', 'opposite'] + "S7left": ["left", "opposite", "right"], + "S7right": ["left", "opposite"], + "S7opposite": ["right", "left", "opposite"], + "S8left": ["opposite"], + "S9right": ["left", "opposite"], } CONFIG_TLM_TRANSLATION = { - 'left': [INT_CONF_LFT1, INT_CONF_LFT2], - 'right': [INT_CONF_RGT1, INT_CONF_RGT2], - 'opposite': [INT_CONF_OPP1, INT_CONF_OPP2] + "left": [INT_CONF_LFT1, INT_CONF_LFT2], + "right": [INT_CONF_RGT1, INT_CONF_RGT2], + "opposite": [INT_CONF_OPP1, INT_CONF_OPP2], } - def __init__(self, ego_vehicle, subtype, debug=False, name="TrafficLightManipulator"): + def __init__( + self, ego_vehicle, subtype, debug=False, name="TrafficLightManipulator" + ): super(TrafficLightManipulator, self).__init__(name) self.ego_vehicle = ego_vehicle self.subtype = subtype @@ -4145,23 +4689,27 @@ def __init__(self, ego_vehicle, subtype, debug=False, name="TrafficLightManipula self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): - new_status = py_trees.common.Status.RUNNING # 1) Set up the parameters if self.current_step == 1: - # Traffic light affecting the ego vehicle - self.traffic_light = CarlaDataProvider.get_next_traffic_light(self.ego_vehicle, use_cached_location=False) + self.traffic_light = CarlaDataProvider.get_next_traffic_light( + self.ego_vehicle, use_cached_location=False + ) if not self.traffic_light: # nothing else to do in this iteration... return new_status # "Topology" of the intersection - self.annotations = CarlaDataProvider.annotate_trafficlight_in_group(self.traffic_light) + self.annotations = CarlaDataProvider.annotate_trafficlight_in_group( + self.traffic_light + ) # Which traffic light will be modified (apart from the ego lane) - self.configuration = self.get_traffic_light_configuration(self.subtype, self.annotations) + self.configuration = self.get_traffic_light_configuration( + self.subtype, self.annotations + ) if self.configuration is None: self.current_step = 0 # End the behavior return new_status @@ -4175,7 +4723,6 @@ def update(self): # 2) Modify the ego lane to yellow when closeby elif self.current_step == 2: - ego_location = CarlaDataProvider.get_location(self.ego_vehicle) if self.junction_location is None: @@ -4203,21 +4750,25 @@ def update(self): # 3) Modify the ego lane to red and the chosen one to green after several seconds elif self.current_step == 3: - if self.passed_enough_time(self.YELLOW_TIME): - _ = self.set_intersection_state(self.CONFIG_TLM_TRANSLATION[self.configuration][0]) + _ = self.set_intersection_state( + self.CONFIG_TLM_TRANSLATION[self.configuration][0] + ) self.current_step += 1 # 4) Wait a bit to let vehicles enter the intersection, then set the ego lane to green elif self.current_step == 4: - # Get the time in red, dependent on the intersection dimensions if self.waiting_time is None: - self.waiting_time = self.get_waiting_time(self.annotations, self.configuration) + self.waiting_time = self.get_waiting_time( + self.annotations, self.configuration + ) if self.passed_enough_time(self.waiting_time): - _ = self.set_intersection_state(self.CONFIG_TLM_TRANSLATION[self.configuration][1]) + _ = self.set_intersection_state( + self.CONFIG_TLM_TRANSLATION[self.configuration][1] + ) self.current_step += 1 @@ -4266,7 +4817,7 @@ def passed_enough_time(self, time_limit): self.prev_time = GameTime.get_time() timestamp = GameTime.get_time() - self.seconds_waited += (timestamp - self.prev_time) + self.seconds_waited += timestamp - self.prev_time self.prev_time = timestamp if self.debug: @@ -4284,10 +4835,8 @@ def set_intersection_state(self, choice): Changes the intersection to the desired state """ prev_state = CarlaDataProvider.update_light_states( - self.traffic_light, - self.annotations, - choice, - freeze=True) + self.traffic_light, self.annotations, choice, freeze=True + ) return prev_state @@ -4360,7 +4909,6 @@ def variable_cleanup(self): class ScenarioTriggerer(AtomicBehavior): - """ Handles the triggering of the scenarios that are part of a route. @@ -4370,7 +4918,15 @@ class ScenarioTriggerer(AtomicBehavior): WINDOWS_SIZE = 5 - def __init__(self, actor, route, blackboard_list, distance, debug=False, name="ScenarioTriggerer"): + def __init__( + self, + actor, + route, + blackboard_list, + distance, + debug=False, + name="ScenarioTriggerer", + ): """ Setup class members """ @@ -4403,9 +4959,11 @@ def update(self): return new_status lower_bound = self._current_index - upper_bound = min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length) + upper_bound = min( + self._current_index + self.WINDOWS_SIZE + 1, self._route_length + ) - shortest_distance = float('inf') + shortest_distance = float("inf") closest_index = -1 for index in range(lower_bound, upper_bound): @@ -4417,7 +4975,7 @@ def update(self): closest_index = index shortest_distance = dist_to_route - if closest_index == -1 or shortest_distance == float('inf'): + if closest_index == -1 or shortest_distance == float("inf"): return new_status # Update the ego position at the route @@ -4428,7 +4986,6 @@ def update(self): # Check which scenarios can be triggered blackboard = py_trees.blackboard.Blackboard() for black_var_name, scen_location in self._blackboard_list: - # Close enough scen_distance = route_location.distance(scen_location) condition1 = bool(scen_distance < self._distance) @@ -4449,14 +5006,14 @@ def update(self): scen_location + carla.Location(z=4), size=0.5, life_time=0.5, - color=carla.Color(255, 255, 0) + color=carla.Color(255, 255, 0), ) self._world.debug.draw_string( scen_location + carla.Location(z=5), str(black_var_name), False, color=carla.Color(0, 0, 0), - life_time=1000 + life_time=1000, ) return new_status @@ -4475,8 +5032,17 @@ class KeepLongitudinalGap(AtomicBehavior): The behavior terminates after overwritten by other events / when target distance is reached(if continues). """ - def __init__(self, actor, reference_actor, gap, gap_type="distance", max_speed=None, continues=False, - freespace=False, name="AutoKeepDistance"): + def __init__( + self, + actor, + reference_actor, + gap, + gap_type="distance", + max_speed=None, + continues=False, + freespace=False, + name="AutoKeepDistance", + ): """ Setup parameters """ @@ -4491,7 +5057,10 @@ def __init__(self, actor, reference_actor, gap, gap_type="distance", max_speed=N max_speed_limit = 100 self.max_speed = max_speed_limit if max_speed is None else float(max_speed) if freespace and self._gap_type == "distance": - self._gap += self._reference_actor.bounding_box.extent.x + self._actor.bounding_box.extent.x + self._gap += ( + self._reference_actor.bounding_box.extent.x + + self._actor.bounding_box.extent.x + ) self._start_time = None @@ -4508,7 +5077,9 @@ def initialise(self): raise RuntimeError("Actor not found in ActorsWithController BlackBoard") self._start_time = GameTime.get_time() - actor_dict[self._actor.id].update_target_speed(self.max_speed, start_time=self._start_time) + actor_dict[self._actor.id].update_target_speed( + self.max_speed, start_time=self._start_time + ) self._global_rp = CarlaDataProvider.get_global_route_planner() @@ -4527,7 +5098,10 @@ def update(self): if not actor_dict or self._actor.id not in actor_dict: return py_trees.common.Status.FAILURE - if actor_dict[self._actor.id].get_last_longitudinal_command() != self._start_time: + if ( + actor_dict[self._actor.id].get_last_longitudinal_command() + != self._start_time + ): return py_trees.common.Status.SUCCESS new_status = py_trees.common.Status.RUNNING @@ -4535,20 +5109,26 @@ def update(self): actor_velocity = CarlaDataProvider.get_velocity(self._actor) reference_velocity = CarlaDataProvider.get_velocity(self._reference_actor) - gap = sr_tools.scenario_helper.get_distance_between_actors(self._actor, self._reference_actor, - distance_type="longitudinal", - freespace=self._freespace, - global_planner=self._global_rp) + gap = sr_tools.scenario_helper.get_distance_between_actors( + self._actor, + self._reference_actor, + distance_type="longitudinal", + freespace=self._freespace, + global_planner=self._global_rp, + ) actor_transform = CarlaDataProvider.get_transform(self._actor) ref_actor_transform = CarlaDataProvider.get_transform(self._reference_actor) - if is_within_distance(ref_actor_transform, actor_transform, float('inf'), [0, 90]) and \ - operator.le(gap, self._gap): + if is_within_distance( + ref_actor_transform, actor_transform, float("inf"), [0, 90] + ) and operator.le(gap, self._gap): try: - factor = abs(actor_velocity - reference_velocity)/actor_velocity + factor = abs(actor_velocity - reference_velocity) / actor_velocity if actor_velocity > reference_velocity: - actor_velocity = actor_velocity - (factor*actor_velocity) - elif actor_velocity < reference_velocity and operator.gt(gap, self._gap): - actor_velocity = actor_velocity + (factor*actor_velocity) + actor_velocity = actor_velocity - (factor * actor_velocity) + elif actor_velocity < reference_velocity and operator.gt( + gap, self._gap + ): + actor_velocity = actor_velocity + (factor * actor_velocity) except ZeroDivisionError: pass actor_dict[self._actor.id].update_target_speed(actor_velocity) @@ -4559,7 +5139,9 @@ def update(self): else: actor_dict[self._actor.id].update_target_speed(self.max_speed) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -4588,7 +5170,8 @@ def update(self): new_status = py_trees.common.Status.RUNNING try: new_actor = CarlaDataProvider.request_new_actor( - self._actor_type, self._spawn_point, color=self._color) + self._actor_type, self._spawn_point, color=self._color + ) if new_actor: new_status = py_trees.common.Status.SUCCESS except: # pylint: disable=bare-except @@ -4596,7 +5179,6 @@ def update(self): class SwitchWrongDirectionTest(AtomicBehavior): - """ Atomic that switch the OutsideRouteLanesTest criterion. @@ -4613,12 +5195,13 @@ def __init__(self, active, name="SwitchWrongDirectionTest"): super().__init__(name) def update(self): - py_trees.blackboard.Blackboard().set("AC_SwitchWrongDirectionTest", self._active, overwrite=True) + py_trees.blackboard.Blackboard().set( + "AC_SwitchWrongDirectionTest", self._active, overwrite=True + ) return py_trees.common.Status.SUCCESS class SwitchMinSpeedCriteria(AtomicBehavior): - def __init__(self, active, name="ChangeMinSpeed"): """ Setup parameters @@ -4632,7 +5215,9 @@ def update(self): keeps track of gap and update the controller accordingly """ new_status = py_trees.common.Status.SUCCESS - py_trees.blackboard.Blackboard().set("SwitchMinSpeedCriteria", self._active, overwrite=True) + py_trees.blackboard.Blackboard().set( + "SwitchMinSpeedCriteria", self._active, overwrite=True + ) return new_status @@ -4650,11 +5235,20 @@ class WalkerFlow(AtomicBehavior): - sink_locations_prob (list(float)): The probability of each sink_location - spawn_dist_interval (list(float)): Distance between spawned actors - random_seed : Optional. The seed of numpy's random - - sink_distance: Actors closer to the sink than this distance will be deleted. + - sink_distance: Actors closer to the sink than this distance will be deleted. Probably due to the navigation module rerouting the walkers, a sink distance of 2 is reasonable. """ - def __init__(self, source_location, sink_locations, sink_locations_prob, spawn_dist_interval, random_seed=None, sink_dist=2, - name="WalkerFlow"): + + def __init__( + self, + source_location, + sink_locations, + sink_locations_prob, + spawn_dist_interval, + random_seed=None, + sink_dist=2, + name="WalkerFlow", + ): """ Setup class members """ @@ -4666,7 +5260,9 @@ def __init__(self, source_location, sink_locations, sink_locations_prob, spawn_d self._rng = CarlaDataProvider.get_random_seed() self._world = CarlaDataProvider.get_world() - self._controller_bp = self._world.get_blueprint_library().find('controller.ai.walker') + self._controller_bp = self._world.get_blueprint_library().find( + "controller.ai.walker" + ) self._source_location = source_location @@ -4678,7 +5274,7 @@ def __init__(self, source_location, sink_locations, sink_locations_prob, spawn_d self._max_spawn_dist = spawn_dist_interval[1] self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - self._batch_size_list = [1,2,3] + self._batch_size_list = [1, 2, 3] self._walkers = [] @@ -4706,19 +5302,25 @@ def update(self): spawn_tran = carla.Transform(self._source_location) spawn_tran.location.y -= i walker = CarlaDataProvider.request_new_actor( - 'walker.*', spawn_tran, rolename='scenario' + "walker.*", spawn_tran, rolename="scenario" ) if walker is None: continue # Use ai.walker to controll walkers - controller = self._world.try_spawn_actor(self._controller_bp, carla.Transform(), walker) - sink_location = self._rng.choice(a = self._sink_locations, p = self._sink_locations_prob) + controller = self._world.try_spawn_actor( + self._controller_bp, carla.Transform(), walker + ) + sink_location = self._rng.choice( + a=self._sink_locations, p=self._sink_locations_prob + ) controller.start() controller.go_to_location(sink_location) # Add to walkers list self._walkers.append((walker, controller, sink_location)) - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._spawn_dist = self._rng.uniform( + self._min_spawn_dist, self._max_spawn_dist + ) return py_trees.common.Status.RUNNING @@ -4737,6 +5339,7 @@ def terminate(self, new_status): except RuntimeError: pass # Actor was already destroyed + class AIWalkerBehavior(AtomicBehavior): """ Behavior that creates a walker controlled by AI Walker controller. @@ -4748,15 +5351,16 @@ class AIWalkerBehavior(AtomicBehavior): - sink_location (carla.Location): Location at which the actor will be deleted """ - def __init__(self, source_location, sink_location, - name="AIWalkerBehavior"): + def __init__(self, source_location, sink_location, name="AIWalkerBehavior"): """ Setup class members """ super(AIWalkerBehavior, self).__init__(name) self._world = CarlaDataProvider.get_world() - self._controller_bp = self._world.get_blueprint_library().find('controller.ai.walker') + self._controller_bp = self._world.get_blueprint_library().find( + "controller.ai.walker" + ) self._source_location = source_location @@ -4776,13 +5380,14 @@ def initialise(self): """ spawn_tran = carla.Transform(self._source_location) self._walker = CarlaDataProvider.request_new_actor( - 'walker.*', spawn_tran, rolename='scenario' + "walker.*", spawn_tran, rolename="scenario" ) if self._walker is None: raise RuntimeError("Couldn't spawn the walker") # Use ai.walker to controll the walker self._controller = self._world.try_spawn_actor( - self._controller_bp, carla.Transform(), self._walker) + self._controller_bp, carla.Transform(), self._walker + ) self._controller.start() self._controller.go_to_location(self._sink_location) @@ -4818,7 +5423,6 @@ def terminate(self, new_status): class ScenarioTimeout(AtomicBehavior): - """ This class is an idle behavior that waits for a set amount of time before stoping. @@ -4849,7 +5453,9 @@ def initialise(self): Set start time """ self._start_time = GameTime.get_time() - py_trees.blackboard.Blackboard().set("AC_SwitchActorBlockedTest", False, overwrite=True) + py_trees.blackboard.Blackboard().set( + "AC_SwitchActorBlockedTest", False, overwrite=True + ) super().initialise() def update(self): @@ -4868,16 +5474,25 @@ def terminate(self, new_status): """ Modifies the blackboard to tell the `ScenarioTimeoutTest` if the timeout was triggered """ - if not self._terminated: # py_trees calls the terminate several times for some reason. - py_trees.blackboard.Blackboard().set(f"ScenarioTimeout_{self._scenario_name}", self._scenario_timeout, overwrite=True) - py_trees.blackboard.Blackboard().set("AC_SwitchActorBlockedTest", True, overwrite=True) + if ( + not self._terminated + ): # py_trees calls the terminate several times for some reason. + py_trees.blackboard.Blackboard().set( + f"ScenarioTimeout_{self._scenario_name}", + self._scenario_timeout, + overwrite=True, + ) + py_trees.blackboard.Blackboard().set( + "AC_SwitchActorBlockedTest", True, overwrite=True + ) self._terminated = True super().terminate(new_status) class MovePedestrianWithEgo(AtomicBehavior): - - def __init__(self, reference_actor, actor, distance, displacement=0, name="TrackActor"): + def __init__( + self, reference_actor, actor, distance, displacement=0, name="TrackActor" + ): """ Setup actor """ @@ -4912,6 +5527,8 @@ def update(self): if GameTime.get_time() - self._start_time > self._teleport_time: added_location = carla.Location(x=self._displacement, z=-self._distance) - self._actor.set_location(self._reference_actor.get_location() + added_location) + self._actor.set_location( + self._reference_actor.get_location() + added_location + ) self._start_time = GameTime.get_time() return new_status diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 45dc961..952084d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -21,7 +21,6 @@ import shapely.geometry import carla -from agents.tools.misc import get_speed from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.timer import GameTime @@ -29,7 +28,6 @@ class Criterion(py_trees.behaviour.Behaviour): - """ Base class for all criteria used to evaluate a scenario for success/failure @@ -46,11 +44,7 @@ class Criterion(py_trees.behaviour.Behaviour): - units: units of the 'actual_value'. This is a string and is used by the result writter """ - def __init__(self, - name, - actor, - optional=False, - terminate_on_failure=False): + def __init__(self, name, actor, optional=False, terminate_on_failure=False): super(Criterion, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) @@ -58,7 +52,9 @@ def __init__(self, self.actor = actor self.optional = optional self._terminate_on_failure = terminate_on_failure - self.test_status = "INIT" # Either "INIT", "RUNNING", "SUCCESS", "ACCEPTABLE" or "FAILURE" + self.test_status = ( + "INIT" # Either "INIT", "RUNNING", "SUCCESS", "ACCEPTABLE" or "FAILURE" + ) # Attributes to compare the current state (actual_value), with the expected ones self.success_value = 0 @@ -78,14 +74,16 @@ def terminate(self, new_status): """ Terminate the criterion. Can be extended by the user-derived class """ - if self.test_status in ('RUNNING', 'INIT'): + if self.test_status in ("RUNNING", "INIT"): self.test_status = "SUCCESS" - self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.terminate()[%s->%s]" + % (self.__class__.__name__, self.status, new_status) + ) class MaxVelocityTest(Criterion): - """ This class contains an atomic test for maximum velocity. @@ -95,7 +93,9 @@ class MaxVelocityTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, actor, max_velocity, optional=False, name="CheckMaximumVelocity"): + def __init__( + self, actor, max_velocity, optional=False, name="CheckMaximumVelocity" + ): """ Setup actor and maximum allowed velovity """ @@ -123,7 +123,9 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -141,7 +143,14 @@ class DrivenDistanceTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, actor, distance, acceptable_distance=None, optional=False, name="CheckDrivenDistance"): + def __init__( + self, + actor, + distance, + acceptable_distance=None, + optional=False, + name="CheckDrivenDistance", + ): """ Setup actor """ @@ -177,8 +186,10 @@ def update(self): if self.actual_value > self.success_value: self.test_status = "SUCCESS" - elif (self.acceptable_value is not None and - self.actual_value > self.acceptable_value): + elif ( + self.acceptable_value is not None + and self.actual_value > self.acceptable_value + ): self.test_status = "ACCEPTABLE" else: self.test_status = "RUNNING" @@ -186,7 +197,9 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -201,7 +214,6 @@ def terminate(self, new_status): class AverageVelocityTest(Criterion): - """ This class contains an atomic test for average velocity. @@ -214,8 +226,14 @@ class AverageVelocityTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, actor, velocity, acceptable_velocity=None, optional=False, - name="CheckAverageVelocity"): + def __init__( + self, + actor, + velocity, + acceptable_velocity=None, + optional=False, + name="CheckAverageVelocity", + ): """ Setup actor and average velovity expected """ @@ -256,8 +274,10 @@ def update(self): if self.actual_value > self.success_value: self.test_status = "SUCCESS" - elif (self.acceptable_value is not None and - self.actual_value > self.acceptable_value): + elif ( + self.acceptable_value is not None + and self.actual_value > self.acceptable_value + ): self.test_status = "ACCEPTABLE" else: self.test_status = "RUNNING" @@ -265,7 +285,9 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -279,7 +301,6 @@ def terminate(self, new_status): class CollisionTest(Criterion): - """ This class contains an atomic test for collisions. @@ -293,11 +314,22 @@ class CollisionTest(Criterion): """ COLLISION_RADIUS = 5 # Two collisions that happen within this distance count as one - MAX_ID_TIME = 5 # Two collisions with the same id that happen within this time count as one - EPSILON = 0.1 # Collisions at lower this speed won't be counted as the actor's fault - - def __init__(self, actor, other_actor=None, other_actor_type=None, - optional=False, terminate_on_failure=False, name="CollisionTest"): + MAX_ID_TIME = ( + 5 # Two collisions with the same id that happen within this time count as one + ) + EPSILON = ( + 0.1 # Collisions at lower this speed won't be counted as the actor's fault + ) + + def __init__( + self, + actor, + other_actor=None, + other_actor_type=None, + optional=False, + terminate_on_failure=False, + name="CollisionTest", + ): """ Construction with sensor setup """ @@ -316,8 +348,10 @@ def initialise(self): """ Creates the sensor and callback""" world = CarlaDataProvider.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.collision') - self._collision_sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self.actor) + blueprint = world.get_blueprint_library().find("sensor.other.collision") + self._collision_sensor = world.spawn_actor( + blueprint, carla.Transform(), attach_to=self.actor + ) self._collision_sensor.listen(lambda event: self._count_collisions(event)) super(CollisionTest, self).initialise() @@ -342,7 +376,9 @@ def update(self): if elapsed_time > self.MAX_ID_TIME: self._collision_id = None - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -356,7 +392,7 @@ def terminate(self, new_status): self._collision_sensor = None super(CollisionTest, self).terminate(new_status) - def _count_collisions(self, event): # pylint: disable=too-many-return-statements + def _count_collisions(self, event): # pylint: disable=too-many-return-statements """Update collision count""" actor_location = CarlaDataProvider.get_location(self.actor) @@ -366,10 +402,13 @@ def _count_collisions(self, event): # pylint: disable=too-many-return-statem if self._other_actor_type: if self._other_actor_type == "miscellaneous": # Special OpenScenario case - if "traffic" not in event.other_actor.type_id and "static" not in event.other_actor.type_id: + if ( + "traffic" not in event.other_actor.type_id + and "static" not in event.other_actor.type_id + ): return elif self._other_actor_type not in event.other_actor.type_id: - return + return # To avoid multiple counts of the same collision, filter some of them. if self._collision_id == event.other_actor.id: @@ -389,33 +428,40 @@ def _count_collisions(self, event): # pylint: disable=too-many-return-statem self._collision_time = GameTime.get_time() self._collision_location = actor_location - if event.other_actor.id != 0: # Number 0: static objects -> ignore it + if event.other_actor.id != 0: # Number 0: static objects -> ignore it self._collision_id = event.other_actor.id - if ('static' in event.other_actor.type_id or 'traffic' in event.other_actor.type_id) \ - and 'sidewalk' not in event.other_actor.type_id: + if ( + "static" in event.other_actor.type_id + or "traffic" in event.other_actor.type_id + ) and "sidewalk" not in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_STATIC - elif 'vehicle' in event.other_actor.type_id: + elif "vehicle" in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_VEHICLE - elif 'walker' in event.other_actor.type_id: + elif "walker" in event.other_actor.type_id: actor_type = TrafficEventType.COLLISION_PEDESTRIAN else: return - collision_event = TrafficEvent(event_type=actor_type, frame=GameTime.get_frame()) - collision_event.set_dict({'other_actor': event.other_actor, 'location': actor_location}) + collision_event = TrafficEvent( + event_type=actor_type, frame=GameTime.get_frame() + ) + collision_event.set_dict( + {"other_actor": event.other_actor, "location": actor_location} + ) collision_event.set_message( "Agent collided against object with type={} and id={} at (x={}, y={}, z={})".format( event.other_actor.type_id, event.other_actor.id, round(actor_location.x, 3), round(actor_location.y, 3), - round(actor_location.z, 3))) + round(actor_location.z, 3), + ) + ) self.events.append(collision_event) class ActorBlockedTest(Criterion): - """ This test will fail if the actor has had its linear velocity lower than a specific value for a specific amount of time @@ -426,7 +472,15 @@ class ActorBlockedTest(Criterion): - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ - def __init__(self, actor, min_speed, max_time, name="ActorBlockedTest", optional=False, terminate_on_failure=False): + def __init__( + self, + actor, + min_speed, + max_time, + name="ActorBlockedTest", + optional=False, + terminate_on_failure=False, + ): """ Class constructor """ @@ -444,35 +498,46 @@ def update(self): new_status = py_trees.common.Status.RUNNING # Deactivate/Activate checking by blackboard message - active = py_trees.blackboard.Blackboard().get('AC_SwitchActorBlockedTest') + active = py_trees.blackboard.Blackboard().get("AC_SwitchActorBlockedTest") if active is not None: self._active = active self._time_last_valid_state = GameTime.get_time() - py_trees.blackboard.Blackboard().set("AC_SwitchActorBlockedTest", None, overwrite=True) + py_trees.blackboard.Blackboard().set( + "AC_SwitchActorBlockedTest", None, overwrite=True + ) if self._active: linear_speed = CarlaDataProvider.get_velocity(self.actor) if linear_speed is not None: if linear_speed < self._min_speed and self._time_last_valid_state: - if (GameTime.get_time() - self._time_last_valid_state) > self._max_time: + if ( + GameTime.get_time() - self._time_last_valid_state + ) > self._max_time: # The actor has been "blocked" for too long, save the data self.test_status = "FAILURE" vehicle_location = CarlaDataProvider.get_location(self.actor) - event = TrafficEvent(event_type=TrafficEventType.VEHICLE_BLOCKED, frame=GameTime.get_frame()) - event.set_message('Agent got blocked at (x={}, y={}, z={})'.format( - round(vehicle_location.x, 3), - round(vehicle_location.y, 3), - round(vehicle_location.z, 3)) + event = TrafficEvent( + event_type=TrafficEventType.VEHICLE_BLOCKED, + frame=GameTime.get_frame(), + ) + event.set_message( + "Agent got blocked at (x={}, y={}, z={})".format( + round(vehicle_location.x, 3), + round(vehicle_location.y, 3), + round(vehicle_location.z, 3), + ) ) - event.set_dict({'location': vehicle_location}) + event.set_dict({"location": vehicle_location}) self.events.append(event) else: self._time_last_valid_state = GameTime.get_time() if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -492,8 +557,10 @@ def __init__(self, actor, optional=False, name="CheckKeepLane"): super(KeepLaneTest, self).__init__(name, actor, optional) world = self.actor.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.lane_invasion') - self._lane_sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self.actor) + blueprint = world.get_blueprint_library().find("sensor.other.lane_invasion") + self._lane_sensor = world.spawn_actor( + blueprint, carla.Transform(), attach_to=self.actor + ) self._lane_sensor.listen(lambda event: self._count_lane_invasion(event)) def update(self): @@ -510,7 +577,9 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -531,7 +600,6 @@ def _count_lane_invasion(self, event): class ReachedRegionTest(Criterion): - """ This class contains the reached region test The test is a success if the actor reaches a specified region @@ -565,7 +633,8 @@ def update(self): in_region = False if self.test_status != "SUCCESS": in_region = (location.x > self._min_x and location.x < self._max_x) and ( - location.y > self._min_y and location.y < self._max_y) + location.y > self._min_y and location.y < self._max_y + ) if in_region: self.test_status = "SUCCESS" else: @@ -574,12 +643,13 @@ def update(self): if self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class OffRoadTest(Criterion): - """ Atomic containing a test to detect when an actor deviates from the driving lanes. This atomic can fail when actor has spent a specific time outside driving lanes (defined by OpenDRIVE). Simplified @@ -594,7 +664,14 @@ class OffRoadTest(Criterion): terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met. """ - def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="OffRoadTest"): + def __init__( + self, + actor, + duration=0, + optional=False, + terminate_on_failure=False, + name="OffRoadTest", + ): """ Setup of the variables """ @@ -622,14 +699,9 @@ def update(self): current_location = CarlaDataProvider.get_location(self.actor) # Get the waypoint at the current location to see if the actor is offroad - drive_waypoint = self._map.get_waypoint( - current_location, - project_to_road=False - ) + drive_waypoint = self._map.get_waypoint(current_location, project_to_road=False) park_waypoint = self._map.get_waypoint( - current_location, - project_to_road=False, - lane_type=carla.LaneType.Parking + current_location, project_to_road=False, lane_type=carla.LaneType.Parking ) if drive_waypoint or park_waypoint: self._offroad = False @@ -653,13 +725,14 @@ def update(self): if self._terminate_on_failure and self.test_status == "FAILURE": new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class EndofRoadTest(Criterion): - """ Atomic containing a test to detect when an actor has changed to a different road @@ -672,7 +745,14 @@ class EndofRoadTest(Criterion): terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met. """ - def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="EndofRoadTest"): + def __init__( + self, + actor, + duration=0, + optional=False, + terminate_on_failure=False, + name="EndofRoadTest", + ): """ Setup of the variables """ @@ -707,7 +787,6 @@ def update(self): else: # Wait until the actor has left the road if self._road_id != current_waypoint.road_id or self._start_time: - # Start counting if self._start_time is None: self._start_time = GameTime.get_time() @@ -721,12 +800,13 @@ def update(self): self.actual_value += 1 return py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class OnSidewalkTest(Criterion): - """ Atomic containing a test to detect sidewalk invasions of a specific actor. This atomic can fail when actor has spent a specific time outside driving lanes (defined by OpenDRIVE). @@ -740,11 +820,20 @@ class OnSidewalkTest(Criterion): terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met. """ - def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="OnSidewalkTest"): + def __init__( + self, + actor, + duration=0, + optional=False, + terminate_on_failure=False, + name="OnSidewalkTest", + ): """ Construction with sensor setup """ - super(OnSidewalkTest, self).__init__(name, actor, optional, terminate_on_failure) + super(OnSidewalkTest, self).__init__( + name, actor, optional, terminate_on_failure + ) self._map = CarlaDataProvider.get_map() self._onsidewalk_active = False @@ -787,12 +876,16 @@ def update(self): self._sidewalk_start_location = current_loc # Case 2) Not inside allowed zones (Driving and Parking) - elif current_wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): - + elif current_wp.lane_type not in ( + carla.LaneType.Driving, + carla.LaneType.Parking, + ): # Get the vertices of the vehicle heading_vec = current_tra.get_forward_vector() heading_vec.z = 0 - heading_vec = heading_vec / math.sqrt(math.pow(heading_vec.x, 2) + math.pow(heading_vec.y, 2)) + heading_vec = heading_vec / math.sqrt( + math.pow(heading_vec.x, 2) + math.pow(heading_vec.y, 2) + ) perpendicular_vec = carla.Vector3D(-heading_vec.y, heading_vec.x, 0) extent = self.actor.bounding_box.extent @@ -802,23 +895,37 @@ def update(self): bbox = [ current_loc + carla.Location(x_boundary_vector - y_boundary_vector), current_loc + carla.Location(x_boundary_vector + y_boundary_vector), - current_loc + carla.Location(-1 * x_boundary_vector - y_boundary_vector), - current_loc + carla.Location(-1 * x_boundary_vector + y_boundary_vector)] + current_loc + + carla.Location(-1 * x_boundary_vector - y_boundary_vector), + current_loc + + carla.Location(-1 * x_boundary_vector + y_boundary_vector), + ] bbox_wp = [ self._map.get_waypoint(bbox[0], lane_type=carla.LaneType.Any), self._map.get_waypoint(bbox[1], lane_type=carla.LaneType.Any), self._map.get_waypoint(bbox[2], lane_type=carla.LaneType.Any), - self._map.get_waypoint(bbox[3], lane_type=carla.LaneType.Any)] + self._map.get_waypoint(bbox[3], lane_type=carla.LaneType.Any), + ] - lane_type_list = [bbox_wp[0].lane_type, bbox_wp[1].lane_type, bbox_wp[2].lane_type, bbox_wp[3].lane_type] + lane_type_list = [ + bbox_wp[0].lane_type, + bbox_wp[1].lane_type, + bbox_wp[2].lane_type, + bbox_wp[3].lane_type, + ] # Case 2.1) Not quite outside yet - if bbox_wp[0].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \ - or bbox_wp[1].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \ - or bbox_wp[2].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \ - or bbox_wp[3].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking): - + if ( + bbox_wp[0].lane_type + == (carla.LaneType.Driving or carla.LaneType.Parking) + or bbox_wp[1].lane_type + == (carla.LaneType.Driving or carla.LaneType.Parking) + or bbox_wp[2].lane_type + == (carla.LaneType.Driving or carla.LaneType.Parking) + or bbox_wp[3].lane_type + == (carla.LaneType.Driving or carla.LaneType.Parking) + ): self._onsidewalk_active = False self._outside_lane_active = False @@ -829,11 +936,12 @@ def update(self): self._sidewalk_start_location = current_loc else: - distance_vehicle_wp = current_loc.distance(current_wp.transform.location) + distance_vehicle_wp = current_loc.distance( + current_wp.transform.location + ) # Case 2.3) Outside lane if distance_vehicle_wp >= current_wp.lane_width / 2: - if not self._outside_lane_active: self._outside_lane_active = True self._outside_lane_start_location = current_loc @@ -848,15 +956,15 @@ def update(self): # Check for false positives at junctions if current_wp.is_junction: distance_vehicle_wp = math.sqrt( - math.pow(current_wp.transform.location.x - current_loc.x, 2) + - math.pow(current_wp.transform.location.y - current_loc.y, 2)) + math.pow(current_wp.transform.location.x - current_loc.x, 2) + + math.pow(current_wp.transform.location.y - current_loc.y, 2) + ) if distance_vehicle_wp <= current_wp.lane_width / 2: self._onsidewalk_active = False self._outside_lane_active = False # Else, do nothing, the waypoint is too far to consider it a correct position else: - self._onsidewalk_active = False self._outside_lane_active = False @@ -875,8 +983,12 @@ def update(self): self.test_status = "FAILURE" # Update the distances - distance_vector = CarlaDataProvider.get_location(self.actor) - self._actor_location - distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) + distance_vector = ( + CarlaDataProvider.get_location(self.actor) - self._actor_location + ) + distance = math.sqrt( + math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2) + ) if distance >= 0.02: # Used to avoid micro-changes adding to considerable sums self._actor_location = CarlaDataProvider.get_location(self.actor) @@ -889,14 +1001,22 @@ def update(self): # Register the sidewalk event if not self._onsidewalk_active and self._wrong_sidewalk_distance > 0: - self.actual_value += 1 - onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION, frame=GameTime.get_frame()) + onsidewalk_event = TrafficEvent( + event_type=TrafficEventType.ON_SIDEWALK_INFRACTION, + frame=GameTime.get_frame(), + ) self._set_event_message( - onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance) + onsidewalk_event, + self._sidewalk_start_location, + self._wrong_sidewalk_distance, + ) self._set_event_dict( - onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance) + onsidewalk_event, + self._sidewalk_start_location, + self._wrong_sidewalk_distance, + ) self._onsidewalk_active = False self._wrong_sidewalk_distance = 0 @@ -904,20 +1024,30 @@ def update(self): # Register the outside of a lane event if not self._outside_lane_active and self._wrong_outside_lane_distance > 0: - self.actual_value += 1 - outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION, frame=GameTime.get_frame()) + outsidelane_event = TrafficEvent( + event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION, + frame=GameTime.get_frame(), + ) self._set_event_message( - outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance) + outsidelane_event, + self._outside_lane_start_location, + self._wrong_outside_lane_distance, + ) self._set_event_dict( - outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance) + outsidelane_event, + self._outside_lane_start_location, + self._wrong_outside_lane_distance, + ) self._outside_lane_active = False self._wrong_outside_lane_distance = 0 self.events.append(outsidelane_event) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -927,14 +1057,22 @@ def terminate(self, new_status): """ # If currently at a sidewalk, register the event if self._onsidewalk_active: - self.actual_value += 1 - onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION, frame=GameTime.get_frame()) + onsidewalk_event = TrafficEvent( + event_type=TrafficEventType.ON_SIDEWALK_INFRACTION, + frame=GameTime.get_frame(), + ) self._set_event_message( - onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance) + onsidewalk_event, + self._sidewalk_start_location, + self._wrong_sidewalk_distance, + ) self._set_event_dict( - onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance) + onsidewalk_event, + self._sidewalk_start_location, + self._wrong_sidewalk_distance, + ) self._onsidewalk_active = False self._wrong_sidewalk_distance = 0 @@ -942,14 +1080,22 @@ def terminate(self, new_status): # If currently outside of our lane, register the event if self._outside_lane_active: - self.actual_value += 1 - outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION, frame=GameTime.get_frame()) + outsidelane_event = TrafficEvent( + event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION, + frame=GameTime.get_frame(), + ) self._set_event_message( - outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance) + outsidelane_event, + self._outside_lane_start_location, + self._wrong_outside_lane_distance, + ) self._set_event_dict( - outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance) + outsidelane_event, + self._outside_lane_start_location, + self._wrong_outside_lane_distance, + ) self._outside_lane_active = False self._wrong_outside_lane_distance = 0 @@ -962,27 +1108,28 @@ def _set_event_message(self, event, location, distance): Sets the message of the event """ if event.get_type() == TrafficEventType.ON_SIDEWALK_INFRACTION: - message_start = 'Agent invaded the sidewalk' + message_start = "Agent invaded the sidewalk" else: - message_start = 'Agent went outside the lane' + message_start = "Agent went outside the lane" event.set_message( - '{} for about {} meters, starting at (x={}, y={}, z={})'.format( + "{} for about {} meters, starting at (x={}, y={}, z={})".format( message_start, round(distance, 3), round(location.x, 3), round(location.y, 3), - round(location.z, 3))) + round(location.z, 3), + ) + ) def _set_event_dict(self, event, location, distance): """ Sets the dictionary of the event """ - event.set_dict({'location': location, 'distance': distance}) + event.set_dict({"location": location, "distance": distance}) class OutsideRouteLanesTest(Criterion): - """ Atomic to detect if the vehicle is either on a sidewalk or at a wrong lane. The distance spent outside is computed and it is returned as a percentage of the route distance traveled. @@ -993,10 +1140,16 @@ class OutsideRouteLanesTest(Criterion): optional (bool): If True, the result is not considered for an overall pass/fail result """ - ALLOWED_OUT_DISTANCE = 0.5 # At least 0.5, due to the mini-shoulder between lanes and sidewalks + ALLOWED_OUT_DISTANCE = ( + 0.5 # At least 0.5, due to the mini-shoulder between lanes and sidewalks + ) MAX_VEHICLE_ANGLE = 120.0 # Maximum angle between the yaw and waypoint lane - MAX_WAYPOINT_ANGLE = 150.0 # Maximum change between the yaw-lane angle between frames - WINDOWS_SIZE = 3 # Amount of additional waypoints checked (in case the first on fails) + MAX_WAYPOINT_ANGLE = ( + 150.0 # Maximum change between the yaw-lane angle between frames + ) + WINDOWS_SIZE = ( + 3 # Amount of additional waypoints checked (in case the first on fails) + ) def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): """ @@ -1040,20 +1193,26 @@ def update(self): return new_status # Deactivate / activate checking by blackboard message - active = py_trees.blackboard.Blackboard().get('AC_SwitchWrongDirectionTest') + active = py_trees.blackboard.Blackboard().get("AC_SwitchWrongDirectionTest") if active is not None: self._wrong_direction_active = active - py_trees.blackboard.Blackboard().set("AC_SwitchWrongDirectionTest", None, overwrite=True) + py_trees.blackboard.Blackboard().set( + "AC_SwitchWrongDirectionTest", None, overwrite=True + ) self._is_outside_driving_lanes(location) self._is_at_wrong_lane(location) - if self._outside_lane_active or (self._wrong_direction_active and self._wrong_lane_active): + if self._outside_lane_active or ( + self._wrong_direction_active and self._wrong_lane_active + ): self.test_status = "FAILURE" # Get the traveled distance - for index in range(self._current_index + 1, - min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)): + for index in range( + self._current_index + 1, + min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length), + ): # Get the dot product to know if it has passed this location route_transform = self._route_transforms[index] route_location = route_transform.location @@ -1063,12 +1222,16 @@ def update(self): if wp_veh.dot(wp_dir) > 0: # Get the distance traveled and add it to the total distance - prev_route_location = self._route_transforms[self._current_index].location + prev_route_location = self._route_transforms[ + self._current_index + ].location new_dist = prev_route_location.distance(route_location) self._total_distance += new_dist # And to the wrong one if outside route lanes - if self._outside_lane_active or (self._wrong_direction_active and self._wrong_lane_active): + if self._outside_lane_active or ( + self._wrong_direction_active and self._wrong_lane_active + ): self._wrong_distance += new_dist if self._wrong_distance: @@ -1076,7 +1239,9 @@ def update(self): self._current_index = index - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def _set_traffic_event(self): @@ -1084,7 +1249,10 @@ def _set_traffic_event(self): Creates the traffic event / updates it """ if self._traffic_event is None: - self._traffic_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION, frame=GameTime.get_frame()) + self._traffic_event = TrafficEvent( + event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION, + frame=GameTime.get_frame(), + ) self.events.append(self._traffic_event) percentage = self._wrong_distance / self._total_distance * 100 @@ -1093,13 +1261,13 @@ def _set_traffic_event(self): self._traffic_event.set_message( "Agent went outside its route lanes for about {} meters " "({}% of the completed route)".format( - round(self._wrong_distance, 3), - round(percentage, 2))) + round(self._wrong_distance, 3), round(percentage, 2) + ) + ) - self._traffic_event.set_dict({ - 'distance': self._wrong_distance, - 'percentage': percentage - }) + self._traffic_event.set_dict( + {"distance": self._wrong_distance, "percentage": percentage} + ) self._traffic_event.set_frame(GameTime.get_frame()) @@ -1114,7 +1282,7 @@ def _is_outside_driving_lanes(self, location): if parking_wp is not None: # Some towns have no parking parking_distance = location.distance(parking_wp.transform.location) else: - parking_distance = float('inf') + parking_distance = float("inf") if driving_distance >= parking_distance: distance = parking_distance @@ -1123,7 +1291,9 @@ def _is_outside_driving_lanes(self, location): distance = driving_distance lane_width = driving_wp.lane_width - self._outside_lane_active = bool(distance > (lane_width / 2 + self.ALLOWED_OUT_DISTANCE)) + self._outside_lane_active = bool( + distance > (lane_width / 2 + self.ALLOWED_OUT_DISTANCE) + ) def _is_at_wrong_lane(self, location): """ @@ -1137,14 +1307,15 @@ def _is_at_wrong_lane(self, location): if waypoint.is_junction: self._wrong_lane_active = False elif self._last_road_id != road_id or self._last_lane_id != lane_id: - if self._last_ego_waypoint.is_junction: # Just exited a junction, check the wp direction vs the ego's one wp_yaw = waypoint.transform.rotation.yaw % 360 actor_yaw = self.actor.get_transform().rotation.yaw % 360 angle = (wp_yaw - actor_yaw) % 360 - if angle < self.MAX_VEHICLE_ANGLE or angle > (360 - self.MAX_VEHICLE_ANGLE): + if angle < self.MAX_VEHICLE_ANGLE or angle > ( + 360 - self.MAX_VEHICLE_ANGLE + ): self._wrong_lane_active = False else: self._wrong_lane_active = True @@ -1155,8 +1326,9 @@ def _is_at_wrong_lane(self, location): wp_yaw = waypoint.transform.rotation.yaw % 360 angle = (last_wp_yaw - wp_yaw) % 360 - if angle > self.MAX_WAYPOINT_ANGLE and angle < (360 - self.MAX_WAYPOINT_ANGLE): - + if angle > self.MAX_WAYPOINT_ANGLE and angle < ( + 360 - self.MAX_WAYPOINT_ANGLE + ): # Is the ego vehicle going back to the lane, or going out? Take the opposite self._wrong_lane_active = not bool(self._wrong_lane_active) @@ -1167,7 +1339,6 @@ def _is_at_wrong_lane(self, location): class WrongLaneTest(Criterion): - """ This class contains an atomic test to detect invasions to wrong direction lanes. @@ -1175,6 +1346,7 @@ class WrongLaneTest(Criterion): - actor: CARLA actor to be used for this test - optional [optional]: If True, the result is not considered for an overall pass/fail result """ + MAX_ALLOWED_ANGLE = 120.0 MAX_WAYPOINT_ANGLE = 150.0 @@ -1209,8 +1381,10 @@ def update(self): current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id - if (self._last_road_id != current_road_id or self._last_lane_id != current_lane_id) \ - and not lane_waypoint.is_junction: + if ( + self._last_road_id != current_road_id + or self._last_lane_id != current_lane_id + ) and not lane_waypoint.is_junction: next_waypoint = lane_waypoint.next(2.0)[0] if not next_waypoint: @@ -1218,18 +1392,33 @@ def update(self): # The waypoint route direction can be considered continuous. # Therefore just check for a big gap in waypoint directions. - previous_lane_direction = self._previous_lane_waypoint.transform.get_forward_vector() + previous_lane_direction = ( + self._previous_lane_waypoint.transform.get_forward_vector() + ) current_lane_direction = lane_waypoint.transform.get_forward_vector() - p_lane_vector = np.array([previous_lane_direction.x, previous_lane_direction.y]) - c_lane_vector = np.array([current_lane_direction.x, current_lane_direction.y]) + p_lane_vector = np.array( + [previous_lane_direction.x, previous_lane_direction.y] + ) + c_lane_vector = np.array( + [current_lane_direction.x, current_lane_direction.y] + ) waypoint_angle = math.degrees( - math.acos(np.clip(np.dot(p_lane_vector, c_lane_vector) / - (np.linalg.norm(p_lane_vector) * np.linalg.norm(c_lane_vector)), -1.0, 1.0))) + math.acos( + np.clip( + np.dot(p_lane_vector, c_lane_vector) + / ( + np.linalg.norm(p_lane_vector) + * np.linalg.norm(c_lane_vector) + ), + -1.0, + 1.0, + ) + ) + ) if waypoint_angle > self.MAX_WAYPOINT_ANGLE and self._in_lane: - self.test_status = "FAILURE" self._in_lane = False self.actual_value += 1 @@ -1241,18 +1430,34 @@ def update(self): # Continuity is broken after a junction so check vehicle-lane angle instead if self._previous_lane_waypoint.is_junction: - - vector_wp = np.array([next_waypoint.transform.location.x - lane_waypoint.transform.location.x, - next_waypoint.transform.location.y - lane_waypoint.transform.location.y]) - - vector_actor = np.array([math.cos(math.radians(self.actor.get_transform().rotation.yaw)), - math.sin(math.radians(self.actor.get_transform().rotation.yaw))]) + vector_wp = np.array( + [ + next_waypoint.transform.location.x + - lane_waypoint.transform.location.x, + next_waypoint.transform.location.y + - lane_waypoint.transform.location.y, + ] + ) + + vector_actor = np.array( + [ + math.cos(math.radians(self.actor.get_transform().rotation.yaw)), + math.sin(math.radians(self.actor.get_transform().rotation.yaw)), + ] + ) vehicle_lane_angle = math.degrees( - math.acos(np.clip(np.dot(vector_actor, vector_wp) / (np.linalg.norm(vector_wp)), -1.0, 1.0))) + math.acos( + np.clip( + np.dot(vector_actor, vector_wp) + / (np.linalg.norm(vector_wp)), + -1.0, + 1.0, + ) + ) + ) if vehicle_lane_angle > self.MAX_ALLOWED_ANGLE: - self.test_status = "FAILURE" self._in_lane = False self.actual_value += 1 @@ -1260,9 +1465,13 @@ def update(self): # Keep adding "meters" to the counter distance_vector = self.actor.get_location() - self._actor_location - distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) + distance = math.sqrt( + math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2) + ) - if distance >= 0.02: # Used to avoid micro-changes adding add to considerable sums + if ( + distance >= 0.02 + ): # Used to avoid micro-changes adding add to considerable sums self._actor_location = CarlaDataProvider.get_location(self.actor) if not self._in_lane and not lane_waypoint.is_junction: @@ -1270,12 +1479,24 @@ def update(self): # Register the event if self._in_lane and self._wrong_distance > 0: - - wrong_way_event = TrafficEvent(event_type=TrafficEventType.WRONG_WAY_INFRACTION, frame=GameTime.get_frame()) - self._set_event_message(wrong_way_event, self._wrong_lane_start_location, - self._wrong_distance, current_road_id, current_lane_id) - self._set_event_dict(wrong_way_event, self._wrong_lane_start_location, - self._wrong_distance, current_road_id, current_lane_id) + wrong_way_event = TrafficEvent( + event_type=TrafficEventType.WRONG_WAY_INFRACTION, + frame=GameTime.get_frame(), + ) + self._set_event_message( + wrong_way_event, + self._wrong_lane_start_location, + self._wrong_distance, + current_road_id, + current_lane_id, + ) + self._set_event_dict( + wrong_way_event, + self._wrong_lane_start_location, + self._wrong_distance, + current_road_id, + current_lane_id, + ) self.events.append(wrong_way_event) self._wrong_distance = 0 @@ -1285,7 +1506,9 @@ def update(self): self._last_road_id = current_road_id self._previous_lane_waypoint = lane_waypoint - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -1294,16 +1517,28 @@ def terminate(self, new_status): If there is currently an event running, it is registered """ if not self._in_lane: - lane_waypoint = self._map.get_waypoint(self.actor.get_location()) current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id - wrong_way_event = TrafficEvent(event_type=TrafficEventType.WRONG_WAY_INFRACTION, frame=GameTime.get_frame()) - self._set_event_message(wrong_way_event, self._wrong_lane_start_location, - self._wrong_distance, current_road_id, current_lane_id) - self._set_event_dict(wrong_way_event, self._wrong_lane_start_location, - self._wrong_distance, current_road_id, current_lane_id) + wrong_way_event = TrafficEvent( + event_type=TrafficEventType.WRONG_WAY_INFRACTION, + frame=GameTime.get_frame(), + ) + self._set_event_message( + wrong_way_event, + self._wrong_lane_start_location, + self._wrong_distance, + current_road_id, + current_lane_id, + ) + self._set_event_dict( + wrong_way_event, + self._wrong_lane_start_location, + self._wrong_distance, + current_road_id, + current_lane_id, + ) self._wrong_distance = 0 self._in_lane = True @@ -1324,21 +1559,25 @@ def _set_event_message(self, event, location, distance, road_id, lane_id): round(location.y, 3), round(location.z, 3), road_id, - lane_id)) + lane_id, + ) + ) def _set_event_dict(self, event, location, distance, road_id, lane_id): """ Sets the dictionary of the event """ - event.set_dict({ - 'location': location, - 'distance': distance, - 'road_id': road_id, - 'lane_id': lane_id}) + event.set_dict( + { + "location": location, + "distance": distance, + "road_id": road_id, + "lane_id": lane_id, + } + ) class InRadiusRegionTest(Criterion): - """ The test is a success if the actor is within a given radius of a specified region @@ -1348,12 +1587,11 @@ class InRadiusRegionTest(Criterion): """ def __init__(self, actor, x, y, radius, name="InRadiusRegionTest"): - """ - """ + """ """ super(InRadiusRegionTest, self).__init__(name, actor) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._x = x # pylint: disable=invalid-name - self._y = y # pylint: disable=invalid-name + self._x = x # pylint: disable=invalid-name + self._y = y # pylint: disable=invalid-name self._radius = radius def update(self): @@ -1367,10 +1605,18 @@ def update(self): return new_status if self.test_status != "SUCCESS": - in_radius = math.sqrt(((location.x - self._x)**2) + ((location.y - self._y)**2)) < self._radius + in_radius = ( + math.sqrt(((location.x - self._x) ** 2) + ((location.y - self._y) ** 2)) + < self._radius + ) if in_radius: - route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED, frame=GameTime.get_frame()) - route_completion_event.set_message("Destination was successfully reached") + route_completion_event = TrafficEvent( + event_type=TrafficEventType.ROUTE_COMPLETED, + frame=GameTime.get_frame(), + ) + route_completion_event.set_message( + "Destination was successfully reached" + ) self.events.append(route_completion_event) self.test_status = "SUCCESS" else: @@ -1379,13 +1625,14 @@ def update(self): if self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InRouteTest(Criterion): - """ The test is a success if the actor is never outside route. The actor can go outside of the route but only for a certain amount of distance @@ -1397,13 +1644,23 @@ class InRouteTest(Criterion): - offroad_min: Maximum safe distance (in meters). Might eventually cause failure - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ + MAX_ROUTE_PERCENTAGE = 30 # % WINDOWS_SIZE = 5 # Amount of additional waypoints checked - def __init__(self, actor, route, offroad_min=None, offroad_max=30, name="InRouteTest", terminate_on_failure=False): - """ - """ - super(InRouteTest, self).__init__(name, actor, terminate_on_failure=terminate_on_failure) + def __init__( + self, + actor, + route, + offroad_min=None, + offroad_max=30, + name="InRouteTest", + terminate_on_failure=False, + ): + """ """ + super(InRouteTest, self).__init__( + name, actor, terminate_on_failure=terminate_on_failure + ) self.units = None # We care about whether or not it fails, no units attached self._route = route @@ -1448,23 +1705,27 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - elif self.test_status in ('RUNNING', 'INIT'): - + elif self.test_status in ("RUNNING", "INIT"): off_route = True - shortest_distance = float('inf') + shortest_distance = float("inf") closest_index = -1 # Get the closest distance - for index in range(self._current_index, - min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)): + for index in range( + self._current_index, + min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length), + ): ref_location = self._route_transforms[index].location - distance = math.sqrt(((location.x - ref_location.x) ** 2) + ((location.y - ref_location.y) ** 2)) + distance = math.sqrt( + ((location.x - ref_location.x) ** 2) + + ((location.y - ref_location.y) ** 2) + ) if distance <= shortest_distance: closest_index = index shortest_distance = distance - if closest_index == -1 or shortest_distance == float('inf'): + if closest_index == -1 or shortest_distance == float("inf"): return new_status # Check if the actor is out of route @@ -1474,13 +1735,17 @@ def update(self): # If actor advanced a step, record the distance if self._current_index != closest_index: - - new_dist = self._accum_meters[closest_index] - self._accum_meters[self._current_index] + new_dist = ( + self._accum_meters[closest_index] + - self._accum_meters[self._current_index] + ) # If too far from the route, add it and check if its value if not self._in_safe_route: self._out_route_distance += new_dist - out_route_percentage = 100 * self._out_route_distance / self._accum_meters[-1] + out_route_percentage = ( + 100 * self._out_route_distance / self._accum_meters[-1] + ) if out_route_percentage > self.MAX_ROUTE_PERCENTAGE: off_route = True @@ -1491,13 +1756,16 @@ def update(self): blackv = py_trees.blackboard.Blackboard() _ = blackv.set("InRoute", False) - route_deviation_event = TrafficEvent(event_type=TrafficEventType.ROUTE_DEVIATION, frame=GameTime.get_frame()) + route_deviation_event = TrafficEvent( + event_type=TrafficEventType.ROUTE_DEVIATION, + frame=GameTime.get_frame(), + ) route_deviation_event.set_message( "Agent deviated from the route at (x={}, y={}, z={})".format( - round(location.x, 3), - round(location.y, 3), - round(location.z, 3))) - route_deviation_event.set_dict({'location': location}) + round(location.x, 3), round(location.y, 3), round(location.z, 3) + ) + ) + route_deviation_event.set_dict({"location": location}) self.events.append(route_deviation_event) @@ -1505,13 +1773,14 @@ def update(self): self.actual_value += 1 new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class RouteCompletionTest(Criterion): - """ Check at which stage of the route is the actor at each tick @@ -1520,16 +1789,20 @@ class RouteCompletionTest(Criterion): - route: Route to be checked - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ + WINDOWS_SIZE = 2 # Thresholds to return that a route has been completed DISTANCE_THRESHOLD = 10.0 # meters PERCENTAGE_THRESHOLD = 99 # % - def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False): - """ - """ - super(RouteCompletionTest, self).__init__(name, actor, terminate_on_failure=terminate_on_failure) + def __init__( + self, actor, route, name="RouteCompletionTest", terminate_on_failure=False + ): + """ """ + super(RouteCompletionTest, self).__init__( + name, actor, terminate_on_failure=terminate_on_failure + ) self.units = "%" self.success_value = 100 self._route = route @@ -1542,9 +1815,13 @@ def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failur self.target_location = self._route_transforms[-1].location - self._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION, frame=0) - self._traffic_event.set_dict({'route_completed': self.actual_value}) - self._traffic_event.set_message("Agent has completed {} of the route".format(self.actual_value)) + self._traffic_event = TrafficEvent( + event_type=TrafficEventType.ROUTE_COMPLETION, frame=0 + ) + self._traffic_event.set_dict({"route_completed": self.actual_value}) + self._traffic_event.set_message( + "Agent has completed {} of the route".format(self.actual_value) + ) self.events.append(self._traffic_event) def _get_acummulated_percentages(self): @@ -1574,32 +1851,42 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - elif self.test_status in ('RUNNING', 'INIT'): - - for index in range(self._index, min(self._index + self.WINDOWS_SIZE + 1, self._route_length)): + elif self.test_status in ("RUNNING", "INIT"): + for index in range( + self._index, + min(self._index + self.WINDOWS_SIZE + 1, self._route_length), + ): # Get the dot product to know if it has passed this location route_transform = self._route_transforms[index] route_location = route_transform.location - wp_dir = route_transform.get_forward_vector() # Waypoint's forward vector - wp_veh = location - route_location # vector route - vehicle + wp_dir = ( + route_transform.get_forward_vector() + ) # Waypoint's forward vector + wp_veh = location - route_location # vector route - vehicle if wp_veh.dot(wp_dir) > 0: self._index = index self.actual_value = self._route_accum_perc[self._index] self.actual_value = round(self.actual_value, 2) - self._traffic_event.set_dict({'route_completed': self.actual_value}) - self._traffic_event.set_message("Agent has completed {} of the route".format(self.actual_value)) - - if self.actual_value > self.PERCENTAGE_THRESHOLD \ - and location.distance(self.target_location) < self.DISTANCE_THRESHOLD: + self._traffic_event.set_dict({"route_completed": self.actual_value}) + self._traffic_event.set_message( + "Agent has completed {} of the route".format(self.actual_value) + ) + + if ( + self.actual_value > self.PERCENTAGE_THRESHOLD + and location.distance(self.target_location) < self.DISTANCE_THRESHOLD + ): self.test_status = "SUCCESS" self.actual_value = 100 elif self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -1609,8 +1896,10 @@ def terminate(self, new_status): """ self.actual_value = round(self.actual_value, 2) - self._traffic_event.set_dict({'route_completed': self.actual_value}) - self._traffic_event.set_message("Agent has completed {} of the route".format(self.actual_value)) + self._traffic_event.set_dict({"route_completed": self.actual_value}) + self._traffic_event.set_message( + "Agent has completed {} of the route".format(self.actual_value) + ) if self.test_status == "INIT": self.test_status = "FAILURE" @@ -1618,7 +1907,6 @@ def terminate(self, new_status): class RunningRedLightTest(Criterion): - """ Check if an actor is running a red light @@ -1626,13 +1914,16 @@ class RunningRedLightTest(Criterion): - actor: CARLA actor to be used for this test - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ + DISTANCE_LIGHT = 15 # m def __init__(self, actor, name="RunningRedLightTest", terminate_on_failure=False): """ Init """ - super(RunningRedLightTest, self).__init__(name, actor, terminate_on_failure=terminate_on_failure) + super(RunningRedLightTest, self).__init__( + name, actor, terminate_on_failure=terminate_on_failure + ) self._world = CarlaDataProvider.get_world() self._map = CarlaDataProvider.get_map() self._list_traffic_lights = [] @@ -1641,7 +1932,7 @@ def __init__(self, actor, name="RunningRedLightTest", terminate_on_failure=False all_actors = CarlaDataProvider.get_all_actors() for _actor in all_actors: - if 'traffic_light' in _actor.type_id: + if "traffic_light" in _actor.type_id: center, waypoints = self.get_traffic_light_waypoints(_actor) self._list_traffic_lights.append((_actor, center, waypoints)) @@ -1650,8 +1941,12 @@ def is_vehicle_crossing_line(self, seg1, seg2): """ check if vehicle crosses a line segment """ - line1 = shapely.geometry.LineString([(seg1[0].x, seg1[0].y), (seg1[1].x, seg1[1].y)]) - line2 = shapely.geometry.LineString([(seg2[0].x, seg2[0].y), (seg2[1].x, seg2[1].y)]) + line1 = shapely.geometry.LineString( + [(seg1[0].x, seg1[0].y), (seg1[1].x, seg1[1].y)] + ) + line2 = shapely.geometry.LineString( + [(seg2[0].x, seg2[0].y), (seg2[1].x, seg2[1].y)] + ) inter = line1.intersection(line2) return not inter.is_empty @@ -1669,14 +1964,17 @@ def update(self): veh_extent = self.actor.bounding_box.extent.x - tail_close_pt = self.rotate_point(carla.Vector3D(-0.8 * veh_extent, 0, 0), transform.rotation.yaw) + tail_close_pt = self.rotate_point( + carla.Vector3D(-0.8 * veh_extent, 0, 0), transform.rotation.yaw + ) tail_close_pt = location + carla.Location(tail_close_pt) - tail_far_pt = self.rotate_point(carla.Vector3D(-veh_extent - 1, 0, 0), transform.rotation.yaw) + tail_far_pt = self.rotate_point( + carla.Vector3D(-veh_extent - 1, 0, 0), transform.rotation.yaw + ) tail_far_pt = location + carla.Location(tail_far_pt) for traffic_light, center, waypoints in self._list_traffic_lights: - if self.debug: z = 2.1 if traffic_light.state == carla.TrafficLightState.Red: @@ -1685,16 +1983,27 @@ def update(self): color = carla.Color(0, 155, 0) else: color = carla.Color(155, 155, 0) - self._world.debug.draw_point(center + carla.Location(z=z), size=0.2, color=color, life_time=0.01) + self._world.debug.draw_point( + center + carla.Location(z=z), size=0.2, color=color, life_time=0.01 + ) for wp in waypoints: text = "{}.{}".format(wp.road_id, wp.lane_id) self._world.debug.draw_string( - wp.transform.location + carla.Location(x=1, z=z), text, color=color, life_time=0.01) + wp.transform.location + carla.Location(x=1, z=z), + text, + color=color, + life_time=0.01, + ) self._world.debug.draw_point( - wp.transform.location + carla.Location(z=z), size=0.1, color=color, life_time=0.01) + wp.transform.location + carla.Location(z=z), + size=0.1, + color=color, + life_time=0.01, + ) center_loc = carla.Location(center) + # skip traffic lights that have already been passed, are too far or are not red if self._last_red_light_id and self._last_red_light_id == traffic_light.id: continue if center_loc.distance(location) > self.DISTANCE_LIGHT: @@ -1703,39 +2012,56 @@ def update(self): continue for wp in waypoints: - tail_wp = self._map.get_waypoint(tail_far_pt) # Calculate the dot product (Might be unscaled, as only its sign is important) - ve_dir = CarlaDataProvider.get_transform(self.actor).get_forward_vector() + ve_dir = CarlaDataProvider.get_transform( + self.actor + ).get_forward_vector() wp_dir = wp.transform.get_forward_vector() # Check the lane until all the "tail" has passed - if tail_wp.road_id == wp.road_id and tail_wp.lane_id == wp.lane_id and ve_dir.dot(wp_dir) > 0: + if ( + tail_wp.road_id == wp.road_id + and tail_wp.lane_id == wp.lane_id + and ve_dir.dot(wp_dir) > 0 + ): # This light is red and is affecting our lane yaw_wp = wp.transform.rotation.yaw lane_width = wp.lane_width location_wp = wp.transform.location - lft_lane_wp = self.rotate_point(carla.Vector3D(0.6 * lane_width, 0, 0), yaw_wp + 90) + lft_lane_wp = self.rotate_point( + carla.Vector3D(0.6 * lane_width, 0, 0), yaw_wp + 90 + ) lft_lane_wp = location_wp + carla.Location(lft_lane_wp) - rgt_lane_wp = self.rotate_point(carla.Vector3D(0.6 * lane_width, 0, 0), yaw_wp - 90) + rgt_lane_wp = self.rotate_point( + carla.Vector3D(0.6 * lane_width, 0, 0), yaw_wp - 90 + ) rgt_lane_wp = location_wp + carla.Location(rgt_lane_wp) # Is the vehicle traversing the stop line? - if self.is_vehicle_crossing_line((tail_close_pt, tail_far_pt), (lft_lane_wp, rgt_lane_wp)): - + if self.is_vehicle_crossing_line( + (tail_close_pt, tail_far_pt), (lft_lane_wp, rgt_lane_wp) + ): self.test_status = "FAILURE" self.actual_value += 1 location = traffic_light.get_transform().location - red_light_event = TrafficEvent(event_type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION, frame=GameTime.get_frame()) + red_light_event = TrafficEvent( + event_type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION, + frame=GameTime.get_frame(), + ) red_light_event.set_message( "Agent ran a red light {} at (x={}, y={}, z={})".format( traffic_light.id, round(location.x, 3), round(location.y, 3), - round(location.z, 3))) - red_light_event.set_dict({'id': traffic_light.id, 'location': location}) + round(location.z, 3), + ) + ) + red_light_event.set_dict( + {"id": traffic_light.id, "location": location} + ) self.events.append(red_light_event) self._last_red_light_id = traffic_light.id @@ -1744,7 +2070,9 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status @@ -1752,8 +2080,14 @@ def rotate_point(self, point, angle): """ rotate a given point by a given angle """ - x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y - y_ = math.sin(math.radians(angle)) * point.x + math.cos(math.radians(angle)) * point.y + x_ = ( + math.cos(math.radians(angle)) * point.x + - math.sin(math.radians(angle)) * point.y + ) + y_ = ( + math.sin(math.radians(angle)) * point.x + + math.cos(math.radians(angle)) * point.y + ) return carla.Vector3D(x_, y_, point.z) def get_traffic_light_waypoints(self, traffic_light): @@ -1766,7 +2100,9 @@ def get_traffic_light_waypoints(self, traffic_light): # Discretize the trigger box into points area_ext = traffic_light.trigger_volume.extent - x_values = np.arange(-0.9 * area_ext.x, 0.9 * area_ext.x, 1.0) # 0.9 to avoid crossing to adjacent lanes + x_values = np.arange( + -0.9 * area_ext.x, 0.9 * area_ext.x, 1.0 + ) # 0.9 to avoid crossing to adjacent lanes area = [] for x in x_values: @@ -1779,7 +2115,11 @@ def get_traffic_light_waypoints(self, traffic_light): for pt in area: wpx = self._map.get_waypoint(pt) # As x_values are arranged in order, only the last one has to be checked - if not ini_wps or ini_wps[-1].road_id != wpx.road_id or ini_wps[-1].lane_id != wpx.lane_id: + if ( + not ini_wps + or ini_wps[-1].road_id != wpx.road_id + or ini_wps[-1].lane_id != wpx.lane_id + ): ini_wps.append(wpx) # Advance them until the intersection @@ -1797,7 +2137,6 @@ def get_traffic_light_waypoints(self, traffic_light): class RunningStopTest(Criterion): - """ Check if an actor is running a stop sign @@ -1805,14 +2144,16 @@ class RunningStopTest(Criterion): - actor: CARLA actor to be used for this test - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ + PROXIMITY_THRESHOLD = 4.0 # Stops closer than this distance will be detected [m] - SPEED_THRESHOLD = 0.1 # Minimum speed to consider the actor has stopped [m/s] + SPEED_THRESHOLD = 0.1 # Minimum speed to consider the actor has stopped [m/s] WAYPOINT_STEP = 0.5 # m def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): - """ - """ - super(RunningStopTest, self).__init__(name, actor, terminate_on_failure=terminate_on_failure) + """ """ + super(RunningStopTest, self).__init__( + name, actor, terminate_on_failure=terminate_on_failure + ) self._world = CarlaDataProvider.get_world() self._map = CarlaDataProvider.get_map() self._list_stop_signs = [] @@ -1822,16 +2163,25 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): self._last_failed_stop = None for _actor in CarlaDataProvider.get_all_actors(): - if 'traffic.stop' in _actor.type_id: + if "traffic.stop" in _actor.type_id: self._list_stop_signs.append(_actor) def point_inside_boundingbox(self, point, bb_center, bb_extent, multiplier=1.2): """Checks whether or not a point is inside a bounding box.""" # pylint: disable=invalid-name - A = carla.Vector2D(bb_center.x - multiplier * bb_extent.x, bb_center.y - multiplier * bb_extent.y) - B = carla.Vector2D(bb_center.x + multiplier * bb_extent.x, bb_center.y - multiplier * bb_extent.y) - D = carla.Vector2D(bb_center.x - multiplier * bb_extent.x, bb_center.y + multiplier * bb_extent.y) + A = carla.Vector2D( + bb_center.x - multiplier * bb_extent.x, + bb_center.y - multiplier * bb_extent.y, + ) + B = carla.Vector2D( + bb_center.x + multiplier * bb_extent.x, + bb_center.y - multiplier * bb_extent.y, + ) + D = carla.Vector2D( + bb_center.x - multiplier * bb_extent.x, + bb_center.y + multiplier * bb_extent.y, + ) M = carla.Vector2D(point.x, point.y) AB = B - A @@ -1859,7 +2209,9 @@ def is_actor_affected_by_stop(self, wp_list, stop): # Using more than one waypoint removes issues with small trigger volumes and backwards movement stop_extent = stop.trigger_volume.extent for actor_wp in wp_list: - if self.point_inside_boundingbox(actor_wp.transform.location, stop_location, stop_extent): + if self.point_inside_boundingbox( + actor_wp.transform.location, stop_location, stop_extent + ): return True return False @@ -1915,7 +2267,9 @@ def update(self): check_wps = self._get_waypoints(self.actor) if not self._target_stop_sign: - self._target_stop_sign = self._scan_for_stop_sign(actor_transform, check_wps) + self._target_stop_sign = self._scan_for_stop_sign( + actor_transform, check_wps + ) return new_status if not self._stop_completed: @@ -1924,19 +2278,29 @@ def update(self): self._stop_completed = True if not self.is_actor_affected_by_stop(check_wps, self._target_stop_sign): - if not self._stop_completed and self._last_failed_stop != self._target_stop_sign.id: + if ( + not self._stop_completed + and self._last_failed_stop != self._target_stop_sign.id + ): # did we stop? self.actual_value += 1 self.test_status = "FAILURE" stop_location = self._target_stop_sign.get_transform().location - running_stop_event = TrafficEvent(event_type=TrafficEventType.STOP_INFRACTION, frame=GameTime.get_frame()) + running_stop_event = TrafficEvent( + event_type=TrafficEventType.STOP_INFRACTION, + frame=GameTime.get_frame(), + ) running_stop_event.set_message( "Agent ran a stop with id={} at (x={}, y={}, z={})".format( self._target_stop_sign.id, round(stop_location.x, 3), round(stop_location.y, 3), - round(stop_location.z, 3))) - running_stop_event.set_dict({'id': self._target_stop_sign.id, 'location': stop_location}) + round(stop_location.z, 3), + ) + ) + running_stop_event.set_dict( + {"id": self._target_stop_sign.id, "location": stop_location} + ) self.events.append(running_stop_event) @@ -1949,13 +2313,14 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class MinimumSpeedRouteTest(Criterion): - """ Check at which stage of the route is the actor at each tick @@ -1964,12 +2329,19 @@ class MinimumSpeedRouteTest(Criterion): - route: Route to be checked - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ + WINDOWS_SIZE = 2 RATIO = 1 - def __init__(self, actor, route, checkpoints=1, name="MinimumSpeedRouteTest", terminate_on_failure=False): - """ - """ + def __init__( + self, + actor, + route, + checkpoints=1, + name="MinimumSpeedRouteTest", + terminate_on_failure=False, + ): + """ """ super().__init__(name, actor, terminate_on_failure=terminate_on_failure) self.units = "%" self.success_value = 100 @@ -1999,7 +2371,6 @@ def __init__(self, actor, route, checkpoints=1, name="MinimumSpeedRouteTest", te self._index = 0 - def update(self): """ Check if the actor location is within trigger region @@ -2009,12 +2380,14 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - # Check the actor progress through the route + # Check the actor progress through the route location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status - for index in range(self._index, min(self._index + self.WINDOWS_SIZE + 1, self._route_length)): + for index in range( + self._index, min(self._index + self.WINDOWS_SIZE + 1, self._route_length) + ): # Get the dot product to know if it has passed this location route_transform = self._route[index][0] route_location = route_transform.location @@ -2037,8 +2410,10 @@ def update(self): return new_status # Get the speed of the surrounding Background Activity - all_vehicles = CarlaDataProvider.get_all_actors().filter('vehicle*') - background_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'background'] + all_vehicles = CarlaDataProvider.get_all_actors().filter("vehicle*") + background_vehicles = [ + v for v in all_vehicles if v.attributes["role_name"] == "background" + ] if background_vehicles: frame_mean_speed = 0 @@ -2051,16 +2426,19 @@ def update(self): self._actor_speed += velocity self._speed_points += 1 - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def _set_traffic_event(self): - if self._speed_points > 0 and self._mean_speed: self._mean_speed /= self._speed_points self._actor_speed /= self._speed_points - checkpoint_value = round(self._actor_speed / (self.RATIO * self._mean_speed) * 100, 2) + checkpoint_value = round( + self._actor_speed / (self.RATIO * self._mean_speed) * 100, 2 + ) else: checkpoint_value = 100 @@ -2069,9 +2447,13 @@ def _set_traffic_event(self): else: self.test_status = "FAILURE" - self._traffic_event = TrafficEvent(TrafficEventType.MIN_SPEED_INFRACTION, GameTime.get_frame()) - self._traffic_event.set_dict({'percentage': checkpoint_value}) - self._traffic_event.set_message(f"Average speed is {checkpoint_value}% of the surrounding traffic's one") + self._traffic_event = TrafficEvent( + TrafficEventType.MIN_SPEED_INFRACTION, GameTime.get_frame() + ) + self._traffic_event.set_dict({"percentage": checkpoint_value}) + self._traffic_event.set_message( + f"Average speed is {checkpoint_value}% of the surrounding traffic's one" + ) self.events.append(self._traffic_event) self._checkpoint_values.append(checkpoint_value) @@ -2086,12 +2468,13 @@ def terminate(self, new_status): self._set_traffic_event() if len(self._checkpoint_values): - self.actual_value = round(sum(self._checkpoint_values) / len(self._checkpoint_values), 2) + self.actual_value = round( + sum(self._checkpoint_values) / len(self._checkpoint_values), 2 + ) super().terminate(new_status) class YieldToEmergencyVehicleTest(Criterion): - """ Atomic Criterion to detect if the actor yields its lane to the emergency vehicle behind it. Detection is done by checking if the ev is in front of the actor @@ -2102,7 +2485,7 @@ class YieldToEmergencyVehicleTest(Criterion): optional (bool): If True, the result is not considered for an overall pass/fail result """ - WAITING_TIME_THRESHOLD = 15 # Maximum time for actor to block ev + WAITING_TIME_THRESHOLD = 15 # Maximum time for actor to block ev def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest"): """ @@ -2136,7 +2519,9 @@ def update(self): else: self.test_status = "SUCCESS" - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -2144,8 +2529,12 @@ def terminate(self, new_status): # Terminates are called multiple times. Do this only once if not self._terminated: if self.test_status == "FAILURE": - traffic_event = TrafficEvent(TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, GameTime.get_frame()) - traffic_event.set_message("Agent failed to yield to an emergency vehicle") + traffic_event = TrafficEvent( + TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, GameTime.get_frame() + ) + traffic_event.set_message( + "Agent failed to yield to an emergency vehicle" + ) self.events.append(traffic_event) self._terminated = True @@ -2154,7 +2543,6 @@ def terminate(self, new_status): class ScenarioTimeoutTest(Criterion): - """ Atomic Criterion to detect if the actor has been incapable of finishing an scenario @@ -2163,7 +2551,9 @@ class ScenarioTimeoutTest(Criterion): optional (bool): If True, the result is not considered for an overall pass/fail result """ - def __init__(self, actor, scenario_name, optional=False, name="ScenarioTimeoutTest"): + def __init__( + self, actor, scenario_name, optional=False, name="ScenarioTimeoutTest" + ): """ Constructor """ @@ -2175,7 +2565,9 @@ def __init__(self, actor, scenario_name, optional=False, name="ScenarioTimeoutTe def update(self): """wait""" new_status = py_trees.common.Status.RUNNING - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status def terminate(self, new_status): @@ -2188,7 +2580,9 @@ def terminate(self, new_status): self.actual_value = 1 self.test_status = "FAILURE" - traffic_event = TrafficEvent(event_type=TrafficEventType.SCENARIO_TIMEOUT, frame=GameTime.get_frame()) + traffic_event = TrafficEvent( + event_type=TrafficEventType.SCENARIO_TIMEOUT, frame=GameTime.get_frame() + ) traffic_event.set_message("Agent timed out a scenario") self.events.append(traffic_event) py_trees.blackboard.Blackboard().set(blackboard_name, None, True) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_repeat.py b/srunner/scenariomanager/scenarioatomics/atomic_repeat.py index 9fd3bfb..c60fb35 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_repeat.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_repeat.py @@ -11,14 +11,12 @@ class Decorator(behaviour.Behaviour): TypeError: if the child is not an instance of :class:`~py_trees.behaviour.Behaviour` """ - def __init__( - self, - child: behaviour.Behaviour, - name - ): + def __init__(self, child: behaviour.Behaviour, name): # Checks if not isinstance(child, behaviour.Behaviour): - raise TypeError("A decorator's child must be an instance of py_trees.behaviours.Behaviour") + raise TypeError( + "A decorator's child must be an instance of py_trees.behaviours.Behaviour" + ) # Initialise super().__init__(name=name) self.children.append(child) @@ -44,7 +42,8 @@ def tick(self): new_status = self.update() if new_status not in list(common.Status): self.logger.error( - "A behaviour returned an invalid status, setting to INVALID [%s][%s]" % (new_status, self.name) + "A behaviour returned an invalid status, setting to INVALID [%s][%s]" + % (new_status, self.name) ) new_status = common.Status.INVALID if new_status != common.Status.RUNNING: @@ -100,7 +99,9 @@ def update(self): :class:`~py_trees.common.Status`: the behaviour's new status :class:`~py_trees.common.Status` """ if self.decorated.status == common.Status.SUCCESS: - self.feedback_message = "success is running [%s]" % self.decorated.feedback_message + self.feedback_message = ( + "success is running [%s]" % self.decorated.feedback_message + ) if self.repaet_num < self.period: return common.Status.SUCCESS self.repaet_num += 1 diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index bacb2fa..952cfed 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -31,7 +31,10 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import calculate_distance from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.timer import GameTime -from srunner.tools.scenario_helper import get_distance_along_route, get_distance_between_actors +from srunner.tools.scenario_helper import ( + get_distance_along_route, + get_distance_between_actors, +) import srunner.tools as sr_tools @@ -39,7 +42,6 @@ class AtomicCondition(py_trees.behaviour.Behaviour): - """ Base class for all atomic conditions used to setup a scenario @@ -74,12 +76,16 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ - self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.terminate()[%s->%s]" + % (self.__class__.__name__, self.status, new_status) + ) class IfTriggerer(AtomicCondition): - - def __init__(self, actor_ego, actor_npc, comparison_operator=operator.gt, name="IfTriggerer"): + def __init__( + self, actor_ego, actor_npc, comparison_operator=operator.gt, name="IfTriggerer" + ): super(IfTriggerer, self).__init__(name) self.actor_ego = actor_ego self.actor_npc = actor_npc @@ -119,8 +125,14 @@ def update(self): class InTriggerNearCollision(AtomicCondition): - def __init__(self, reference_actor, actor, distance, comparison_operator=operator.lt, - name="InTriggerNearCollision"): + def __init__( + self, + reference_actor, + actor, + distance, + comparison_operator=operator.lt, + name="InTriggerNearCollision", + ): """ Setup trigger distance """ @@ -144,20 +156,23 @@ def update(self): if location is None or reference_location is None: return new_status - if self._comparison_operator(calculate_distance(location, reference_location), self._distance): + if self._comparison_operator( + calculate_distance(location, reference_location), self._distance + ): new_status = py_trees.common.Status.SUCCESS print("Too close, collision!") self._control.throttle = 0 self._control.brake = 1 - print('decelerate!!!') + print("decelerate!!!") self._reference_actor.apply_control(self._control) - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerDistanceToOSCPosition(AtomicCondition): - """ OpenSCENARIO atomic This class contains the trigger condition for a distance to an OpenSCENARIO position @@ -171,8 +186,15 @@ class InTriggerDistanceToOSCPosition(AtomicCondition): The condition terminates with SUCCESS, when the actor reached the target distance to the openSCENARIO position """ - def __init__(self, actor, osc_position, distance, along_route=False, - comparison_operator=operator.lt, name="InTriggerDistanceToOSCPosition"): + def __init__( + self, + actor, + osc_position, + distance, + along_route=False, + comparison_operator=operator.lt, + name="InTriggerDistanceToOSCPosition", + ): """ Setup parameters """ @@ -202,7 +224,8 @@ def update(self): # calculate transform with method in openscenario_parser.py osc_transform = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( - self._osc_position) + self._osc_position + ) if osc_transform is not None: osc_location = osc_transform.location @@ -210,7 +233,9 @@ def update(self): if self._along_route: # Global planner needs a location at a driving lane - actor_location = self._map.get_waypoint(actor_location).transform.location + actor_location = self._map.get_waypoint( + actor_location + ).transform.location osc_location = self._map.get_waypoint(osc_location).transform.location distance = calculate_distance(actor_location, osc_location, self._grp) @@ -222,7 +247,6 @@ def update(self): class InTimeToArrivalToOSCPosition(AtomicCondition): - """ OpenSCENARIO atomic This class contains a trigger if an actor arrives within a given time to an OpenSCENARIO position @@ -236,8 +260,15 @@ class InTimeToArrivalToOSCPosition(AtomicCondition): The condition terminates with SUCCESS, when the actor can reach the position within the given time """ - def __init__(self, actor, osc_position, time, along_route=False, - comparison_operator=operator.lt, name="InTimeToArrivalToOSCPosition"): + def __init__( + self, + actor, + osc_position, + time, + along_route=False, + comparison_operator=operator.lt, + name="InTimeToArrivalToOSCPosition", + ): """ Setup parameters """ @@ -268,7 +299,8 @@ def update(self): # calculate transform with method in openscenario_parser.py try: osc_transform = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( - self._osc_position) + self._osc_position + ) except AttributeError: return py_trees.common.Status.FAILURE target_location = osc_transform.location @@ -292,7 +324,7 @@ def update(self): elif distance == 0: time_to_arrival = 0 else: - time_to_arrival = float('inf') + time_to_arrival = float("inf") if self._comparison_operator(time_to_arrival, self._time): new_status = py_trees.common.Status.SUCCESS @@ -300,7 +332,6 @@ def update(self): class StandStill(AtomicCondition): - """ This class contains a standstill behavior of a scenario @@ -344,13 +375,14 @@ def update(self): if GameTime.get_time() - self._start_time > self._duration: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class RelativeVelocityToOtherActor(AtomicCondition): - """ Atomic containing a comparison between an actor's velocity and another actor's one. The behavior returns SUCCESS when the @@ -364,8 +396,14 @@ class RelativeVelocityToOtherActor(AtomicCondition): comparison_operator: Type "operator", used to compare the two velocities """ - def __init__(self, actor, other_actor, speed, comparison_operator=operator.gt, - name="RelativeVelocityToOtherActor"): + def __init__( + self, + actor, + other_actor, + speed, + comparison_operator=operator.gt, + name="RelativeVelocityToOtherActor", + ): """ Setup the parameters """ @@ -394,13 +432,14 @@ def update(self): if self._comparison_operator(relative_speed, self._relative_speed): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class TriggerVelocity(AtomicCondition): - """ Atomic containing a comparison between an actor's speed and a reference one. The behavior returns SUCCESS when the expected comparison (greater than / @@ -413,7 +452,13 @@ class TriggerVelocity(AtomicCondition): comparison_operator: Type "operator", used to compare the two velocities """ - def __init__(self, actor, target_velocity, comparison_operator=operator.gt, name="TriggerVelocity"): + def __init__( + self, + actor, + target_velocity, + comparison_operator=operator.gt, + name="TriggerVelocity", + ): """ Setup the atomic parameters """ @@ -438,13 +483,14 @@ def update(self): if self._comparison_operator(actor_speed, self._target_velocity): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class TriggerAcceleration(AtomicCondition): - """ Atomic containing a comparison between an actor's acceleration and a reference one. The behavior returns SUCCESS when the @@ -457,7 +503,13 @@ class TriggerAcceleration(AtomicCondition): comparison_operator (operator): Type "operator", used to compare the two acceleration """ - def __init__(self, actor, target_acceleration, comparison_operator=operator.gt, name="TriggerAcceleration"): + def __init__( + self, + actor, + target_acceleration, + comparison_operator=operator.gt, + name="TriggerAcceleration", + ): """ Setup trigger acceleration """ @@ -478,20 +530,23 @@ def update(self): new_status = py_trees.common.Status.RUNNING acceleration = self._actor.get_acceleration() - linear_accel = math.sqrt(math.pow(acceleration.x, 2) + - math.pow(acceleration.y, 2) + - math.pow(acceleration.z, 2)) + linear_accel = math.sqrt( + math.pow(acceleration.x, 2) + + math.pow(acceleration.y, 2) + + math.pow(acceleration.z, 2) + ) if self._comparison_operator(linear_accel, self._target_acceleration): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class TimeOfDayComparison(AtomicCondition): - """ Atomic containing a comparison between the current time of day of the simulation and a given one. The behavior returns SUCCESS when the @@ -504,9 +559,10 @@ class TimeOfDayComparison(AtomicCondition): comparison_operator (operator): Type "operator", used to compare the two acceleration """ - def __init__(self, dattime, comparison_operator=operator.gt, name="TimeOfDayComparison"): - """ - """ + def __init__( + self, dattime, comparison_operator=operator.gt, name="TimeOfDayComparison" + ): + """ """ super(TimeOfDayComparison, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._datetime = datetime.datetime.strptime(dattime, "%Y-%m-%dT%H:%M:%S") @@ -531,13 +587,14 @@ def update(self): if self._comparison_operator(dtime, self._datetime): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class OSCStartEndCondition(AtomicCondition): - """ This class contains a check if a named story element has started/terminated. @@ -575,18 +632,21 @@ def update(self): new_status = py_trees.common.Status.RUNNING if new_status == py_trees.common.Status.RUNNING: - blackboard_variable_name = "({}){}-{}".format(self._element_type, self._element_name, self._rule) + blackboard_variable_name = "({}){}-{}".format( + self._element_type, self._element_name, self._rule + ) element_start_time = self._blackboard.get(blackboard_variable_name) if element_start_time and element_start_time >= self._start_time: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerRegion(AtomicCondition): - """ This class contains the trigger region (condition) of a scenario @@ -622,18 +682,20 @@ def update(self): if location is None: return new_status - not_in_region = (location.x < self._min_x or location.x > self._max_x) or \ - (location.y < self._min_y or location.y > self._max_y) + not_in_region = (location.x < self._min_x or location.x > self._max_x) or ( + location.y < self._min_y or location.y > self._max_y + ) if not not_in_region: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerDistanceToVehicle(AtomicCondition): - """ This class contains the trigger distance (condition) between to actors of a scenario @@ -650,8 +712,16 @@ class InTriggerDistanceToVehicle(AtomicCondition): The condition terminates with SUCCESS, when the actor reached the target distance to the other actor """ - def __init__(self, reference_actor, actor, distance, comparison_operator=operator.lt, - distance_type="cartesianDistance", freespace=False, name="TriggerDistanceToVehicle"): + def __init__( + self, + reference_actor, + actor, + distance, + comparison_operator=operator.lt, + distance_type="cartesianDistance", + freespace=False, + name="TriggerDistanceToVehicle", + ): """ Setup trigger distance """ @@ -682,18 +752,24 @@ def update(self): return new_status distance = get_distance_between_actors( - self._actor, self._reference_actor, self._distance_type, self._freespace, self._global_rp) + self._actor, + self._reference_actor, + self._distance_type, + self._freespace, + self._global_rp, + ) if self._comparison_operator(distance, self._distance): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerDistanceToLocation(AtomicCondition): - """ This class contains the trigger (condition) for a distance to a fixed location of a scenario @@ -707,12 +783,14 @@ class InTriggerDistanceToLocation(AtomicCondition): The condition terminates with SUCCESS, when the actor reached the target distance to the given location """ - def __init__(self, - actor, - target_location, - distance, - comparison_operator=operator.lt, - name="InTriggerDistanceToLocation"): + def __init__( + self, + actor, + target_location, + distance, + comparison_operator=operator.lt, + name="InTriggerDistanceToLocation", + ): """ Setup trigger distance """ @@ -735,17 +813,19 @@ def update(self): if location is None: return new_status - if self._comparison_operator(calculate_distance( - location, self._target_location), self._distance): + if self._comparison_operator( + calculate_distance(location, self._target_location), self._distance + ): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerDistanceToNextIntersection(AtomicCondition): - """ This class contains the trigger (condition) for a distance to the next intersection of a scenario @@ -780,19 +860,24 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - current_waypoint = self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)) - distance = calculate_distance(current_waypoint.transform.location, self._final_location) + current_waypoint = self._map.get_waypoint( + CarlaDataProvider.get_location(self._actor) + ) + distance = calculate_distance( + current_waypoint.transform.location, self._final_location + ) if distance < self._distance: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTriggerDistanceToLocationAlongRoute(AtomicCondition): - """ Implementation for a behavior that will check if a given actor is within a given distance to a given location considering a given route @@ -808,7 +893,14 @@ class InTriggerDistanceToLocationAlongRoute(AtomicCondition): along its route to the given location """ - def __init__(self, actor, route, location, distance, name="InTriggerDistanceToLocationAlongRoute"): + def __init__( + self, + actor, + route, + location, + distance, + name="InTriggerDistanceToLocationAlongRoute", + ): """ Setup class members """ @@ -819,7 +911,9 @@ def __init__(self, actor, route, location, distance, name="InTriggerDistanceToLo self._route = route self._distance = distance - self._location_distance, _ = get_distance_along_route(self._route, self._location) + self._location_distance, _ = get_distance_along_route( + self._route, self._location + ) def update(self): new_status = py_trees.common.Status.RUNNING @@ -830,20 +924,19 @@ def update(self): return new_status if current_location.distance(self._location) < self._distance + 20: - actor_distance, _ = get_distance_along_route(self._route, current_location) # If closer than self._distance and hasn't passed the trigger point - if (self._location_distance < actor_distance + self._distance and - actor_distance < self._location_distance) or \ - self._location_distance < 1.0: + if ( + self._location_distance < actor_distance + self._distance + and actor_distance < self._location_distance + ) or self._location_distance < 1.0: new_status = py_trees.common.Status.SUCCESS return new_status class InTimeToArrivalToLocation(AtomicCondition): - """ This class contains a check if a actor arrives within a given time at a given location. @@ -857,9 +950,16 @@ class InTimeToArrivalToLocation(AtomicCondition): The condition terminates with SUCCESS, when the actor can reach the target location within the given time """ - _max_time_to_arrival = float('inf') # time to arrival in seconds + _max_time_to_arrival = float("inf") # time to arrival in seconds - def __init__(self, actor, time, location, comparison_operator=operator.lt, name="TimeToArrival"): + def __init__( + self, + actor, + time, + location, + comparison_operator=operator.lt, + name="TimeToArrival", + ): """ Setup parameters """ @@ -892,13 +992,14 @@ def update(self): if self._comparison_operator(time_to_arrival, self._time): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTimeToArrivalToVehicle(AtomicCondition): - """ This class contains a check if a actor arrives within a given time at another actor. @@ -912,10 +1013,18 @@ class InTimeToArrivalToVehicle(AtomicCondition): The condition terminates with SUCCESS, when the actor can reach the other vehicle within the given time """ - _max_time_to_arrival = float('inf') # time to arrival in seconds + _max_time_to_arrival = float("inf") # time to arrival in seconds - def __init__(self, actor, other_actor, time, condition_freespace=False, - along_route=False, comparison_operator=operator.lt, name="TimeToArrival"): + def __init__( + self, + actor, + other_actor, + time, + condition_freespace=False, + along_route=False, + comparison_operator=operator.lt, + name="TimeToArrival", + ): """ Setup parameters """ @@ -966,7 +1075,9 @@ def update(self): if self._along_route: # Global planner needs a location at a driving lane - current_location = self._map.get_waypoint(current_location).transform.location + current_location = self._map.get_waypoint( + current_location + ).transform.location other_location = self._map.get_waypoint(other_location).transform.location distance = calculate_distance(current_location, other_location, self._grp) @@ -976,20 +1087,23 @@ def update(self): if current_velocity > other_velocity: if self._condition_freespace: - time_to_arrival = (distance - actor_extent - other_extent) / (current_velocity - other_velocity) + time_to_arrival = (distance - actor_extent - other_extent) / ( + current_velocity - other_velocity + ) else: time_to_arrival = distance / (current_velocity - other_velocity) if self._comparison_operator(time_to_arrival, self._time): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class InTimeToArrivalToVehicleSideLane(InTimeToArrivalToLocation): - """ This class contains a check if a actor arrives within a given time at another actor's side lane. Inherits from InTimeToArrivalToLocation @@ -1004,10 +1118,17 @@ class InTimeToArrivalToVehicleSideLane(InTimeToArrivalToLocation): The condition terminates with SUCCESS, when the actor can reach the other vehicle within the given time """ - _max_time_to_arrival = float('inf') # time to arrival in seconds + _max_time_to_arrival = float("inf") # time to arrival in seconds - def __init__(self, actor, other_actor, time, side_lane, - comparison_operator=operator.lt, name="InTimeToArrivalToVehicleSideLane"): + def __init__( + self, + actor, + other_actor, + time, + side_lane, + comparison_operator=operator.lt, + name="InTimeToArrivalToVehicleSideLane", + ): """ Setup parameters """ @@ -1020,9 +1141,9 @@ def __init__(self, actor, other_actor, time, side_lane, other_location = other_actor.get_transform().location other_waypoint = self._map.get_waypoint(other_location) - if self._side_lane == 'right': + if self._side_lane == "right": other_side_waypoint = other_waypoint.get_left_lane() - elif self._side_lane == 'left': + elif self._side_lane == "left": other_side_waypoint = other_waypoint.get_right_lane() else: raise Exception("cut_in_lane must be either 'left' or 'right'") @@ -1030,7 +1151,8 @@ def __init__(self, actor, other_actor, time, side_lane, other_side_location = other_side_waypoint.transform.location super(InTimeToArrivalToVehicleSideLane, self).__init__( - actor, time, other_side_location, comparison_operator, name) + actor, time, other_side_location, comparison_operator, name + ) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): @@ -1042,9 +1164,9 @@ def update(self): other_location = CarlaDataProvider.get_location(self._other_actor) other_waypoint = self._map.get_waypoint(other_location) - if self._side_lane == 'right': + if self._side_lane == "right": other_side_waypoint = other_waypoint.get_left_lane() - elif self._side_lane == 'left': + elif self._side_lane == "left": other_side_waypoint = other_waypoint.get_right_lane() else: raise Exception("cut_in_lane must be either 'left' or 'right'") @@ -1062,7 +1184,6 @@ def update(self): class WaitUntilInFront(AtomicCondition): - """ Behavior that support the creation of cut ins. It waits until the actor has passed another actor @@ -1074,7 +1195,9 @@ class WaitUntilInFront(AtomicCondition): 1: The front of the other_actor and the back of the actor are right next to each other """ - def __init__(self, actor, other_actor, factor=1, check_distance=True, name="WaitUntilInFront"): + def __init__( + self, actor, other_actor, factor=1, check_distance=True, name="WaitUntilInFront" + ): """ Init """ @@ -1128,7 +1251,10 @@ def update(self): # Wait for it to be close-by if not self._check_distance: close_by = True - elif actor_location.distance(other_next_waypoint.transform.location) < self._distance: + elif ( + actor_location.distance(other_next_waypoint.transform.location) + < self._distance + ): close_by = True if in_front and close_by: @@ -1138,7 +1264,6 @@ def update(self): class WaitUntilInFrontPosition(AtomicCondition): - """ Behavior that support the creation of cut ins. It waits until the actor has passed another actor @@ -1147,7 +1272,14 @@ class WaitUntilInFrontPosition(AtomicCondition): - transform: the reference transform that the actor will have to get in front of """ - def __init__(self, actor, transform, check_distance=True, distance=10, name="WaitUntilInFrontPosition"): + def __init__( + self, + actor, + transform, + check_distance=True, + distance=10, + name="WaitUntilInFrontPosition", + ): """ Init """ @@ -1177,7 +1309,10 @@ def update(self): in_front = actor_dir.dot(self._ref_vector) > 0.0 # Is the actor close-by? - if not self._check_distance or location.distance(self._ref_location) < self._distance: + if ( + not self._check_distance + or location.distance(self._ref_location) < self._distance + ): close_by = True else: close_by = False @@ -1189,7 +1324,6 @@ def update(self): class DriveDistance(AtomicCondition): - """ This class contains an atomic behavior to drive a certain distance. @@ -1231,12 +1365,13 @@ def update(self): if self._distance > self._target_distance: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class AtRightmostLane(AtomicCondition): - """ This class contains an atomic behavior to check if the actor is at the rightest driving lane. @@ -1273,12 +1408,13 @@ def update(self): if lane_type != carla.LaneType.Driving: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class WaitForTrafficLightState(AtomicCondition): - """ This class contains an atomic behavior to wait for a given traffic light to have the desired state. @@ -1311,13 +1447,14 @@ def update(self): if self._actor.state == self._state: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class WaitForTrafficLightControllerState(AtomicCondition): - """ This class contains an atomic behavior to wait for a given traffic light to have the desired state. @@ -1329,8 +1466,15 @@ class WaitForTrafficLightControllerState(AtomicCondition): The condition terminates with SUCCESS, when the traffic light switches to the desired state """ - def __init__(self, traffic_signal_id, state, duration, delay=None, ref_id=None, - name="WaitForTrafficLightControllerState"): + def __init__( + self, + traffic_signal_id, + state, + duration, + delay=None, + ref_id=None, + name="WaitForTrafficLightControllerState", + ): """ Init """ @@ -1346,8 +1490,9 @@ def __init__(self, traffic_signal_id, state, duration, delay=None, ref_id=None, self.logger.debug("%s.__init__()" % self.__class__.__name__) def initialise(self): - - self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id(self.actor_id) + self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id( + self.actor_id + ) if self._actor is None: return py_trees.common.Status.FAILURE @@ -1375,23 +1520,29 @@ def update(self): if self._start_time is None: self._start_time = GameTime.get_time() - if self._actor.state == self._state and GameTime.get_time() - self._start_time >= self.duration_time: + if ( + self._actor.state == self._state + and GameTime.get_time() - self._start_time >= self.duration_time + ): new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class WaitEndIntersection(AtomicCondition): - """ Atomic behavior that waits until the vehicles has gone outside the junction. If currently inside no intersection, it will wait until one is found. If 'junction_id' is given, it will wait until that specific junction has finished """ - def __init__(self, actor, junction_id=None, debug=False, name="WaitEndIntersection"): + def __init__( + self, actor, junction_id=None, debug=False, name="WaitEndIntersection" + ): super(WaitEndIntersection, self).__init__(name) self.actor = actor self.debug = debug @@ -1400,7 +1551,6 @@ def __init__(self, actor, junction_id=None, debug=False, name="WaitEndIntersecti self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): - new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self.actor) @@ -1424,7 +1574,6 @@ def update(self): class WaitForBlackboardVariable(AtomicCondition): - """ Atomic behavior that keeps running until the blackboard variable is set to the corresponding value. Used to avoid returning FAILURE if the blackboard comparison fails. @@ -1432,8 +1581,14 @@ class WaitForBlackboardVariable(AtomicCondition): It also initially sets the variable to a given value, if given """ - def __init__(self, variable_name, variable_value, var_init_value=None, - debug=False, name="WaitForBlackboardVariable"): + def __init__( + self, + variable_name, + variable_value, + var_init_value=None, + debug=False, + name="WaitForBlackboardVariable", + ): super(WaitForBlackboardVariable, self).__init__(name) self._debug = debug self._variable_name = variable_name @@ -1445,7 +1600,6 @@ def __init__(self, variable_name, variable_value, var_init_value=None, _ = blackboard.set(variable_name, var_init_value) def update(self): - new_status = py_trees.common.Status.RUNNING blackv = py_trees.blackboard.Blackboard() @@ -1464,7 +1618,14 @@ class CheckParameter(AtomicCondition): The condition terminates with SUCCESS, when the comparison_operator is evaluated successfully. """ - def __init__(self, parameter_ref, value, comparison_operator, debug=False, name="CheckParameter"): + def __init__( + self, + parameter_ref, + value, + comparison_operator, + debug=False, + name="CheckParameter", + ): super(CheckParameter, self).__init__(name) self._parameter_ref = parameter_ref self._value = value @@ -1476,7 +1637,9 @@ def update(self): keeps comparing global osc value with given value till it is successful. """ new_status = py_trees.common.Status.RUNNING - current_value = CarlaDataProvider.get_osc_global_param_value(self._parameter_ref) + current_value = CarlaDataProvider.get_osc_global_param_value( + self._parameter_ref + ) if self._comparison_operator(current_value, self._value): new_status = py_trees.common.Status.SUCCESS diff --git a/srunner/scenariomanager/timer.py b/srunner/scenariomanager/timer.py index de5d202..cb89cb6 100644 --- a/srunner/scenariomanager/timer.py +++ b/srunner/scenariomanager/timer.py @@ -18,7 +18,6 @@ class GameTime(object): - """ This (static) class provides access to the CARLA game time. @@ -86,7 +85,6 @@ def get_frame(): class SimulationTimeCondition(py_trees.behaviour.Behaviour): - """ This class contains an atomic simulation time condition behavior. It uses the CARLA game time, not the system time which is used by @@ -95,7 +93,9 @@ class SimulationTimeCondition(py_trees.behaviour.Behaviour): Returns, if the provided rule was successfully evaluated """ - def __init__(self, timeout, comparison_operator=operator.gt, name="SimulationTimeCondition"): + def __init__( + self, timeout, comparison_operator=operator.gt, name="SimulationTimeCondition" + ): """ Setup timeout """ @@ -126,13 +126,14 @@ def update(self): else: new_status = py_trees.common.Status.SUCCESS - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status class TimeOut(SimulationTimeCondition): - """ This class contains an atomic timeout behavior. It uses the CARLA game time, not the system time which is used by @@ -164,10 +165,18 @@ class RouteTimeoutBehavior(py_trees.behaviour.Behaviour): Behavior responsible of the route's timeout. With an initial value, it increases every time the agent advanced through the route, and is dependent on the road's speed. """ + MIN_TIMEOUT = 300 TIMEOUT_ROUTE_PERC = 10 - def __init__(self, ego_vehicle, route, debug=False, name="RouteTimeoutBehavior"): + def __init__( + self, + ego_vehicle, + route, + min_timeout=None, + debug=False, + name="RouteTimeoutBehavior", + ): """ Setup timeout """ @@ -178,7 +187,9 @@ def __init__(self, ego_vehicle, route, debug=False, name="RouteTimeoutBehavior") self._debug = debug self._start_time = None - self._timeout_value = self.MIN_TIMEOUT + self._timeout_value = ( + self.MIN_TIMEOUT if min_timeout is None else float(min_timeout) + ) self.timeout = False # Route variables @@ -219,7 +230,10 @@ def update(self): new_index = self._current_index - for index in range(self._current_index, min(self._current_index + self._wsize + 1, self._route_length)): + for index in range( + self._current_index, + min(self._current_index + self._wsize + 1, self._route_length), + ): route_transform = self._route_transforms[index] route_veh_vec = ego_location - route_transform.location if route_veh_vec.dot(route_transform.get_forward_vector()) > 0: @@ -227,7 +241,10 @@ def update(self): # Update the timeout value if new_index > self._current_index: - dist = self._route_accum_meters[new_index] - self._route_accum_meters[self._current_index] + dist = ( + self._route_accum_meters[new_index] + - self._route_accum_meters[self._current_index] + ) max_speed = self._ego_vehicle.get_speed_limit() / 3.6 timeout_speed = max_speed * self.TIMEOUT_ROUTE_PERC / 100 self._timeout_value += dist / timeout_speed @@ -238,6 +255,8 @@ def update(self): new_status = py_trees.common.Status.SUCCESS self.timeout = True - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + self.logger.debug( + "%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status) + ) return new_status diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index d7fe1bd..3b49692 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -11,7 +11,6 @@ class TrafficEventType(Enum): - """ This enum represents different traffic events that occur during driving. """ @@ -36,7 +35,6 @@ class TrafficEventType(Enum): class TrafficEvent(object): - """ TrafficEvent definition """ @@ -46,7 +44,7 @@ def __init__(self, event_type, frame, message="", dictionary=None): Initialize object :param event_type: TrafficEventType defining the type of traffic event - :param frame: frame in which the event happened + :param frame: frame in which the event happened :param message: optional message to inform users of the event :param dictionary: optional dictionary with arbitrary keys and values """ diff --git a/srunner/scenariomanager/watchdog.py b/srunner/scenariomanager/watchdog.py index 26e5b5e..14423bc 100644 --- a/srunner/scenariomanager/watchdog.py +++ b/srunner/scenariomanager/watchdog.py @@ -9,9 +9,11 @@ This module provides a simple watchdog timer to detect timeouts It is for example used in the ScenarioManager """ + from __future__ import print_function import simple_watchdog_timer as swt + try: import thread except ImportError: @@ -36,7 +38,9 @@ def __init__(self, timeout=1.0, interval=None): """Class constructor""" self._watchdog = None self._timeout = timeout + 1.0 - self._interval = min(interval if interval is not None else self._timeout / 100, 1.0) + self._interval = min( + interval if interval is not None else self._timeout / 100, 1.0 + ) self._failed = False self._watchdog_stopped = False @@ -45,7 +49,7 @@ def start(self): self._watchdog = swt.WDT( check_interval_sec=self._interval, trigger_delta_sec=self._timeout, - callback=self._callback + callback=self._callback, ) def stop(self): @@ -77,7 +81,9 @@ def _callback(self, watchdog): """Method called when the timer triggers. Raises a KeyboardInterrupt on the main thread and stops the watchdog.""" self.pause() # Good practice to stop it after the event occurs - print('Watchdog exception - Timeout of {} seconds occured'.format(self._timeout)) + print( + "Watchdog exception - Timeout of {} seconds occured".format(self._timeout) + ) self._failed = True thread.interrupt_main() diff --git a/srunner/scenarios/junction_crossing_route.py b/srunner/scenarios/junction_crossing_route.py new file mode 100644 index 0000000..ac61acc --- /dev/null +++ b/srunner/scenarios/junction_crossing_route.py @@ -0,0 +1,110 @@ +#!/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 . + +""" +All intersection related scenarios that are part of a route. +""" + +from __future__ import print_function + +import py_trees + +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ( + TrafficLightManipulator, +) + +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import ( + DriveDistance, +) +from srunner.scenarios.basic_scenario import BasicScenario + + +class SignalJunctionCrossingRoute(BasicScenario): + """ + At routes, these scenarios are simplified, as they can be triggered making + use of the background activity. To ensure interactions with this background + activity, the traffic lights are modified, setting two of them to green + """ + + # ego vehicle parameters + _ego_max_velocity_allowed = 20 # Maximum allowed velocity [m/s] + _ego_expected_driven_distance = 50 # Expected driven distance [m] + _ego_distance_to_drive = 20 # Allowed distance to drive + + _traffic_light = None + + # Depending on the route, decide which traffic lights can be modified + + def __init__( + self, + world, + ego_vehicles, + config, + randomize=False, + debug_mode=False, + criteria_enable=True, + timeout=180, + ): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + # Timeout of scenario in seconds + self.timeout = timeout + self.subtype = config.subtype + + super(SignalJunctionCrossingRoute, self).__init__( + "SignalJunctionCrossingRoute", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable, + ) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + + def _create_behavior(self): + """ + Scenario behavior: + When close to an intersection, the traffic lights will turn green for + both the ego_vehicle and another lane, allowing the background activity + to "run" their red light, creating scenarios 7, 8 and 9. + + If this does not happen within 120 seconds, a timeout stops the scenario + """ + + # Changes traffic lights + traffic_hack = TrafficLightManipulator(self.ego_vehicles[0], self.subtype) + + # finally wait that ego vehicle drove a specific distance + wait = DriveDistance(self.ego_vehicles[0], self._ego_distance_to_drive) + + # Build behavior tree + sequence = py_trees.composites.Sequence("SignalJunctionCrossingRoute") + sequence.add_child(traffic_hack) + sequence.add_child(wait) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + criteria = [] + return criteria + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self._traffic_light = None + self.remove_all_actors() diff --git a/srunner/scenarios/no_signal_junction_crossing.py b/srunner/scenarios/no_signal_junction_crossing.py index 0add829..02c28a1 100644 --- a/srunner/scenarios/no_signal_junction_crossing.py +++ b/srunner/scenarios/no_signal_junction_crossing.py @@ -15,18 +15,23 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, - ActorDestroy, - SyncArrival, - KeepVelocity, - StopVehicle) +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ( + ActorTransformSetter, + ActorDestroy, + SyncArrival, + KeepVelocity, + StopVehicle, +) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerRegion, DriveDistance, WaitEndIntersection +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import ( + InTriggerRegion, + DriveDistance, + WaitEndIntersection, +) from srunner.scenarios.basic_scenario import BasicScenario class NoSignalJunctionCrossing(BasicScenario): - """ Implementation class for 'Non-signalized junctions: crossing negotiation' scenario, @@ -43,8 +48,16 @@ class NoSignalJunctionCrossing(BasicScenario): _other_actor_max_brake = 1.0 _other_actor_target_velocity = 15 - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, - timeout=60): + def __init__( + self, + world, + ego_vehicles, + config, + randomize=False, + debug_mode=False, + criteria_enable=True, + timeout=60, + ): """ Setup all relevant parameters and create scenario """ @@ -53,12 +66,14 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals # Timeout of scenario in seconds self.timeout = timeout - super(NoSignalJunctionCrossing, self).__init__("NoSignalJunctionCrossing", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super(NoSignalJunctionCrossing, self).__init__( + "NoSignalJunctionCrossing", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable, + ) def _initialize_actors(self, config): """ @@ -66,11 +81,16 @@ def _initialize_actors(self, config): """ self._other_actor_transform = config.other_actors[0].transform first_vehicle_transform = carla.Transform( - carla.Location(config.other_actors[0].transform.location.x, - config.other_actors[0].transform.location.y, - config.other_actors[0].transform.location.z - 500), - config.other_actors[0].transform.rotation) - first_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, first_vehicle_transform) + carla.Location( + config.other_actors[0].transform.location.x, + config.other_actors[0].transform.location.y, + config.other_actors[0].transform.location.z - 500, + ), + config.other_actors[0].transform.rotation, + ) + first_vehicle = CarlaDataProvider.request_new_actor( + config.other_actors[0].model, first_vehicle_transform + ) first_vehicle.set_simulate_physics(enabled=False) self.other_actors.append(first_vehicle) @@ -87,50 +107,43 @@ def _create_behavior(self): """ # Creating leaf nodes - start_other_trigger = InTriggerRegion( - self.ego_vehicles[0], - -80, -70, - -75, -60) + start_other_trigger = InTriggerRegion(self.ego_vehicles[0], -80, -70, -75, -60) sync_arrival = SyncArrival( - self.other_actors[0], self.ego_vehicles[0], - carla.Location(x=-74.63, y=-136.34)) + self.other_actors[0], + self.ego_vehicles[0], + carla.Location(x=-74.63, y=-136.34), + ) pass_through_trigger = InTriggerRegion( - self.ego_vehicles[0], - -90, -70, - -124, -119) + self.ego_vehicles[0], -90, -70, -124, -119 + ) keep_velocity_other = KeepVelocity( - self.other_actors[0], - self._other_actor_target_velocity) + self.other_actors[0], self._other_actor_target_velocity + ) - stop_other_trigger = InTriggerRegion( - self.other_actors[0], - -45, -35, - -140, -130) + stop_other_trigger = InTriggerRegion(self.other_actors[0], -45, -35, -140, -130) - stop_other = StopVehicle( - self.other_actors[0], - self._other_actor_max_brake) + stop_other = StopVehicle(self.other_actors[0], self._other_actor_max_brake) - end_condition = InTriggerRegion( - self.ego_vehicles[0], - -90, -70, - -170, -156 - ) + end_condition = InTriggerRegion(self.ego_vehicles[0], -90, -70, -170, -156) # Creating non-leaf nodes root = py_trees.composites.Sequence() scenario_sequence = py_trees.composites.Sequence() sync_arrival_parallel = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE + ) keep_velocity_other_parallel = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE + ) # Building tree root.add_child(scenario_sequence) - scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) + scenario_sequence.add_child( + ActorTransformSetter(self.other_actors[0], self._other_actor_transform) + ) scenario_sequence.add_child(start_other_trigger) scenario_sequence.add_child(sync_arrival_parallel) scenario_sequence.add_child(keep_velocity_other_parallel) @@ -165,15 +178,22 @@ def __del__(self): class NoSignalJunctionCrossingRoute(BasicScenario): - """ At routes, these scenarios are simplified, as they can be triggered making use of the background activity. For unsignalized intersections, just wait until the ego_vehicle has left the intersection. """ - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, - timeout=180): + def __init__( + self, + world, + ego_vehicles, + config, + randomize=False, + debug_mode=False, + criteria_enable=True, + timeout=180, + ): """ Setup all relevant parameters and create scenario and instantiate scenario manager @@ -182,12 +202,14 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._end_distance = 50 - super(NoSignalJunctionCrossingRoute, self).__init__("NoSignalJunctionCrossingRoute", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super(NoSignalJunctionCrossingRoute, self).__init__( + "NoSignalJunctionCrossingRoute", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable, + ) def _create_behavior(self): """ diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py deleted file mode 100644 index c7f58d0..0000000 --- a/srunner/scenarios/open_scenario.py +++ /dev/null @@ -1,617 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019-2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" -Basic scenario class using the OpenSCENARIO definition -""" - -from __future__ import print_function - -from distutils.util import strtobool -import itertools -import os -import py_trees - -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeWeather, ChangeRoadFriction, ChangeParameter, \ - ChangeActorLaneOffset, ChangeActorWaypoints, ChangeLateralDistance -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeActorControl, ChangeActorTargetSpeed -from srunner.scenariomanager.timer import GameTime -from srunner.scenariomanager.weather_sim import OSCWeatherBehavior -from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.openscenario_parser import OpenScenarioParser, oneshot_with_check, ParameterRef -from srunner.tools.py_trees_port import Decorator - - -def repeatable_behavior(behaviour, name=None): - """ - This behaviour allows a composite with oneshot ancestors to run multiple - times, resetting the oneshot variables after each execution - """ - if not name: - name = behaviour.name - clear_descendant_variables = ClearBlackboardVariablesStartingWith( - name="Clear Descendant Variables of {}".format(name), - variable_name_beginning=name + ">" - ) - # If it's a sequence, don't double-nest it in a redundant manner - if isinstance(behaviour, py_trees.composites.Sequence): - behaviour.add_child(clear_descendant_variables) - sequence = behaviour - else: - sequence = py_trees.composites.Sequence(name="RepeatableBehaviour of {}".format(name)) - sequence.add_children([behaviour, clear_descendant_variables]) - return sequence - - -class ClearBlackboardVariablesStartingWith(py_trees.behaviours.Success): - - """ - Clear the values starting with the specified string from the blackboard. - - Args: - name (:obj:`str`): name of the behaviour - variable_name_beginning (:obj:`str`): beginning of the names of variable to clear - """ - - def __init__(self, - name="Clear Blackboard Variable Starting With", - variable_name_beginning="dummy", - ): - super(ClearBlackboardVariablesStartingWith, self).__init__(name) - self.variable_name_beginning = variable_name_beginning - - def initialise(self): - """ - Delete the variables from the blackboard. - """ - blackboard_variables = [key for key, _ in py_trees.blackboard.Blackboard().__dict__.items( - ) if key.startswith(self.variable_name_beginning)] - for variable in blackboard_variables: - delattr(py_trees.blackboard.Blackboard(), variable) - - -class StoryElementStatusToBlackboard(Decorator): - - """ - Reflect the status of the decorator's child story element to the blackboard. - - Args: - child: the child behaviour or subtree - story_element_type: the element type [act,scene,maneuver,event,action] - element_name: the story element's name attribute - """ - - def __init__(self, child, story_element_type, element_name): - super(StoryElementStatusToBlackboard, self).__init__(name=child.name, child=child) - self.story_element_type = story_element_type - self.element_name = element_name - self.blackboard = py_trees.blackboard.Blackboard() - - def initialise(self): - """ - Record the elements's start time on the blackboard - """ - self.blackboard.set( - name="({}){}-{}".format(self.story_element_type.upper(), - self.element_name, "START"), - value=GameTime.get_time(), - overwrite=True - ) - - def update(self): - """ - Reflect the decorated child's status - Returns: the decorated child's status - """ - return self.decorated.status - - def terminate(self, new_status): - """ - Terminate and mark Blackboard entry with END - """ - # Report whether we ended with End or Cancel - # If we were ended or cancelled, our state will be INVALID and - # We will have an ancestor (a parallel SUCCESS_ON_ALL) with a successful child/children - # It's possible we ENDed AND CANCELled if both condition groups were true simultaneously - # NOTE 'py_trees.common.Status.INVALID' is the status of a behaviur which was terminated by a parent - rules = [] - if new_status == py_trees.common.Status.INVALID: - # We were terminated from above unnaturally - # Figure out if were ended or cancelled - terminating_ancestor = self.parent - while terminating_ancestor.status == py_trees.common.Status.INVALID: - terminating_ancestor = terminating_ancestor.parent - # We have found an ancestory which was not terminated by a parent - # Check what caused it to terminate its children - if terminating_ancestor.status == py_trees.common.Status.SUCCESS: - successful_children = [ - child.name - for child - in terminating_ancestor.children - if child.status == py_trees.common.Status.SUCCESS] - if "StopTrigger" in successful_children: - rules.append("END") - - # END is the default status unless we have a more detailed one - rules = rules or ["END"] - - for rule in rules: - self.blackboard.set( - name="({}){}-{}".format(self.story_element_type.upper(), - self.element_name, rule), - value=GameTime.get_time(), - overwrite=True - ) - - -def get_xml_path(tree, node): - """ - Extract the full path of a node within an XML tree - - Note: Catalogs are pulled from a separate file so the XML tree is split. - This means that in order to get the XML path, it must be done in 2 steps. - Some places in this python script do that by concatenating the results - of 2 get_xml_path calls with another ">". - Example: "Behavior>AutopilotSequence" + ">" + "StartAutopilot>StartAutopilot>StartAutopilot" - """ - - path = "" - parent_map = {c: p for p in tree.iter() for c in p} - - cur_node = node - while cur_node != tree: - path = "{}>{}".format(cur_node.attrib.get('name'), path) - cur_node = parent_map[cur_node] - - path = path[:-1] - return path - - -class OpenScenario(BasicScenario): - - """ - Implementation of the OpenSCENARIO scenario - """ - - def __init__(self, world, ego_vehicles, config, config_file, debug_mode=False, criteria_enable=True, timeout=300): - """ - Setup all relevant parameters and create scenario - """ - self.config = config - self.route = None - self.config_file = config_file - # Timeout of scenario in seconds - self.timeout = timeout - - super(OpenScenario, self).__init__(self.config.name, ego_vehicles=ego_vehicles, config=config, - world=world, debug_mode=debug_mode, - terminate_on_failure=False, criteria_enable=criteria_enable) - - def _initialize_parameters(self): - """ - Parse ParameterAction from Init and update global osc parameters. - """ - param_behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="ParametersInit") - for i, global_action in enumerate(self.config.init.find('Actions').iter('GlobalAction')): - maneuver_name = 'InitParams' - if global_action.find('ParameterAction') is not None: - parameter_action = global_action.find('ParameterAction') - parameter_ref = parameter_action.attrib.get('parameterRef') - if parameter_action.find('ModifyAction') is not None: - action_rule = parameter_action.find('ModifyAction').find("Rule") - if action_rule.find("AddValue") is not None: - rule, value = '+', action_rule.find("AddValue").attrib.get('value') - else: - rule, value = '*', action_rule.find("MultiplyByValue").attrib.get('value') - else: - rule, value = None, parameter_action.find('SetAction').attrib.get('value') - parameter_update = ChangeParameter(parameter_ref, value=ParameterRef(value), rule=rule, - name=maneuver_name + '_%d' % i) - param_behavior.add_child(oneshot_with_check(variable_name="InitialParameters" + '_%d' % i, - behaviour=parameter_update)) - - return param_behavior - - def _initialize_environment(self, world): - """ - Initialization of weather and road friction. - """ - pass - - def _create_environment_behavior(self): - # Set the appropriate weather conditions - - env_behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior") - - weather_update = ChangeWeather( - OpenScenarioParser.get_weather_from_env_action(self.config.init, self.config.catalogs)) - road_friction = ChangeRoadFriction( - OpenScenarioParser.get_friction_from_env_action(self.config.init, self.config.catalogs)) - env_behavior.add_child(oneshot_with_check(variable_name="InitialWeather", behaviour=weather_update)) - env_behavior.add_child(oneshot_with_check(variable_name="InitRoadFriction", behaviour=road_friction)) - - return env_behavior - - def _create_init_behavior(self): - - init_behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="InitBehaviour") - - actor_list = self.other_actors + self.ego_vehicles + [None] - - for actor in self.config.other_actors + self.config.ego_vehicles: - for carla_actor in self.other_actors + self.ego_vehicles: - if (carla_actor is not None and 'role_name' in carla_actor.attributes and - carla_actor.attributes['role_name'] == actor.rolename): - actor_init_behavior = py_trees.composites.Sequence(name="InitActor{}".format(actor.rolename)) - - controller_atomic = None - atomic = None - for private in self.config.init.iter("Private"): - if private.attrib.get('entityRef', None) == actor.rolename: - for private_action in private.iter("PrivateAction"): - if private_action.find('ControllerAction') is not None: - for controller_action in private_action.iter('ControllerAction'): - module, args = OpenScenarioParser.get_controller( - controller_action, self.config.catalogs) - controller_atomic = ChangeActorControl( - carla_actor, control_py_module=module, args=args, - scenario_file_path=os.path.dirname(self.config.filename)) - - elif private_action.find('LateralAction') is not None: - private_action = private_action.find('LateralAction') - if private_action.find('LaneOffsetAction') is not None: - lat_maneuver = private_action.find('LaneOffsetAction') - continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "true"))) - # Parsing of the different Dynamic shapes is missing - lane_target_offset = lat_maneuver.find('LaneOffsetTarget') - if lane_target_offset.find('AbsoluteTargetLaneOffset') is not None: - absolute_offset = ParameterRef( - lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', - 0)) - atomic = ChangeActorLaneOffset( - carla_actor, absolute_offset, continuous=continuous, - name='LaneOffsetAction') - - elif lane_target_offset.find('RelativeTargetLaneOffset') is not None: - relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset') - relative_offset = ParameterRef( - relative_target_offset.attrib.get('value', 0)) - relative_actor_name = relative_target_offset.attrib.get('entityRef', None) - relative_actor = None - for _actor in actor_list: - if _actor is not None and 'role_name' in _actor.attributes: - if relative_actor_name == _actor.attributes['role_name']: - relative_actor = _actor - break - if relative_actor is None: - raise AttributeError( - "Cannot find actor '{}' for condition".format(relative_actor_name)) - atomic = ChangeActorLaneOffset(carla_actor, relative_offset, relative_actor, - continuous=continuous, - name='LaneOffsetAction') - if private_action.find("LateralDistanceAction") is not None: - lat_maneuver = private_action.find('LateralDistanceAction') - maneuver_name = "LateralDistanceActionInit" - continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "false"))) - freespace = bool(strtobool(lat_maneuver.attrib.get('freespace', "false"))) - distance = ParameterRef(lat_maneuver.attrib.get('distance', float("inf"))) - constraints = lat_maneuver.find('DynamicConstraints') - max_speed = constraints.attrib.get('maxSpeed', - None) if constraints is not None else None - relative_actor = None - relative_actor_name = lat_maneuver.attrib.get('entityRef', None) - for _actor in actor_list: - if _actor is not None and 'role_name' in _actor.attributes: - if relative_actor_name == _actor.attributes['role_name']: - relative_actor = _actor - break - if relative_actor is None: - raise AttributeError( - "Cannot find actor '{}' for condition".format(relative_actor_name)) - atomic = ChangeLateralDistance(carla_actor, distance, relative_actor, - continuous=continuous, freespace=freespace, - name=maneuver_name) - - elif private_action.find('RoutingAction') is not None: - private_action = private_action.find('RoutingAction') - if private_action.find('AssignRouteAction') is not None: - route_action = private_action.find('AssignRouteAction') - waypoints = OpenScenarioParser.get_route(route_action, self.config.catalogs) - atomic = ChangeActorWaypoints(carla_actor, waypoints=waypoints, - name="AssignRouteAction") - elif private_action.find('FollowTrajectoryAction') is not None: - trajectory_action = private_action.find('FollowTrajectoryAction') - waypoints, times = OpenScenarioParser.get_trajectory(trajectory_action, - self.config.catalogs) - atomic = ChangeActorWaypoints(carla_actor, waypoints=list( - zip(waypoints, ['shortest'] * len(waypoints))), - times=times, name="FollowTrajectoryAction") - elif private_action.find('AcquirePositionAction') is not None: - route_action = private_action.find('AcquirePositionAction') - osc_position = route_action.find('Position') - waypoints = [(osc_position, 'fastest')] - atomic = ChangeActorWaypoints(carla_actor, waypoints=waypoints, - name="AcquirePositionAction") - - if controller_atomic is None: - controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={}) - actor_init_behavior.add_child(controller_atomic) - if atomic is not None: - actor_init_behavior.add_child(atomic) - - if actor.speed > 0: - actor_init_behavior.add_child(ChangeActorTargetSpeed(carla_actor, actor.speed, init_speed=True)) - - init_behavior.add_child(actor_init_behavior) - break - - return init_behavior - - def _create_behavior(self): - """ - Basic behavior do nothing, i.e. Idle - """ - - stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, - name="OSCStories") - joint_actor_list = self.other_actors + self.ego_vehicles + [None] - - for story in self.config.stories: - story_name = story.get("name") - story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, - name=story_name) - for act in story.iter("Act"): - - act_sequence = py_trees.composites.Sequence( - name="Act StartConditions and behaviours") - - start_conditions = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="StartConditions Group") - - parallel_behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Maneuver + EndConditions Group") - - parallel_sequences = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Maneuvers") - - for sequence in act.iter("ManeuverGroup"): - sequence_behavior = py_trees.composites.Sequence(name=sequence.attrib.get('name')) - repetitions = sequence.attrib.get('maximumExecutionCount', 1) - - for _ in range(int(repetitions)): - - actor_ids = [] - for actor in sequence.iter("Actors"): - for entity in actor.iter("EntityRef"): - entity_name = entity.attrib.get('entityRef', None) - for k, _ in enumerate(joint_actor_list): - if (joint_actor_list[k] and - entity_name == joint_actor_list[k].attributes['role_name']): - actor_ids.append(k) - break - - if not actor_ids: - print("Warning: Maneuvergroup {} does not use reference actors!".format( - sequence.attrib.get('name'))) - actor_ids.append(len(joint_actor_list) - 1) - - # Collect catalog reference maneuvers in order to process them at the same time as normal maneuvers - catalog_maneuver_list = [] - for catalog_reference in sequence.iter("CatalogReference"): - catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs, - catalog_reference) - catalog_maneuver_list.append(catalog_maneuver) - all_maneuvers = itertools.chain(iter(catalog_maneuver_list), sequence.iter("Maneuver")) - single_sequence_iteration = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=sequence_behavior.name) - for maneuver in all_maneuvers: # Iterates through both CatalogReferences and Maneuvers - maneuver_parallel = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, - name="Maneuver " + maneuver.attrib.get('name')) - for event in maneuver.iter("Event"): - event_sequence = py_trees.composites.Sequence( - name="Event " + event.attrib.get('name')) - parallel_actions = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Actions") - for child in event.iter(): - if child.tag == "Action": - for actor_id in actor_ids: - maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic( - child, joint_actor_list[actor_id], - joint_actor_list, self.config.catalogs, self.config) - maneuver_behavior = StoryElementStatusToBlackboard( - maneuver_behavior, "ACTION", child.attrib.get('name')) - parallel_actions.add_child( - oneshot_with_check(variable_name= # See note in get_xml_path - get_xml_path(story, sequence) + '>' + \ - get_xml_path(maneuver, child) + '>' + \ - str(actor_id), - behaviour=maneuver_behavior)) - - if child.tag == "StartTrigger": - # There is always one StartConditions block per Event - parallel_condition_groups = self._create_condition_container( - child, story, "Parallel Condition Groups", sequence, maneuver) - event_sequence.add_child( - parallel_condition_groups) - - parallel_actions = StoryElementStatusToBlackboard( - parallel_actions, "EVENT", event.attrib.get('name')) - event_sequence.add_child(parallel_actions) - maneuver_parallel.add_child( - oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' + - get_xml_path(maneuver, event), # See get_xml_path - behaviour=event_sequence)) - maneuver_parallel = StoryElementStatusToBlackboard( - maneuver_parallel, "MANEUVER", maneuver.attrib.get('name')) - single_sequence_iteration.add_child( - oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' + - maneuver.attrib.get('name'), # See get_xml_path - behaviour=maneuver_parallel)) - - # OpenSCENARIO refers to Sequences as Scenes in this instance - single_sequence_iteration = StoryElementStatusToBlackboard( - single_sequence_iteration, "SCENE", sequence.attrib.get('name')) - single_sequence_iteration = repeatable_behavior( - single_sequence_iteration, get_xml_path(story, sequence)) - - sequence_behavior.add_child(single_sequence_iteration) - - if sequence_behavior.children: - parallel_sequences.add_child( - oneshot_with_check(variable_name=get_xml_path(story, sequence), - behaviour=sequence_behavior)) - - if parallel_sequences.children: - parallel_sequences = StoryElementStatusToBlackboard( - parallel_sequences, "ACT", act.attrib.get('name')) - parallel_behavior.add_child(parallel_sequences) - - start_triggers = act.find("StartTrigger") - if list(start_triggers) is not None: - for start_condition in start_triggers: - parallel_start_criteria = self._create_condition_container(start_condition, - story, - "StartConditions") - if parallel_start_criteria.children: - start_conditions.add_child(parallel_start_criteria) - end_triggers = act.find("StopTrigger") - if end_triggers is not None and list(end_triggers) is not None: - for end_condition in end_triggers: - parallel_end_criteria = self._create_condition_container( - end_condition, story, "EndConditions", success_on_all=False) - if parallel_end_criteria.children: - parallel_behavior.add_child(parallel_end_criteria) - - if start_conditions.children: - act_sequence.add_child(start_conditions) - if parallel_behavior.children: - act_sequence.add_child(parallel_behavior) - - if act_sequence.children: - story_behavior.add_child(act_sequence) - - stories_behavior.add_child(oneshot_with_check(variable_name=get_xml_path(story, story) + '>' + - story_name, # See get_xml_path - behaviour=story_behavior)) - - # Build behavior tree - behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="behavior") - - init_parameters = self._initialize_parameters() - if init_parameters is not None: - behavior.add_child(oneshot_with_check(variable_name="InitialParameterSettings", behaviour=init_parameters)) - - env_behavior = self._create_environment_behavior() - if env_behavior is not None: - behavior.add_child(oneshot_with_check(variable_name="InitialEnvironmentSettings", behaviour=env_behavior)) - - init_behavior = self._create_init_behavior() - if init_behavior is not None: - behavior.add_child(oneshot_with_check(variable_name="InitialActorSettings", behaviour=init_behavior)) - - behavior.add_child(stories_behavior) - - return behavior - - def _create_weather_behavior(self): - """ - Sets the osc weather behavior, which will monitor other behaviors, changing the weather - """ - return OSCWeatherBehavior() - - def _create_condition_container(self, node, story, name='Conditions Group', sequence=None, - maneuver=None, success_on_all=True): - """ - This is a generic function to handle conditions utilising ConditionGroups - Each ConditionGroup is represented as a Sequence of Conditions - The ConditionGroups are grouped under a SUCCESS_ON_ONE Parallel - """ - - parallel_condition_groups = py_trees.composites.Parallel(name, - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - - for condition_group in node.iter("ConditionGroup"): - if success_on_all: - condition_group_sequence = py_trees.composites.Parallel( - name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) - else: - condition_group_sequence = py_trees.composites.Parallel( - name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - for condition in condition_group.iter("Condition"): - criterion = OpenScenarioParser.convert_condition_to_atomic( - condition, self.other_actors + self.ego_vehicles) - if sequence is not None and maneuver is not None: - xml_path = get_xml_path(story, sequence) + '>' + \ - get_xml_path(maneuver, condition) # See note in get_xml_path - else: - xml_path = get_xml_path(story, condition) - criterion = oneshot_with_check(variable_name=xml_path, behaviour=criterion) - condition_group_sequence.add_child(criterion) - - if condition_group_sequence.children: - parallel_condition_groups.add_child(condition_group_sequence) - - return parallel_condition_groups - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - parallel_criteria = py_trees.composites.Parallel("EndConditions (Criteria Group)", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - - criteria = [] - for endcondition in self.config.storyboard.iter("StopTrigger"): - for condition in endcondition.iter("Condition"): - if condition.attrib.get('name').startswith('criteria_'): - criteria.append(condition) - - for condition in criteria: - criterion = OpenScenarioParser.convert_condition_to_atomic(condition, self.ego_vehicles) - parallel_criteria.add_child(criterion) - - return parallel_criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() - - def _initialize_actors(self, config): - """ - Override the superclass method to initialize other actors - """ - if config.other_actors: - for global_action in self.config.init.find("Actions").iter("GlobalAction"): - if global_action.find("EntityAction") is not None: - entity_action = global_action.find("EntityAction") - entity_ref = entity_action.attrib.get("entityRef") - if entity_action.find('AddEntityAction') is not None: - position = entity_action.find('AddEntityAction').find("Position") - actor_transform = OpenScenarioParser.convert_position_to_transform( - position, actor_list=config.other_actors + config.ego_vehicles) - for actor in config.other_actors: - if actor.rolename == entity_ref: - actor.transform = actor_transform - elif entity_action.find('DeleteEntityAction') is not None: - for actor in config.other_actors: - if actor.rolename == entity_ref: - config.other_actors.remove(actor) - - new_actors = CarlaDataProvider.request_new_actors(config.other_actors) - if not new_actors: - raise Exception("Error: Unable to add actors") - for new_actor in new_actors: - self.other_actors.append(new_actor) diff --git a/srunner/scenarios/osc2_scenario.py b/srunner/scenarios/osc2_scenario.py deleted file mode 100644 index f36dc6a..0000000 --- a/srunner/scenarios/osc2_scenario.py +++ /dev/null @@ -1,1447 +0,0 @@ -from __future__ import print_function - -import copy -import math -import operator -import random -import re -import sys -from typing import List, Tuple - -import py_trees -from agents.navigation.global_route_planner import GlobalRoutePlanner - -from srunner.osc2.ast_manager import ast_node -from srunner.osc2.ast_manager.ast_vistor import ASTVisitor - -# OSC2 -from srunner.osc2.symbol_manager.method_symbol import MethodSymbol -from srunner.osc2.symbol_manager.parameter_symbol import ParameterSymbol -from srunner.osc2.utils.log_manager import (LOG_INFO, LOG_ERROR, LOG_WARNING) -from srunner.osc2.utils.relational_operator import RelationalOperator -from srunner.osc2_dm.physical_types import Physical, Range - -# from sqlalchemy import true -# from srunner.osc2_stdlib import event, variables -from srunner.osc2_stdlib.modifier import ( - AccelerationModifier, - ChangeLaneModifier, - ChangeSpeedModifier, - LaneModifier, - PositionModifier, - SpeedModifier, -) - -# OSC2 -from srunner.scenarioconfigs.osc2_scenario_configuration import ( - OSC2ScenarioConfiguration, -) -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ( - ActorTransformSetter, - ChangeTargetSpeed, - LaneChange, - UniformAcceleration, - WaypointFollower, - calculate_distance, -) -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import ( - IfTriggerer, - TimeOfWaitComparison, -) -from srunner.scenariomanager.timer import TimeOut -from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.openscenario_parser import oneshot_with_check -from srunner.tools.osc2_helper import OSC2Helper - - -def para_type_str_sequence(config, arguments, line, column, node): - retrieval_name = "" - if isinstance(arguments, List): - for arg in arguments: - if isinstance(arg, Tuple): - if isinstance(arg[1], int): - retrieval_name = retrieval_name + "#int" - elif isinstance(arg[1], float): - retrieval_name = retrieval_name + "#float" - elif isinstance(arg[1], str): - retrieval_name = retrieval_name + "#str" - elif isinstance(arg[1], bool): - retrieval_name = retrieval_name + "#bool" - elif isinstance(arg[1], Physical): - physical_type_name = OSC2Helper.find_physical_type( - config.physical_dict, arg[1].unit.physical.si_base_exponent - ) - - if physical_type_name is None: - pass - else: - physical_type = ( - node.get_scope().resolve(physical_type_name).name - ) - retrieval_name += "#" + physical_type - else: - pass - elif isinstance(arg, str): - retrieval_name = retrieval_name + arg.split(".", 1)[-1] - else: - pass - elif isinstance(arguments, Tuple): - if isinstance(arguments[1], int): - retrieval_name = retrieval_name + "#int" - elif isinstance(arguments[1], float): - retrieval_name = retrieval_name + "#float" - elif isinstance(arguments[1], str): - retrieval_name = retrieval_name + "#str" - elif isinstance(arguments[1], bool): - retrieval_name = retrieval_name + "#bool" - elif isinstance(arguments[1], Physical): - physical_type_name = OSC2Helper.find_physical_type( - config.physical_dict, arguments[1].unit.physical.si_base_exponent - ) - - if physical_type_name is None: - pass - else: - physical_type = node.get_scope().resolve(physical_type_name).name - retrieval_name += "#" + physical_type - else: - pass - elif isinstance(arguments, int): - retrieval_name = retrieval_name + "#int" - elif isinstance(arguments, float): - retrieval_name = retrieval_name + "#float" - elif isinstance(arguments, str): - retrieval_name = retrieval_name + "#str" - elif isinstance(arguments, bool): - retrieval_name = retrieval_name + "#bool" - elif isinstance(arguments, Physical): - physical_type_name = OSC2Helper.find_physical_type( - config.physical_dict, arguments.unit.physical.si_base_exponent - ) - - if physical_type_name is None: - pass - else: - physical_type = node.get_scope().resolve(physical_type_name).name - retrieval_name += "#" + physical_type - else: - pass - return retrieval_name - - -def process_speed_modifier( - config, modifiers, duration: float, all_duration: float, father_tree -): - if not modifiers: - return - - for modifier in modifiers: - actor_name = modifier.get_actor_name() - - if isinstance(modifier, SpeedModifier): - # en_value_mps() The speed unit in Carla is m/s, so the default conversion unit is m/s - target_speed = modifier.get_speed().gen_physical_value() - # target_speed = float(modifier.get_speed())*0.27777778 - actor = CarlaDataProvider.get_actor_by_name(actor_name) - car_driving = WaypointFollower(actor, target_speed) - # car_driving.set_duration(duration) - - father_tree.add_child(car_driving) - - car_config = config.get_car_config(actor_name) - car_config.set_arg({"target_speed": target_speed}) - LOG_WARNING( - f"{actor_name} car speed will be set to {target_speed * 3.6} km/h" - ) - - # # _velocity speed, go straight down the driveway, and will hit the wall - # keep_speed = KeepVelocity(actor, target_speed, duration=father_duration.num) - elif isinstance(modifier, ChangeSpeedModifier): - # speed_delta indicates the increment of velocity - speed_delta = modifier.get_speed().gen_physical_value() - speed_delta = speed_delta * 3.6 - current_car_conf = config.get_car_config(actor_name) - current_car_speed = current_car_conf.get_arg("target_speed") - current_car_speed = current_car_speed * 3.6 - target_speed = current_car_speed + speed_delta - LOG_WARNING( - f"{actor_name} car speed will be changed to {target_speed} km/h" - ) - - actor = CarlaDataProvider.get_actor_by_name(actor_name) - change_speed = ChangeTargetSpeed(actor, target_speed) - - car_driving = WaypointFollower(actor) - # car_driving.set_duration(duration) - - father_tree.add_child(change_speed) - father_tree.add_child(car_driving) - elif isinstance(modifier, AccelerationModifier): - current_car_conf = config.get_car_config(actor_name) - current_car_speed = current_car_conf.get_arg("target_speed") - accelerate_speed = modifier.get_accelerate().gen_physical_value() - target_velocity = current_car_speed + accelerate_speed * duration - actor = CarlaDataProvider.get_actor_by_name(actor_name) - start_time = all_duration - duration - uniform_accelerate_speed = UniformAcceleration( - actor, current_car_speed, target_velocity, accelerate_speed, start_time - ) - print("END ACCELERATION") - car_driving = WaypointFollower(actor) - - father_tree.add_child(uniform_accelerate_speed) - father_tree.add_child(car_driving) - else: - LOG_WARNING("not implement modifier") - - -def process_location_modifier(config, modifiers, duration: float, father_tree): - # position([distance: ] | time: