From c64b186c92bd1e24d92fc1b70e390a70d35e9d8a Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 16 Feb 2022 10:06:53 +0100 Subject: [PATCH 01/86] New parsing (#858) * Improved junction scenarios to work on routes * Added basic agent to the SyncArrival behavior * Improved all routes * Updated to use new route planner * Improved synchornization for S7 adn S9 * WIP: Added new background activity * Minor fixes * Planner changes * Added compatibility with CARLA 0.9.12 * Pretty code * Changed S9 to an actor flow * Improved synchronization of crossing scenarios * WIP: Improve synchronization * First working behavior * Fixed other leading vehicle scenario speed bug * Created new S2 for routes and added it to routes * Better ControlLoss + fix missplaced route scenarios * Improved actor flows with new python agent * Minor fixes * Cleanup and some example changes * Added CHANGELOG * Removed unused atomic behaviors * Pretty code * Added missing BA code * Removed constant velocity agent * Added other parameters * Pretty code * Fixed CHANGELOG * Pretty code * Fixed sceario parsing bug * Minor fixes * CHANGELOG * Minor fixes * Fixed bug with agent's route --- Docs/CHANGELOG.md | 2 + manual_control.py | 31 +- .../openscenario_configuration.py | 2 + .../route_scenario_configuration.py | 6 +- .../scenarioconfigs/scenario_configuration.py | 48 ++- srunner/scenarios/basic_scenario.py | 12 + srunner/scenarios/master_scenario.py | 114 ------- srunner/scenarios/route_scenario.py | 289 +++++------------- srunner/tools/route_parser.py | 265 +++++++--------- srunner/tools/scenario_parser.py | 142 +++++---- 10 files changed, 348 insertions(+), 563 deletions(-) delete mode 100644 srunner/scenarios/master_scenario.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 9b5b15783..9ad7e48eb 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -14,6 +14,8 @@ ## Latest changes ### :rocket: New Features +* Scenarios can now parse and use all parameters present at the configuration file. +* Improved overall parsing of routes and scenarios. * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. * The BackgroundActivity has been readded to the routes, which the objective of creating the sensation of traffic around the ego diff --git a/manual_control.py b/manual_control.py index d144807c4..58fc60a89 100755 --- a/manual_control.py +++ b/manual_control.py @@ -67,7 +67,6 @@ from pygame.locals import K_F1 from pygame.locals import KMOD_CTRL from pygame.locals import KMOD_SHIFT - from pygame.locals import K_BACKSPACE from pygame.locals import K_TAB from pygame.locals import K_SPACE from pygame.locals import K_UP @@ -114,8 +113,6 @@ def get_actor_display_name(actor, truncate=250): class World(object): - restarted = False - def __init__(self, carla_world, hud, args): self.world = carla_world try: @@ -140,10 +137,6 @@ def __init__(self, carla_world, hud, args): def restart(self): - if self.restarted: - return - self.restarted = True - self.player_max_speed = 1.589 self.player_max_speed_fast = 3.713 @@ -161,7 +154,7 @@ def restart(self): print("Ego vehicle found") self.player = vehicle break - + self.player_name = self.player.type_id # Set up the sensors. @@ -177,9 +170,14 @@ def restart(self): self.world.wait_for_tick() - def tick(self, clock): + def tick(self, clock, wait_for_repetitions): if len(self.world.get_actors().filter(self.player_name)) < 1: - return False + if not wait_for_repetitions: + return False + else: + self.player = None + self.destroy() + self.restart() self.hud.tick(self, clock) return True @@ -232,13 +230,6 @@ def parse_events(self, client, world, clock): elif event.type == pygame.KEYUP: if self._is_quit_shortcut(event.key): return True - elif event.key == K_BACKSPACE: - if self._autopilot_enabled: - world.player.set_autopilot(False) - world.restart() - world.player.set_autopilot(True) - else: - world.restart() elif event.key == K_F1: world.hud.toggle_info() elif event.key == K_TAB: @@ -917,7 +908,7 @@ def game_loop(args): clock.tick_busy_loop(60) if controller.parse_events(client, world, clock): return - if not world.tick(clock): + if not world.tick(clock, args.wait_for_repetitions): return world.render(display) pygame.display.flip() @@ -978,6 +969,10 @@ def main(): '--keep_ego_vehicle', action='store_true', help='do not destroy ego vehicle on exit') + argparser.add_argument( + '--wait-for-repetitions', + action='store_true', + help='Avoids stopping the manual control when the scenario ends.') args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index ce4d99b09..5fb1a5878 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -33,6 +33,8 @@ class OpenScenarioConfiguration(ScenarioConfiguration): def __init__(self, filename, client, custom_params): + super(OpenScenarioConfiguration, self).__init__() + self.xml_tree = ET.parse(filename) self.filename = filename self._custom_params = custom_params if custom_params is not None else {} diff --git a/srunner/scenarioconfigs/route_scenario_configuration.py b/srunner/scenarioconfigs/route_scenario_configuration.py index 63f4318cc..55da841bb 100644 --- a/srunner/scenarioconfigs/route_scenario_configuration.py +++ b/srunner/scenarioconfigs/route_scenario_configuration.py @@ -46,5 +46,7 @@ class RouteScenarioConfiguration(ScenarioConfiguration): Basic configuration of a RouteScenario """ - trajectory = None - scenario_file = None + def __init__(self): + super(RouteScenarioConfiguration, self).__init__() + self.trajectory = None + self.scenario_file = None diff --git a/srunner/scenarioconfigs/scenario_configuration.py b/srunner/scenarioconfigs/scenario_configuration.py index 388a7d2cf..03a5d4720 100644 --- a/srunner/scenarioconfigs/scenario_configuration.py +++ b/srunner/scenarioconfigs/scenario_configuration.py @@ -61,6 +61,28 @@ def parse_from_node(node, rolename): return ActorConfigurationData(model, transform, rolename, speed, autopilot, random_location, color) + @staticmethod + 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.*' + + 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 + + return ActorConfigurationData(model, transform, rolename, speed, autopilot, random_location, color) + class ScenarioConfiguration(object): @@ -72,15 +94,17 @@ class ScenarioConfiguration(object): - type is the class of scenario (e.g. ControlLoss) """ - trigger_points = [] - ego_vehicles = [] - other_actors = [] - town = None - name = None - type = None - route = None - agent = None - weather = carla.WeatherParameters() - friction = None - subtype = None - route_var_name = None + def __init__(self): + self.trigger_points = [] + self.ego_vehicles = [] + self.other_actors = [] + self.other_parameters = {} + self.town = None + self.name = None + self.type = None + self.route = None + self.agent = None + self.weather = carla.WeatherParameters(sun_altitude_angle=70) + self.friction = None + self.subtype = None + self.route_var_name = None diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index e80cfba04..4226d44d2 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -50,6 +50,9 @@ def __init__(self, name, ego_vehicles, config, world, self._initialize_environment(world) + is_route_scenario = CarlaDataProvider.get_ego_vehicle_route() is not None + self._initialize_other_parameters(config, is_route_scenario) + # Initializing adversarial actors self._initialize_actors(config) if CarlaDataProvider.is_sync_mode(): @@ -119,6 +122,15 @@ def _initialize_actors(self, config): for new_actor in new_actors: self.other_actors.append(new_actor) + def _initialize_other_parameters(self, config, is_route_scenario): + """ + Initializes other parameters part of the config file. + As routes and scenarios are parsed differently, use 'is_route_scenario' + to know if the scenario is being triggered as part of a route. + Override this method in child class to provide custom initialization. + """ + pass + def _setup_scenario_trigger(self, config): """ This function creates a trigger maneuver, that has to be finished before the real scenario starts. diff --git a/srunner/scenarios/master_scenario.py b/srunner/scenarios/master_scenario.py deleted file mode 100644 index 87fc4ff05..000000000 --- a/srunner/scenarios/master_scenario.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python - -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" -Basic CARLA Autonomous Driving training scenario -""" - -import py_trees - -from srunner.scenarioconfigs.route_scenario_configuration import RouteConfiguration -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle -from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest, - InRouteTest, - RouteCompletionTest, - OutsideRouteLanesTest, - RunningRedLightTest, - RunningStopTest, - ActorSpeedAboveThresholdTest) -from srunner.scenarios.basic_scenario import BasicScenario - - -class MasterScenario(BasicScenario): - - """ - Implementation of a Master scenario that controls the route. - - This is a single ego vehicle scenario - """ - - radius = 10.0 # meters - - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, - timeout=300): - """ - Setup all relevant parameters and create scenario - """ - self.config = config - self.route = None - # Timeout of scenario in seconds - self.timeout = timeout - - if hasattr(self.config, 'route'): - self.route = self.config.route - else: - raise ValueError("Master scenario must have a route") - - super(MasterScenario, self).__init__("MasterScenario", ego_vehicles=ego_vehicles, config=config, - world=world, debug_mode=debug_mode, - terminate_on_failure=True, criteria_enable=criteria_enable) - - def _create_behavior(self): - """ - Basic behavior do nothing, i.e. Idle - """ - - # Build behavior tree - sequence = py_trees.composites.Sequence("MasterScenario") - idle_behavior = Idle() - sequence.add_child(idle_behavior) - - return sequence - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - - if isinstance(self.route, RouteConfiguration): - route = self.route.data - else: - route = self.route - - collision_criterion = CollisionTest(self.ego_vehicles[0], terminate_on_failure=False) - - route_criterion = InRouteTest(self.ego_vehicles[0], - route=route, - offroad_max=30, - terminate_on_failure=True) - - completion_criterion = RouteCompletionTest(self.ego_vehicles[0], route=route) - - outsidelane_criterion = OutsideRouteLanesTest(self.ego_vehicles[0], route=route) - - red_light_criterion = RunningRedLightTest(self.ego_vehicles[0]) - - stop_criterion = RunningStopTest(self.ego_vehicles[0]) - - blocked_criterion = ActorSpeedAboveThresholdTest(self.ego_vehicles[0], - speed_threshold=0.1, - below_threshold_max_time=90.0, - terminate_on_failure=True) - - parallel_criteria = py_trees.composites.Parallel("group_criteria", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - - parallel_criteria.add_child(completion_criterion) - parallel_criteria.add_child(collision_criterion) - parallel_criteria.add_child(route_criterion) - parallel_criteria.add_child(outsidelane_criterion) - parallel_criteria.add_child(red_light_criterion) - parallel_criteria.add_child(stop_criterion) - parallel_criteria.add_child(blocked_criterion) - - return parallel_criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 962a0d834..b81aceee9 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -11,23 +11,20 @@ from __future__ import print_function -import math import traceback -import xml.etree.ElementTree as ET import py_trees +from numpy import random import carla from agents.navigation.local_planner import RoadOption -# pylint: disable=line-too-long -from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData -# pylint: enable=line-too-long +from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle, ScenarioTriggerer from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.route_parser import RouteParser, TRIGGER_THRESHOLD, TRIGGER_ANGLE_THRESHOLD +from srunner.tools.route_parser import RouteParser from srunner.tools.route_manipulation import interpolate_trajectory from srunner.tools.py_trees_port import oneshot_behavior @@ -67,28 +64,6 @@ } -def convert_json_to_transform(actor_dict): - """ - Convert a JSON string to a CARLA transform - """ - return carla.Transform(location=carla.Location(x=float(actor_dict['x']), y=float(actor_dict['y']), - z=float(actor_dict['z'])), - rotation=carla.Rotation(roll=0.0, pitch=0.0, yaw=float(actor_dict['yaw']))) - - -def convert_json_to_actor(actor_dict): - """ - Convert a JSON string to an ActorConfigurationData dictionary - """ - node = ET.Element('waypoint') - node.set('x', actor_dict['x']) - node.set('y', actor_dict['y']) - node.set('z', actor_dict['z']) - node.set('yaw', actor_dict['yaw']) - - return ActorConfigurationData.parse_from_node(node, 'simulation') - - def convert_transform_to_location(transform_vec): """ Convert a vector of transforms to a vector of locations @@ -100,44 +75,6 @@ def convert_transform_to_location(transform_vec): return location_vec -def compare_scenarios(scenario_choice, existent_scenario): - """ - Compare function for scenarios based on distance of the scenario start position - """ - def transform_to_pos_vec(scenario): - """ - Convert left/right/front to a meaningful CARLA position - """ - position_vec = [scenario['trigger_position']] - if scenario['other_actors'] is not None: - if 'left' in scenario['other_actors']: - position_vec += scenario['other_actors']['left'] - if 'front' in scenario['other_actors']: - position_vec += scenario['other_actors']['front'] - if 'right' in scenario['other_actors']: - position_vec += scenario['other_actors']['right'] - - return position_vec - - # put the positions of the scenario choice into a vec of positions to be able to compare - - choice_vec = transform_to_pos_vec(scenario_choice) - existent_vec = transform_to_pos_vec(existent_scenario) - for pos_choice in choice_vec: - for pos_existent in existent_vec: - - dx = float(pos_choice['x']) - float(pos_existent['x']) - dy = float(pos_choice['y']) - float(pos_existent['y']) - dz = float(pos_choice['z']) - float(pos_existent['z']) - dist_position = math.sqrt(dx * dx + dy * dy + dz * dz) - dyaw = float(pos_choice['yaw']) - float(pos_choice['yaw']) - dist_angle = math.sqrt(dyaw * dyaw) - if dist_position < TRIGGER_THRESHOLD and dist_angle < TRIGGER_ANGLE_THRESHOLD: - return True - - return False - - class RouteScenario(BasicScenario): """ @@ -151,19 +88,23 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou """ self.config = config - self.route = None - self.sampled_scenarios_definitions = None + self.route = self._get_route(config) + sampled_scenario_definitions = self._get_scenarios(config) + ego_vehicle = self._spawn_ego_vehicle() + self.timeout = self._estimate_route_timeout() - self._update_route(world, config, debug_mode) + if debug_mode: + self._draw_waypoints(world, self.route, vertical_shift=0.1, size=0.1, persistency=self.timeout) - ego_vehicle = self._update_ego_vehicle() + self.list_scenarios = self._build_scenarios(world, + ego_vehicle, + sampled_scenario_definitions, + scenarios_per_tick=5, + timeout=self.timeout, + debug_mode=debug_mode) - self.list_scenarios = self._build_scenario_instances(world, - ego_vehicle, - self.sampled_scenarios_definitions, - scenarios_per_tick=5, - timeout=self.timeout, - debug_mode=debug_mode) + self.list_scenarios.append(BackgroundActivity( + world, ego_vehicle, self.config, self.route, timeout=self.timeout)) self.list_scenarios.append(BackgroundActivity( world, ego_vehicle, self.config, self.route, timeout=self.timeout)) @@ -176,44 +117,40 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou terminate_on_failure=False, criteria_enable=criteria_enable) - def _update_route(self, world, config, debug_mode): + def _get_route(self, config): """ - Update the input route, i.e. refine waypoint list, and extract possible scenario locations + Gets the route from the configuration, interpolating it to the desired density, + saving it to the CarlaDataProvider and sending it to the agent Parameters: - world: CARLA world - config: Scenario configuration (RouteConfiguration) + - debug_mode: boolean to decide whether or not the route poitns are printed """ - - # Transform the scenario file into a dictionary - world_annotations = RouteParser.parse_annotations_file(config.scenario_file) - # prepare route's trajectory (interpolate and add the GPS route) gps_route, route = interpolate_trajectory(config.trajectory) - - potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(config.town, route, world_annotations) - - self.route = route - CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route)) - + CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(route)) if config.agent is not None: - config.agent.set_global_plan(gps_route, self.route) + config.agent.set_global_plan(gps_route, route) - # Sample the scenarios to be used for this route instance. - self.sampled_scenarios_definitions = self._scenario_sampling(potential_scenarios_definitions) - - # Timeout of scenario in seconds - self.timeout = self._estimate_route_timeout() + return route - # Print route in debug mode - if debug_mode: - self._draw_waypoints(world, self.route, vertical_shift=0.1, persistency=50000.0) - - def _update_ego_vehicle(self): + def _get_scenarios(self, config): """ - Set/Update the start position of the ego_vehicle + Gets the scenarios that will be part of the route. Automatically filters the scenarios + that affect the route and, if there are two or more scenarios with very similar triggering positions, + one of those is randomly chosen + + Parameters: + - config: Scenario configuration (RouteConfiguration) """ - # move ego to correct position + scenario_dict = RouteParser.parse_scenario_file_to_dict(config.scenario_file) + potential_scenarios = RouteParser.scan_route_for_scenarios(config.town, self.route, scenario_dict) + sampled_scenarios = self._scenario_sampling(potential_scenarios) + return sampled_scenarios + + def _spawn_ego_vehicle(self): + """Spawn the ego vehicle at the first waypoint of the route""" elevate_transform = self.route[0][0] elevate_transform.location.z += 0.5 @@ -225,7 +162,7 @@ def _update_ego_vehicle(self): def _estimate_route_timeout(self): """ - Estimate the duration of the route + Estimate the duration of the route, as a proportinal value of its length """ route_length = 0.0 # in meters @@ -238,7 +175,7 @@ def _estimate_route_timeout(self): return int(SECONDS_GIVEN_PER_METERS * route_length) # pylint: disable=no-self-use - def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): + def _draw_waypoints(self, world, waypoints, vertical_shift, size, persistency=-1): """ Draw a list of waypoints at a certain height given in vertical_shift. """ @@ -246,105 +183,65 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1): wp = w[0].location + carla.Location(z=vertical_shift) if w[1] == RoadOption.LEFT: # Yellow - color = carla.Color(255, 255, 0) + color = carla.Color(128, 128, 0) elif w[1] == RoadOption.RIGHT: # Cyan - color = carla.Color(0, 255, 255) + color = carla.Color(0, 128, 128) elif w[1] == RoadOption.CHANGELANELEFT: # Orange - color = carla.Color(255, 64, 0) + color = carla.Color(128, 32, 0) elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan - color = carla.Color(0, 64, 255) + color = carla.Color(0, 32, 128) elif w[1] == RoadOption.STRAIGHT: # Gray - color = carla.Color(128, 128, 128) + color = carla.Color(64, 64, 64) else: # LANEFOLLOW - color = carla.Color(0, 255, 0) # Green + color = carla.Color(0, 128, 0) # Green world.debug.draw_point(wp, size=0.1, color=color, life_time=persistency) - world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=0.2, - color=carla.Color(0, 0, 255), life_time=persistency) - world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=0.2, - color=carla.Color(255, 0, 0), life_time=persistency) - - def _scenario_sampling(self, potential_scenarios_definitions): - """ - The function used to sample the scenarios that are going to happen for this route. - """ + world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=2*size, + color=carla.Color(0, 0, 128), life_time=persistency) + world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=2*size, + color=carla.Color(128, 0, 0), life_time=persistency) - # fix the random seed for reproducibility - rng = CarlaDataProvider.get_random_seed() + def _scenario_sampling(self, potential_scenarios, random_seed=0): + """Sample the scenarios that are going to happen for this route.""" + # Fix the random seed for reproducibility, and randomly sample a scenario per trigger position. + rng = random.RandomState(random_seed) - def position_sampled(scenario_choice, sampled_scenarios): - """ - Check if a position was already sampled, i.e. used for another scenario - """ - for existent_scenario in sampled_scenarios: - # If the scenarios have equal positions then it is true. - if compare_scenarios(scenario_choice, existent_scenario): - return True - - return False - - # The idea is to randomly sample a scenario per trigger position. sampled_scenarios = [] - for trigger in potential_scenarios_definitions.keys(): - possible_scenarios = potential_scenarios_definitions[trigger] - - scenario_choice = rng.choice(possible_scenarios) - del possible_scenarios[possible_scenarios.index(scenario_choice)] - # We keep sampling and testing if this position is present on any of the scenarios. - while position_sampled(scenario_choice, sampled_scenarios): - if possible_scenarios is None or not possible_scenarios: - scenario_choice = None - break - scenario_choice = rng.choice(possible_scenarios) - del possible_scenarios[possible_scenarios.index(scenario_choice)] - - if scenario_choice is not None: - sampled_scenarios.append(scenario_choice) + for trigger in list(potential_scenarios): + scenario_list = potential_scenarios[trigger] + sampled_scenarios.append(rng.choice(scenario_list)) return sampled_scenarios - def _build_scenario_instances(self, world, ego_vehicle, scenario_definitions, - scenarios_per_tick=5, timeout=300, debug_mode=False): + def _build_scenarios(self, world, ego_vehicle, scenario_definitions, + scenarios_per_tick=5, timeout=300, debug_mode=False): """ - Based on the parsed route and possible scenarios, build all the scenario classes. + Initializes the class of all the scenarios that will be present in the route. + If a class fails to be initialized, a warning is printed but the route execution isn't stopped """ scenario_instance_vec = [] if debug_mode: - for scenario in scenario_definitions: - loc = carla.Location(scenario['trigger_position']['x'], - scenario['trigger_position']['y'], - scenario['trigger_position']['z']) + carla.Location(z=2.0) - world.debug.draw_point(loc, size=0.3, color=carla.Color(255, 0, 0), life_time=100000) - world.debug.draw_string(loc, str(scenario['name']), draw_shadow=False, - color=carla.Color(0, 0, 255), life_time=100000, persistent_lines=True) - - for scenario_number, definition in enumerate(scenario_definitions): - # Get the class possibilities for this scenario number - scenario_class = NUMBER_CLASS_TRANSLATION[definition['name']] - - # Create the other actors that are going to appear - if definition['other_actors'] is not None: - list_of_actor_conf_instances = self._get_actors_instances(definition['other_actors']) - else: - list_of_actor_conf_instances = [] - # Create an actor configuration for the ego-vehicle trigger position - - egoactor_trigger_position = convert_json_to_transform(definition['trigger_position']) - scenario_configuration = ScenarioConfiguration() - scenario_configuration.other_actors = list_of_actor_conf_instances - scenario_configuration.trigger_points = [egoactor_trigger_position] - scenario_configuration.subtype = definition['scenario_type'] - scenario_configuration.ego_vehicles = [ActorConfigurationData('vehicle.lincoln.mkz_2017', - ego_vehicle.get_transform(), - 'hero')] - route_var_name = "ScenarioRouteNumber{}".format(scenario_number) - scenario_configuration.route_var_name = route_var_name + tmap = CarlaDataProvider.get_map() + for scenario_config in scenario_definitions: + scenario_loc = scenario_config.trigger_points[0].location + debug_loc = tmap.get_waypoint(scenario_loc).transform.location + carla.Location(z=0.2) + world.debug.draw_point(debug_loc, size=0.2, color=carla.Color(128, 0, 0), life_time=timeout) + world.debug.draw_string(debug_loc, str(scenario_config.type), draw_shadow=False, + color=carla.Color(0, 0, 128), life_time=timeout, persistent_lines=True) + + for scenario_number, scenario_config in enumerate(scenario_definitions): + scenario_config.ego_vehicles = [ActorConfigurationData(ego_vehicle.type_id, + ego_vehicle.get_transform(), + 'hero')] + scenario_config.route_var_name = "ScenarioRouteNumber{}".format(scenario_number) try: - scenario_instance = scenario_class(world, [ego_vehicle], scenario_configuration, + scenario_class = NUMBER_CLASS_TRANSLATION[scenario_config.type] + scenario_instance = scenario_class(world, [ego_vehicle], scenario_config, criteria_enable=False, timeout=timeout) + # Do a tick every once in a while to avoid spawning everything at the same time if scenario_number % scenarios_per_tick == 0: if CarlaDataProvider.is_sync_mode(): @@ -352,47 +249,17 @@ def _build_scenario_instances(self, world, ego_vehicle, scenario_definitions, else: world.wait_for_tick() - scenario_number += 1 except Exception as e: # pylint: disable=broad-except if debug_mode: traceback.print_exc() - print("Skipping scenario '{}' due to setup error: {}".format(definition['name'], e)) + print("Skipping scenario '{}' due to setup error: {}".format(scenario_config.type, e)) continue scenario_instance_vec.append(scenario_instance) return scenario_instance_vec - def _get_actors_instances(self, list_of_antagonist_actors): - """ - Get the full list of actor instances. - """ - - def get_actors_from_list(list_of_actor_def): - """ - Receives a list of actor definitions and creates an actual list of ActorConfigurationObjects - """ - sublist_of_actors = [] - for actor_def in list_of_actor_def: - sublist_of_actors.append(convert_json_to_actor(actor_def)) - - return sublist_of_actors - - list_of_actors = [] - # Parse vehicles to the left - if 'front' in list_of_antagonist_actors: - list_of_actors += get_actors_from_list(list_of_antagonist_actors['front']) - - if 'left' in list_of_antagonist_actors: - list_of_actors += get_actors_from_list(list_of_antagonist_actors['left']) - - if 'right' in list_of_antagonist_actors: - list_of_actors += get_actors_from_list(list_of_antagonist_actors['right']) - - return list_of_actors - # pylint: enable=no-self-use - def _initialize_actors(self, config): """ Set other_actors to the superset of all scenario actors diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index c0e3f980b..d07f63490 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -14,12 +14,21 @@ import carla from agents.navigation.local_planner import RoadOption from srunner.scenarioconfigs.route_scenario_configuration import RouteScenarioConfiguration +from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData # TODO check this threshold, it could be a bit larger but not so large that we cluster scenarios. TRIGGER_THRESHOLD = 2.0 # Threshold to say if a trigger position is new or repeated, works for matching positions TRIGGER_ANGLE_THRESHOLD = 10 # Threshold to say if two angles can be considering matching when matching transforms. +def convert_dict_to_transform(scenario_dict): + """Convert a JSON dict to a CARLA transform""" + return carla.Transform( + carla.Location(float(scenario_dict['x']), float(scenario_dict['y']), float(scenario_dict['z'])), + carla.Rotation(roll=0.0, pitch=0.0, yaw=float(scenario_dict['yaw'])) + ) + + class RouteParser(object): """ @@ -27,19 +36,19 @@ class RouteParser(object): """ @staticmethod - def parse_annotations_file(annotation_filename): + def parse_scenario_file_to_dict(scenario_file): """ - Return the annotations of which positions where the scenarios are going to happen. - :param annotation_filename: the filename for the anotations file + Parses and returns the scenario file into a dictionary + :param scenario_file: the filename for the scenario file :return: """ - with open(annotation_filename, 'r', encoding='utf-8') as f: - annotation_dict = json.loads(f.read()) + with open(scenario_file, 'r', encoding='utf-8') as f: + scenario_dict = json.loads(f.read()) final_dict = {} - for town_dict in annotation_dict['available_scenarios']: + for town_dict in scenario_dict['available_scenarios']: final_dict.update(town_dict) return final_dict # the file has a current maps name that is an one element vec @@ -47,7 +56,7 @@ def parse_annotations_file(annotation_filename): @staticmethod def parse_routes_file(route_filename, scenario_file, single_route=None): """ - Returns a list of route elements. + Returns a list of route configuration elements. :param route_filename: the path to a set of routes. :param single_route: If set, only this route shall be returned :return: List of dicts containing the waypoints, id and town of the routes @@ -113,110 +122,87 @@ def parse_weather(route): weather.fog_distance = float(weather_attrib.attrib['fog_distance']) if 'fog_density' in weather_attrib.attrib: weather.fog_density = float(weather_attrib.attrib['fog_density']) + if 'scattering_intensity' in weather_attrib.attrib: + weather.scattering_intensity = float(weather_attrib.attrib['scattering_intensity']) + if 'mie_scattering_scale' in weather_attrib.attrib: + weather.mie_scattering_scale = float(weather_attrib.attrib['mie_scattering_scale']) + if 'rayleigh_scattering_scale' in weather_attrib.attrib: + weather.rayleigh_scattering_scale = float(weather_attrib.attrib['rayleigh_scattering_scale']) return weather @staticmethod - def check_trigger_position(new_trigger, existing_triggers): + def get_trigger_position(scenario_trigger, existing_triggers): """ Check if this trigger position already exists or if it is a new one. - :param new_trigger: - :param existing_triggers: + :param scenario_trigger: position to be checked + :param existing_triggers: list with all the already found position :return: """ - - for trigger_id in existing_triggers.keys(): - trigger = existing_triggers[trigger_id] - dx = trigger['x'] - new_trigger['x'] - dy = trigger['y'] - new_trigger['y'] + for trigger in existing_triggers: + dx = trigger.location.x - scenario_trigger.location.x + dy = trigger.location.y - scenario_trigger.location.y distance = math.sqrt(dx * dx + dy * dy) - dyaw = (trigger['yaw'] - new_trigger['yaw']) % 360 + dyaw = (trigger.rotation.yaw - scenario_trigger.rotation.yaw) % 360 if distance < TRIGGER_THRESHOLD \ and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)): - return trigger_id + return trigger return None @staticmethod - def convert_waypoint_float(waypoint): + def match_trigger_to_route(trigger_transform, route): """ - Convert waypoint values to float + Check if the scenario is affecting the route. + This is true if the trigger position is very close to any route point """ - waypoint['x'] = float(waypoint['x']) - waypoint['y'] = float(waypoint['y']) - waypoint['z'] = float(waypoint['z']) - waypoint['yaw'] = float(waypoint['yaw']) - - @staticmethod - def match_world_location_to_route(world_location, route_description): - """ - We match this location to a given route. - world_location: - route_description: - """ - def match_waypoints(waypoint1, wtransform): - """ - Check if waypoint1 and wtransform are similar - """ - dx = float(waypoint1['x']) - wtransform.location.x - dy = float(waypoint1['y']) - wtransform.location.y - dz = float(waypoint1['z']) - wtransform.location.z + def is_trigger_close(trigger_transform, route_transform): + """Check if the two transforms are similar""" + dx = trigger_transform.location.x - route_transform.location.x + dy = trigger_transform.location.y - route_transform.location.y + dz = trigger_transform.location.z - route_transform.location.z dpos = math.sqrt(dx * dx + dy * dy + dz * dz) - dyaw = (float(waypoint1['yaw']) - wtransform.rotation.yaw) % 360 + dyaw = (float(trigger_transform.rotation.yaw) - route_transform.rotation.yaw) % 360 return dpos < TRIGGER_THRESHOLD \ and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)) - match_position = 0 - # TODO this function can be optimized to run on Log(N) time - for route_waypoint in route_description: - if match_waypoints(world_location, route_waypoint[0]): - return match_position - match_position += 1 + for position, [route_transform, _] in enumerate(route): + if is_trigger_close(trigger_transform, route_transform): + return position return None @staticmethod - def get_scenario_type(scenario, match_position, trajectory): + def get_scenario_subtype(scenario, route): """ - Some scenarios have different types depending on the route. + Some scenarios have subtypes depending on the route trajectory, + even being invalid if there isn't a valid one. As an example, + some scenarios need the route to turn in a specific direction, + and if this isn't the case, the scenario should not be considered valid. + This is currently only used for validity purposes. + :param scenario: the scenario name - :param match_position: the matching position for the scenarion - :param trajectory: the route trajectory the ego is following + :param route: route starting at the triggering point of the scenario :return: tag representing this subtype - - Also used to check which are not viable (Such as an scenario - that triggers when turning but the route doesnt') - WARNING: These tags are used at: - - VehicleTurningRoute - - SignalJunctionCrossingRoute - and changes to these tags will affect them """ - def check_this_waypoint(tuple_wp_turn): - """ - Decides whether or not the waypoint will define the scenario behavior - """ - if RoadOption.LANEFOLLOW == tuple_wp_turn[1]: - return False - elif RoadOption.CHANGELANELEFT == tuple_wp_turn[1]: - return False - elif RoadOption.CHANGELANERIGHT == tuple_wp_turn[1]: + def is_junction_option(option): + """Whether or not an option is part of a junction""" + if option in (RoadOption.LANEFOLLOW, RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT): return False return True - # Unused tag for the rest of scenarios, - # can't be None as they are still valid scenarios - subtype = 'valid' + subtype = None - if scenario == 'Scenario4': - for tuple_wp_turn in trajectory[match_position:]: - if check_this_waypoint(tuple_wp_turn): - if RoadOption.LEFT == tuple_wp_turn[1]: + if scenario == 'Scenario4': # Only if the route turns + for _, option in route: + if is_junction_option(option): + if option == RoadOption.LEFT: subtype = 'S4left' - elif RoadOption.RIGHT == tuple_wp_turn[1]: + elif option == RoadOption.RIGHT: subtype = 'S4right' else: subtype = None @@ -224,98 +210,75 @@ def check_this_waypoint(tuple_wp_turn): subtype = None if scenario == 'Scenario7': - for tuple_wp_turn in trajectory[match_position:]: - if check_this_waypoint(tuple_wp_turn): - if RoadOption.STRAIGHT == tuple_wp_turn[1]: + for _, option in route: + if is_junction_option(option): + if RoadOption.STRAIGHT == option: subtype = 'S7opposite' - else: - subtype = None - break # Avoid checking all of them - subtype = None - - if scenario == 'Scenario8': - for tuple_wp_turn in trajectory[match_position:]: - if check_this_waypoint(tuple_wp_turn): - if RoadOption.LEFT == tuple_wp_turn[1]: + break + elif scenario == 'Scenario8': # Only if the route turns left + for _, option in route: + if is_junction_option(option): + if option == RoadOption.LEFT: subtype = 'S8left' - else: - subtype = None - break # Avoid checking all of them - subtype = None - - if scenario == 'Scenario9': - for tuple_wp_turn in trajectory[match_position:]: - if check_this_waypoint(tuple_wp_turn): - if RoadOption.RIGHT == tuple_wp_turn[1]: + break + elif scenario == 'Scenario9': # Only if the route turns right + for _, option in route: + if is_junction_option(option): + if option == RoadOption.RIGHT: subtype = 'S9right' - else: - subtype = None - break # Avoid checking all of them - subtype = None + break + else: + subtype = 'valid' return subtype @staticmethod def scan_route_for_scenarios(route_name, trajectory, world_annotations): """ - Just returns a plain list of possible scenarios that can happen in this route by matching - the locations from the scenario into the route description - - :return: A list of scenario definitions with their correspondent parameters + Filters all the scenarios that are affecting the route. + Returns a dictionary where each item is a list of all the scenarios that are close to each other """ - - # the triggers dictionaries: - existent_triggers = {} - # We have a table of IDs and trigger positions associated possible_scenarios = {} - # Keep track of the trigger ids being added - latest_trigger_id = 0 - for town_name in world_annotations.keys(): if town_name != route_name: continue - scenarios = world_annotations[town_name] - for scenario in scenarios: # For each existent scenario - if "scenario_type" not in scenario: + town_scenarios = world_annotations[town_name] + for scenario_data in town_scenarios: + + if "scenario_type" not in scenario_data: break - scenario_name = scenario["scenario_type"] - for event in scenario["available_event_configurations"]: - waypoint = event['transform'] # trigger point of this scenario - RouteParser.convert_waypoint_float(waypoint) - # We match trigger point to the route, now we need to check if the route affects - match_position = RouteParser.match_world_location_to_route( - waypoint, trajectory) - if match_position is not None: - # We match a location for this scenario, create a scenario object so this scenario - # can be instantiated later - - if 'other_actors' in event: - other_vehicles = event['other_actors'] - else: - other_vehicles = None - scenario_subtype = RouteParser.get_scenario_type(scenario_name, match_position, - trajectory) - if scenario_subtype is None: - continue - scenario_description = { - 'name': scenario_name, - 'other_actors': other_vehicles, - 'trigger_position': waypoint, - 'scenario_type': scenario_subtype, # some scenarios have route dependent configs - } - - trigger_id = RouteParser.check_trigger_position(waypoint, existent_triggers) - if trigger_id is None: - # This trigger does not exist create a new reference on existent triggers - existent_triggers.update({latest_trigger_id: waypoint}) - # Update a reference for this trigger on the possible scenarios - possible_scenarios.update({latest_trigger_id: []}) - trigger_id = latest_trigger_id - # Increment the latest trigger - latest_trigger_id += 1 - - possible_scenarios[trigger_id].append(scenario_description) - - return possible_scenarios, existent_triggers + scenario_name = scenario_data["scenario_type"] + + for scenario in scenario_data["available_event_configurations"]: + # Get the trigger point of the scenario + trigger_point = convert_dict_to_transform(scenario.pop('transform')) + + # Check if the route passes through the scenario + match_position = RouteParser.match_trigger_to_route(trigger_point, trajectory) + if match_position is None: + continue + + # Check the route has the correct topology + subtype = RouteParser.get_scenario_subtype(scenario_name, trajectory[match_position:]) + if subtype is None: + continue + + # Parse the scenario data + scenario_config = ScenarioConfiguration() + scenario_config.type = scenario_name + scenario_config.subtype = subtype + scenario_config.trigger_points = [trigger_point] + for other in scenario.pop('other_actors', []): + scenario_config.other_actors.append(ActorConfigurationData.parse_from_dict(other, 'scenario')) + scenario_config.other_parameters.update(scenario) + + # Check if its location overlaps with other scenarios + existing_trigger = RouteParser.get_trigger_position(trigger_point, possible_scenarios.keys()) + if existing_trigger: + possible_scenarios[existing_trigger].append(scenario_config) + else: + possible_scenarios.update({trigger_point: [scenario_config]}) + + return possible_scenarios diff --git a/srunner/tools/scenario_parser.py b/srunner/tools/scenario_parser.py index 39f913801..8ea64b08c 100644 --- a/srunner/tools/scenario_parser.py +++ b/srunner/tools/scenario_parser.py @@ -13,6 +13,8 @@ import os import xml.etree.ElementTree as ET +import carla + from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData from srunner.scenarioconfigs.route_scenario_configuration import RouteConfiguration @@ -24,7 +26,7 @@ class ScenarioConfigurationParser(object): """ @staticmethod - def parse_scenario_configuration(scenario_name, config_file_name): + def parse_scenario_configuration(scenario_name, additional_config_file_name): """ Parse all scenario configuration files at srunner/examples and the additional config files, providing a list of ScenarioConfigurations @return @@ -34,18 +36,18 @@ def parse_scenario_configuration(scenario_name, config_file_name): scenario that matches the scenario_name is parsed and returned. """ - list_of_config_files = glob.glob("{}/srunner/examples/*.xml".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) - - if config_file_name != '': - list_of_config_files.append(config_file_name) - - single_scenario_only = True if scenario_name.startswith("group:"): - single_scenario_only = False + scenario_group = True scenario_name = scenario_name[6:] + else: + scenario_group = False scenario_configurations = [] + list_of_config_files = glob.glob("{}/srunner/examples/*.xml".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) + if additional_config_file_name != '': + list_of_config_files.append(additional_config_file_name) + for file_name in list_of_config_files: tree = ET.parse(file_name) @@ -54,62 +56,92 @@ def parse_scenario_configuration(scenario_name, config_file_name): scenario_config_name = scenario.attrib.get('name', None) scenario_config_type = scenario.attrib.get('type', None) - if single_scenario_only: - # Check the scenario is the correct one - if scenario_config_name != scenario_name: - continue - else: - # Check the scenario is of the correct type - if scenario_config_type != scenario_name: - continue - - new_config = ScenarioConfiguration() - new_config.town = scenario.attrib.get('town', None) - new_config.name = scenario_config_name - new_config.type = scenario_config_type - new_config.other_actors = [] - new_config.ego_vehicles = [] - new_config.trigger_points = [] - - for weather in scenario.iter("weather"): - new_config.weather.cloudiness = float(weather.attrib.get("cloudiness", 0)) - new_config.weather.precipitation = float(weather.attrib.get("precipitation", 0)) - new_config.weather.precipitation_deposits = float(weather.attrib.get("precipitation_deposits", 0)) - new_config.weather.wind_intensity = float(weather.attrib.get("wind_intensity", 0.35)) - new_config.weather.sun_azimuth_angle = float(weather.attrib.get("sun_azimuth_angle", 0.0)) - new_config.weather.sun_altitude_angle = float(weather.attrib.get("sun_altitude_angle", 15.0)) - new_config.weather.fog_density = float(weather.attrib.get("fog_density", 0.0)) - new_config.weather.fog_distance = float(weather.attrib.get("fog_distance", 0.0)) - new_config.weather.wetness = float(weather.attrib.get("wetness", 0.0)) - - for ego_vehicle in scenario.iter("ego_vehicle"): - - new_config.ego_vehicles.append(ActorConfigurationData.parse_from_node(ego_vehicle, 'hero')) - new_config.trigger_points.append(new_config.ego_vehicles[-1].transform) - - for route in scenario.iter("route"): - route_conf = RouteConfiguration() - route_conf.parse_xml(route) - new_config.route = route_conf - - for other_actor in scenario.iter("other_actor"): - new_config.other_actors.append(ActorConfigurationData.parse_from_node(other_actor, 'scenario')) - - scenario_configurations.append(new_config) - + # Check that the scenario is the correct one + if not scenario_group and scenario_config_name != scenario_name: + continue + # Check that the scenario is of the correct type + elif scenario_group and scenario_config_type != scenario_name: + continue + + config = ScenarioConfiguration() + config.town = scenario.attrib.get('town') + config.name = scenario_config_name + config.type = scenario_config_type + + for elem in scenario.getchildren(): + # Elements with special parsing + if elem.tag == 'ego_vehicle': + config.ego_vehicles.append(ActorConfigurationData.parse_from_node(elem, 'hero')) + config.trigger_points.append(config.ego_vehicles[-1].transform) + elif elem.tag == 'other_actor': + config.other_actors.append(ActorConfigurationData.parse_from_node(elem, 'scenario')) + elif elem.tag == 'weather': + config.weather.cloudiness = float(elem.attrib.get("cloudiness", 50)) + config.weather.precipitation = float(elem.attrib.get("precipitation", 10)) + config.weather.precipitation_deposits = float(elem.attrib.get("precipitation_deposits", 10)) + config.weather.wind_intensity = float(elem.attrib.get("wind_intensity", 0.35)) + config.weather.sun_azimuth_angle = float(elem.attrib.get("sun_azimuth_angle", 0.0)) + config.weather.sun_altitude_angle = float(elem.attrib.get("sun_altitude_angle", 70.0)) + config.weather.fog_density = float(elem.attrib.get("fog_density", 0.0)) + config.weather.fog_distance = float(elem.attrib.get("fog_distance", 0.0)) + config.weather.wetness = float(elem.attrib.get("wetness", 0.0)) + config.weather.fog_falloff = float(elem.attrib.get("fog_falloff", 0.0)) + config.weather.scattering_intensity = float(elem.attrib.get("scattering_intensity", 0.0)) + config.weather.mie_scattering_scale = float(elem.attrib.get("mie_scattering_scale", 0.0)) + config.weather.rayleigh_scattering_scale = float( + elem.attrib.get("rayleigh_scattering_scale", 0.0331)) + + elif elem.tag == 'route': + route_conf = RouteConfiguration() + route_conf.parse_xml(elem) + config.route = route_conf + + # Any other possible element, add it as a config attribute + else: + config.other_parameters[elem.tag] = elem.attrib + + scenario_configurations.append(config) return scenario_configurations @staticmethod - def get_list_of_scenarios(config_file_name): + def parse_weather(weather_element): + """Parses the weather element into a carla.WeatherParameters. + """ + weather = carla.WeatherParameters() + + for weather_attrib in weather_element: + + if 'cloudiness' in weather_attrib.attrib: + weather.cloudiness = float(weather_attrib.attrib['cloudiness']) + if 'precipitation' in weather_attrib.attrib: + weather.precipitation = float(weather_attrib.attrib['precipitation']) + if 'precipitation_deposits' in weather_attrib.attrib: + weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits']) + if 'wind_intensity' in weather_attrib.attrib: + weather.wind_intensity = float(weather_attrib.attrib['wind_intensity']) + if 'sun_azimuth_angle' in weather_attrib.attrib: + weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle']) + if 'sun_altitude_angle' in weather_attrib.attrib: + weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle']) + if 'wetness' in weather_attrib.attrib: + weather.wetness = float(weather_attrib.attrib['wetness']) + if 'fog_distance' in weather_attrib.attrib: + weather.fog_distance = float(weather_attrib.attrib['fog_distance']) + if 'fog_density' in weather_attrib.attrib: + weather.fog_density = float(weather_attrib.attrib['fog_density']) + + return weather + + @staticmethod + def get_list_of_scenarios(additional_config_file_name): """ Parse *all* config files and provide a list with all scenarios @return """ list_of_config_files = glob.glob("{}/srunner/examples/*.xml".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) list_of_config_files += glob.glob("{}/srunner/examples/*.xosc".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) - - if config_file_name != '': - list_of_config_files.append(config_file_name) + if additional_config_file_name != '': + list_of_config_files.append(additional_config_file_name) scenarios = [] for file_name in list_of_config_files: From cf487141393b7c9d42597c90aa562bed44f62cfc Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 16 Feb 2022 16:52:10 +0100 Subject: [PATCH 02/86] Improve actor flow scenarios (#859) * Improved actor flow behavior * CHANGELOG Co-authored-by: Guillermo --- Docs/CHANGELOG.md | 1 + .../scenarioatomics/atomic_behaviors.py | 80 +++++++++++++++++-- .../opposite_vehicle_taking_priority.py | 4 +- 3 files changed, 77 insertions(+), 8 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 9ad7e48eb..751848ac1 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -14,6 +14,7 @@ ## Latest changes ### :rocket: New Features +* Actor Flows are now more consistent * Scenarios can now parse and use all parameters present at the configuration file. * Improved overall parsing of routes and scenarios. * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index f98beb047..5570d5b74 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -30,6 +30,7 @@ import carla from agents.navigation.basic_agent import BasicAgent +from agents.navigation.constant_velocity_agent import ConstantVelocityAgent from agents.navigation.local_planner import RoadOption, LocalPlanner from agents.navigation.global_route_planner import GlobalRoutePlanner from agents.tools.misc import is_within_distance @@ -1760,7 +1761,6 @@ def update(self): class BasicAgentBehavior(AtomicBehavior): - """ This class contains an atomic behavior, which uses the basic_agent from CARLA to control the actor until @@ -1772,14 +1772,67 @@ class BasicAgentBehavior(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="BasicAgentBehavior"): + def __init__(self, actor, target_location, name="BasicAgentBehavior"): """ Setup actor and maximum steer value """ super(BasicAgentBehavior, self).__init__(name, actor) self._map = CarlaDataProvider.get_map() + self._target_location = target_location + self._control = carla.VehicleControl() + self._agent = None + self._plan = None + + def initialise(self): + """Initialises the agent""" + self._agent = BasicAgent(self._actor) + self._plan = self._agent.trace_route( + self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), + self._map.get_waypoint(self._target_location)) + self._agent.set_global_plan(self._plan) + + def update(self): + new_status = py_trees.common.Status.RUNNING + + if self._agent.done(): + new_status = py_trees.common.Status.SUCCESS + 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)) + return new_status + + def terminate(self, new_status): + """Resets the control""" + self._control.throttle = 0.0 + self._control.brake = 0.0 + self._actor.apply_control(self._control) + super(BasicAgentBehavior, self).terminate(new_status) + + +class ConstantVelocityAgentBehavior(AtomicBehavior): + + """ + This class contains an atomic behavior, which uses the + constant_velocity_agent from CARLA to control the actor until + reaching a target location. + Important parameters: + - actor: CARLA actor to execute the behavior + - target_location: Is the desired target location (carla.location), + the actor should move to + - plan: List of [carla.Waypoint, RoadOption] to pass to the controller + - target_speed: Desired speed of the actor + 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"): + """ + Set up actor and local planner + """ + super(ConstantVelocityAgentBehavior, self).__init__(name, actor) self._target_speed = target_speed + self._map = CarlaDataProvider.get_map() self._target_location = target_location self._opt_dict = opt_dict if opt_dict else {} self._control = carla.VehicleControl() @@ -1788,21 +1841,24 @@ def __init__(self, actor, target_location, target_speed=None, def initialise(self): """Initialises the agent""" - self._agent = BasicAgent(self._actor, self._target_speed * 3.6, opt_dict=self._opt_dict) + self._agent = ConstantVelocityAgent(self._actor, self._target_speed * 3.6, opt_dict=self._opt_dict) self._plan = self._agent.trace_route( self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), self._map.get_waypoint(self._target_location)) self._agent.set_global_plan(self._plan) def update(self): + """Moves the actor and waits for it to finish the plan""" new_status = py_trees.common.Status.RUNNING if self._agent.done(): new_status = py_trees.common.Status.SUCCESS + 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)) + return new_status def terminate(self, new_status): @@ -1810,7 +1866,7 @@ def terminate(self, new_status): self._control.throttle = 0.0 self._control.brake = 0.0 self._actor.apply_control(self._control) - super(BasicAgentBehavior, self).terminate(new_status) + super(ConstantVelocityAgentBehavior, self).terminate(new_status) class Idle(AtomicBehavior): @@ -2473,9 +2529,21 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, actors_ self._route = [] self._grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) self._route = self._grp.trace_route(self._source_wp.transform.location, self._sink_wp.transform.location) + self._opt_dict = { + "ignore_vehicles": True, + } def update(self): """Controls the created actors and creaes / removes other when needed""" + # To avoid multiple collisions, activate collision detection if one vehicle has already collided + stopped_actors = [] + for actor, agent in list(self._actor_agent_list): + if not agent.is_constant_velocity_active: + stopped_actors.append(actor) + if stopped_actors: + for actor, agent in list(self._actor_agent_list): + agent.ignore_vehicles(False) + # Control the vehicles, removing them when needed for actor_info in list(self._actor_agent_list): actor, agent = actor_info @@ -2499,7 +2567,7 @@ def update(self): 'vehicle.*', self._source_transform, rolename='scenario', safe_blueprint=True, tick=False ) if actor is not None: - actor_agent = BasicAgent(actor, 3.6 * self._speed) + actor_agent = ConstantVelocityAgent(actor, 3.6 * self._speed, opt_dict=self._opt_dict) actor_agent.set_global_plan(self._route, False) self._actor_agent_list.append([actor, actor_agent]) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index e38ed6e0e..fed6a7a81 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -19,7 +19,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, ActorDestroy, TrafficLightFreezer, - BasicAgentBehavior) + ConstantVelocityAgentBehavior) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -174,7 +174,7 @@ def _create_behavior(self): self.ego_vehicles[0], self._collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) - sequence.add_child(BasicAgentBehavior( + sequence.add_child(ConstantVelocityAgentBehavior( self.other_actors[0], target_location=self._sink_wp.transform.location, target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True}, name="AdversaryCrossing")) From a51e9c8fdcb69eb7e4447ff304541a2859c921a3 Mon Sep 17 00:00:00 2001 From: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com> Date: Tue, 1 Mar 2022 17:02:15 +0100 Subject: [PATCH 03/86] Changes in Actor Flow, implementation of Accident and Construction, changes to BA (#863) * Initial testing * Changed ActorFlow to Town04 * WIP: ActorFLow at routes * WIP2: Actor Flow * Changes to Accident and Construction blocking scenarios Implementation of chages in BA to allow TM lane changes through set_path * Improve road behavior, actor flow and accident scenarios * Construction scenario implementation Co-authored-by: Guillermo --- scenario_runner.py | 3 + srunner/examples/ActorFlow.xml | 17 + srunner/examples/HighwayCutIn.xml | 7 + srunner/examples/RouteObstacles.xml | 15 + .../scenarioconfigs/scenario_configuration.py | 2 +- .../scenarioatomics/atomic_behaviors.py | 45 +- .../atomic_trigger_conditions.py | 20 +- srunner/scenarios/actor_flow.py | 226 +++++ srunner/scenarios/background_activity.py | 831 ++++++++++-------- srunner/scenarios/bycicle_group.py | 113 +++ .../scenarios/construction_crash_vehicle.py | 124 +-- srunner/scenarios/follow_leading_vehicle.py | 10 +- srunner/scenarios/highway_cut_in.py | 115 +++ .../scenarios/object_crash_intersection.py | 4 +- .../opposite_vehicle_taking_priority.py | 4 +- srunner/scenarios/route_obstacles.py | 155 ++++ .../signalized_junction_left_turn.py | 4 +- .../signalized_junction_right_turn.py | 4 +- srunner/tools/background_manager.py | 151 ++-- srunner/tools/scenario_helper.py | 71 +- 20 files changed, 1385 insertions(+), 536 deletions(-) create mode 100644 srunner/examples/ActorFlow.xml create mode 100644 srunner/examples/HighwayCutIn.xml create mode 100644 srunner/examples/RouteObstacles.xml create mode 100644 srunner/scenarios/actor_flow.py create mode 100644 srunner/scenarios/bycicle_group.py create mode 100644 srunner/scenarios/highway_cut_in.py create mode 100644 srunner/scenarios/route_obstacles.py diff --git a/scenario_runner.py b/scenario_runner.py index 5d2dd6015..740877a5b 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -230,6 +230,9 @@ def _prepare_ego_vehicles(self, ego_vehicles): for i, _ in enumerate(self.ego_vehicles): self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) + self.ego_vehicles[i].set_target_velocity(carla.Vector3D()) + self.ego_vehicles[i].set_target_angular_velocity(carla.Vector3D()) + self.ego_vehicles[i].apply_control(carla.VehicleControl()) CarlaDataProvider.register_actor(self.ego_vehicles[i]) # sync state diff --git a/srunner/examples/ActorFlow.xml b/srunner/examples/ActorFlow.xml new file mode 100644 index 000000000..beeba0599 --- /dev/null +++ b/srunner/examples/ActorFlow.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/srunner/examples/HighwayCutIn.xml b/srunner/examples/HighwayCutIn.xml new file mode 100644 index 000000000..cb069e59d --- /dev/null +++ b/srunner/examples/HighwayCutIn.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/srunner/examples/RouteObstacles.xml b/srunner/examples/RouteObstacles.xml new file mode 100644 index 000000000..71b4a6d32 --- /dev/null +++ b/srunner/examples/RouteObstacles.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/srunner/scenarioconfigs/scenario_configuration.py b/srunner/scenarioconfigs/scenario_configuration.py index 03a5d4720..a6dc6aab7 100644 --- a/srunner/scenarioconfigs/scenario_configuration.py +++ b/srunner/scenarioconfigs/scenario_configuration.py @@ -104,7 +104,7 @@ def __init__(self): self.type = None self.route = None self.agent = None - self.weather = carla.WeatherParameters(sun_altitude_angle=70) + self.weather = carla.WeatherParameters(sun_altitude_angle=70, cloudiness=50) self.friction = None self.subtype = None self.route_var_name = None diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 5570d5b74..68e57fab7 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2018,7 +2018,8 @@ def _apply_local_planner(self, actor): local_planner = LocalPlanner( # pylint: disable=undefined-variable actor, opt_dict={ 'target_speed': self._target_speed * 3.6, - 'lateral_control_dict': self._args_lateral_dict}) + 'lateral_control_dict': self._args_lateral_dict, + 'max_throttle': 1.0}) if self._plan is not None: if isinstance(self._plan[0], carla.Location): @@ -2502,14 +2503,16 @@ class ActorFlow(AtomicBehavior): - sink_location (carla.Location): Location at which actors will be deleted - spawn_distance: Distance between spawned actors - sink_distance: Actors closer to the sink than this distance will be deleted + - actors_speed: Speed of the actors part of the flow [m/s] """ - def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, actors_speed=30/3.6, name="ActorFlow"): + def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, + actor_speed=20 / 3.6, acceleration=float('inf'), name="ActorFlow"): """ Setup class members """ super(ActorFlow, self).__init__(name) - self._rng = random.RandomState(2000) + self._rng = CarlaDataProvider.get_random_seed() self._world = CarlaDataProvider.get_world() self._source_wp = source_wp @@ -2519,7 +2522,7 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, actors_ self._source_transform = self._source_wp.transform self._sink_dist = sink_dist - self._speed = actors_speed + self._speed = actor_speed self._min_spawn_dist = spawn_dist_interval[0] self._max_spawn_dist = spawn_dist_interval[1] @@ -2530,23 +2533,33 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, actors_ self._grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) self._route = self._grp.trace_route(self._source_wp.transform.location, self._sink_wp.transform.location) self._opt_dict = { - "ignore_vehicles": True, + 'ignore_vehicles': True, + 'use_basic_behavior': True, } def update(self): """Controls the created actors and creaes / removes other when needed""" # To avoid multiple collisions, activate collision detection if one vehicle has already collided - stopped_actors = [] - for actor, agent in list(self._actor_agent_list): + collision_found = False + for actor, agent, _ in list(self._actor_agent_list): if not agent.is_constant_velocity_active: - stopped_actors.append(actor) - if stopped_actors: - for actor, agent in list(self._actor_agent_list): + collision_found = True + break + if collision_found: + for actor, agent, _ in list(self._actor_agent_list): agent.ignore_vehicles(False) + agent.stop_constant_velocity() # Control the vehicles, removing them when needed for actor_info in list(self._actor_agent_list): - actor, agent = actor_info + actor, agent, just_spawned = actor_info + if just_spawned: # Move the vehicle to the ground. Important if flow starts at turns + ground_loc = self._world.ground_projection(actor.get_location(), 2) + if ground_loc: + initial_location = ground_loc.location + actor.set_location(initial_location) + actor_info[2] = False + sink_distance = self._sink_location.distance(CarlaDataProvider.get_location(actor)) if sink_distance < self._sink_dist: actor.destroy() @@ -2569,15 +2582,9 @@ def update(self): if actor is not None: actor_agent = ConstantVelocityAgent(actor, 3.6 * self._speed, opt_dict=self._opt_dict) actor_agent.set_global_plan(self._route, False) - self._actor_agent_list.append([actor, actor_agent]) + self._actor_agent_list.append([actor, actor_agent, True]) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - ground_loc = self._world.ground_projection(self._source_transform.location, 2) - if ground_loc.location: - initial_location = ground_loc.location - initial_location.z += 0.06 - actor.set_location(initial_location) - return py_trees.common.Status.RUNNING def terminate(self, new_status): @@ -2585,7 +2592,7 @@ def terminate(self, new_status): Default terminate. Can be extended in derived class """ try: - for actor, _ in self._actor_agent_list: + for actor, _, _ in self._actor_agent_list: actor.destroy() except RuntimeError: pass # Actor was already destroyed diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index 167959e15..9eb0867fa 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -1191,14 +1191,16 @@ 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 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, 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 - self.inside_junction = False + self._junction_id = junction_id + self._inside_junction = False self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): @@ -1208,12 +1210,16 @@ def update(self): location = CarlaDataProvider.get_location(self.actor) waypoint = CarlaDataProvider.get_map().get_waypoint(location) - # Wait for the actor to enter a junction - if not self.inside_junction and waypoint.is_junction: - self.inside_junction = True + # Wait for the actor to enter a / the junction + if not self._inside_junction: + junction = waypoint.get_junction() + if not junction: + return new_status + if not self._junction_id or junction.id == self._junction_id: + self._inside_junction = True # And to leave it - if self.inside_junction and not waypoint.is_junction: + elif self._inside_junction and not waypoint.is_junction: if self.debug: print("--- Leaving the junction") new_status = py_trees.common.Status.SUCCESS diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py new file mode 100644 index 000000000..c82819889 --- /dev/null +++ b/srunner/scenarios/actor_flow.py @@ -0,0 +1,226 @@ +#!/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 . + +""" +Scenarios in which another (opposite) vehicle 'illegally' takes +priority, e.g. by running a red traffic light. +""" + +from __future__ import print_function + +import py_trees +import carla + +from agents.navigation.global_route_planner import GlobalRoutePlanner + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToLocation, WaitEndIntersection +from srunner.scenarios.basic_scenario import BasicScenario + +from srunner.tools.background_manager import ChangeRoadBehavior, JunctionScenarioManager, ExtentExitRoadSpace, StopEntries +from srunner.tools.scenario_helper import get_same_dir_lanes + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + +class EnterActorFlow(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._sink_distance = 2 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + super(EnterActorFlow, self).__init__("EnterActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + # Get all lanes + source_wps = get_same_dir_lanes(source_wp) + sink_wps = get_same_dir_lanes(sink_wp) + num_flows = min(len(source_wps), len(sink_wps)) + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + for i in range(num_flows): + root.add_child(ActorFlow( + source_wps[i], sink_wps[i], self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wps[i].transform.location, self._sink_distance)) + + sequence = py_trees.composites.Sequence() + if CarlaDataProvider.get_ego_vehicle_route(): + sequence.add_child(JunctionScenarioManager('left', True, False, True)) + + grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + extra_space = 0 + for i in range(-2, -len(route)-1, -1): + current_wp = route[i][0] + extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + if current_wp.is_junction: + sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='left')) + break + + sequence.add_child(ChangeRoadBehavior(switch_source=False)) + sequence.add_child(root) + if CarlaDataProvider.get_ego_vehicle_route(): + sequence.add_child(ChangeRoadBehavior(switch_source=True)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() + + +class CrossActorFlow(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._sink_distance = 2 + + self._end_distance = 40 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + super(CrossActorFlow, self).__init__("CrossActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + self.world.debug.draw_point(source_wp.transform.location + carla.Location(z=3)) + self.world.debug.draw_point(sink_wp.transform.location + carla.Location(z=3)) + + grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + junction_id = None + for wp, _ in route: + if wp.is_junction: + junction_id = wp.get_junction().id + break + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) + + root.add_child(end_condition) + + sequence = py_trees.composites.Sequence() + if CarlaDataProvider.get_ego_vehicle_route(): + sequence.add_child(JunctionScenarioManager('opposite', True, True, True)) + sequence.add_child(StopEntries()) + sequence.add_child(root) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 02e4d8461..0369f008c 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -18,6 +18,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes DEBUG_COLORS = { 'road': carla.Color(0, 0, 255), # Blue @@ -34,6 +35,14 @@ 'large': [0.2, 0.2], } +JUNCTION_ENTRY_NAME = 'junction_entry' +JUNCTION_MIDDLE_NAME = 'junction_middle' +JUNCTION_EXIT_NAME = 'junction_exit' +JUNCTION_INACTIVE_NAME = 'junction_inactive' + +EGO_JUNCTION_NAME = 'junction' +EGO_ROAD_NAME = 'road' + def draw_string(world, location, string='', debug_type='road', persistent=False): """Utility function to draw debugging strings""" @@ -53,65 +62,6 @@ def draw_point(world, location, point_type='small', debug_type='road', persisten world.debug.draw_point(location + l_shift, size, color, life_time) -def get_same_dir_lanes(waypoint): - """Gets all the lanes with the same direction of the road of a wp""" - same_dir_wps = [waypoint] - - # Check roads on the right - right_wp = waypoint - while True: - possible_right_wp = right_wp.get_right_lane() - if possible_right_wp is None or possible_right_wp.lane_type != carla.LaneType.Driving: - break - right_wp = possible_right_wp - same_dir_wps.append(right_wp) - - # Check roads on the left - left_wp = waypoint - while True: - possible_left_wp = left_wp.get_left_lane() - if possible_left_wp is None or possible_left_wp.lane_type != carla.LaneType.Driving: - break - if possible_left_wp.lane_id * left_wp.lane_id < 0: - break - left_wp = possible_left_wp - same_dir_wps.append(left_wp) - - return same_dir_wps - - -def get_opposite_dir_lanes(waypoint): - """Gets all the lanes with opposite direction of the road of a wp""" - other_dir_wps = [] - other_dir_wp = None - - # Get the first lane of the opposite direction - left_wp = waypoint - while True: - possible_left_wp = left_wp.get_left_lane() - if possible_left_wp is None: - break - if possible_left_wp.lane_id * left_wp.lane_id < 0: - other_dir_wp = possible_left_wp - break - left_wp = possible_left_wp - - if not other_dir_wp: - return other_dir_wps - - # Check roads on the right - right_wp = other_dir_wp - while True: - if right_wp.lane_type == carla.LaneType.Driving: - other_dir_wps.append(right_wp) - possible_right_wp = right_wp.get_right_lane() - if possible_right_wp is None: - break - right_wp = possible_right_wp - - return other_dir_wps - - def get_lane_key(waypoint): """Returns a key corresponding to the waypoint lane. Equivalent to a 'Lane' object and used to compare waypoint lanes""" @@ -130,12 +80,13 @@ class Source(object): Source object to store its position and its responsible actors """ - def __init__(self, wp, actors, entry_lane_wp=''): # pylint: disable=invalid-name + def __init__(self, wp, actors, entry_lane_wp='', dist_to_ego=0): # pylint: disable=invalid-name self.wp = wp # pylint: disable=invalid-name self.actors = actors + self.active = True # For road sources - self.mapped_key = get_lane_key(wp) + self.dist_to_ego = dist_to_ego # For junction sources self.entry_lane_wp = entry_lane_wp @@ -166,15 +117,17 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind # State self.entry_sources = [] - self.exit_sources = [] self.exit_dict = OrderedDict() self.actor_dict = OrderedDict() + + # Scenario interactions self.scenario_info = { 'direction': None, 'remove_entries': False, 'remove_middle': False, 'remove_exits': False, } + self.stop_entries = False def contains(self, other_junction): """Checks whether or not a carla.Junction is part of the class""" @@ -256,7 +209,7 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr # Global variables self._ego_actor = ego_actor - self._ego_state = 'road' + self._ego_state = EGO_ROAD_NAME self._route_index = 0 self._get_route_data(route) @@ -267,11 +220,7 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._fake_lane_pair_keys = [] # Road variables - self._road_actors = [] - self._road_back_actors = {} # Dictionary mapping the actors behind the ego to their lane - self._road_ego_key = None - self._road_extra_front_actors = 0 - self._road_sources = [] + self._road_dict = {} # Dictionary lane key -> actor source self._road_checker_index = 0 self._road_ego_key = "" @@ -281,10 +230,16 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._road_spawn_dist = 11 # Initial distance between spawned road vehicles [m] self._road_new_sources_dist = 20 # Distance of the source to the start of the new lanes self._radius_increase_ratio = 1.8 # Meters the radius increases per m/s of the ego - self._extra_radius = 0.0 # Extra distance to avoid the road behavior from blocking + + # TODO: Remove these 3 and refine road radiuses + self._extra_radius = 0.0 # Extra distance to avoid the road behavior from blocking. self._extra_radius_increase_ratio = 0.5 * timestep # Distance the radius increases per tick (0.5 m/s) self._max_extra_radius = 10 # Max extra distance + self._road_num_front_vehicles = self._road_front_vehicles # Checks the real amount of actors in the front of the ego + self._road_extra_front_actors = 0 # For cases where we want more space but not more vehicles + self._road_time_delay = 3 + self._base_min_radius = 0 self._base_max_radius = 0 self._min_radius = 0 @@ -313,15 +268,11 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._opposite_sources_max_actors = 8 # Maximum vehicles alive at the same time per source # Scenario 2 variables - self._is_scenario_2_active = False - self._scenario_2_actors = [] - self._activate_break_scenario = False - self._break_duration = 7 # Duration of the scenario - self._next_scenario_time = float('inf') + self._stopped_road_actors = [] # Scenario 4 variables - self._is_scenario_4_active = False - self._scenario_4_actors = [] + self._is_crossing_scenario_active = False + self._crossing_scenario_actors = [] self._ego_exitted_junction = False self._crossing_dist = None # Distance between the crossing object and the junction exit self._start_ego_wp = None @@ -357,8 +308,8 @@ def _get_road_radius(self): Computes the min and max radius of the road behaviorm which will determine the speed of the vehicles. Vehicles closer than the min radius maintain full speed, while those further than max radius are stopped. Between the two, the velocity decreases linearly""" - self._base_min_radius = (self._road_front_vehicles + self._road_extra_front_actors) * self._road_spawn_dist - self._base_max_radius = (self._road_front_vehicles + self._road_extra_front_actors + 1) * self._road_spawn_dist + self._base_min_radius = (self._road_num_front_vehicles + self._road_extra_front_actors) * self._road_spawn_dist + self._base_max_radius = (self._road_num_front_vehicles + self._road_extra_front_actors + 1) * self._road_spawn_dist self._min_radius = self._base_min_radius self._max_radius = self._base_max_radius @@ -368,53 +319,47 @@ def initialise(self): ego_wp = self._route[0] self._road_ego_key = get_lane_key(ego_wp) same_dir_wps = get_same_dir_lanes(ego_wp) - self._initialise_road_behavior(same_dir_wps, ego_wp) + + self._initialise_road_behavior(same_dir_wps) self._initialise_opposite_sources() self._initialise_road_checker() def update(self): - new_status = py_trees.common.Status.RUNNING - prev_ego_index = self._route_index + # Check if the TM destroyed an actor - if self._route_index > 0: + if self._route_index > 0: # TODO: This check is due to intialization problem. self._check_background_actors() - # Get ego's odometry. For robustness, the closest route point will be used - location = CarlaDataProvider.get_location(self._ego_actor) - ego_wp = self._update_ego_route_location(location) - ego_transform = ego_wp.transform - if self.debug: - string = 'EGO_' + self._ego_state[0].upper() - draw_string(self._world, location, string, self._ego_state, False) + # Update ego's route position. For robustness, the route point is used for most calculus + self._update_ego_route_location() # Parameters and scenarios self._update_parameters() - self._manage_break_scenario() - # Update ego state - if self._ego_state == 'junction': - self._monitor_ego_junction_exit(ego_wp) - self._monitor_nearby_junctions() + # Update ego state. + if self._ego_state == EGO_JUNCTION_NAME: + self._monitor_ego_junction_exit() + self._monitor_incoming_junctions() # Update_actors - if self._ego_state == 'junction': - self._monitor_ego_junction_exit(ego_wp) + if self._ego_state == EGO_JUNCTION_NAME: self._update_junction_actors() self._update_junction_sources() else: - self._update_road_actors(prev_ego_index, self._route_index) - self._move_road_checker(prev_ego_index, self._route_index) - self._move_opposite_sources(prev_ego_index, self._route_index) + self._update_road_actors() + self._update_road_sources(prev_ego_index) + self._move_road_checker(prev_ego_index) + self._move_opposite_sources(prev_ego_index) + self._monitor_road_changes(prev_ego_index) self._update_opposite_sources() - # Update non junction sources - self._update_opposite_actors(ego_transform) - self._update_road_sources(ego_transform.location) + # Update non junction sources. + self._update_opposite_actors() - self._monitor_scenario_4_end(ego_transform.location) + self._monitor_crossing_scenario_end() - return new_status + return py_trees.common.Status.RUNNING def terminate(self, new_status): """Destroy all actors""" @@ -425,7 +370,9 @@ def terminate(self, new_status): def _get_actors(self): """Returns a list of all actors part of the background activity""" - actors = self._road_actors + self._opposite_actors + actors = list(self._opposite_actors) + for lane in self._road_dict: + actors.extend(self._road_dict[lane].actors) for junction in self._active_junctions: actors.extend(list(junction.actor_dict)) return actors @@ -781,6 +728,13 @@ def _add_junctions_topology(self, route_data): junction_data.entry_wps = entry_lane_wps junction_data.exit_wps = exit_lane_wps + for exit_wp in exit_lane_wps: + junction_data.exit_dict[get_lane_key(exit_wp)] = { + 'actors': [], + 'max_actors': 0, + 'ref_wp': None, + 'max_distance': 0, + } # Filter the entries and exits that correspond to the route route_entry_wp = self._route[junction_data.route_entry_index] @@ -867,10 +821,6 @@ def _add_junctions_topology(self, route_data): route_oppo_exit += oppo_exit_key + ' ' * (6 - len(oppo_exit_key)) print(route_oppo_exit) - ################################ - ## Waypoint related functions ## - ################################ - def _is_junction(self, waypoint): if not waypoint.is_junction or waypoint.junction_id in self._fake_junction_ids: return False @@ -881,24 +831,40 @@ def _is_junction(self, waypoint): ################################ def _add_actor_dict_element(self, actor_dict, actor, exit_lane_key='', at_oppo_entry_lane=False): - """Adds a new actor to the actor dictionary""" + """ + Adds a new actor to the actor dictionary. + 'exit_lane_key' is used to know at which exit lane (if any) is the vehicle + 'at_oppo_entry_lane' whether or not the actor is part of the entry at the opposite lane the route exits through. + This will be the ones that aren't removed + """ actor_dict[actor] = { - 'state': 'junction_entry' if not exit_lane_key else 'junction_exit', - 'exit_lane_key': exit_lane_key, + 'state': JUNCTION_ENTRY_NAME if not exit_lane_key else JUNCTION_EXIT_NAME, + 'exit_lane_key': exit_lane_key, 'at_oppo_entry_lane': at_oppo_entry_lane } def _switch_to_junction_mode(self, junction): - """Prepares the junction mode, changing the state of the actors""" - self._ego_state = 'junction' - for actor in list(self._road_actors): - self._add_actor_dict_element(junction.actor_dict, actor) - self._road_actors.remove(actor) - if not self._is_scenario_2_active: - self._tm.vehicle_percentage_speed_difference(actor, 0) - - self._road_back_actors.clear() - self._road_extra_front_actors = 0 + """ + Prepares the junction mode, removing all road behaviours. + Actors that are stopped via a scenario will still wait. + """ + self._ego_state = EGO_JUNCTION_NAME + for lane in self._road_dict: + for actor in self._road_dict[lane].actors: + # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' + self._add_actor_dict_element(junction.actor_dict, actor) + if actor not in self._stopped_road_actors: + self._tm.vehicle_percentage_speed_difference(actor, 0) + + for lane_key in self._road_dict: + source = self._road_dict[lane_key] + if get_lane_key(source.wp) in junction.route_entry_keys: + source.actors.reverse() # TODO: dont do this + junction.entry_sources.append(Source(source.wp, source.actors, entry_lane_wp=source.wp)) + # TODO: Else should map the source to the entry and add it + + self._road_dict.clear() + self._road_num_front_vehicles = self._road_front_vehicles self._opposite_sources.clear() def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, remove_middle): @@ -923,7 +889,7 @@ def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, # Source is affected actors = entry_source.actors for actor in list(actors): - if actor_dict[actor]['state'] == 'junction_entry': + if actor_dict[actor]['state'] == JUNCTION_ENTRY_NAME: # Actor is at the entry lane self._destroy_actor(actor) @@ -935,9 +901,17 @@ def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, if remove_middle: actor_dict = scenario_junction.actor_dict for actor in list(actor_dict): - if actor_dict[actor]['state'] == 'junction_middle': + if actor_dict[actor]['state'] == JUNCTION_MIDDLE_NAME: self._destroy_actor(actor) + elif self._junctions: + self._junctions[0].scenario_info = { + 'direction': direction, + 'remove_entries': remove_entries, + 'remove_middle': remove_middle, + 'remove_exits': remove_exits, + } + def _handle_junction_scenario_end(self, junction): """Ends the junction scenario interaction. This is pretty much useless as the junction scenario ends at the same time as the active junction, but in the future it might not""" @@ -948,24 +922,25 @@ def _handle_junction_scenario_end(self, junction): 'remove_exits': False, } - def _monitor_scenario_4_end(self, ego_location): + def _monitor_crossing_scenario_end(self): """Monitors the ego distance to the junction to know if the scenario 4 has ended""" if self._ego_exitted_junction: ref_location = self._start_ego_wp.transform.location + ego_location = self._ego_wp.transform.location if ego_location.distance(ref_location) > self._crossing_dist: - for actor in self._scenario_4_actors: + for actor in self._crossing_scenario_actors: self._tm.vehicle_percentage_speed_difference(actor, 0) - self._is_scenario_4_active = False - self._scenario_4_actors.clear() + self._is_crossing_scenario_active = False + self._crossing_scenario_actors.clear() self._ego_exitted_junction = False self._crossing_dist = None - def _handle_scenario_4_interaction(self, junction, ego_wp): + def _handle_crossing_scenario_interaction(self, junction, ego_wp): """ Handles the interation between the scenario 4 of the Leaderboard and the background activity. This removes all vehicles near the bycicle path, and stops the others so that they don't interfere """ - if not self._is_scenario_4_active: + if not self._is_crossing_scenario_active: return self._ego_exitted_junction = True @@ -999,7 +974,7 @@ def _handle_scenario_4_interaction(self, junction, ego_wp): self._destroy_actor(actor) continue # Actor at the ego lane and between the ego and scenario - self._scenario_4_actors.append(actor) + self._crossing_scenario_actors.append(actor) # Actor entering the junction for entry_source in junction.entry_sources: @@ -1020,14 +995,14 @@ def _handle_scenario_4_interaction(self, junction, ego_wp): self._destroy_actor(actor) continue # Actors blocking the path of the crossing obstacle - self._scenario_4_actors.append(actor) + self._crossing_scenario_actors.append(actor) # Actors entering the next junction if len(self._active_junctions) > 1: next_junction = self._active_junctions[1] actors_dict = next_junction.actor_dict for actor in list(actors_dict): - if actors_dict[actor]['state'] != 'junction_entry': + if actors_dict[actor]['state'] != JUNCTION_ENTRY_NAME: continue actor_location = CarlaDataProvider.get_location(actor) @@ -1049,31 +1024,47 @@ def _handle_scenario_4_interaction(self, junction, ego_wp): self._destroy_actor(actor) continue # Actor at the ego lane and between the ego and scenario - self._scenario_4_actors.append(actor) + self._crossing_scenario_actors.append(actor) # Immediately freeze the actors - for actor in self._scenario_4_actors: + for actor in self._crossing_scenario_actors: try: actor.set_target_velocity(carla.Vector3D(0, 0, 0)) self._tm.vehicle_percentage_speed_difference(actor, 100) except RuntimeError: pass # Just in case the actor is not alive - def _end_junction_behavior(self, ego_wp, junction): + def _end_junction_behavior(self, junction): """ - Destroys unneeded actors (those behind the ego), moves the rest to other data structures - and cleans up the variables. If no other junctions are active, starts road mode + Destroys unneeded actors (those that aren't part of the route's road), + moving the rest to other data structures and cleaning up the variables. + If no other junctions are active, starts road mode """ actor_dict = junction.actor_dict route_exit_keys = junction.route_exit_keys self._active_junctions.pop(0) + # Prepare the road dictionary + if not self._active_junctions: + for wp in junction.exit_wps: + if get_lane_key(wp) in route_exit_keys: + self._road_dict[get_lane_key(wp)] = Source(wp, []) + + else: + for wp in junction.exit_wps: + if get_lane_key(wp) in route_exit_keys: + # TODO: entry_lane_wp isn't really this one (for cases with road changes) + self._active_junctions[0].entry_sources.append(Source(wp, [], entry_lane_wp=wp)) + + # Handle the actors for actor in list(actor_dict): location = CarlaDataProvider.get_location(actor) if not location or self._is_location_behind_ego(location): self._destroy_actor(actor) continue + # Don't destroy those that are on the route's road opposite lane. + # Instead, let them move freely until they are automatically destroyed. self._tm.vehicle_percentage_speed_difference(actor, 0) if actor_dict[actor]['at_oppo_entry_lane']: self._opposite_actors.append(actor) @@ -1081,31 +1072,30 @@ def _end_junction_behavior(self, ego_wp, junction): self._tm.ignore_signs_percentage(actor, 100) continue - if not self._active_junctions and actor_dict[actor]['exit_lane_key'] in route_exit_keys: - self._road_actors.append(actor) + # Save those that are on the route's road + exit_key = actor_dict[actor]['exit_lane_key'] + if exit_key in route_exit_keys: + if not self._active_junctions: + self._road_dict[exit_key].actors.append(actor) + else: + entry_sources = self._active_junctions[0].entry_sources + for entry_source in entry_sources: # Add it to the back source + if exit_key == get_lane_key(entry_source.wp): + entry_sources.actors.append(actor) + break continue + # Destroy the rest self._destroy_actor(actor) - self._handle_scenario_4_interaction(junction, ego_wp) - self._handle_junction_scenario_end(junction) - self._switch_junction_road_sources(junction) + self._handle_crossing_scenario_interaction(junction, self._ego_wp) # TODO: Do a better scenario case + self._handle_junction_scenario_end(junction) # TODO: See if this can be removed (as it changes a popped element) if not self._active_junctions: - self._ego_state = 'road' + self._ego_state = EGO_ROAD_NAME self._initialise_opposite_sources() self._initialise_road_checker() - self._road_ego_key = self._get_ego_route_lane_key(ego_wp) - for source in junction.exit_sources: - self._road_back_actors[source.mapped_key] = [] - - def _switch_junction_road_sources(self, junction): - """ - Removes the sources part of the previous road and gets the ones of the exitted junction. - """ - self._road_sources.clear() - new_sources = junction.exit_sources - self._road_sources.extend(new_sources) + self._road_ego_key = self._get_ego_route_lane_key(self._ego_wp) def _search_for_next_junction(self): """Check if closeby to a junction. The closest one will always be the first""" @@ -1135,30 +1125,31 @@ def _initialise_connecting_lanes(self, junction): self._add_actor_dict_element(junction.actor_dict, actor) self._tm.vehicle_percentage_speed_difference(actor, 0) - def _monitor_nearby_junctions(self): + def _monitor_incoming_junctions(self): """ - Monitors when the ego approaches a junction, preparing the junction mode when it happens. - This can be triggered even if there is another junction behavior happening + Monitors when the ego approaches a junction, triggering that junction when it happens. + This can be triggered even if there is another junction happening are they work independently """ junction = self._search_for_next_junction() if not junction: return - if self._ego_state == 'road': + if self._ego_state == EGO_ROAD_NAME: self._switch_to_junction_mode(junction) self._initialise_junction_sources(junction) self._initialise_junction_exits(junction) + self._initialise_connecting_lanes(junction) self._active_junctions.append(junction) - def _monitor_ego_junction_exit(self, ego_wp): + def _monitor_ego_junction_exit(self): """ Monitors when the ego exits the junctions, preparing the road mode when that happens """ current_junction = self._active_junctions[0] exit_index = current_junction.route_exit_index if exit_index and self._route_index >= exit_index: - self._end_junction_behavior(ego_wp, current_junction) + self._end_junction_behavior(current_junction) def _add_incoming_actors(self, junction, source): """Checks nearby actors that will pass through the source, adding them to it""" @@ -1190,22 +1181,53 @@ def _add_incoming_actors(self, junction, source): return actor - def _update_road_sources(self, ego_location): + def _update_road_sources(self, prev_ego_index): """ Manages the sources that spawn actors behind the ego. - Sources are destroyed after their actors are spawned + These are always behidn the ego and will constinuously spawn actors. + These sources also track the amount of vehicles in front of the ego, + removing actors if the amount is too high. """ - for source in list(self._road_sources): + if prev_ego_index != self._route_index: + route_wp = self._route[self._route_index] + prev_route_wp = self._route[prev_ego_index] + dist = route_wp.transform.location.distance(prev_route_wp.transform.location) + + min_distance = self._road_back_vehicles * self._road_spawn_dist + for lane_key in self._road_dict: + source = self._road_dict[lane_key] + if source.dist_to_ego < min_distance: + source.dist_to_ego += dist + else: + new_source_wps = source.wp.next(dist) + if not new_source_wps: + continue + source.wp = new_source_wps[0] + + for lane_key in self._road_dict: + source = self._road_dict[lane_key] if self.debug: draw_point(self._world, source.wp.transform.location, 'small', self._ego_state, False) draw_string(self._world, source.wp.transform.location, str(len(source.actors)), self._ego_state, False) - if len(source.actors) >= self._road_back_vehicles: - self._road_sources.remove(source) + # Ensure not too many actors are in front of the ego + front_veh = 0 + for actor in source.actors: + location = CarlaDataProvider.get_location(actor) + if not location: + continue + if not self._is_location_behind_ego(location): + front_veh += 1 + if front_veh > self._road_front_vehicles: + self._destroy_actor(source.actors[0]) # This is always the front most vehicle + + if not source.active: + continue + if len(source.actors) >= self._road_back_vehicles + self._road_front_vehicles: continue if len(source.actors) == 0: - location = ego_location + location = self._ego_wp.transform.location else: location = CarlaDataProvider.get_location(source.actors[-1]) if not location: @@ -1221,50 +1243,32 @@ def _update_road_sources(self, ego_location): self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) source.actors.append(actor) - if self._ego_state == 'road': - self._road_actors.append(actor) - if source.mapped_key in self._road_back_actors: - self._road_back_actors[source.mapped_key].append(actor) - elif self._ego_state == 'junction': - self._add_actor_dict_element(self._active_junctions[0].actor_dict, actor) ################################ ## Behavior related functions ## ################################ - def _initialise_road_behavior(self, road_wps, ego_wp): - """Initialises the road behavior, consisting on several vehicle in front of the ego, - and several on the back. The ones on the back are spawned only outside junctions, - and if not enough are spawned, sources are created that will do so later on""" - spawn_wps = [] + def _initialise_road_behavior(self, road_wps): + """ + Initialises the road behavior, consisting on several vehicle in front of the ego, + and several on the back and are only spawned outside junctions. + If there aren't enough actors behind, road sources will be created that will do so later on + """ # Vehicles in front for wp in road_wps: + spawn_wps = [] + + # Front spawn points next_wp = wp for _ in range(self._road_front_vehicles): next_wps = next_wp.next(self._road_spawn_dist) if len(next_wps) != 1 or self._is_junction(next_wps[0]): break # Stop when there's no next or found a junction next_wp = next_wps[0] - spawn_wps.append(next_wp) - - for actor in self._spawn_actors(spawn_wps): - self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) - self._road_actors.append(actor) - - # Vehicles on the side - for wp in road_wps: - self._road_back_actors[get_lane_key(wp)] = [] - if wp.lane_id == ego_wp.lane_id: - continue - - actor = self._spawn_actors([wp])[0] - self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) - self._road_actors.append(actor) - self._road_back_actors[get_lane_key(wp)].append(actor) + spawn_wps.insert(0, next_wp) - # Vehicles behind - for wp in road_wps: - spawn_wps = [] + # Back spawn points + source_dist = 0 prev_wp = wp for _ in range(self._road_back_vehicles): prev_wps = prev_wp.previous(self._road_spawn_dist) @@ -1272,21 +1276,21 @@ def _initialise_road_behavior(self, road_wps, ego_wp): break # Stop when there's no next or found a junction prev_wp = prev_wps[0] spawn_wps.append(prev_wp) + source_dist += self._road_spawn_dist + # Spawn actors actors = self._spawn_actors(spawn_wps) for actor in actors: self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) - self._road_actors.append(actor) - self._road_back_actors[get_lane_key(wp)].append(actor) - # If not spawned enough, create actor soruces behind the ego - if len(actors) < self._road_back_vehicles: - self._road_sources.append(Source(prev_wp, actors)) + self._road_dict[get_lane_key(wp)] = Source(prev_wp, actors, dist_to_ego=source_dist) def _initialise_opposite_sources(self): """ - Gets the waypoints where the actor sources that spawn actors in the opposite direction - will be located. These are at a fixed distance from the ego, but never entering junctions + All opposite lanes have actor sources that will continually create vehicles, + creating the sensation of permanent traffic. The actor spawning will be done later on + (_update_opposite_sources). These sources are at a (somewhat) fixed distance + from the ego, but they never entering junctions. """ self._opposite_route_index = None if not self._junctions: @@ -1331,7 +1335,7 @@ def _initialise_junction_sources(self, junction): """ Initializes the actor sources to ensure the junction is always populated. They are placed at certain distance from the junction, but are stopped if another junction is found, - to ensure the spawned actors always move towards the activated one + to ensure the spawned actors always move towards the activated one. """ remove_entries = junction.scenario_info['remove_entries'] direction = junction.scenario_info['direction'] @@ -1370,7 +1374,7 @@ def _initialise_junction_exits(self, junction): for wp in exit_wps: max_actors = 0 - max_distance = 0 + max_distance = junction.exit_dict[get_lane_key(wp)]['max_distance'] exiting_wps = [] next_wp = wp @@ -1403,8 +1407,6 @@ def _initialise_junction_exits(self, junction): continue # The direction is prohibited as a junction scenario is active if exit_lane_key in route_exit_keys: - junction.exit_sources.append(Source(wp, [])) - actors = self._spawn_actors(exiting_wps) for actor in actors: self._tm.distance_to_leading_vehicle(actor, self._junction_vehicle_dist) @@ -1439,12 +1441,13 @@ def _update_junction_sources(self): # Calculate distance to the last created actor if len(source.actors) == 0: - distance = self._junction_spawn_dist + 1 + actor_location = CarlaDataProvider.get_location(self._ego_actor) else: actor_location = CarlaDataProvider.get_location(source.actors[-1]) - if not actor_location: - continue - distance = actor_location.distance(source.wp.transform.location) + + if not actor_location: + continue + distance = actor_location.distance(source.wp.transform.location) # Spawn a new actor if the last one is far enough if distance > self._junction_spawn_dist: @@ -1452,6 +1455,8 @@ def _update_junction_sources(self): if not actor: continue + if junction.stop_entries: + self._tm.vehicle_percentage_speed_difference(actor, 100) self._tm.distance_to_leading_vehicle(actor, self._junction_vehicle_dist) self._add_actor_dict_element(actor_dict, actor, at_oppo_entry_lane=at_oppo_entry_lane) source.actors.append(actor) @@ -1473,7 +1478,7 @@ def _found_a_road_change(self, old_index, new_index, ignore_false_junctions=True return True - def _move_road_checker(self, prev_index, current_index): + def _move_road_checker(self, prev_index): """ Continually check the road in front to see if it has changed its topology. If so and the number of lanes have reduced, remove the actor of the lane that merges into others @@ -1482,13 +1487,13 @@ def _move_road_checker(self, prev_index, current_index): checker_wp = self._route[self._road_checker_index] draw_point(self._world, checker_wp.transform.location, 'small', 'road', False) - if prev_index == current_index: + if prev_index == self._route_index: return # Get the new route tracking wp checker_index = None last_index = self._junctions[0].route_entry_index if self._junctions else self._route_length - 1 - current_accum_dist = self._accum_dist[current_index] + current_accum_dist = self._accum_dist[self._route_index] for i in range(self._road_checker_index, last_index): accum_dist = self._accum_dist[i] if accum_dist - current_accum_dist >= self._max_radius: @@ -1520,17 +1525,14 @@ def _move_road_checker(self, prev_index, current_index): if not mapped_wp: unmapped_lane_keys.append(get_lane_key(old_wp)) - for actor in list(self._road_actors): - location = CarlaDataProvider.get_location(actor) - if not location: - continue - wp = self._map.get_waypoint(location) - if get_lane_key(wp) in unmapped_lane_keys: - self._destroy_actor(actor) + for lane in self._road_dict: + if lane in unmapped_lane_keys: + for actor in list(self._road_dict[lane].actors): + self._destroy_actor(actor) self._road_checker_index = checker_index - def _move_opposite_sources(self, prev_index, current_index): + def _move_opposite_sources(self, prev_index): """ Moves the sources of the opposite direction back. Additionally, tracks a point a certain distance in front of the ego to see if the road topology has to be recalculated @@ -1542,13 +1544,13 @@ def _move_opposite_sources(self, prev_index, current_index): route_wp = self._route[self._opposite_route_index] draw_point(self._world, route_wp.transform.location, 'small', 'opposite', False) - if prev_index == current_index: + if prev_index == self._route_index: return # Get the new route tracking wp oppo_route_index = None last_index = self._junctions[0].route_entry_index if self._junctions else self._route_length - 1 - current_accum_dist = self._accum_dist[current_index] + current_accum_dist = self._accum_dist[self._route_index] for i in range(self._opposite_route_index, last_index): accum_dist = self._accum_dist[i] if accum_dist - current_accum_dist >= self._opposite_sources_dist: @@ -1584,7 +1586,7 @@ def _move_opposite_sources(self, prev_index, current_index): self._opposite_sources = new_opposite_sources else: prev_accum_dist = self._accum_dist[prev_index] - current_accum_dist = self._accum_dist[current_index] + current_accum_dist = self._accum_dist[self._route_index] move_dist = current_accum_dist - prev_accum_dist if move_dist <= 0: return @@ -1627,38 +1629,57 @@ def _update_opposite_sources(self): source.actors.append(actor) def _update_parameters(self): - """Changes the parameters depending on the blackboard variables and / or the speed of the ego""" - road_behavior_data = py_trees.blackboard.Blackboard().get("BA_RoadBehavior") + """ + Changes those parameters that have dynamic behaviors and / or that can be changed by external source. + This is done using py_trees' Blackboard variables and all behaviors should be at `background_manager.py`. + The blackboard variable is reset to None to avoid changing them back again next time. + """ + + # Road behavior + road_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeRoadBehavior") if road_behavior_data: - num_front_vehicles, num_back_vehicles, vehicle_dist, spawn_dist = road_behavior_data - if num_front_vehicles: + num_front_vehicles, num_back_vehicles, vehicle_dist, spawn_dist, sources_active = road_behavior_data + if num_front_vehicles is not None: self._road_front_vehicles = num_front_vehicles - if num_back_vehicles: + if num_back_vehicles is not None: self._road_back_vehicles = num_back_vehicles - if vehicle_dist: + if vehicle_dist is not None: self._road_vehicle_dist = vehicle_dist - if spawn_dist: + if spawn_dist is not None: self._road_spawn_dist = spawn_dist + if sources_active is not None: + for lane_key in self._road_dict: + self._road_dict[lane_key].active = sources_active + self._get_road_radius() - py_trees.blackboard.Blackboard().set("BA_RoadBehavior", None, True) + py_trees.blackboard.Blackboard().set("BA_ChangeRoadBehavior", None, True) - opposite_behavior_data = py_trees.blackboard.Blackboard().get("BA_OppositeBehavior") + # Extend the space of a specific exit lane + road_exit_data = py_trees.blackboard.Blackboard().get("BA_ExtentExitRoadSpace") + if road_exit_data: + space, direction = road_exit_data + self._extent_road_exit_space(space, direction) + py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", None, True) + + # Opposite behavior + opposite_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeOppositeBehavior") if opposite_behavior_data: source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data - if source_dist: + if source_dist is not None: if source_dist < self._junction_sources_dist: print("WARNING: Opposite sources distance is lower than the junction ones. Ignoring it") else: self._opposite_sources_dist = source_dist - if vehicle_dist: + if vehicle_dist is not None: self._opposite_vehicle_dist = vehicle_dist - if spawn_dist: + if spawn_dist is not None: self._opposite_spawn_dist = spawn_dist - if max_actors: + if max_actors is not None: self._opposite_sources_max_actors = max_actors - py_trees.blackboard.Blackboard().set("BA_OppositeBehavior", None, True) + py_trees.blackboard.Blackboard().set("BA_ChangeOppositeBehavior", None, True) - junction_behavior_data = py_trees.blackboard.Blackboard().get("BA_JunctionBehavior") + # Junction behavior + junction_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeJunctionBehavior") if junction_behavior_data: source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data if source_dist: @@ -1672,39 +1693,52 @@ def _update_parameters(self): self._junction_spawn_dist = spawn_dist if max_actors: self._junction_sources_max_actors = max_actors - py_trees.blackboard.Blackboard().set("BA_JunctionBehavior", None, True) - - break_duration = py_trees.blackboard.Blackboard().get("BA_Scenario2") - if break_duration: - if self._is_scenario_2_active: - print("WARNING: A break scenario was requested but another one is already being triggered.") - else: - self._activate_break_scenario = True - self._break_duration = break_duration - py_trees.blackboard.Blackboard().set("BA_Scenario2", None, True) - - crossing_dist = py_trees.blackboard.Blackboard().get("BA_Scenario4") + py_trees.blackboard.Blackboard().set("BA_ChangeJunctionBehavior", None, True) + + # Stop front vehicles + stop_data = py_trees.blackboard.Blackboard().get("BA_StopFrontVehicles") + if stop_data: + self._stop_road_front_vehicles() + py_trees.blackboard.Blackboard().set("BA_StopFrontVehicles", None, True) + + # Start front vehicles + start_data = py_trees.blackboard.Blackboard().get("BA_StartFrontVehicles") + if start_data: + self._start_road_front_vehicles() + py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) + + # Handles crossing actor scenarios. This currently only works for Scenario4 + crossing_dist = py_trees.blackboard.Blackboard().get("BA_HandleCrossingActor") if crossing_dist: - self._is_scenario_4_active = True + self._is_crossing_scenario_active = True self._crossing_dist = crossing_dist - py_trees.blackboard.Blackboard().set("BA_Scenario4", None, True) - - direction = py_trees.blackboard.Blackboard().get("BA_Scenario7") - if direction: - self._initialise_junction_scenario(direction, True, True, True) - py_trees.blackboard.Blackboard().set("BA_Scenario7", None, True) - direction = py_trees.blackboard.Blackboard().get("BA_Scenario8") - if direction: - self._initialise_junction_scenario(direction, True, True, True) - py_trees.blackboard.Blackboard().set("BA_Scenario8", None, True) - direction = py_trees.blackboard.Blackboard().get("BA_Scenario9") - if direction: - self._initialise_junction_scenario(direction, True, False, True) - py_trees.blackboard.Blackboard().set("BA_Scenario9", None, True) - direction = py_trees.blackboard.Blackboard().get("BA_Scenario10") - if direction: - self._initialise_junction_scenario(direction, False, False, False) - py_trees.blackboard.Blackboard().set("BA_Scenario10", None, True) + py_trees.blackboard.Blackboard().set("BA_HandleCrossingActor", None, True) + + # Handles junction scenarios (scenarios 7 to 10) + junction_scenario_data = py_trees.blackboard.Blackboard().get("BA_JunctionScenario") + if junction_scenario_data: + direction, remove_entries, remove_exits, remove_middle = junction_scenario_data + self._initialise_junction_scenario(direction, remove_entries, remove_exits, remove_middle) + py_trees.blackboard.Blackboard().set("BA_JunctionScenario", None, True) + + # Handles road accident scenario (Accident and Construction) + handle_start_accident_data = py_trees.blackboard.Blackboard().get("BA_HandleStartAccidentScenario") + if handle_start_accident_data: + accident_wp, distance = handle_start_accident_data + self._handle_lanechange_scenario(accident_wp, distance) + py_trees.blackboard.Blackboard().set("BA_HandleStartAccidentScenario", None, True) + + # Handles road accident scenario (Accident and Construction) + handle_end_accident_data = py_trees.blackboard.Blackboard().get("BA_HandleEndAccidentScenario") + if handle_end_accident_data: + self._road_extra_front_actors = 0 + py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", None, True) + + # Stops non route entries + stop_entry_data = py_trees.blackboard.Blackboard().get("BA_StopEntries") + if stop_entry_data: + self._stop_non_route_entries() + py_trees.blackboard.Blackboard().set("BA_StopEntries", None, True) self._compute_parameters() @@ -1716,7 +1750,7 @@ def _compute_parameters(self): # Partially avoid this by adding an extra distance to the radius when the vehicle is stopped # in the middle of the road and unaffected by any object such as traffic lights or stops. if ego_speed == 0 \ - and not self._is_scenario_2_active \ + and len(self._stopped_road_actors) == 0 \ and not self._ego_actor.is_at_traffic_light() \ and len(self._active_junctions) <= 0: self._extra_radius = min(self._extra_radius + self._extra_radius_increase_ratio, self._max_extra_radius) @@ -1729,36 +1763,81 @@ def _compute_parameters(self): self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed + self._extra_radius self._junction_detection_dist = self._max_radius - def _manage_break_scenario(self): + def _stop_road_front_vehicles(self): """ Manages the break scenario, where all road vehicles in front of the ego suddenly stop, wait for a bit, and start moving again. This will never trigger unless done so from outside. """ - if self._is_scenario_2_active: - self._next_scenario_time -= self._world.get_snapshot().timestamp.delta_seconds - if self._next_scenario_time <= 0: - for actor in self._scenario_2_actors: - self._tm.vehicle_percentage_speed_difference(actor, 0) - lights = actor.get_light_state() - lights &= ~carla.VehicleLightState.Brake - actor.set_light_state(carla.VehicleLightState(lights)) - self._scenario_2_actors = [] - - self._is_scenario_2_active = False - - elif self._activate_break_scenario: - for actor in self._road_actors: + for lane in self._road_dict: + for actor in self._road_dict[lane].actors: location = CarlaDataProvider.get_location(actor) if location and not self._is_location_behind_ego(location): - self._scenario_2_actors.append(actor) + self._stopped_road_actors.append(actor) self._tm.vehicle_percentage_speed_difference(actor, 100) lights = actor.get_light_state() lights |= carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) - self._is_scenario_2_active = True - self._activate_break_scenario = False - self._next_scenario_time = self._break_duration + def _start_road_front_vehicles(self): + """ + Manages the break scenario, where all road vehicles in front of the ego suddenly stop, + wait for a bit, and start moving again. This will never trigger unless done so from outside. + """ + for actor in self._stopped_road_actors: + self._tm.vehicle_percentage_speed_difference(actor, 0) + lights = actor.get_light_state() + lights &= ~carla.VehicleLightState.Brake + actor.set_light_state(carla.VehicleLightState(lights)) + self._stopped_road_actors = [] + + def _extent_road_exit_space(self, space, direction): + """Increases the space left by the exit vehicles at a specific road""" + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + actor_dict = self._active_junctions[0].actor_dict + for actor in actor_dict: + if actor_dict[actor]['state'] == JUNCTION_INACTIVE_NAME: + self._tm.vehicle_percentage_speed_difference(actor, 0) + actor_dict[actor]['state'] = JUNCTION_EXIT_NAME + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return + + direction_lane_keys = junction.exit_directions[direction] + exit_dict = junction.exit_dict + for exit_lane_key in exit_dict: + if exit_lane_key in direction_lane_keys: + exit_dict[exit_lane_key]['max_distance'] += space + + def _stop_non_route_entries(self): + if len(self._active_junctions) > 0: + entry_sources = self._active_junctions[0].entry_sources + for source in entry_sources: + for actor in source.actors: + self._tm.vehicle_percentage_speed_difference(actor, 100) + elif len(self._junctions) > 0: + self._junctions[0].stop_entries = True + + def _handle_lanechange_scenario(self, accident_wp, distance): + """ + Handles the scenario in which the BA has to change lane. + """ + ego_wp = self._route[self._route_index] + lane_change_actors = self._road_dict[get_lane_key(ego_wp)].actors + for actor in lane_change_actors: + location = CarlaDataProvider.get_location(actor) + if not self._is_location_behind_ego(location): + lanechange_wp = accident_wp.get_left_lane().next(distance/4)[0] + end_lanechange_wp = lanechange_wp.next(distance/2)[0] + vehicle_path = [lanechange_wp.transform.location, + end_lanechange_wp.transform.location, + end_lanechange_wp.get_right_lane().next(distance/4)[0].transform.location] + self._tm.set_path(actor, vehicle_path) + ## maybe check here to activate lane changing lights + self._road_extra_front_actors += 1 + else: + self._tm.vehicle_percentage_speed_difference(actor, 100) ############################# ## Actor functions ## @@ -1849,50 +1928,53 @@ def _get_ego_route_lane_key(self, route_wp): leftmost_wp = possible_left_wp return get_lane_key(leftmost_wp) - def _update_road_actors(self, prev_ego_index, current_ego_index): + def _update_road_actors(self): """ Dynamically controls the actor speed in front of the ego. Not applied to those behind it so that they can catch up it """ - route_wp = self._route[current_ego_index] - scenario_actors = self._scenario_4_actors + self._scenario_2_actors - for actor in self._road_actors: - location = CarlaDataProvider.get_location(actor) - if not location: - continue - if self.debug: - back_actor = False - for lane in self._road_back_actors: - if actor in self._road_back_actors[lane]: - back_actor = True - if back_actor: - draw_string(self._world, location, 'R(B)', 'road', False) - else: - draw_string(self._world, location, 'R(F)', 'road', False) - if actor in scenario_actors: - continue - if self._is_location_behind_ego(location): - continue + # Updates their speed + route_wp = self._route[self._route_index] + scenario_actors = self._crossing_scenario_actors + self._stopped_road_actors + for lane in self._road_dict: + for actor in self._road_dict[lane].actors: + location = CarlaDataProvider.get_location(actor) + if not location: + continue + if self.debug: + if self._is_location_behind_ego(location): + draw_string(self._world, location, 'R(B)', 'road', False) + else: + draw_string(self._world, location, 'R(F)', 'road', False) + if actor in scenario_actors or self._is_location_behind_ego(location): + continue - distance = location.distance(route_wp.transform.location) - speed_red = (distance - self._min_radius) / (self._max_radius - self._min_radius) * 100 - speed_red = np.clip(speed_red, 0, 100) - self._tm.vehicle_percentage_speed_difference(actor, speed_red) + distance = location.distance(route_wp.transform.location) + speed_red = (distance - self._min_radius) / (self._max_radius - self._min_radius) * 100 + speed_red = np.clip(speed_red, 0, 100) + self._tm.vehicle_percentage_speed_difference(actor, speed_red) - # Check how the vehicles behind are - self._check_back_vehicles(prev_ego_index, current_ego_index) + if not self._road_ego_key in self._road_dict: + return + + front_actors = 0 + for actor in self._road_dict[self._road_ego_key].actors: + if not self._is_location_behind_ego(actor.get_location()): + front_actors += 1 + self._road_num_front_vehicles = front_actors - def _check_back_vehicles(self, prev_route_index, current_route_index): + self._get_road_radius() + self._compute_parameters() + + def _monitor_road_changes(self, prev_route_index): """ - Checks if any of the vehicles that should be behind the ego are in front, updating the road radius. - This is done by monitoring the closest lane key to the ego that is part of the route, - and needs some remaping when the ego enters a new road + Checks if the ego changes road, remapping the route keys, and creating new sources (if needed) """ - route_wp = self._route[current_route_index] + route_wp = self._route[ self._route_index] prev_route_wp = self._route[prev_route_index] check_dist = 1.1 * route_wp.transform.location.distance(prev_route_wp.transform.location) - if prev_route_index != current_route_index: - road_change = self._found_a_road_change(prev_route_index, current_route_index, ignore_false_junctions=False) + if prev_route_index != self._route_index: + road_change = self._found_a_road_change(prev_route_index, self._route_index, ignore_false_junctions=False) if not self._is_junction(prev_route_wp) and road_change: # Get all the wps of the new road if not route_wp.is_junction: @@ -1925,30 +2007,26 @@ def _check_back_vehicles(self, prev_route_index, current_route_index): if mapped_wp: unmapped_wps.remove(mapped_wp) + old_wps.remove(old_wp) mapped_keys[get_lane_key(old_wp)] = get_lane_key(mapped_wp) # Remake the road back actors dictionary - new_road_back_actors = {} - for lane_key in self._road_back_actors: + new_road_dict = {} + for lane_key in self._road_dict: if lane_key not in mapped_keys: continue # A lane ended at that road new_lane_key = mapped_keys[lane_key] - new_road_back_actors[new_lane_key] = self._road_back_actors[lane_key] + new_road_dict[new_lane_key] = self._road_dict[lane_key] - # For the active sources, change the mapped key to the new road keys - for source in self._road_sources: - if source.mapped_key in mapped_keys: - source.mapped_key = mapped_keys[source.mapped_key] - self._road_back_actors = new_road_back_actors + self._road_dict = new_road_dict # New lanes, add new sources for unmapped_wp in unmapped_wps: source_wps = unmapped_wp.next(self._road_new_sources_dist) if len(source_wps) != 1: continue - new_source = Source(source_wps[0], []) - self._road_sources.append(new_source) - self._road_back_actors[new_source.mapped_key] = [] + new_lane_wp = source_wps[0] + self._road_dict[get_lane_key(new_lane_wp)] = Source(new_lane_wp, []) if not self._road_ego_key in mapped_keys: # Return the default. This might happen when the route lane ends and should be fixed next frame @@ -1958,18 +2036,6 @@ def _check_back_vehicles(self, prev_route_index, current_route_index): else: self._road_ego_key = self._get_ego_route_lane_key(route_wp) - # Get the amount of vehicles in front of the ego - if not self._road_ego_key in self._road_back_actors: - return - - self._road_extra_front_actors = 0 - for actor in self._road_back_actors[self._road_ego_key]: - if not self._is_location_behind_ego(actor.get_location()): - self._road_extra_front_actors += 1 - - self._get_road_radius() - self._compute_parameters() - def _update_junction_actors(self): """ Handles an actor depending on their previous state. Actors entering the junction have its exit @@ -2008,16 +2074,16 @@ def _update_junction_actors(self): draw_string(self._world, location, string, self._ego_state, False) # Monitor its entry - if state == 'junction_entry': + if state == JUNCTION_ENTRY_NAME: actor_wp = self._map.get_waypoint(location) if self._is_junction(actor_wp) and junction.contains(actor_wp.get_junction()): if remove_middle: self._destroy_actor(actor) # Don't clutter the junction if a junction scenario is active continue - actor_dict[actor]['state'] = 'junction_middle' + actor_dict[actor]['state'] = JUNCTION_MIDDLE_NAME # Monitor its exit and destroy an actor if needed - elif state == 'junction_middle': + elif state == JUNCTION_MIDDLE_NAME: actor_wp = self._map.get_waypoint(location) actor_lane_key = get_lane_key(actor_wp) if not self._is_junction(actor_wp) and actor_lane_key in exit_dict: @@ -2038,7 +2104,7 @@ def _update_junction_actors(self): else: # Check the lane capacity exit_dict[actor_lane_key]['ref_wp'] = actor_wp - actor_dict[actor]['state'] = 'junction_exit' + actor_dict[actor]['state'] = JUNCTION_EXIT_NAME actor_dict[actor]['exit_lane_key'] = actor_lane_key actors = exit_dict[actor_lane_key]['actors'] @@ -2047,20 +2113,20 @@ def _update_junction_actors(self): actors.append(actor) # Deactivate them when far from the junction - elif state == 'junction_exit': + elif state == JUNCTION_EXIT_NAME: distance = location.distance(exit_dict[exit_lane_key]['ref_wp'].transform.location) if distance > exit_dict[exit_lane_key]['max_distance']: self._tm.vehicle_percentage_speed_difference(actor, 100) - actor_dict[actor]['state'] = 'junction_inactive' + actor_dict[actor]['state'] = JUNCTION_INACTIVE_NAME # Wait for something to happen - elif state == 'junction_inactive': + elif state == JUNCTION_INACTIVE_NAME: pass - def _update_opposite_actors(self, ref_transform): + def _update_opposite_actors(self): """ Updates the opposite actors. This involves tracking their position, - removing if too far behind the ego + removing them if too far behind the ego. """ max_dist = max(self._opposite_removal_dist, self._opposite_spawn_dist) for actor in list(self._opposite_actors): @@ -2069,31 +2135,25 @@ def _update_opposite_actors(self, ref_transform): continue if self.debug: draw_string(self._world, location, 'O', 'opposite', False) - distance = location.distance(ref_transform.location) + distance = location.distance(self._ego_wp.transform.location) if distance > max_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) def _remove_actor_info(self, actor): """Removes all the references of the actor""" - if actor in self._road_actors: - self._road_actors.remove(actor) - if actor in self._opposite_actors: - self._opposite_actors.remove(actor) - if actor in self._scenario_2_actors: - self._scenario_2_actors.remove(actor) - if actor in self._scenario_4_actors: - self._scenario_4_actors.remove(actor) - - for road_source in self._road_sources: - if actor in road_source.actors: - road_source.actors.remove(actor) - break - for lane in self._road_back_actors: - if actor in self._road_back_actors[lane]: - self._road_back_actors[lane].remove(actor) + for lane in self._road_dict: + if actor in self._road_dict[lane].actors: + self._road_dict[lane].actors.remove(actor) break + if actor in self._opposite_actors: + self._opposite_actors.remove(actor) + if actor in self._stopped_road_actors: + self._stopped_road_actors.remove(actor) + if actor in self._crossing_scenario_actors: + self._crossing_scenario_actors.remove(actor) + for opposite_source in self._opposite_sources: if actor in opposite_source.actors: opposite_source.actors.remove(actor) @@ -2102,11 +2162,6 @@ def _remove_actor_info(self, actor): for junction in self._active_junctions: junction.actor_dict.pop(actor, None) - for exit_source in junction.exit_sources: - if actor in exit_source.actors: - exit_source.actors.remove(actor) - break - for entry_source in junction.entry_sources: if actor in entry_source.actors: entry_source.actors.remove(actor) @@ -2126,15 +2181,25 @@ def _destroy_actor(self, actor): except RuntimeError: pass - def _update_ego_route_location(self, location): - """Returns the closest route location to the ego""" - for index in range(self._route_index, min(self._route_index + self._route_buffer, self._route_length)): + def _update_ego_route_location(self): + """ + Checks the ego location to see if i has moved closer to the next route waypoint, + updating its information. This never checks for backwards movements to avoid unnedded confusion + """ + location = CarlaDataProvider.get_location(self._ego_actor) + for index in range(self._route_index, min(self._route_index + self._route_buffer, self._route_length)): route_wp = self._route[index] + route_wp_dir = route_wp.transform.get_forward_vector() # Waypoint's forward vector veh_wp_dir = location - route_wp.transform.location # vector waypoint - vehicle dot_ve_wp = veh_wp_dir.x * route_wp_dir.x + veh_wp_dir.y * route_wp_dir.y + veh_wp_dir.z * route_wp_dir.z + if dot_ve_wp > 0: self._route_index = index - return self._route[self._route_index] + self._ego_wp = self._route[self._route_index] + + if self.debug: + string = 'EGO_' + self._ego_state[0].upper() + draw_string(self._world, location, string, self._ego_state, False) diff --git a/srunner/scenarios/bycicle_group.py b/srunner/scenarios/bycicle_group.py new file mode 100644 index 000000000..b8c064875 --- /dev/null +++ b/srunner/scenarios/bycicle_group.py @@ -0,0 +1,113 @@ +#!/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 . + +""" +Scenarios in which another (opposite) vehicle 'illegally' takes +priority, e.g. by running a red traffic light. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, BasicAgentBehavior +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTimeToArrivalToVehicle +from srunner.scenarios.basic_scenario import BasicScenario + +def convert_dict_to_transform(actor_dict): + """ + Convert a JSON string to a CARLA transform + """ + return carla.Transform( + carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ), + carla.Rotation( + roll=0.0, + pitch=0.0, + yaw=float(actor_dict['yaw']) + ) + ) + +class BycicleGroup(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self._bycicle_speed = float(config.other_parameters['bycicle_speed']['value']) + self._offset = float(config.other_parameters['bycicle_offset']['value']) + self.timeout = timeout + self._drive_distance = 200 + self._arrival_time = 7 + + super(BycicleGroup, self).__init__("BycicleGroup", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + bycicle_transform = config.other_actors[0].transform + bycicle_wp = self._map.get_waypoint(bycicle_transform.location) + + # Displace the wp to the side + self._displacement = self._offset * bycicle_wp.lane_width / 2 + r_vec = bycicle_wp.transform.get_right_vector() + w_loc = bycicle_wp.transform.location + w_loc += carla.Location(x=self._displacement * r_vec.x, y=self._displacement * r_vec.y) + bycicle_transform = carla.Transform(w_loc, bycicle_wp.transform.rotation) + + bycicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, bycicle_transform) + self.other_actors.append(bycicle) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + sequence = py_trees.composites.Sequence() + behavior = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + sequence.add_child(InTimeToArrivalToVehicle(self.ego_vehicles[0], self.other_actors[0], self._arrival_time)) + behavior.add_child(BasicAgentBehavior( + self.other_actors[0], target_speed=self._bycicle_speed, opt_dict={'offset': self._displacement})) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + sequence.add_child(behavior) + sequence.add_child(ActorDestroy(self.other_actors[0])) + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index d6c2f228d..11e0fff0f 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -16,10 +16,27 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle -from srunner.tools.scenario_helper import get_location_in_distance_from_wp from srunner.scenarios.object_crash_vehicle import StationaryObjectCrossing +from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario +def convert_dict_to_transform(actor_dict): + """ + Convert a JSON string to a CARLA transform + """ + return carla.Transform( + carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ), + carla.Rotation( + roll=0.0, + pitch=0.0, + yaw=float(actor_dict['yaw']) + ) + ) class ConstructionSetupCrossing(StationaryObjectCrossing): @@ -31,49 +48,46 @@ class ConstructionSetupCrossing(StationaryObjectCrossing): This is a single ego vehicle scenario """ - 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 + and instantiate scenario manager """ - super( - ConstructionSetupCrossing, - self).__init__( - world, - ego_vehicles=ego_vehicles, - config=config, - randomize=randomize, - debug_mode=debug_mode, - criteria_enable=criteria_enable) + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 150 + self._distance_to_construction = 100 + self.construction_wp = None + + super(ConstructionSetupCrossing, self).__init__(world, + ego_vehicles, + config, + randomize, + debug_mode, + criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ - _start_distance = 40 lane_width = self._reference_waypoint.lane_width - location, _ = get_location_in_distance_from_wp( - self._reference_waypoint, _start_distance) - waypoint = self._wmap.get_waypoint(location) - self._create_construction_setup(waypoint.transform, lane_width) - - def create_cones_side( - self, - start_transform, - forward_vector, - z_inc=0, - cone_length=0, - cone_offset=0): - """ - Creates One Side of the Cones - """ + starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + construction_wps = starting_wp.next(self._distance_to_construction) + if not construction_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + self.construction_wp = construction_wps[0] + + self._create_construction_setup(self.construction_wp.transform, lane_width) + pre_accident_wps = starting_wp.next(self._distance_to_construction/2) + if not construction_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + self.construction_wp = pre_accident_wps[0] + + def create_cones_side(self, start_transform, forward_vector, z_inc=0, + cone_length=0, cone_offset=0): + _dist = 0 while _dist < (cone_length * cone_offset): # Move forward @@ -104,7 +118,7 @@ def _create_construction_setup(self, start_transform, lane_width): _setup = {'lengths': [0, 6, 3], 'offsets': [0, 2, 1]} _z_increment = 0.1 - ############################# Traffic Warning and Debris ############## + # Traffic warning and debris for key, value in _initial_offset.items(): if key == 'cones': continue @@ -121,7 +135,7 @@ def _create_construction_setup(self, start_transform, lane_width): static.set_simulate_physics(True) self.other_actors.append(static) - ############################# Cones ################################### + # Cones side_transform = carla.Transform( start_transform.location, start_transform.rotation) @@ -145,21 +159,27 @@ def _create_behavior(self): """ Only behavior here is to wait """ - # leaf nodes - actor_stand = Idle(15) - - end_condition = DriveDistance( - self.ego_vehicles[0], - self._ego_vehicle_distance_driven) - - # non leaf nodes - scenario_sequence = py_trees.composites.Sequence() + root = py_trees.composites.Sequence() + if CarlaDataProvider.get_ego_vehicle_route(): + root.add_child(HandleStartAccidentScenario(self.construction_wp, self._distance_to_construction)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if CarlaDataProvider.get_ego_vehicle_route(): + root.add_child(HandleEndAccidentScenario()) + root.add_child(Idle(15)) + for i, _ in enumerate(self.other_actors): + root.add_child(ActorDestroy(self.other_actors[i])) + return root - # building tree - scenario_sequence.add_child(actor_stand) - for i, _ in enumerate(self.other_actors): - scenario_sequence.add_child(ActorDestroy(self.other_actors[i])) - scenario_sequence.add_child(end_condition) + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] - return scenario_sequence + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/follow_leading_vehicle.py b/srunner/scenarios/follow_leading_vehicle.py index b87a97912..dc3a7af57 100644 --- a/srunner/scenarios/follow_leading_vehicle.py +++ b/srunner/scenarios/follow_leading_vehicle.py @@ -38,7 +38,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_waypoint_in_distance -from srunner.tools.background_manager import Scenario2Manager +from srunner.tools.background_manager import StopFrontVehicles, StartFrontVehicles class FollowLeadingVehicle(BasicScenario): @@ -330,7 +330,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals """ self.timeout = timeout self._stop_duration = 15 - self._end_time_condition = 30 + self.end_distance = 15 super(FollowLeadingVehicleRoute, self).__init__("FollowLeadingVehicleRoute", ego_vehicles, @@ -351,8 +351,10 @@ def _create_behavior(self): then waits for a bit to check if the actor has collided. """ sequence = py_trees.composites.Sequence("FollowLeadingVehicleRoute") - sequence.add_child(Scenario2Manager(self._stop_duration)) - sequence.add_child(Idle(self._end_time_condition)) + sequence.add_child(StopFrontVehicles()) + sequence.add_child(Idle(self._stop_duration)) + sequence.add_child(StartFrontVehicles()) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self.end_distance)) return sequence diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py new file mode 100644 index 000000000..22f2586e9 --- /dev/null +++ b/srunner/scenarios/highway_cut_in.py @@ -0,0 +1,115 @@ +#!/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 . + +""" +Scenarios in which another (opposite) vehicle 'illegally' takes +priority, e.g. by running a red traffic light. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, WaypointFollower, LaneChange, AccelerateToVelocity, SetInitSpeed +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, WaitUntilInFront +from srunner.scenarios.basic_scenario import BasicScenario + +class HighwayEntryCutIn(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 100 + self._other_init_speed = 100 / 3.6 + self._other_end_speed = 70 / 3.6 + self._factor = 0 + + super(HighwayEntryCutIn, self).__init__("HighwayEntryCutIn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + starting_location = config.other_actors[0].transform.location + starting_waypoint = self._map.get_waypoint(starting_location) + + other_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, starting_waypoint.transform) + self.other_actors.append(other_vehicle) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + + behaviour = py_trees.composites.Sequence("ExitFreewayCutIn") + + # Make the vehicle "sync" with the ego_vehicle + sync_arrival = py_trees.composites.Parallel( + "Sync Arrival", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + sync_arrival.add_child(WaitUntilInFront(self.other_actors[0], self.ego_vehicles[0], factor=self._factor)) + + sync_behavior = py_trees.composites.Sequence() + sync_behavior.add_child(SetInitSpeed(self.other_actors[0], self._other_init_speed)) + sync_behavior.add_child(WaypointFollower(self.other_actors[0], self._other_init_speed * 1000)) + sync_arrival.add_child(sync_behavior) + + # Force a lane change + lane_change = LaneChange( + self.other_actors[0], + speed=self._other_end_speed, + direction='left', + distance_same_lane=1, + distance_other_lane=30, + distance_lane_change=60) + + # End of the scenario + end_condition = py_trees.composites.Parallel( + "End condition", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(WaypointFollower(self.other_actors[0], self._other_end_speed)) + + # behaviour.add_child(init_speed) + behaviour.add_child(sync_arrival) + behaviour.add_child(lane_change) + behaviour.add_child(end_condition) + behaviour.add_child(ActorDestroy(self.other_actors[0])) + + return behaviour + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index da11294f9..9ad74a61c 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -27,7 +27,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route -from srunner.tools.background_manager import Scenario4Manager +from srunner.tools.background_manager import HandleCrossingActor def get_sidewalk_transform(waypoint): @@ -163,7 +163,7 @@ def _create_behavior(self): # On trigger behavior if self._ego_route is not None: - sequence.add_child(Scenario4Manager(self._spawn_dist)) + sequence.add_child(HandleCrossingActor(self._spawn_dist)) # First waiting behavior sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index fed6a7a81..0e4ff40da 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -31,7 +31,7 @@ filter_junction_wp_direction, get_closest_traffic_light) -from srunner.tools.background_manager import Scenario7Manager +from srunner.tools.background_manager import JunctionScenarioManager class OppositeVehicleRunningRedLight(BasicScenario): @@ -184,7 +184,7 @@ def _create_behavior(self): root = py_trees.composites.Sequence() if CarlaDataProvider.get_ego_vehicle_route(): - root.add_child(Scenario7Manager(self._direction)) + root.add_child(JunctionScenarioManager(self._direction, True, True, True)) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(main_behavior) root.add_child(ActorDestroy(self.other_actors[0])) diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py new file mode 100644 index 000000000..04acbde3c --- /dev/null +++ b/srunner/scenarios/route_obstacles.py @@ -0,0 +1,155 @@ +#!/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 . + +""" +Scenarios in which another (opposite) vehicle 'illegally' takes +priority, e.g. by running a red traffic light. +""" + +from __future__ import print_function +from distutils.log import error + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario + +def convert_dict_to_transform(actor_dict): + """ + Convert a JSON string to a CARLA transform + """ + return carla.Transform( + carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ), + carla.Rotation( + roll=0.0, + pitch=0.0, + yaw=float(actor_dict['yaw']) + ) + ) + +class Accident(BasicScenario): + """ + This class holds everything required for a scenario in which there is an accident + in front of the ego, forcing it to react. A police vehicle is located before + two other cars that have been in an accident. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 120 + self._distance_to_accident = 100 + self._offset = 0.75 + self._first_distance = 10 + self._second_distance = 6 + self._accident_wp = None + + super(Accident, self).__init__("Accident", + ego_vehicles, + config, + world, + randomize, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + accident_wps = starting_wp.next(self._distance_to_accident) + pre_accident_wps = starting_wp.next(self._distance_to_accident/2) + if not accident_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + if not pre_accident_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + self._accident_wp = accident_wps[0] + + # Create the police vehicle + displacement = self._offset * self._accident_wp.lane_width / 2 + r_vec = self._accident_wp.transform.get_right_vector() + w_loc = self._accident_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) + police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) + police_lights = carla.VehicleLightState.Special1 + police_lights |= carla.VehicleLightState.Special2 + police_lights |= carla.VehicleLightState.Position + police_car.set_light_state(carla.VehicleLightState(police_lights)) + self.other_actors.append(police_car) + + # Create the first vehicle that has been in the accident + vehicle_wps = self._accident_wp.next(self._first_distance) + if not vehicle_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + vehicle_wp = vehicle_wps[0] + self._accident_wp = pre_accident_wps[0] + displacement = self._offset * vehicle_wp.lane_width / 2 + r_vec = vehicle_wp.transform.get_right_vector() + w_loc = vehicle_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + vehicle_1_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) + vehicle_1_car = CarlaDataProvider.request_new_actor('vehicle.tesla.model3', vehicle_1_transform) + self.other_actors.append(vehicle_1_car) + + # Create the second vehicle that has been in the accident + vehicle_wps = vehicle_wp.next(self._second_distance) + if not vehicle_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + vehicle_wp = vehicle_wps[0] + + displacement = self._offset * vehicle_wp.lane_width / 2 + r_vec = vehicle_wp.transform.get_right_vector() + w_loc = vehicle_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + vehicle_2_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) + vehicle_2_car = CarlaDataProvider.request_new_actor('vehicle.mercedes.coupe_2020', vehicle_2_transform) + self.other_actors.append(vehicle_2_car) + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. + """ + root = py_trees.composites.Sequence() + if CarlaDataProvider.get_ego_vehicle_route(): + root.add_child(HandleStartAccidentScenario(self._accident_wp, self._distance_to_accident)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if CarlaDataProvider.get_ego_vehicle_route(): + root.add_child(HandleEndAccidentScenario()) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(ActorDestroy(self.other_actors[2])) + + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index 2e04497ad..a05aa783a 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -24,7 +24,7 @@ filter_junction_wp_direction, get_closest_traffic_light) -from srunner.tools.background_manager import Scenario8Manager +from srunner.tools.background_manager import JunctionScenarioManager class SignalizedJunctionLeftTurn(BasicScenario): @@ -144,7 +144,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence("Sequence Behavior") if CarlaDataProvider.get_ego_vehicle_route(): - sequence.add_child(Scenario8Manager(self._direction)) + sequence.add_child(JunctionScenarioManager(self._direction, True, True, True)) sequence.add_child(root) return sequence diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index 47e855c8d..631541212 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -26,7 +26,7 @@ filter_junction_wp_direction, get_closest_traffic_light) -from srunner.tools.background_manager import Scenario9Manager +from srunner.tools.background_manager import JunctionScenarioManager class SignalizedJunctionRightTurn(BasicScenario): @@ -144,7 +144,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence("Sequence") if CarlaDataProvider.get_ego_vehicle_route(): - sequence.add_child(Scenario9Manager(self._direction)) + sequence.add_child(JunctionScenarioManager(self._direction, True, False, True)) sequence.add_child(root) return sequence diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 5519a5af8..27058e470 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -13,36 +13,38 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior -class RoadBehaviorManager(AtomicBehavior): +class ChangeRoadBehavior(AtomicBehavior): """ Updates the blackboard to change the parameters of the road behavior. - None values imply that these values won't be changed + None values imply that these values won't be changed. Args: num_front_vehicles (int): Amount of vehicles in front of the ego. Can't be negative num_back_vehicles (int): Amount of vehicles behind it. Can't be negative vehicle_dist (float): Minimum distance between the road vehicles. Must between 0 and 'spawn_dist' spawn_dist (float): Minimum distance between spawned vehicles. Must be positive + switch_source (bool): (De)activatea the road sources. """ def __init__(self, num_front_vehicles=None, num_back_vehicles=None, - vehicle_dist=None, spawn_dist=None, name="RoadBehaviorManager"): + vehicle_dist=None, spawn_dist=None, switch_source=None, name="ChangeRoadBehavior"): self._num_front_vehicles = num_front_vehicles self._num_back_vehicles = num_back_vehicles self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist - super(RoadBehaviorManager, self).__init__(name) + self._switch_source = switch_source + super(ChangeRoadBehavior, self).__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_RoadBehavior", - [self._num_front_vehicles, self._num_back_vehicles, self._vehicle_dist, self._spawn_dist], + "BA_ChangeRoadBehavior", + [self._num_front_vehicles, self._num_back_vehicles, self._vehicle_dist, self._spawn_dist, self._switch_source], overwrite=True ) return py_trees.common.Status.SUCCESS -class OppositeBehaviorManager(AtomicBehavior): +class ChangeOppositeBehavior(AtomicBehavior): """ Updates the blackboard to change the parameters of the opposite road behavior. None values imply that these values won't be changed @@ -55,23 +57,23 @@ class OppositeBehaviorManager(AtomicBehavior): """ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, - max_actors=None, name="OppositeBehaviorManager"): + max_actors=None, name="ChangeOppositeBehavior"): self._source_dist = source_dist self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist self._max_actors = max_actors - super(OppositeBehaviorManager, self).__init__(name) + super(ChangeOppositeBehavior, self).__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_OppositeBehavior", + "BA_ChangeOppositeBehavior", [self._source_dist, self._vehicle_dist, self._spawn_dist, self._max_actors], overwrite=True ) return py_trees.common.Status.SUCCESS -class JunctionBehaviorManager(AtomicBehavior): +class ChangeJunctionBehavior(AtomicBehavior): """ Updates the blackboard to change the parameters of the junction behavior. None values imply that these values won't be changed @@ -85,116 +87,153 @@ class JunctionBehaviorManager(AtomicBehavior): """ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, - max_actors=None, name="JunctionBehaviorManager"): + max_actors=None, name="ChangeJunctionBehavior"): self._source_dist = source_dist self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist self._max_actors = max_actors - super(JunctionBehaviorManager, self).__init__(name) + super(ChangeJunctionBehavior, self).__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_JunctionBehavior", + "BA_ChangeJunctionBehavior", [self._source_dist, self._vehicle_dist, self._spawn_dist, self._max_actors], overwrite=True ) return py_trees.common.Status.SUCCESS -class Scenario2Manager(AtomicBehavior): +class ActivateHardBreakScenario(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario2 has to be triggered. + Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. 'stop_duration' is the amount of time, in seconds, the vehicles will be stopped """ - def __init__(self, stop_duration=10, name="Scenario2Manager"): + def __init__(self, stop_duration=10, name="ActivateHardBreakScenario"): self._stop_duration = stop_duration - super(Scenario2Manager, self).__init__(name) + super(ActivateHardBreakScenario, self).__init__(name) + + def update(self): + py_trees.blackboard.Blackboard().set("BA_ActivateHardBreakScenario", self._stop_duration, overwrite=True) + return py_trees.common.Status.SUCCESS + +class StopFrontVehicles(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. + 'stop_duration' is the amount of time, in seconds, the vehicles will be stopped + """ + + def __init__(self, name="StopFrontVehicles"): + super(StopFrontVehicles, self).__init__(name) + + def update(self): + py_trees.blackboard.Blackboard().set("BA_StopFrontVehicles", True, overwrite=True) + return py_trees.common.Status.SUCCESS + + +class StartFrontVehicles(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. + 'stop_duration' is the amount of time, in seconds, the vehicles will be stopped + """ + + def __init__(self, name="StartFrontVehicles"): + super(StartFrontVehicles, self).__init__(name) def update(self): - py_trees.blackboard.Blackboard().set("BA_Scenario2", self._stop_duration, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", True, overwrite=True) return py_trees.common.Status.SUCCESS -class Scenario4Manager(AtomicBehavior): +class HandleCrossingActor(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario4 has been triggered. + Updates the blackboard to tell the background activity that a crossing actor has been triggered. 'crossing_dist' is the distance between the crossing actor and the junction """ - def __init__(self, crossing_dist=10, name="Scenario4Manager"): + def __init__(self, crossing_dist=10, name="HandleCrossingActor"): self._crossing_dist = crossing_dist - super(Scenario4Manager, self).__init__(name) + super(HandleCrossingActor, self).__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_Scenario4", self._crossing_dist, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_HandleCrossingActor", self._crossing_dist, overwrite=True) return py_trees.common.Status.SUCCESS -class Scenario7Manager(AtomicBehavior): +class JunctionScenarioManager(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario7 has been triggered + Updates the blackboard to tell the background activity that a JunctionScenarioManager has been triggered 'entry_direction' is the direction from which the incoming traffic enters the junction. It should be something like 'left', 'right' or 'opposite' """ - def __init__(self, entry_direction, name="Scenario7Manager"): + def __init__(self, entry_direction, remove_entry, remove_exit, remove_middle, name="Scenario7Manager"): self._entry_direction = entry_direction - super(Scenario7Manager, self).__init__(name) + self._remove_entry = remove_entry + self._remove_exit = remove_exit + self._remove_middle = remove_middle + super(JunctionScenarioManager, self).__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_Scenario7", self._entry_direction, overwrite=True) + py_trees.blackboard.Blackboard().set( + "BA_JunctionScenario", + [self._entry_direction, self._remove_entry, self._remove_exit, self._remove_middle], + overwrite=True + ) return py_trees.common.Status.SUCCESS -class Scenario8Manager(AtomicBehavior): +class ExtentExitRoadSpace(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario8 has been triggered - 'entry_direction' is the direction from which the incoming traffic enters the junction. It should be - something like 'left', 'right' or 'opposite' + Updates the blackboard to tell the background activity that an exit road needs more space """ - - def __init__(self, entry_direction, name="Scenario8Manager"): - self._entry_direction = entry_direction - super(Scenario8Manager, self).__init__(name) + def __init__(self, distance, direction, name="ExtentExitRoadSpace"): + self._distance = distance + self._direction = direction + super(ExtentExitRoadSpace, self).__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_Scenario8", self._entry_direction, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", [self._distance, self._direction], overwrite=True) return py_trees.common.Status.SUCCESS -class Scenario9Manager(AtomicBehavior): +class StopEntries(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario9 has been triggered - 'entry_direction' is the direction from which the incoming traffic enters the junction. It should be - something like 'left', 'right' or 'opposite' + Updates the blackboard to tell the background activity that an exit road needs more space """ - - def __init__(self, entry_direction, name="Scenario9Manager"): - self._entry_direction = entry_direction - super(Scenario9Manager, self).__init__(name) + def __init__(self, name="StopEntries"): + super(StopEntries, self).__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_Scenario9", self._entry_direction, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_StopEntries", True, overwrite=True) return py_trees.common.Status.SUCCESS - -class Scenario10Manager(AtomicBehavior): +class HandleStartAccidentScenario(AtomicBehavior): """ - Updates the blackboard to tell the background activity that a Scenario10 has been triggered - 'entry_direction' is the direction from which the incoming traffic enters the junction. It should be - something like 'left', 'right' or 'opposite' + Updates the blackboard to tell the background activity that the road behavior has to be initialized """ + def __init__(self, accident_wp, distance, name="HandleStartAccidentScenario"): + self._accident_wp = accident_wp + self._distance = distance + super(HandleStartAccidentScenario, self).__init__(name) - def __init__(self, entry_direction, name="Scenario10Manager"): - self._entry_direction = entry_direction - super(Scenario10Manager, self).__init__(name) + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_HandleStartAccidentScenario", [self._accident_wp, self._distance], overwrite=True) + return py_trees.common.Status.SUCCESS + +class HandleEndAccidentScenario(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that the road behavior has to be initialized + """ + def __init__(self, name="HandleEndAccidentScenario"): + super(HandleEndAccidentScenario, self).__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_Scenario10", self._entry_direction, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", True, overwrite=True) return py_trees.common.Status.SUCCESS diff --git a/srunner/tools/scenario_helper.py b/srunner/tools/scenario_helper.py index ecfe49935..eb24583fb 100644 --- a/srunner/tools/scenario_helper.py +++ b/srunner/tools/scenario_helper.py @@ -371,6 +371,7 @@ def generate_target_waypoint_in_route(waypoint, route): This method follow waypoints to a junction @returns a waypoint list according to turn input """ + target_waypoint = None wmap = CarlaDataProvider.get_map() reached_junction = False @@ -388,11 +389,9 @@ def generate_target_waypoint_in_route(waypoint, route): route_location = route[closest_index][0] index = closest_index - while True: - # Get the next route location - index = min(index + 1, len(route)) - route_location = route[index][0] - road_option = route[index][1] + for i in range(index, len(route)): + route_location = route[i][0] + road_option = route[i][1] # Enter the junction if not reached_junction and (road_option in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): @@ -400,9 +399,10 @@ def generate_target_waypoint_in_route(waypoint, route): # End condition for the behavior, at the end of the junction if reached_junction and (road_option not in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): + target_waypoint = route_location break - return wmap.get_waypoint(route_location) + return wmap.get_waypoint(target_waypoint) def choose_at_junction(current_waypoint, next_choices, direction=0): @@ -718,6 +718,65 @@ def get_distance_between_actors(current, target, distance_type="euclidianDistanc return distance +def get_same_dir_lanes(waypoint): + """Gets all the lanes with the same direction of the road of a wp""" + same_dir_wps = [waypoint] + + # Check roads on the right + right_wp = waypoint + while True: + possible_right_wp = right_wp.get_right_lane() + if possible_right_wp is None or possible_right_wp.lane_type != carla.LaneType.Driving: + break + right_wp = possible_right_wp + same_dir_wps.append(right_wp) + + # Check roads on the left + left_wp = waypoint + while True: + possible_left_wp = left_wp.get_left_lane() + if possible_left_wp is None or possible_left_wp.lane_type != carla.LaneType.Driving: + break + if possible_left_wp.lane_id * left_wp.lane_id < 0: + break + left_wp = possible_left_wp + same_dir_wps.insert(0, left_wp) + + return same_dir_wps + + +def get_opposite_dir_lanes(waypoint): + """Gets all the lanes with opposite direction of the road of a wp""" + other_dir_wps = [] + other_dir_wp = None + + # Get the first lane of the opposite direction + left_wp = waypoint + while True: + possible_left_wp = left_wp.get_left_lane() + if possible_left_wp is None: + break + if possible_left_wp.lane_id * left_wp.lane_id < 0: + other_dir_wp = possible_left_wp + break + left_wp = possible_left_wp + + if not other_dir_wp: + return other_dir_wps + + # Check roads on the right + right_wp = other_dir_wp + while True: + if right_wp.lane_type == carla.LaneType.Driving: + other_dir_wps.append(right_wp) + possible_right_wp = right_wp.get_right_lane() + if possible_right_wp is None: + break + right_wp = possible_right_wp + + return other_dir_wps + + class RotatedRectangle(object): """ From d61329df52dd48d5ac8a12e91153120700711890 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 9 Mar 2022 16:38:54 +0100 Subject: [PATCH 04/86] Changed ActorFlow to use the TrafficManager --- .../scenariomanager/carla_data_provider.py | 5 +- .../scenarioatomics/atomic_behaviors.py | 84 +++-- srunner/scenarios/actor_flow.py | 9 +- srunner/scenarios/background_activity.py | 325 +++++++++++------- srunner/tools/background_manager.py | 37 +- 5 files changed, 264 insertions(+), 196 deletions(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 6e855f8c3..64760319c 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -456,10 +456,13 @@ def create_blueprint(model, rolename='scenario', color=None, actor_category="car blueprints = CarlaDataProvider._blueprint_library.filter(model) blueprints_ = [] if safe: + # Safe vehicles are those that can move through all roads. + # This means that big vehicles such as ambulance and trucks are removed, + # as well as 2-wheeled ones as they can go through highways for bp in blueprints: if bp.id.endswith('firetruck') or bp.id.endswith('ambulance') \ + or bp.id.endswith('carlacola') or bp.id.endswith('cybertruck') \ or int(bp.get_attribute('number_of_wheels')) != 4: - # Two wheeled vehicles take much longer to render + bicicles shouldn't behave like cars continue blueprints_.append(bp) else: diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 68e57fab7..8290f29e4 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2507,19 +2507,24 @@ class ActorFlow(AtomicBehavior): """ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, - actor_speed=20 / 3.6, acceleration=float('inf'), name="ActorFlow"): + actor_speed=20 / 3.6, name="ActorFlow"): """ Setup class members """ super(ActorFlow, self).__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._collision_bp = self._world.get_blueprint_library().find('sensor.other.collision') + self._is_constant_velocity_active = True self._source_wp = source_wp self._sink_wp = sink_wp self._sink_location = self._sink_wp.transform.location self._source_transform = self._source_wp.transform + self._source_location = self._source_transform.location self._sink_dist = sink_dist self._speed = actor_speed @@ -2528,74 +2533,65 @@ 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._actor_agent_list = [] - self._route = [] - self._grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) - self._route = self._grp.trace_route(self._source_wp.transform.location, self._sink_wp.transform.location) - self._opt_dict = { - 'ignore_vehicles': True, - 'use_basic_behavior': True, - } + self._actor_list = [] + self._collision_sensor_list = [] def update(self): """Controls the created actors and creaes / removes other when needed""" - # To avoid multiple collisions, activate collision detection if one vehicle has already collided - collision_found = False - for actor, agent, _ in list(self._actor_agent_list): - if not agent.is_constant_velocity_active: - collision_found = True - break - if collision_found: - for actor, agent, _ in list(self._actor_agent_list): - agent.ignore_vehicles(False) - agent.stop_constant_velocity() - # Control the vehicles, removing them when needed - for actor_info in list(self._actor_agent_list): - actor, agent, just_spawned = actor_info - if just_spawned: # Move the vehicle to the ground. Important if flow starts at turns - ground_loc = self._world.ground_projection(actor.get_location(), 2) - if ground_loc: - initial_location = ground_loc.location - actor.set_location(initial_location) - actor_info[2] = False - + for actor in list(self._actor_list): sink_distance = self._sink_location.distance(CarlaDataProvider.get_location(actor)) if sink_distance < self._sink_dist: actor.destroy() - self._actor_agent_list.remove(actor_info) - else: - control = agent.run_step() - actor.apply_control(control) + self._actor_list.remove(actor) # Spawn new actors if needed - if len(self._actor_agent_list) == 0: + if len(self._actor_list) == 0: distance = self._spawn_dist + 1 else: - actor_location = CarlaDataProvider.get_location(self._actor_agent_list[-1][0]) + actor_location = CarlaDataProvider.get_location(self._actor_list[-1]) distance = self._source_transform.location.distance(actor_location) if distance > self._spawn_dist: actor = CarlaDataProvider.request_new_actor( 'vehicle.*', self._source_transform, rolename='scenario', safe_blueprint=True, tick=False ) - if actor is not None: - actor_agent = ConstantVelocityAgent(actor, 3.6 * self._speed, opt_dict=self._opt_dict) - actor_agent.set_global_plan(self._route, False) - self._actor_agent_list.append([actor, actor_agent, True]) - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + if actor is None: + return py_trees.common.Status.RUNNING + + actor.set_autopilot(True) + self._tm.set_path(actor, [self._source_location, self._sink_location]) + + if self._is_constant_velocity_active: + self._tm.ignore_vehicles_percentage(actor, 100) + self._tm.auto_lane_change(actor, False) + self._tm.set_desired_speed(actor, 3.6 * self._speed) + actor.enable_constant_velocity(carla.Vector3D(self._speed, 0, 0)) # For when physics are active + self._actor_list.append(actor) + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) + sensor.listen(lambda _: self.stop_constant_velocity()) + self._collision_sensor_list.append(sensor) return py_trees.common.Status.RUNNING + def stop_constant_velocity(self): + """Stops the constant velocity behavior""" + self._is_constant_velocity_active = False + for actor in self._actor_list: + actor.disable_constant_velocity() + self._tm.ignore_vehicles_percentage(actor, 0) + def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ - try: - for actor, _, _ in self._actor_agent_list: + for actor in self._actor_list: + try: actor.destroy() - except RuntimeError: - pass # Actor was already destroyed + except RuntimeError: + pass # Actor was already destroyed class TrafficLightFreezer(AtomicBehavior): diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index c82819889..ca07cc76f 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -23,7 +23,10 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToLocation, WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import ChangeRoadBehavior, JunctionScenarioManager, ExtentExitRoadSpace, StopEntries +from srunner.tools.background_manager import (SwitchRouteSources, + JunctionScenarioManager, + ExtentExitRoadSpace, + StopEntries) from srunner.tools.scenario_helper import get_same_dir_lanes def convert_dict_to_location(actor_dict): @@ -112,10 +115,10 @@ def _create_behavior(self): sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='left')) break - sequence.add_child(ChangeRoadBehavior(switch_source=False)) + sequence.add_child(SwitchRouteSources(False)) sequence.add_child(root) if CarlaDataProvider.get_ego_vehicle_route(): - sequence.add_child(ChangeRoadBehavior(switch_source=True)) + sequence.add_child(SwitchRouteSources(True)) return sequence diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 0369f008c..bca1a54a5 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -20,44 +20,54 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes +JUNCTION_ENTRY = 'entry' +JUNCTION_MIDDLE = 'middle' +JUNCTION_EXIT = 'exit' +JUNCTION_INACTIVE = 'inactive' + +EGO_JUNCTION = 'junction' +EGO_ROAD = 'road' + +DEBUG_ROAD = 'road' +DEBUG_OPPOSITE = 'opposite' +DEBUG_JUNCTION = 'junction' +DEBUG_ENTRY = 'entry' +DEBUG_EXIT = 'exit' +DEBUG_CONNECT = 'connect' + +DEBUG_SMALL = 'small' +DEBUG_MEDIUM = 'medium' +DEBUG_LARGE = 'large' + DEBUG_COLORS = { - 'road': carla.Color(0, 0, 255), # Blue - 'opposite': carla.Color(255, 0, 0), # Red - 'junction': carla.Color(0, 0, 0), # Black - 'entry': carla.Color(255, 255, 0), # Yellow - 'exit': carla.Color(0, 255, 255), # Teal - 'connect': carla.Color(0, 255, 0), # Green + DEBUG_ROAD: carla.Color(0, 0, 255), # Blue + DEBUG_OPPOSITE: carla.Color(255, 0, 0), # Red + DEBUG_JUNCTION: carla.Color(0, 0, 0), # Black + DEBUG_ENTRY: carla.Color(255, 255, 0), # Yellow + DEBUG_EXIT: carla.Color(0, 255, 255), # Teal + DEBUG_CONNECT: carla.Color(0, 255, 0), # Green } DEBUG_TYPE = { - 'small': [0.8, 0.1], - 'medium': [0.5, 0.15], - 'large': [0.2, 0.2], + DEBUG_SMALL: [0.8, 0.1], + DEBUG_MEDIUM: [0.5, 0.15], + DEBUG_LARGE: [0.2, 0.2], } -JUNCTION_ENTRY_NAME = 'junction_entry' -JUNCTION_MIDDLE_NAME = 'junction_middle' -JUNCTION_EXIT_NAME = 'junction_exit' -JUNCTION_INACTIVE_NAME = 'junction_inactive' - -EGO_JUNCTION_NAME = 'junction' -EGO_ROAD_NAME = 'road' - - -def draw_string(world, location, string='', debug_type='road', persistent=False): +def draw_string(world, location, string='', debug_type=DEBUG_ROAD, persistent=False): """Utility function to draw debugging strings""" - v_shift, _ = DEBUG_TYPE.get('small') + v_shift, _ = DEBUG_TYPE.get(DEBUG_SMALL) l_shift = carla.Location(z=v_shift) - color = DEBUG_COLORS.get(debug_type, 'road') + color = DEBUG_COLORS.get(debug_type, DEBUG_ROAD) life_time = 0.07 if not persistent else 100000 world.debug.draw_string(location + l_shift, string, False, color, life_time) -def draw_point(world, location, point_type='small', debug_type='road', persistent=False): +def draw_point(world, location, point_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, persistent=False): """Utility function to draw debugging points""" - v_shift, size = DEBUG_TYPE.get(point_type, 'small') + v_shift, size = DEBUG_TYPE.get(point_type, DEBUG_SMALL) l_shift = carla.Location(z=v_shift) - color = DEBUG_COLORS.get(debug_type, 'road') + color = DEBUG_COLORS.get(debug_type, DEBUG_ROAD) life_time = 0.07 if not persistent else 100000 world.debug.draw_point(location + l_shift, size, color, life_time) @@ -80,10 +90,10 @@ class Source(object): Source object to store its position and its responsible actors """ - def __init__(self, wp, actors, entry_lane_wp='', dist_to_ego=0): # pylint: disable=invalid-name + def __init__(self, wp, actors, entry_lane_wp='', dist_to_ego=0, active=True): # pylint: disable=invalid-name self.wp = wp # pylint: disable=invalid-name self.actors = actors - self.active = True + self.active = active # For road sources self.dist_to_ego = dist_to_ego @@ -193,7 +203,7 @@ class BackgroundBehavior(AtomicBehavior): Handles the background activity """ - def __init__(self, ego_actor, route, night_mode=False, debug=False, name="BackgroundBehavior"): + def __init__(self, ego_actor, route, night_mode=False, debug=True, name="BackgroundBehavior"): """ Setup class members """ @@ -209,7 +219,7 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr # Global variables self._ego_actor = ego_actor - self._ego_state = EGO_ROAD_NAME + self._ego_state = EGO_ROAD self._route_index = 0 self._get_route_data(route) @@ -285,6 +295,8 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr 'remove_exits': False, } # Same as the Junction.scenario_info, but this stores the data in case no junctions are active + self._route_sources_active = True + def _get_route_data(self, route): """Extract the information from the route""" self._route = [] # Transform the route into a list of waypoints @@ -338,12 +350,12 @@ def update(self): self._update_parameters() # Update ego state. - if self._ego_state == EGO_JUNCTION_NAME: + if self._ego_state == EGO_JUNCTION: self._monitor_ego_junction_exit() self._monitor_incoming_junctions() # Update_actors - if self._ego_state == EGO_JUNCTION_NAME: + if self._ego_state == EGO_JUNCTION: self._update_junction_actors() self._update_junction_sources() else: @@ -679,7 +691,7 @@ def _add_junctions_topology(self, route_data): used_entry_lanes.append(get_lane_key(entry_wp)) entry_lane_wps.append(entry_wp) if self.debug: - draw_point(self._world, entry_wp.transform.location, 'small', 'entry', True) + draw_point(self._world, entry_wp.transform.location, DEBUG_SMALL, DEBUG_ENTRY, True) exit_wp = self._get_junction_exit_wp(exit_wp) if not exit_wp: @@ -688,7 +700,7 @@ def _add_junctions_topology(self, route_data): used_exit_lanes.append(get_lane_key(exit_wp)) exit_lane_wps.append(exit_wp) if self.debug: - draw_point(self._world, exit_wp.transform.location, 'small', 'exit', True) + draw_point(self._world, exit_wp.transform.location, DEBUG_SMALL, DEBUG_EXIT, True) # Check for connecting lanes. This is pretty much for the roundabouts, but some weird geometries # make it possible for single junctions to have the same road entering and exiting. Two cases, @@ -699,13 +711,13 @@ def _add_junctions_topology(self, route_data): if get_lane_key(wp) in exit_lane_keys: entry_lane_wps.remove(wp) if self.debug: - draw_point(self._world, wp.transform.location, 'small', 'connect', True) + draw_point(self._world, wp.transform.location, DEBUG_SMALL, DEBUG_CONNECT, True) for wp in list(exit_lane_wps): if get_lane_key(wp) in entry_lane_keys: exit_lane_wps.remove(wp) if self.debug: - draw_point(self._world, wp.transform.location, 'small', 'connect', True) + draw_point(self._world, wp.transform.location, DEBUG_SMALL, DEBUG_CONNECT, True) # Lanes with a fake junction in the middle (maps junction exit to fake junction entry and viceversa) for entry_key, exit_key in self._fake_lane_pair_keys: @@ -723,8 +735,8 @@ def _add_junctions_topology(self, route_data): entry_lane_wps.remove(entry_wp) exit_lane_wps.remove(exit_wp) if self.debug: - draw_point(self._world, entry_wp.transform.location, 'small', 'connect', True) - draw_point(self._world, exit_wp.transform.location, 'small', 'connect', True) + draw_point(self._world, entry_wp.transform.location, DEBUG_SMALL, DEBUG_CONNECT, True) + draw_point(self._world, exit_wp.transform.location, DEBUG_SMALL, DEBUG_CONNECT, True) junction_data.entry_wps = entry_lane_wps junction_data.exit_wps = exit_lane_wps @@ -838,7 +850,7 @@ def _add_actor_dict_element(self, actor_dict, actor, exit_lane_key='', at_oppo_e This will be the ones that aren't removed """ actor_dict[actor] = { - 'state': JUNCTION_ENTRY_NAME if not exit_lane_key else JUNCTION_EXIT_NAME, + 'state': JUNCTION_ENTRY if not exit_lane_key else JUNCTION_EXIT, 'exit_lane_key': exit_lane_key, 'at_oppo_entry_lane': at_oppo_entry_lane } @@ -848,7 +860,7 @@ def _switch_to_junction_mode(self, junction): Prepares the junction mode, removing all road behaviours. Actors that are stopped via a scenario will still wait. """ - self._ego_state = EGO_JUNCTION_NAME + self._ego_state = EGO_JUNCTION for lane in self._road_dict: for actor in self._road_dict[lane].actors: # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' @@ -859,8 +871,9 @@ def _switch_to_junction_mode(self, junction): for lane_key in self._road_dict: source = self._road_dict[lane_key] if get_lane_key(source.wp) in junction.route_entry_keys: - source.actors.reverse() # TODO: dont do this - junction.entry_sources.append(Source(source.wp, source.actors, entry_lane_wp=source.wp)) + junction.entry_sources.append(Source( + source.wp, source.actors, entry_lane_wp=source.wp, active=self._route_sources_active) + ) # TODO: Else should map the source to the entry and add it self._road_dict.clear() @@ -889,7 +902,7 @@ def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, # Source is affected actors = entry_source.actors for actor in list(actors): - if actor_dict[actor]['state'] == JUNCTION_ENTRY_NAME: + if actor_dict[actor]['state'] == JUNCTION_ENTRY: # Actor is at the entry lane self._destroy_actor(actor) @@ -901,7 +914,7 @@ def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, if remove_middle: actor_dict = scenario_junction.actor_dict for actor in list(actor_dict): - if actor_dict[actor]['state'] == JUNCTION_MIDDLE_NAME: + if actor_dict[actor]['state'] == JUNCTION_MIDDLE: self._destroy_actor(actor) elif self._junctions: @@ -1002,7 +1015,7 @@ def _handle_crossing_scenario_interaction(self, junction, ego_wp): next_junction = self._active_junctions[1] actors_dict = next_junction.actor_dict for actor in list(actors_dict): - if actors_dict[actor]['state'] != JUNCTION_ENTRY_NAME: + if actors_dict[actor]['state'] != JUNCTION_ENTRY: continue actor_location = CarlaDataProvider.get_location(actor) @@ -1048,13 +1061,15 @@ def _end_junction_behavior(self, junction): if not self._active_junctions: for wp in junction.exit_wps: if get_lane_key(wp) in route_exit_keys: - self._road_dict[get_lane_key(wp)] = Source(wp, []) + self._road_dict[get_lane_key(wp)] = Source(wp, [], active=self._route_sources_active) else: for wp in junction.exit_wps: if get_lane_key(wp) in route_exit_keys: # TODO: entry_lane_wp isn't really this one (for cases with road changes) - self._active_junctions[0].entry_sources.append(Source(wp, [], entry_lane_wp=wp)) + self._active_junctions[0].entry_sources.append( + Source(wp, [], entry_lane_wp=wp, active=self._route_sources_active) + ) # Handle the actors for actor in list(actor_dict): @@ -1092,7 +1107,7 @@ def _end_junction_behavior(self, junction): self._handle_junction_scenario_end(junction) # TODO: See if this can be removed (as it changes a popped element) if not self._active_junctions: - self._ego_state = EGO_ROAD_NAME + self._ego_state = EGO_ROAD self._initialise_opposite_sources() self._initialise_road_checker() self._road_ego_key = self._get_ego_route_lane_key(self._ego_wp) @@ -1134,7 +1149,7 @@ def _monitor_incoming_junctions(self): if not junction: return - if self._ego_state == EGO_ROAD_NAME: + if self._ego_state == EGO_ROAD: self._switch_to_junction_mode(junction) self._initialise_junction_sources(junction) self._initialise_junction_exits(junction) @@ -1191,24 +1206,30 @@ def _update_road_sources(self, prev_ego_index): if prev_ego_index != self._route_index: route_wp = self._route[self._route_index] prev_route_wp = self._route[prev_ego_index] - dist = route_wp.transform.location.distance(prev_route_wp.transform.location) + added_dist = route_wp.transform.location.distance(prev_route_wp.transform.location) min_distance = self._road_back_vehicles * self._road_spawn_dist for lane_key in self._road_dict: source = self._road_dict[lane_key] if source.dist_to_ego < min_distance: - source.dist_to_ego += dist - else: - new_source_wps = source.wp.next(dist) - if not new_source_wps: - continue - source.wp = new_source_wps[0] + source.dist_to_ego += added_dist + continue # Don't move the source that is too close to the ego, + + distance = source.wp.transform.location.distance(source.actors[-1].get_location()) + if distance <= self._road_spawn_dist: + continue # or is it is too close to the last vehicle + + added_dist += distance - self._road_spawn_dist # But don't let it stagger behind + new_source_wps = source.wp.next(added_dist) + if not new_source_wps: + continue + source.wp = new_source_wps[0] for lane_key in self._road_dict: source = self._road_dict[lane_key] if self.debug: - draw_point(self._world, source.wp.transform.location, 'small', self._ego_state, False) - draw_string(self._world, source.wp.transform.location, str(len(source.actors)), self._ego_state, False) + draw_point(self._world, source.wp.transform.location, DEBUG_SMALL, DEBUG_ROAD, False) + draw_string(self._world, source.wp.transform.location, str(len(source.actors)), DEBUG_ROAD, False) # Ensure not too many actors are in front of the ego front_veh = 0 @@ -1283,7 +1304,9 @@ def _initialise_road_behavior(self, road_wps): for actor in actors: self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) - self._road_dict[get_lane_key(wp)] = Source(prev_wp, actors, dist_to_ego=source_dist) + self._road_dict[get_lane_key(wp)] = Source( + prev_wp, actors, dist_to_ego=source_dist, active=self._route_sources_active + ) def _initialise_opposite_sources(self): """ @@ -1346,6 +1369,7 @@ def _initialise_junction_sources(self, junction): if entry_lane_key in junction.route_entry_keys: continue # Ignore the road from which the route enters + # TODO: Use the source.active to do this if remove_entries and entry_lane_key in entry_lanes: continue # Ignore entries that are part of active junction scenarios @@ -1378,6 +1402,13 @@ def _initialise_junction_exits(self, junction): exiting_wps = [] next_wp = wp + + # Move the initial distance + if max_distance > 0: + next_wps = next_wp.next(max_distance) + if len(next_wps) > 0: + next_wp = next_wps[0] + for i in range(max(self._road_front_vehicles, 1)): # Get the moving distance (first jump is higher to allow space for another vehicle) @@ -1418,19 +1449,21 @@ def _update_junction_sources(self): for junction in self._active_junctions: remove_entries = junction.scenario_info['remove_entries'] direction = junction.scenario_info['direction'] - entry_lanes = [] if not direction else junction.entry_directions[direction] + entry_keys = [] if not direction else junction.entry_directions[direction] actor_dict = junction.actor_dict for source in junction.entry_sources: if self.debug: - draw_point(self._world, source.wp.transform.location, 'small', 'junction', False) - draw_string(self._world, source.wp.transform.location, str(len(source.actors)), 'junction', False) + draw_point(self._world, source.wp.transform.location, DEBUG_SMALL, DEBUG_JUNCTION, False) + draw_string(self._world, source.wp.transform.location, str(len(source.actors)), DEBUG_JUNCTION, False) entry_lane_key = get_lane_key(source.entry_lane_wp) at_oppo_entry_lane = entry_lane_key in junction.opposite_entry_keys - # The direction is prohibited as a junction scenario is active - if remove_entries and entry_lane_key in entry_lanes: + if not source.active: + continue + # The direction is prohibited + if remove_entries and entry_lane_key in entry_keys: continue self._add_incoming_actors(junction, source) @@ -1454,7 +1487,6 @@ def _update_junction_sources(self): actor = self._spawn_source_actor(source) if not actor: continue - if junction.stop_entries: self._tm.vehicle_percentage_speed_difference(actor, 100) self._tm.distance_to_leading_vehicle(actor, self._junction_vehicle_dist) @@ -1485,7 +1517,7 @@ def _move_road_checker(self, prev_index): """ if self.debug: checker_wp = self._route[self._road_checker_index] - draw_point(self._world, checker_wp.transform.location, 'small', 'road', False) + draw_point(self._world, checker_wp.transform.location, DEBUG_SMALL, DEBUG_ROAD, False) if prev_index == self._route_index: return @@ -1539,10 +1571,10 @@ def _move_opposite_sources(self, prev_index): """ if self.debug: for source in self._opposite_sources: - draw_point(self._world, source.wp.transform.location, 'small', 'opposite', False) - draw_string(self._world, source.wp.transform.location, str(len(source.actors)), 'opposite', False) + draw_point(self._world, source.wp.transform.location, DEBUG_SMALL, DEBUG_OPPOSITE, False) + draw_string(self._world, source.wp.transform.location, str(len(source.actors)), DEBUG_OPPOSITE, False) route_wp = self._route[self._opposite_route_index] - draw_point(self._world, route_wp.transform.location, 'small', 'opposite', False) + draw_point(self._world, route_wp.transform.location, DEBUG_SMALL, DEBUG_OPPOSITE, False) if prev_index == self._route_index: return @@ -1636,8 +1668,8 @@ def _update_parameters(self): """ # Road behavior - road_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeRoadBehavior") - if road_behavior_data: + road_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeRoadBehavior') + if road_behavior_data is not None: num_front_vehicles, num_back_vehicles, vehicle_dist, spawn_dist, sources_active = road_behavior_data if num_front_vehicles is not None: self._road_front_vehicles = num_front_vehicles @@ -1652,22 +1684,28 @@ def _update_parameters(self): self._road_dict[lane_key].active = sources_active self._get_road_radius() - py_trees.blackboard.Blackboard().set("BA_ChangeRoadBehavior", None, True) + py_trees.blackboard.Blackboard().set('BA_ChangeRoadBehavior', None, True) # Extend the space of a specific exit lane - road_exit_data = py_trees.blackboard.Blackboard().get("BA_ExtentExitRoadSpace") - if road_exit_data: + road_exit_data = py_trees.blackboard.Blackboard().get('BA_ExtentExitRoadSpace') + if road_exit_data is not None: space, direction = road_exit_data self._extent_road_exit_space(space, direction) py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", None, True) + # Switch route sources + switch_sources_data = py_trees.blackboard.Blackboard().get('BA_SwitchRouteSources') + if switch_sources_data is not None: + self._switch_route_sources(switch_sources_data) + py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", None, True) + # Opposite behavior - opposite_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeOppositeBehavior") - if opposite_behavior_data: + opposite_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeOppositeBehavior') + if opposite_behavior_data is not None: source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data if source_dist is not None: if source_dist < self._junction_sources_dist: - print("WARNING: Opposite sources distance is lower than the junction ones. Ignoring it") + print('WARNING: Opposite sources distance is lower than the junction ones. Ignoring it') else: self._opposite_sources_dist = source_dist if vehicle_dist is not None: @@ -1676,69 +1714,69 @@ def _update_parameters(self): self._opposite_spawn_dist = spawn_dist if max_actors is not None: self._opposite_sources_max_actors = max_actors - py_trees.blackboard.Blackboard().set("BA_ChangeOppositeBehavior", None, True) + py_trees.blackboard.Blackboard().set('BA_ChangeOppositeBehavior', None, True) # Junction behavior - junction_behavior_data = py_trees.blackboard.Blackboard().get("BA_ChangeJunctionBehavior") - if junction_behavior_data: + junction_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeJunctionBehavior') + if junction_behavior_data is not None: source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data - if source_dist: + if source_dist is not None: if source_dist > self._opposite_sources_dist: - print("WARNING: Junction sources distance is higher than the opposite ones. Ignoring it") + print('WARNING: Junction sources distance is higher than the opposite ones. Ignoring it') else: self._junction_sources_dist = source_dist - if vehicle_dist: + if vehicle_dist is not None: self._junction_vehicle_dist = vehicle_dist - if spawn_dist: + if spawn_dist is not None: self._junction_spawn_dist = spawn_dist - if max_actors: + if max_actors is not None: self._junction_sources_max_actors = max_actors - py_trees.blackboard.Blackboard().set("BA_ChangeJunctionBehavior", None, True) + py_trees.blackboard.Blackboard().set('BA_ChangeJunctionBehavior', None, True) # Stop front vehicles - stop_data = py_trees.blackboard.Blackboard().get("BA_StopFrontVehicles") - if stop_data: + stop_data = py_trees.blackboard.Blackboard().get('BA_StopFrontVehicles') + if stop_data is not None: self._stop_road_front_vehicles() - py_trees.blackboard.Blackboard().set("BA_StopFrontVehicles", None, True) + py_trees.blackboard.Blackboard().set('BA_StopFrontVehicles', None, True) # Start front vehicles - start_data = py_trees.blackboard.Blackboard().get("BA_StartFrontVehicles") - if start_data: + start_data = py_trees.blackboard.Blackboard().get('BA_StartFrontVehicles') + if start_data is not None: self._start_road_front_vehicles() py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) # Handles crossing actor scenarios. This currently only works for Scenario4 - crossing_dist = py_trees.blackboard.Blackboard().get("BA_HandleCrossingActor") - if crossing_dist: + crossing_dist = py_trees.blackboard.Blackboard().get('BA_HandleCrossingActor') + if crossing_dist is not None: self._is_crossing_scenario_active = True self._crossing_dist = crossing_dist - py_trees.blackboard.Blackboard().set("BA_HandleCrossingActor", None, True) + py_trees.blackboard.Blackboard().set('BA_HandleCrossingActor', None, True) # Handles junction scenarios (scenarios 7 to 10) - junction_scenario_data = py_trees.blackboard.Blackboard().get("BA_JunctionScenario") - if junction_scenario_data: + junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_JunctionScenario') + if junction_scenario_data is not None: direction, remove_entries, remove_exits, remove_middle = junction_scenario_data self._initialise_junction_scenario(direction, remove_entries, remove_exits, remove_middle) - py_trees.blackboard.Blackboard().set("BA_JunctionScenario", None, True) + py_trees.blackboard.Blackboard().set('BA_JunctionScenario', None, True) # Handles road accident scenario (Accident and Construction) - handle_start_accident_data = py_trees.blackboard.Blackboard().get("BA_HandleStartAccidentScenario") - if handle_start_accident_data: + handle_start_accident_data = py_trees.blackboard.Blackboard().get('BA_HandleStartAccidentScenario') + if handle_start_accident_data is not None: accident_wp, distance = handle_start_accident_data self._handle_lanechange_scenario(accident_wp, distance) - py_trees.blackboard.Blackboard().set("BA_HandleStartAccidentScenario", None, True) + py_trees.blackboard.Blackboard().set('BA_HandleStartAccidentScenario', None, True) # Handles road accident scenario (Accident and Construction) - handle_end_accident_data = py_trees.blackboard.Blackboard().get("BA_HandleEndAccidentScenario") - if handle_end_accident_data: + handle_end_accident_data = py_trees.blackboard.Blackboard().get('BA_HandleEndAccidentScenario') + if handle_end_accident_data is not None: self._road_extra_front_actors = 0 - py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", None, True) + py_trees.blackboard.Blackboard().set('BA_HandleEndAccidentScenario', None, True) # Stops non route entries - stop_entry_data = py_trees.blackboard.Blackboard().get("BA_StopEntries") - if stop_entry_data: + stop_entry_data = py_trees.blackboard.Blackboard().get('BA_StopEntries') + if stop_entry_data is not None: self._stop_non_route_entries() - py_trees.blackboard.Blackboard().set("BA_StopEntries", None, True) + py_trees.blackboard.Blackboard().set('BA_StopEntries', None, True) self._compute_parameters() @@ -1794,11 +1832,6 @@ def _extent_road_exit_space(self, space, direction): """Increases the space left by the exit vehicles at a specific road""" if len(self._active_junctions) > 0: junction = self._active_junctions[0] - actor_dict = self._active_junctions[0].actor_dict - for actor in actor_dict: - if actor_dict[actor]['state'] == JUNCTION_INACTIVE_NAME: - self._tm.vehicle_percentage_speed_difference(actor, 0) - actor_dict[actor]['state'] = JUNCTION_EXIT_NAME elif len(self._junctions) > 0: junction = self._junctions[0] else: @@ -1810,6 +1843,24 @@ def _extent_road_exit_space(self, space, direction): if exit_lane_key in direction_lane_keys: exit_dict[exit_lane_key]['max_distance'] += space + # For all the actors present, teleport them a bit to the front and activate them + for actor in exit_dict[exit_lane_key]['actors']: + location = CarlaDataProvider.get_location(actor) + if not location: + continue + + actor_wp = self._map.get_waypoint(location) + new_actor_wps = actor_wp.next(space) + if len(new_actor_wps) > 0: + new_transform = new_actor_wps[0].transform + new_transform.location.z += 0.2 + actor.set_transform(new_transform) + + if junction.actor_dict[actor]['state'] == JUNCTION_INACTIVE: + self._tm.vehicle_percentage_speed_difference(actor, 0) + junction.actor_dict[actor]['state'] = JUNCTION_EXIT + + def _stop_non_route_entries(self): if len(self._active_junctions) > 0: entry_sources = self._active_junctions[0].entry_sources @@ -1839,6 +1890,19 @@ def _handle_lanechange_scenario(self, accident_wp, distance): else: self._tm.vehicle_percentage_speed_difference(actor, 100) + def _switch_route_sources(self, enabled): + """ + Disables all sources that are part of the ego's route + """ + self._route_sources_active = enabled + for lane in self._road_dict: + self._road_dict[lane].active = enabled + + for junction in self._active_junctions: + for source in junction.entry_sources: + if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: + source.active = enabled + ############################# ## Actor functions ## ############################# @@ -1937,15 +2001,15 @@ def _update_road_actors(self): route_wp = self._route[self._route_index] scenario_actors = self._crossing_scenario_actors + self._stopped_road_actors for lane in self._road_dict: - for actor in self._road_dict[lane].actors: + for i, actor in enumerate(self._road_dict[lane].actors): location = CarlaDataProvider.get_location(actor) if not location: continue if self.debug: - if self._is_location_behind_ego(location): - draw_string(self._world, location, 'R(B)', 'road', False) - else: - draw_string(self._world, location, 'R(F)', 'road', False) + string = 'R_' + string += 'B' if self._is_location_behind_ego(location) else 'F' + string += '(' + str(i) + ')' + draw_string(self._world, location, string, DEBUG_ROAD, False) if actor in scenario_actors or self._is_location_behind_ego(location): continue @@ -2052,16 +2116,16 @@ def _update_junction_actors(self): route_oppo_keys = junction.opposite_entry_keys + junction.opposite_exit_keys for wp in junction.entry_wps + junction.exit_wps: if get_lane_key(wp) in route_keys: - draw_point(self._world, wp.transform.location, 'medium', 'road', False) + draw_point(self._world, wp.transform.location, DEBUG_MEDIUM, DEBUG_ROAD, False) elif get_lane_key(wp) in route_oppo_keys: - draw_point(self._world, wp.transform.location, 'medium', 'opposite', False) + draw_point(self._world, wp.transform.location, DEBUG_MEDIUM, DEBUG_OPPOSITE, False) else: - draw_point(self._world, wp.transform.location, 'medium', 'junction', False) + draw_point(self._world, wp.transform.location, DEBUG_MEDIUM, DEBUG_JUNCTION, False) actor_dict = junction.actor_dict exit_dict = junction.exit_dict remove_middle = junction.scenario_info['remove_middle'] - for actor in list(actor_dict): + for j, actor in enumerate(list(actor_dict)): if actor not in actor_dict: continue # Actor was removed during the loop location = CarlaDataProvider.get_location(actor) @@ -2070,20 +2134,20 @@ def _update_junction_actors(self): state, exit_lane_key, _ = actor_dict[actor].values() if self.debug: - string = 'J' + str(i+1) + "_" + state[9:11] - draw_string(self._world, location, string, self._ego_state, False) + string = 'J' + str(i+1) + '_' + state[:2] + '(' + str(j) + ')' + draw_string(self._world, location, string, DEBUG_JUNCTION, False) # Monitor its entry - if state == JUNCTION_ENTRY_NAME: + if state == JUNCTION_ENTRY: actor_wp = self._map.get_waypoint(location) if self._is_junction(actor_wp) and junction.contains(actor_wp.get_junction()): if remove_middle: self._destroy_actor(actor) # Don't clutter the junction if a junction scenario is active continue - actor_dict[actor]['state'] = JUNCTION_MIDDLE_NAME + actor_dict[actor]['state'] = JUNCTION_MIDDLE # Monitor its exit and destroy an actor if needed - elif state == JUNCTION_MIDDLE_NAME: + elif state == JUNCTION_MIDDLE: actor_wp = self._map.get_waypoint(location) actor_lane_key = get_lane_key(actor_wp) if not self._is_junction(actor_wp) and actor_lane_key in exit_dict: @@ -2104,23 +2168,23 @@ def _update_junction_actors(self): else: # Check the lane capacity exit_dict[actor_lane_key]['ref_wp'] = actor_wp - actor_dict[actor]['state'] = JUNCTION_EXIT_NAME + actor_dict[actor]['state'] = JUNCTION_EXIT actor_dict[actor]['exit_lane_key'] = actor_lane_key actors = exit_dict[actor_lane_key]['actors'] if len(actors) > 0 and len(actors) >= exit_dict[actor_lane_key]['max_actors']: - self._destroy_actor(actors[0]) # This is always the front most vehicle + self._destroy_actor(actors[-1]) # This is always the front most vehicle actors.append(actor) # Deactivate them when far from the junction - elif state == JUNCTION_EXIT_NAME: + elif state == JUNCTION_EXIT: distance = location.distance(exit_dict[exit_lane_key]['ref_wp'].transform.location) if distance > exit_dict[exit_lane_key]['max_distance']: self._tm.vehicle_percentage_speed_difference(actor, 100) - actor_dict[actor]['state'] = JUNCTION_INACTIVE_NAME + actor_dict[actor]['state'] = JUNCTION_INACTIVE # Wait for something to happen - elif state == JUNCTION_INACTIVE_NAME: + elif state == JUNCTION_INACTIVE: pass def _update_opposite_actors(self): @@ -2134,7 +2198,7 @@ def _update_opposite_actors(self): if not location: continue if self.debug: - draw_string(self._world, location, 'O', 'opposite', False) + draw_string(self._world, location, 'O', DEBUG_OPPOSITE, False) distance = location.distance(self._ego_wp.transform.location) if distance > max_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) @@ -2202,4 +2266,5 @@ def _update_ego_route_location(self): if self.debug: string = 'EGO_' + self._ego_state[0].upper() - draw_string(self._world, location, string, self._ego_state, False) + debug_name = DEBUG_ROAD if self._ego_state == EGO_ROAD else DEBUG_JUNCTION + draw_string(self._world, location, string, debug_name, False) diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 27058e470..055540be3 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -27,18 +27,17 @@ class ChangeRoadBehavior(AtomicBehavior): """ def __init__(self, num_front_vehicles=None, num_back_vehicles=None, - vehicle_dist=None, spawn_dist=None, switch_source=None, name="ChangeRoadBehavior"): + vehicle_dist=None, spawn_dist=None, name="ChangeRoadBehavior"): self._num_front_vehicles = num_front_vehicles self._num_back_vehicles = num_back_vehicles self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist - self._switch_source = switch_source super(ChangeRoadBehavior, self).__init__(name) def update(self): py_trees.blackboard.Blackboard().set( "BA_ChangeRoadBehavior", - [self._num_front_vehicles, self._num_back_vehicles, self._vehicle_dist, self._spawn_dist, self._switch_source], + [self._num_front_vehicles, self._num_back_vehicles, self._vehicle_dist, self._spawn_dist], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -103,20 +102,6 @@ def update(self): return py_trees.common.Status.SUCCESS -class ActivateHardBreakScenario(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. - 'stop_duration' is the amount of time, in seconds, the vehicles will be stopped - """ - - def __init__(self, stop_duration=10, name="ActivateHardBreakScenario"): - self._stop_duration = stop_duration - super(ActivateHardBreakScenario, self).__init__(name) - - def update(self): - py_trees.blackboard.Blackboard().set("BA_ActivateHardBreakScenario", self._stop_duration, overwrite=True) - return py_trees.common.Status.SUCCESS - class StopFrontVehicles(AtomicBehavior): """ Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. @@ -202,7 +187,7 @@ def update(self): class StopEntries(AtomicBehavior): """ - Updates the blackboard to tell the background activity that an exit road needs more space + Updates the blackboard to tell the background activity to stop all junction entries """ def __init__(self, name="StopEntries"): super(StopEntries, self).__init__(name) @@ -212,6 +197,21 @@ def update(self): py_trees.blackboard.Blackboard().set("BA_StopEntries", True, overwrite=True) return py_trees.common.Status.SUCCESS + +class SwitchRouteSources(AtomicBehavior): + """ + Updates the blackboard to tell the background activity to (de)activate all route sources + """ + def __init__(self, enabled=True, name="SwitchRouteSources"): + self._enabled = enabled + super(SwitchRouteSources, self).__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", self._enabled, overwrite=True) + return py_trees.common.Status.SUCCESS + + class HandleStartAccidentScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity that the road behavior has to be initialized @@ -226,6 +226,7 @@ def update(self): py_trees.blackboard.Blackboard().set("BA_HandleStartAccidentScenario", [self._accident_wp, self._distance], overwrite=True) return py_trees.common.Status.SUCCESS + class HandleEndAccidentScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity that the road behavior has to be initialized From 5c2f3492cda20a76b8c89b2e92f3883ecb4f0536 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Fri, 11 Mar 2022 09:28:32 +0100 Subject: [PATCH 05/86] Routes can now have scenario specific criterias --- srunner/metrics/examples/criteria_filter.py | 2 +- .../scenariomanager/carla_data_provider.py | 17 - srunner/scenariomanager/result_writer.py | 19 +- srunner/scenariomanager/scenario_manager.py | 9 +- .../scenarioatomics/atomic_criteria.py | 535 +++++++----------- srunner/scenariomanager/traffic_events.py | 35 +- srunner/scenarios/actor_flow.py | 8 +- srunner/scenarios/background_activity.py | 4 +- srunner/scenarios/basic_scenario.py | 233 ++++---- .../scenarios/construction_crash_vehicle.py | 7 +- srunner/scenarios/control_loss.py | 6 +- srunner/scenarios/follow_leading_vehicle.py | 8 +- srunner/scenarios/junction_crossing_route.py | 4 +- .../scenarios/object_crash_intersection.py | 18 +- srunner/scenarios/object_crash_vehicle.py | 12 +- .../opposite_vehicle_taking_priority.py | 8 +- srunner/scenarios/route_obstacles.py | 6 +- srunner/scenarios/route_scenario.py | 179 +++--- .../signalized_junction_left_turn.py | 8 +- .../signalized_junction_right_turn.py | 8 +- srunner/tools/background_manager.py | 2 +- srunner/tools/route_parser.py | 1 + srunner/tools/scenario_helper.py | 8 +- 23 files changed, 479 insertions(+), 658 deletions(-) diff --git a/srunner/metrics/examples/criteria_filter.py b/srunner/metrics/examples/criteria_filter.py index 78938f7cb..e1eacf25e 100644 --- a/srunner/metrics/examples/criteria_filter.py +++ b/srunner/metrics/examples/criteria_filter.py @@ -37,7 +37,7 @@ def _create_metric(self, town_map, log, criteria): { "test_status": criterion["test_status"], "actual_value": criterion["actual_value"], - "success_value": criterion["expected_value_success"] + "success_value": criterion["success_value"] } } ) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 64760319c..0c5b15413 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -404,23 +404,6 @@ def get_next_traffic_light(actor, use_cached_location=True): return relevant_traffic_light - @staticmethod - def set_ego_vehicle_route(route): - """ - Set the route of the ego vehicle - - @todo extend ego_vehicle_route concept to support multi ego_vehicle scenarios - """ - CarlaDataProvider._ego_vehicle_route = route - - @staticmethod - def get_ego_vehicle_route(): - """ - returns the currently set route of the ego vehicle - Note: Can be None - """ - return CarlaDataProvider._ego_vehicle_route - @staticmethod def generate_spawn_points(): """ diff --git a/srunner/scenariomanager/result_writer.py b/srunner/scenariomanager/result_writer.py index b7e0dce20..7e7711ed8 100644 --- a/srunner/scenariomanager/result_writer.py +++ b/srunner/scenariomanager/result_writer.py @@ -101,23 +101,20 @@ def create_output_text(self): # Criteria part output += " > Criteria Information\n" - header = ['Actor', 'Criterion', 'Result', 'Actual Value', 'Expected Value'] + header = ['Actor', 'Criterion', 'Result', 'Actual Value', 'Success Value'] list_statistics = [header] for criterion in self._data.scenario.get_criteria(): - name_string = criterion.name + name = criterion.name if criterion.optional: - name_string += " (Opt.)" + name += " (Opt.)" else: - name_string += " (Req.)" + name += " (Req.)" actor = "{} (id={})".format(criterion.actor.type_id[8:], criterion.actor.id) - criteria = name_string - result = "FAILURE" if criterion.test_status == "RUNNING" else criterion.test_status - actual_value = criterion.actual_value - expected_value = criterion.expected_value_success - list_statistics.extend([[actor, criteria, result, actual_value, expected_value]]) + list_statistics.extend([[ + actor, name, criterion.test_status, criterion.actual_value, criterion.success_value]]) # Timeout actor = "" @@ -182,7 +179,7 @@ def result_dict(name, actor, optional, expected, actual, success): criterion.name, "{}-{}".format(criterion.actor.type_id[8:], criterion.actor.id), criterion.optional, - criterion.expected_value_success, + criterion.success_value, criterion.actual_value, criterion.test_status in ["SUCCESS", "ACCEPTABLE"] ) @@ -255,7 +252,7 @@ def _write_to_junit(self): result_string += " Actual: {}\n".format( criterion.actual_value) result_string += " Expected: {}\n".format( - criterion.expected_value_success) + criterion.success_value) result_string += "\n" result_string += " Exact Value: {} = {}]]>\n".format( criterion.name, criterion.actual_value) diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index 62cffabf1..d832f7f8e 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -48,7 +48,6 @@ def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0): """ self.scenario = None self.scenario_tree = None - self.scenario_class = None self.ego_vehicles = None self.other_actors = None @@ -103,8 +102,7 @@ def load_scenario(self, scenario, agent=None): self._agent = AgentWrapper(agent) if agent else None if self._agent is not None: self._sync_mode = True - self.scenario_class = scenario - self.scenario = scenario.scenario + self.scenario = scenario self.scenario_tree = self.scenario.scenario_tree self.ego_vehicles = scenario.ego_vehicles self.other_actors = scenario.other_actors @@ -211,11 +209,12 @@ def analyze_scenario(self, stdout, filename, junit, json): timeout = False result = "SUCCESS" - if self.scenario.test_criteria is None: + criteria = self.scenario.get_criteria() + if len(criteria) == 0: print("Nothing to analyze, this scenario has no criteria") return True - for criterion in self.scenario.get_criteria(): + for criterion in criteria: if (not criterion.optional and criterion.test_status != "SUCCESS" and criterion.test_status != "ACCEPTABLE"): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 35aa46b85..851bdf745 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -15,7 +15,6 @@ The atomic criteria are implemented with py_trees. """ -import weakref import math import numpy as np import py_trees @@ -35,34 +34,38 @@ class Criterion(py_trees.behaviour.Behaviour): Important parameters (PUBLIC): - name: Name of the criterion - - expected_value_success: Result in case of success - (e.g. max_speed, zero collisions, ...) - - expected_value_acceptable: Result that does not mean a failure, - but is not good enough for a success - - actual_value: Actual result after running the scenario - - test_status: Used to access the result of the criterion + - actor: Actor of the criterion - optional: Indicates if a criterion is optional (not used for overall analysis) + - terminate on failure: Whether or not the criteria stops on failure + + - test_status: Used to access the result of the criterion + - success_value: Result in case of success (e.g. max_speed, zero collisions, ...) + - acceptable_value: Result that does not mean a failure, but is not good enough for a success + - actual_value: Actual result after running the scenario + - units: units of the 'actual_value'. This is a string and is used by the result writter """ def __init__(self, name, actor, - expected_value_success, - expected_value_acceptable=None, optional=False, terminate_on_failure=False): super(Criterion, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._terminate_on_failure = terminate_on_failure self.name = name self.actor = actor - self.test_status = "INIT" - self.expected_value_success = expected_value_success - self.expected_value_acceptable = expected_value_acceptable - self.actual_value = 0 self.optional = optional - self.list_traffic_events = [] + self._terminate_on_failure = terminate_on_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 + self.acceptable_value = None + self.actual_value = 0 + self.units = "times" + + self.events = [] # List of events (i.e collision, sidewalk invasion...) def initialise(self): """ @@ -91,11 +94,12 @@ class MaxVelocityTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, actor, max_velocity_allowed, optional=False, name="CheckMaximumVelocity"): + def __init__(self, actor, max_velocity, optional=False, name="CheckMaximumVelocity"): """ Setup actor and maximum allowed velovity """ - super(MaxVelocityTest, self).__init__(name, actor, max_velocity_allowed, None, optional) + super(MaxVelocityTest, self).__init__(name, actor, optional) + self.success_value = max_velocity def update(self): """ @@ -110,7 +114,7 @@ def update(self): self.actual_value = max(velocity, self.actual_value) - if velocity > self.expected_value_success: + if velocity > self.success_value: self.test_status = "FAILURE" else: self.test_status = "SUCCESS" @@ -124,7 +128,6 @@ def update(self): class DrivenDistanceTest(Criterion): - """ This class contains an atomic test to check the driven distance @@ -137,16 +140,13 @@ class DrivenDistanceTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, - actor, - distance_success, - distance_acceptable=None, - optional=False, - name="CheckDrivenDistance"): + def __init__(self, actor, distance, acceptable_distance=None, optional=False, name="CheckDrivenDistance"): """ Setup actor """ - super(DrivenDistanceTest, self).__init__(name, actor, distance_success, distance_acceptable, optional) + super(DrivenDistanceTest, self).__init__(name, actor, optional) + self.success_value = distance + self.acceptable_value = acceptable_distance self._last_location = None def initialise(self): @@ -174,10 +174,10 @@ def update(self): self.actual_value += location.distance(self._last_location) self._last_location = location - if self.actual_value > self.expected_value_success: + if self.actual_value > self.success_value: self.test_status = "SUCCESS" - elif (self.expected_value_acceptable is not None and - self.actual_value > self.expected_value_acceptable): + elif (self.acceptable_value is not None and + self.actual_value > self.acceptable_value): self.test_status = "ACCEPTABLE" else: self.test_status = "RUNNING" @@ -213,19 +213,14 @@ class AverageVelocityTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - def __init__(self, - actor, - avg_velocity_success, - avg_velocity_acceptable=None, - optional=False, + def __init__(self, actor, velocity, acceptable_velocity=None, optional=False, name="CheckAverageVelocity"): """ Setup actor and average velovity expected """ - super(AverageVelocityTest, self).__init__(name, actor, - avg_velocity_success, - avg_velocity_acceptable, - optional) + super(AverageVelocityTest, self).__init__(name, actor, optional) + self.success_value = velocity + self.acceptable_value = acceptable_velocity self._last_location = None self._distance = 0.0 @@ -258,10 +253,10 @@ def update(self): if elapsed_time > 0.0: self.actual_value = self._distance / elapsed_time - if self.actual_value > self.expected_value_success: + if self.actual_value > self.success_value: self.test_status = "SUCCESS" - elif (self.expected_value_acceptable is not None and - self.actual_value > self.expected_value_acceptable): + elif (self.acceptable_value is not None and + self.actual_value > self.acceptable_value): self.test_status = "ACCEPTABLE" else: self.test_status = "RUNNING" @@ -296,28 +291,33 @@ class CollisionTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ - MIN_AREA_OF_COLLISION = 3 # If closer than this distance, the collision is ignored - MAX_AREA_OF_COLLISION = 5 # If further than this distance, the area is forgotten - MAX_ID_TIME = 5 # Amount of time the last collision if is remembered + 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 def __init__(self, actor, other_actor=None, other_actor_type=None, - optional=False, name="CollisionTest", terminate_on_failure=False): + optional=False, terminate_on_failure=False, name="CollisionTest"): """ Construction with sensor setup """ - super(CollisionTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure) + super(CollisionTest, self).__init__(name, actor, optional, terminate_on_failure) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + self._other_actor = other_actor + self._other_actor_type = other_actor_type - world = self.actor.get_world() + # Attributes to store the last collisions's data + self._collision_sensor = None + self._collision_id = None + self._collision_time = None + self._collision_location = None + + 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) - self._collision_sensor.listen(lambda event: self._count_collisions(weakref.ref(self), event)) - - self.other_actor = other_actor - self.other_actor_type = other_actor_type - self.registered_collisions = [] - self.last_id = None - self.collision_time = None + self._collision_sensor.listen(lambda event: self._count_collisions(event)) + super(CollisionTest, self).initialise() def update(self): """ @@ -329,23 +329,16 @@ def update(self): new_status = py_trees.common.Status.FAILURE actor_location = CarlaDataProvider.get_location(self.actor) - new_registered_collisions = [] - # Loops through all the previous registered collisions - for collision_location in self.registered_collisions: - - # Get the distance to the collision point - distance_vector = actor_location - collision_location - distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) - - # If far away from a previous collision, forget it - if distance <= self.MAX_AREA_OF_COLLISION: - new_registered_collisions.append(collision_location) - - self.registered_collisions = new_registered_collisions - - if self.last_id and GameTime.get_time() - self.collision_time > self.MAX_ID_TIME: - self.last_id = None + # Check if the last collision can be ignored + if self._collision_location: + distance_vector = actor_location - self._collision_location + if distance_vector.length() > self.COLLISION_RADIUS: + self._collision_location = None + if self._collision_id: + elapsed_time = GameTime.get_time() - self._collision_time + 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)) @@ -358,46 +351,39 @@ def terminate(self, new_status): if self._collision_sensor is not None: self._collision_sensor.destroy() self._collision_sensor = None - super(CollisionTest, self).terminate(new_status) - @staticmethod - def _count_collisions(weak_self, event): # pylint: disable=too-many-return-statements - """ - Callback to update collision count - """ - self = weak_self() - if not self: - return - + def _count_collisions(self, event): # pylint: disable=too-many-return-statements + """Update collision count""" actor_location = CarlaDataProvider.get_location(self.actor) - # Ignore the current one if it is the same id as before - if self.last_id == event.other_actor.id: - return - - # Filter to only a specific actor - if self.other_actor and self.other_actor.id != event.other_actor.id: + # Check if the care about the other actor + if self._other_actor and self._other_actor.id != event.other_actor.id: return - # Filter to only a specific type - if self.other_actor_type: - if self.other_actor_type == "miscellaneous": - if "traffic" not in event.other_actor.type_id \ - and "static" not in event.other_actor.type_id: + 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: return - else: - if self.other_actor_type not in event.other_actor.type_id: + elif self._other_actor_type not in event.other_actor.type_id: return - # Ignore it if its too close to a previous collision (avoid micro collisions) - for collision_location in self.registered_collisions: + # To avoid multiple counts of the same collision, filter some of them. + if self._collision_id == event.other_actor.id: + return + if self._collision_location: + distance_vector = actor_location - self._collision_location + if distance_vector.length() <= self.COLLISION_RADIUS: + return - distance_vector = actor_location - collision_location - distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) + # The collision is valid, save the data + self.test_status = "FAILURE" + self.actual_value += 1 - if distance <= self.MIN_AREA_OF_COLLISION: - return + self._collision_time = GameTime.get_time() + self._collision_location = actor_location + 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: @@ -410,12 +396,7 @@ def _count_collisions(weak_self, event): # pylint: disable=too-many-return-s return collision_event = TrafficEvent(event_type=actor_type) - collision_event.set_dict({ - 'type': event.other_actor.type_id, - 'id': event.other_actor.id, - 'x': actor_location.x, - 'y': actor_location.y, - 'z': actor_location.z}) + 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, @@ -423,95 +404,63 @@ def _count_collisions(weak_self, event): # pylint: disable=too-many-return-s round(actor_location.x, 3), round(actor_location.y, 3), round(actor_location.z, 3))) + self.events.append(collision_event) - self.test_status = "FAILURE" - self.actual_value += 1 - self.collision_time = GameTime.get_time() - - self.registered_collisions.append(actor_location) - self.list_traffic_events.append(collision_event) - # Number 0: static objects -> ignore it - if event.other_actor.id != 0: - self.last_id = event.other_actor.id - - -class ActorSpeedAboveThresholdTest(Criterion): +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 Important parameters: - actor: CARLA actor to be used for this test - - speed_threshold: speed required - - below_threshold_max_time: Maximum time (in seconds) the actor can remain under the speed threshold + - min_speed: speed required [m/s] + - max_time: Maximum time (in seconds) the actor can remain under the speed threshold - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ - def __init__(self, actor, speed_threshold, below_threshold_max_time, - name="ActorSpeedAboveThresholdTest", terminate_on_failure=False): + def __init__(self, actor, min_speed, max_time, name="ActorBlockedTest", optional=False, terminate_on_failure=False): """ - Class constructor. + Class constructor """ - super(ActorSpeedAboveThresholdTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor - self._speed_threshold = speed_threshold - self._below_threshold_max_time = below_threshold_max_time + super(ActorBlockedTest, self).__init__(name, actor, optional, terminate_on_failure) + self._min_speed = min_speed + self._max_time = max_time self._time_last_valid_state = None + self.units = None # We care about whether or not it fails, no units attached def update(self): """ - Check if the actor speed is above the speed_threshold + Check if the actor speed is above the min_speed """ new_status = py_trees.common.Status.RUNNING - linear_speed = CarlaDataProvider.get_velocity(self._actor) + linear_speed = CarlaDataProvider.get_velocity(self.actor) if linear_speed is not None: - if linear_speed < self._speed_threshold and self._time_last_valid_state: - if (GameTime.get_time() - self._time_last_valid_state) > self._below_threshold_max_time: - # Game over. The actor has been "blocked" for too long + if linear_speed < self._min_speed and self._time_last_valid_state: + 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" - # record event - vehicle_location = CarlaDataProvider.get_location(self._actor) - blocked_event = TrafficEvent(event_type=TrafficEventType.VEHICLE_BLOCKED) - ActorSpeedAboveThresholdTest._set_event_message(blocked_event, vehicle_location) - ActorSpeedAboveThresholdTest._set_event_dict(blocked_event, vehicle_location) - self.list_traffic_events.append(blocked_event) + vehicle_location = CarlaDataProvider.get_location(self.actor) + event = TrafficEvent(event_type=TrafficEventType.VEHICLE_BLOCKED) + 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}) + 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)) - return new_status - @staticmethod - def _set_event_message(event, location): - """ - Sets the message of the event - """ - - event.set_message('Agent got blocked at (x={}, y={}, z={})'.format(round(location.x, 3), - round(location.y, 3), - round(location.z, 3))) - - @staticmethod - def _set_event_dict(event, location): - """ - Sets the dictionary of the event - """ - event.set_dict({ - 'x': location.x, - 'y': location.y, - 'z': location.z, - }) - class KeepLaneTest(Criterion): - """ This class contains an atomic test for keeping lane. @@ -524,13 +473,12 @@ def __init__(self, actor, optional=False, name="CheckKeepLane"): """ Construction with sensor setup """ - super(KeepLaneTest, self).__init__(name, actor, 0, None, optional) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + 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) - self._lane_sensor.listen(lambda event: self._count_lane_invasion(weakref.ref(self), event)) + self._lane_sensor.listen(lambda event: self._count_lane_invasion(event)) def update(self): """ @@ -556,17 +504,13 @@ def terminate(self, new_status): """ if self._lane_sensor is not None: self._lane_sensor.destroy() - self._lane_sensor = None + self._lane_sensor = None super(KeepLaneTest, self).terminate(new_status) - @staticmethod - def _count_lane_invasion(weak_self, event): + def _count_lane_invasion(self, event): """ Callback to update lane invasion count """ - self = weak_self() - if not self: - return self.actual_value += 1 @@ -586,9 +530,7 @@ def __init__(self, actor, min_x, max_x, min_y, max_y, name="ReachedRegionTest"): Setup trigger region (rectangle provided by [min_x,min_y] and [max_x,max_y] """ - super(ReachedRegionTest, self).__init__(name, actor, 0) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor + super(ReachedRegionTest, self).__init__(name, actor) self._min_x = min_x self._max_x = max_x self._min_y = min_y @@ -600,7 +542,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = CarlaDataProvider.get_location(self._actor) + location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status @@ -617,7 +559,6 @@ def update(self): new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) - return new_status @@ -641,8 +582,7 @@ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False """ Setup of the variables """ - super(OffRoadTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + super(OffRoadTest, self).__init__(name, actor, optional, terminate_on_failure) self._map = CarlaDataProvider.get_map() self._offroad = False @@ -720,8 +660,7 @@ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False """ Setup of the variables """ - super(EndofRoadTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + super(EndofRoadTest, self).__init__(name, actor, optional, terminate_on_failure) self._map = CarlaDataProvider.get_map() self._end_of_road = False @@ -767,7 +706,6 @@ def update(self): return py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) - return new_status @@ -790,15 +728,13 @@ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False """ Construction with sensor setup """ - super(OnSidewalkTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + super(OnSidewalkTest, self).__init__(name, actor, optional, terminate_on_failure) - self._actor = actor self._map = CarlaDataProvider.get_map() self._onsidewalk_active = False self._outside_lane_active = False - self._actor_location = self._actor.get_location() + self._actor_location = self.actor.get_location() self._wrong_sidewalk_distance = 0 self._wrong_outside_lane_distance = 0 self._sidewalk_start_location = None @@ -824,7 +760,7 @@ def update(self): new_status = py_trees.common.Status.FAILURE # Some of the vehicle parameters - current_tra = CarlaDataProvider.get_transform(self._actor) + current_tra = CarlaDataProvider.get_transform(self.actor) current_loc = current_tra.location current_wp = self._map.get_waypoint(current_loc, lane_type=carla.LaneType.Any) @@ -923,11 +859,11 @@ def update(self): self.test_status = "FAILURE" # Update the distances - distance_vector = CarlaDataProvider.get_location(self._actor) - self._actor_location + 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) + self._actor_location = CarlaDataProvider.get_location(self.actor) if self._onsidewalk_active: self._wrong_sidewalk_distance += distance @@ -948,7 +884,7 @@ def update(self): self._onsidewalk_active = False self._wrong_sidewalk_distance = 0 - self.list_traffic_events.append(onsidewalk_event) + self.events.append(onsidewalk_event) # Register the outside of a lane event if not self._outside_lane_active and self._wrong_outside_lane_distance > 0: @@ -963,7 +899,7 @@ def update(self): self._outside_lane_active = False self._wrong_outside_lane_distance = 0 - self.list_traffic_events.append(outsidelane_event) + self.events.append(outsidelane_event) self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) @@ -986,7 +922,7 @@ def terminate(self, new_status): self._onsidewalk_active = False self._wrong_sidewalk_distance = 0 - self.list_traffic_events.append(onsidewalk_event) + self.events.append(onsidewalk_event) # If currently outside of our lane, register the event if self._outside_lane_active: @@ -1001,7 +937,7 @@ def terminate(self, new_status): self._outside_lane_active = False self._wrong_outside_lane_distance = 0 - self.list_traffic_events.append(outsidelane_event) + self.events.append(outsidelane_event) super(OnSidewalkTest, self).terminate(new_status) @@ -1026,11 +962,7 @@ def _set_event_dict(self, event, location, distance): """ Sets the dictionary of the event """ - event.set_dict({ - 'x': location.x, - 'y': location.y, - 'z': location.z, - 'distance': distance}) + event.set_dict({'location': location, 'distance': distance}) class OutsideRouteLanesTest(Criterion): @@ -1054,17 +986,16 @@ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): """ Constructor """ - super(OutsideRouteLanesTest, self).__init__(name, actor, 0, None, optional) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + super(OutsideRouteLanesTest, self).__init__(name, actor, optional) + self.units = "%" - self._actor = actor self._route = route self._current_index = 0 self._route_length = len(self._route) - self._waypoints, _ = zip(*self._route) + self._route_transforms, _ = zip(*self._route) self._map = CarlaDataProvider.get_map() - self._pre_ego_waypoint = self._map.get_waypoint(self._actor.get_location()) + self._pre_ego_waypoint = self._map.get_waypoint(self.actor.get_location()) self._outside_lane_active = False self._wrong_lane_active = False @@ -1084,36 +1015,32 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - if self._terminate_on_failure and (self.test_status == "FAILURE"): - new_status = py_trees.common.Status.FAILURE - # Some of the vehicle parameters - location = CarlaDataProvider.get_location(self._actor) + location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status - # 1) Check if outside route lanes + # Check if outside route lanes self._is_outside_driving_lanes(location) self._is_at_wrong_lane(location) if self._outside_lane_active or self._wrong_lane_active: self.test_status = "FAILURE" - # 2) Get the traveled distance + # Get the traveled distance 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 - index_location = self._waypoints[index] + index_location = self._route_transforms[index].location index_waypoint = self._map.get_waypoint(index_location) wp_dir = index_waypoint.transform.get_forward_vector() # Waypoint's forward vector wp_veh = location - index_location # vector waypoint - vehicle - dot_ve_wp = wp_veh.x * wp_dir.x + wp_veh.y * wp_dir.y + wp_veh.z * wp_dir.z - if dot_ve_wp > 0: + if wp_veh.dot(wp_dir) > 0: # Get the distance traveled - index_location = self._waypoints[index] - current_index_location = self._waypoints[self._current_index] + index_location = self._route_transforms[index].location + current_index_location = self._route_transforms[self._current_index].location new_dist = current_index_location.distance(index_location) # Add it to the total distance @@ -1125,7 +1052,6 @@ def update(self): self._wrong_distance += new_dist self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) - return new_status def _is_outside_driving_lanes(self, location): @@ -1155,7 +1081,6 @@ def _is_at_wrong_lane(self, location): """ Detects if the ego_vehicle has invaded a wrong lane """ - current_waypoint = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving, project_to_road=True) current_lane_id = current_waypoint.lane_id current_road_id = current_waypoint.road_id @@ -1168,7 +1093,7 @@ def _is_at_wrong_lane(self, location): # Route direction can be considered continuous, except after exiting a junction. if self._pre_ego_waypoint.is_junction: yaw_waypt = current_waypoint.transform.rotation.yaw % 360 - yaw_actor = self._actor.get_transform().rotation.yaw % 360 + yaw_actor = self.actor.get_transform().rotation.yaw % 360 vehicle_lane_angle = (yaw_waypt - yaw_actor) % 360 @@ -1222,7 +1147,7 @@ def terminate(self, new_status): }) self._wrong_distance = 0 - self.list_traffic_events.append(outside_lane) + self.events.append(outside_lane) self.actual_value = round(percentage, 2) super(OutsideRouteLanesTest, self).terminate(new_status) @@ -1244,18 +1169,17 @@ def __init__(self, actor, optional=False, name="WrongLaneTest"): """ Construction with sensor setup """ - super(WrongLaneTest, self).__init__(name, actor, 0, None, optional) + super(WrongLaneTest, self).__init__(name, actor, optional) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor self._map = CarlaDataProvider.get_map() self._last_lane_id = None self._last_road_id = None self._in_lane = True self._wrong_distance = 0 - self._actor_location = self._actor.get_location() - self._previous_lane_waypoint = self._map.get_waypoint(self._actor.get_location()) + self._actor_location = self.actor.get_location() + self._previous_lane_waypoint = self._map.get_waypoint(self.actor.get_location()) self._wrong_lane_start_location = None def update(self): @@ -1268,7 +1192,7 @@ def update(self): if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE - lane_waypoint = self._map.get_waypoint(self._actor.get_location()) + lane_waypoint = self._map.get_waypoint(self.actor.get_location()) current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id @@ -1308,8 +1232,8 @@ def update(self): 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_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))) @@ -1319,14 +1243,14 @@ def update(self): self.test_status = "FAILURE" self._in_lane = False self.actual_value += 1 - self._wrong_lane_start_location = self._actor.get_location() + self._wrong_lane_start_location = self.actor.get_location() # Keep adding "meters" to the counter - distance_vector = self._actor.get_location() - self._actor_location + distance_vector = self.actor.get_location() - 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 add to considerable sums - self._actor_location = CarlaDataProvider.get_location(self._actor) + self._actor_location = CarlaDataProvider.get_location(self.actor) if not self._in_lane and not lane_waypoint.is_junction: self._wrong_distance += distance @@ -1340,7 +1264,7 @@ def update(self): self._set_event_dict(wrong_way_event, self._wrong_lane_start_location, self._wrong_distance, current_road_id, current_lane_id) - self.list_traffic_events.append(wrong_way_event) + self.events.append(wrong_way_event) self._wrong_distance = 0 # Remember the last state @@ -1358,7 +1282,7 @@ def terminate(self, new_status): """ if not self._in_lane: - lane_waypoint = self._map.get_waypoint(self._actor.get_location()) + lane_waypoint = self._map.get_waypoint(self.actor.get_location()) current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id @@ -1370,7 +1294,7 @@ def terminate(self, new_status): self._wrong_distance = 0 self._in_lane = True - self.list_traffic_events.append(wrong_way_event) + self.events.append(wrong_way_event) super(WrongLaneTest, self).terminate(new_status) @@ -1394,9 +1318,7 @@ def _set_event_dict(self, event, location, distance, road_id, lane_id): Sets the dictionary of the event """ event.set_dict({ - 'x': location.x, - 'y': location.y, - 'z': location.y, + 'location': location, 'distance': distance, 'road_id': road_id, 'lane_id': lane_id}) @@ -1415,9 +1337,8 @@ class InRadiusRegionTest(Criterion): def __init__(self, actor, x, y, radius, name="InRadiusRegionTest"): """ """ - super(InRadiusRegionTest, self).__init__(name, actor, 0) + super(InRadiusRegionTest, self).__init__(name, actor) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor self._x = x # pylint: disable=invalid-name self._y = y # pylint: disable=invalid-name self._radius = radius @@ -1428,7 +1349,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = CarlaDataProvider.get_location(self._actor) + location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status @@ -1437,7 +1358,7 @@ def update(self): if in_radius: route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED) route_completion_event.set_message("Destination was successfully reached") - self.list_traffic_events.append(route_completion_event) + self.events.append(route_completion_event) self.test_status = "SUCCESS" else: self.test_status = "RUNNING" @@ -1466,38 +1387,36 @@ class InRouteTest(Criterion): MAX_ROUTE_PERCENTAGE = 30 # % WINDOWS_SIZE = 5 # Amount of additional waypoints checked - def __init__(self, actor, route, offroad_min=-1, offroad_max=30, name="InRouteTest", terminate_on_failure=False): + def __init__(self, actor, route, offroad_min=None, offroad_max=30, name="InRouteTest", terminate_on_failure=False): """ """ - super(InRouteTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor + 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 self._offroad_max = offroad_max # Unless specified, halve of the max value - if offroad_min == -1: + if offroad_min is None: self._offroad_min = self._offroad_max / 2 else: self._offroad_min = self._offroad_min self._world = CarlaDataProvider.get_world() - self._waypoints, _ = zip(*self._route) + self._route_transforms, _ = zip(*self._route) self._route_length = len(self._route) self._current_index = 0 self._out_route_distance = 0 self._in_safe_route = True self._accum_meters = [] - prev_wp = self._waypoints[0] - for i, wp in enumerate(self._waypoints): - d = wp.distance(prev_wp) - if i > 0: - accum = self._accum_meters[i - 1] - else: - accum = 0 + prev_loc = self._route_transforms[0].location + for i, tran in enumerate(self._route_transforms): + loc = tran.location + d = loc.distance(prev_loc) + accum = 0 if i == 0 else self._accum_meters[i - 1] self._accum_meters.append(d + accum) - prev_wp = wp + prev_loc = loc # Blackboard variable blackv = py_trees.blackboard.Blackboard() @@ -1509,7 +1428,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = CarlaDataProvider.get_location(self._actor) + location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status @@ -1526,8 +1445,8 @@ def update(self): # Get the closest distance for index in range(self._current_index, min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)): - ref_waypoint = self._waypoints[index] - distance = math.sqrt(((location.x - ref_waypoint.x) ** 2) + ((location.y - ref_waypoint.y) ** 2)) + ref_location = self._route_transforms[index].location + 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 @@ -1565,12 +1484,9 @@ def update(self): round(location.x, 3), round(location.y, 3), round(location.z, 3))) - route_deviation_event.set_dict({ - 'x': location.x, - 'y': location.y, - 'z': location.z}) + route_deviation_event.set_dict({'location': location}) - self.list_traffic_events.append(route_deviation_event) + self.events.append(route_deviation_event) self.test_status = "FAILURE" self.actual_value += 1 @@ -1597,33 +1513,30 @@ class RouteCompletionTest(Criterion): def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False): """ """ - super(RouteCompletionTest, self).__init__(name, actor, 100, terminate_on_failure=terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor + super(RouteCompletionTest, self).__init__(name, actor, terminate_on_failure=terminate_on_failure) + self.units = "%" + self.success_value = 100 self._route = route self._map = CarlaDataProvider.get_map() self._wsize = self.WINDOWS_SIZE self._current_index = 0 self._route_length = len(self._route) - self._waypoints, _ = zip(*self._route) - self.target = self._waypoints[-1] + self._route_transforms, _ = zip(*self._route) + self.target_location = self._route_transforms[-1].location self._accum_meters = [] - prev_wp = self._waypoints[0] - for i, wp in enumerate(self._waypoints): - d = wp.distance(prev_wp) - if i > 0: - accum = self._accum_meters[i - 1] - else: - accum = 0 + prev_loc = self._route_transforms[0].location + for i, tran in enumerate(self._route_transforms): + loc = tran.location + d = loc.distance(prev_loc) + accum = 0 if i == 0 else self._accum_meters[i - 1] self._accum_meters.append(d + accum) - prev_wp = wp + prev_loc = loc self._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION) - self.list_traffic_events.append(self._traffic_event) - self._percentage_route_completed = 0.0 + self.events.append(self._traffic_event) def update(self): """ @@ -1631,7 +1544,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = CarlaDataProvider.get_location(self._actor) + location = CarlaDataProvider.get_location(self.actor) if location is None: return new_status @@ -1642,29 +1555,28 @@ def update(self): for index in range(self._current_index, min(self._current_index + self._wsize + 1, self._route_length)): # Get the dot product to know if it has passed this location - ref_waypoint = self._waypoints[index] - wp = self._map.get_waypoint(ref_waypoint) + ref_location = self._route_transforms[index].location + wp = self._map.get_waypoint(ref_location) wp_dir = wp.transform.get_forward_vector() # Waypoint's forward vector - wp_veh = location - ref_waypoint # vector waypoint - vehicle - dot_ve_wp = wp_veh.x * wp_dir.x + wp_veh.y * wp_dir.y + wp_veh.z * wp_dir.z + wp_veh = location - ref_location # vector waypoint - vehicle - if dot_ve_wp > 0: + if wp_veh.dot(wp_dir) > 0: # good! segment completed! self._current_index = index - self._percentage_route_completed = 100.0 * float(self._accum_meters[self._current_index]) \ + self.actual_value = 100.0 * float(self._accum_meters[self._current_index]) \ / float(self._accum_meters[-1]) self._traffic_event.set_dict({ - 'route_completed': self._percentage_route_completed}) + 'route_completed': self.actual_value}) self._traffic_event.set_message( "Agent has completed > {:.2f}% of the route".format( - self._percentage_route_completed)) + self.actual_value)) - if self._percentage_route_completed > 99.0 and location.distance(self.target) < self.DISTANCE_THRESHOLD: + if self.actual_value > 99.0 and location.distance(self.target_location) < self.DISTANCE_THRESHOLD: route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED) route_completion_event.set_message("Destination was successfully reached") - self.list_traffic_events.append(route_completion_event) + self.events.append(route_completion_event) self.test_status = "SUCCESS" - self._percentage_route_completed = 100 + self.actual_value = 100 elif self.test_status == "SUCCESS": new_status = py_trees.common.Status.SUCCESS @@ -1677,7 +1589,7 @@ def terminate(self, new_status): """ Set test status to failure if not successful and terminate """ - self.actual_value = round(self._percentage_route_completed, 2) + self.actual_value = round(self.actual_value, 2) if self.test_status == "INIT": self.test_status = "FAILURE" @@ -1699,14 +1611,11 @@ def __init__(self, actor, name="RunningRedLightTest", terminate_on_failure=False """ Init """ - super(RunningRedLightTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor - self._world = actor.get_world() + 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 = [] self._last_red_light_id = None - self.actual_value = 0 self.debug = False all_actors = self._world.get_actors() @@ -1732,12 +1641,12 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - transform = CarlaDataProvider.get_transform(self._actor) + transform = CarlaDataProvider.get_transform(self.actor) location = transform.location if location is None: return new_status - veh_extent = self._actor.bounding_box.extent.x + veh_extent = self.actor.bounding_box.extent.x tail_close_pt = self.rotate_point(carla.Vector3D(-0.8 * veh_extent, 0.0, location.z), transform.rotation.yaw) tail_close_pt = location + carla.Location(tail_close_pt) @@ -1777,12 +1686,11 @@ def update(self): 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() - dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z # 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 dot_ve_wp > 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 @@ -1806,13 +1714,9 @@ def update(self): round(location.x, 3), round(location.y, 3), round(location.z, 3))) - red_light_event.set_dict({ - 'id': traffic_light.id, - 'x': location.x, - 'y': location.y, - 'z': location.z}) + red_light_event.set_dict({'id': traffic_light.id, 'location': location}) - self.list_traffic_events.append(red_light_event) + self.events.append(red_light_event) self._last_red_light_id = traffic_light.id break @@ -1887,16 +1791,13 @@ class RunningStopTest(Criterion): def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): """ """ - super(RunningStopTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure) - self.logger.debug("%s.__init__()" % (self.__class__.__name__)) - self._actor = actor + 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 = [] self._target_stop_sign = None self._stop_completed = False self._affected_by_stop = False - self.actual_value = 0 all_actors = self._world.get_actors() for _actor in all_actors: @@ -1965,17 +1866,15 @@ def is_actor_affected_by_stop(self, actor, stop, multi_step=20): def _scan_for_stop_sign(self): target_stop_sign = None - ve_tra = CarlaDataProvider.get_transform(self._actor) + ve_tra = CarlaDataProvider.get_transform(self.actor) ve_dir = ve_tra.get_forward_vector() wp = self._map.get_waypoint(ve_tra.location) wp_dir = wp.transform.get_forward_vector() - dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z - - if dot_ve_wp > 0: # Ignore all when going in a wrong lane + if ve_dir.dot(wp_dir) > 0: # Ignore all when going in a wrong lane for stop_sign in self._list_stop_signs: - if self.is_actor_affected_by_stop(self._actor, stop_sign): + if self.is_actor_affected_by_stop(self.actor, stop_sign): # this stop sign is affecting the vehicle target_stop_sign = stop_sign break @@ -1988,7 +1887,7 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = self._actor.get_location() + location = self.actor.get_location() if location is None: return new_status @@ -1999,7 +1898,7 @@ def update(self): # we were in the middle of dealing with a stop sign if not self._stop_completed: # did the ego-vehicle stop? - current_speed = CarlaDataProvider.get_velocity(self._actor) + current_speed = CarlaDataProvider.get_velocity(self.actor) if current_speed < self.SPEED_THRESHOLD: self._stop_completed = True @@ -2010,7 +1909,7 @@ def update(self): if self.point_inside_boundingbox(location, stop_location, stop_extent): self._affected_by_stop = True - if not self.is_actor_affected_by_stop(self._actor, self._target_stop_sign): + if not self.is_actor_affected_by_stop(self.actor, self._target_stop_sign): # is the vehicle out of the influence of this stop sign now? if not self._stop_completed and self._affected_by_stop: # did we stop? @@ -2024,13 +1923,9 @@ def update(self): 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, - 'x': stop_location.x, - 'y': stop_location.y, - 'z': stop_location.z}) + running_stop_event.set_dict({'id': self._target_stop_sign.id, 'location': stop_location}) - self.list_traffic_events.append(running_stop_event) + self.events.append(running_stop_event) # reset state self._target_stop_sign = None diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index 75f5e82bd..0de653d2a 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -38,7 +38,7 @@ class TrafficEvent(object): TrafficEvent definition """ - def __init__(self, event_type, message=None, dictionary=None): + def __init__(self, event_type, message="", dictionary=None): """ Initialize object @@ -51,34 +51,21 @@ def __init__(self, event_type, message=None, dictionary=None): self._dict = dictionary def get_type(self): - """ - @return type - """ + """return the type""" return self._type - def get_message(self): - """ - @return message - """ - if self._message: - return self._message - - return "" - def set_message(self, message): - """ - Set message - """ + """Set message""" self._message = message - def get_dict(self): - """ - @return dictionary - """ - return self._dict + def get_message(self): + """returns the message""" + return self._message def set_dict(self, dictionary): - """ - Set dictionary - """ + """Set dictionary""" self._dict = dictionary + + def get_dict(self): + """returns the dictionary""" + return self._dict diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index ca07cc76f..999724f4a 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -102,7 +102,7 @@ def _create_behavior(self): root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wps[i].transform.location, self._sink_distance)) sequence = py_trees.composites.Sequence() - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: sequence.add_child(JunctionScenarioManager('left', True, False, True)) grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) @@ -117,7 +117,7 @@ def _create_behavior(self): sequence.add_child(SwitchRouteSources(False)) sequence.add_child(root) - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: sequence.add_child(SwitchRouteSources(True)) return sequence @@ -127,6 +127,8 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): @@ -208,7 +210,7 @@ def _create_behavior(self): root.add_child(end_condition) sequence = py_trees.composites.Sequence() - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: sequence.add_child(JunctionScenarioManager('opposite', True, True, True)) sequence.add_child(StopEntries()) sequence.add_child(root) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index bca1a54a5..9ac88d8c7 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -189,7 +189,7 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - pass + return [] def __del__(self): """ @@ -203,7 +203,7 @@ class BackgroundBehavior(AtomicBehavior): Handles the background activity """ - def __init__(self, ego_actor, route, night_mode=False, debug=True, name="BackgroundBehavior"): + def __init__(self, ego_actor, route, night_mode=False, debug=False, name="BackgroundBehavior"): """ Setup class members """ diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index 4226d44d2..0be76bd08 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -10,17 +10,20 @@ """ from __future__ import print_function +from ast import NameConstant import operator import py_trees import carla -import srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions as conditions +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (WaitForBlackboardVariable, + InTimeToArrivalToLocation) from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.timer import TimeOut from srunner.scenariomanager.weather_sim import WeatherBehavior from srunner.scenariomanager.scenarioatomics.atomic_behaviors import UpdateAllActorControls +from srunner.scenariomanager.scenarioatomics.atomic_criteria import Criterion class BasicScenario(object): @@ -35,56 +38,83 @@ def __init__(self, name, ego_vehicles, config, world, Setup all relevant parameters and create scenario and instantiate scenario manager """ - self.other_actors = [] - if not self.timeout: # pylint: disable=access-member-before-definition - self.timeout = 60 # If no timeout was provided, set it to 60 seconds - - self.criteria_list = [] # List of evaluation criteria - self.scenario = None - self.world = world - - self.ego_vehicles = ego_vehicles self.name = name + self.ego_vehicles = ego_vehicles + self.other_actors = [] self.config = config + self.world = world + self.debug_mode = debug_mode self.terminate_on_failure = terminate_on_failure + self.criteria_enable = criteria_enable - self._initialize_environment(world) + self.route_mode = bool(config.route) + self.behavior_tree = None + self.criteria_tree = None - is_route_scenario = CarlaDataProvider.get_ego_vehicle_route() is not None - self._initialize_other_parameters(config, is_route_scenario) + if not self.timeout: + self.timeout = 60 # If no timeout was provided, set it to 60 seconds + if debug_mode: + py_trees.logging.level = py_trees.logging.Level.DEBUG - # Initializing adversarial actors + self._initialize_environment(world) self._initialize_actors(config) + if CarlaDataProvider.is_sync_mode(): world.tick() else: world.wait_for_tick() - # Setup scenario - if debug_mode: - py_trees.logging.level = py_trees.logging.Level.DEBUG - - behavior = self._create_behavior() + # Main scenario tree + self.scenario_tree = py_trees.composites.Parallel(name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - criteria = None - if criteria_enable: - criteria = self._create_test_criteria() + # Add a trigger and end condition to the behavior to ensure it is only activated when it is relevant + self.behavior_tree = py_trees.composites.Sequence() - # Add a trigger condition for the behavior to ensure the behavior is only activated, when it is relevant - behavior_seq = py_trees.composites.Sequence() trigger_behavior = self._setup_scenario_trigger(config) if trigger_behavior: - behavior_seq.add_child(trigger_behavior) + self.behavior_tree.add_child(trigger_behavior) - if behavior is not None: - behavior_seq.add_child(behavior) - behavior_seq.name = behavior.name + scenario_behavior = self._create_behavior() + self.behavior_tree.add_child(scenario_behavior) + self.behavior_tree.name = scenario_behavior.name end_behavior = self._setup_scenario_end(config) if end_behavior: - behavior_seq.add_child(end_behavior) + self.behavior_tree.add_child(end_behavior) + + # And then add it to the main tree + self.scenario_tree.add_child(self.behavior_tree) + + # Create the criteria tree (if needed) + if self.criteria_enable: + criteria = self._create_test_criteria() - self.scenario = Scenario(behavior_seq, criteria, self.name, self.timeout, self.terminate_on_failure) + # All the work is done, thanks! + if isinstance(criteria, py_trees.composites.Composite): + self.criteria_tree = criteria + + # Lazy mode, but its okay, we'll create the parallel behavior tree for you. + elif isinstance(criteria, list): + for criterion in criteria: + criterion.terminate_on_failure = terminate_on_failure + + self.criteria_tree = py_trees.composites.Parallel(name="Test Criteria", + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + self.criteria_tree.add_children(criteria) + self.criteria_tree.setup(timeout=1) + + else: + raise ValueError("The scenario criteria is neither a list nor a py_trees.composites.Composite") + + self.scenario_tree.add_child(self.criteria_tree) + + # Add other nodes + self.timeout_node = TimeOut(self.timeout, name="TimeOut") # Timeout node + self.scenario_tree.add_child(self.timeout_node) + self.scenario_tree.add_child(WeatherBehavior()) + self.scenario_tree.add_child(UpdateAllActorControls()) + + self.scenario_tree.setup(timeout=1) def _initialize_environment(self, world): """ @@ -122,15 +152,6 @@ def _initialize_actors(self, config): for new_actor in new_actors: self.other_actors.append(new_actor) - def _initialize_other_parameters(self, config, is_route_scenario): - """ - Initializes other parameters part of the config file. - As routes and scenarios are parsed differently, use 'is_route_scenario' - to know if the scenario is being triggered as part of a route. - Override this method in child class to provide custom initialization. - """ - pass - def _setup_scenario_trigger(self, config): """ This function creates a trigger maneuver, that has to be finished before the real scenario starts. @@ -138,31 +159,18 @@ def _setup_scenario_trigger(self, config): The function can be overloaded by a user implementation inside the user-defined scenario class. """ - start_location = None if config.trigger_points and config.trigger_points[0]: - start_location = config.trigger_points[0].location # start location of the scenario - - ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route() - - if start_location: - if ego_vehicle_route: - if config.route_var_name is None: # pylint: disable=no-else-return - return conditions.InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0], - ego_vehicle_route, - start_location, - 5) - else: - check_name = "WaitForBlackboardVariable: {}".format(config.route_var_name) - return conditions.WaitForBlackboardVariable(name=check_name, - variable_name=config.route_var_name, - variable_value=True, - var_init_value=False) - - return conditions.InTimeToArrivalToLocation(self.ego_vehicles[0], - 2.0, - start_location) - - return None + start_location = config.trigger_points[0].location + else: + return None + + # Scenario is not part of a route, wait for the ego to move + if not self.route_mode: + return InTimeToArrivalToLocation(self.ego_vehicles[0], 2.0, start_location) + + # Scenario is part of a route. + check_name = "WaitForBlackboardVariable: {}".format(config.route_var_name) + return WaitForBlackboardVariable(config.route_var_name, True, False, name=check_name) def _setup_scenario_end(self, config): """ @@ -171,15 +179,11 @@ def _setup_scenario_end(self, config): The function can be overloaded by a user implementation inside the user-defined scenario class. """ - ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route() + if not self.route_mode: + return None - if ego_vehicle_route: - if config.route_var_name is not None: - set_name = "Reset Blackboard Variable: {} ".format(config.route_var_name) - return py_trees.blackboard.SetBlackboardVariable(name=set_name, - variable_name=config.route_var_name, - variable_value=False) - return None + set_name = "Reset Blackboard Variable: {} ".format(config.route_var_name) + return py_trees.blackboard.SetBlackboardVariable(set_name, config.route_var_name, False) def _create_behavior(self): """ @@ -208,68 +212,21 @@ def change_control(self, control): # pylint: disable=no-self-use """ return control - def remove_all_actors(self): + def get_criteria(self): """ - Remove all actors + Return the list of test criteria, including all the leaf nodes. + Some criteria might have trigger conditions, which have to be filtered out. """ - for i, _ in enumerate(self.other_actors): - if self.other_actors[i] is not None: - if CarlaDataProvider.actor_id_exists(self.other_actors[i].id): - CarlaDataProvider.remove_actor_by_id(self.other_actors[i].id) - self.other_actors[i] = None - self.other_actors = [] - - -class Scenario(object): - - """ - Basic scenario class. This class holds the behavior_tree describing the - scenario and the test criteria. - - The user must not modify this class. - - Important parameters: - - behavior: User defined scenario with py_tree - - criteria_list: List of user defined test criteria with py_tree - - timeout (default = 60s): Timeout of the scenario in seconds - - terminate_on_failure: Terminate scenario on first failure - """ - - def __init__(self, behavior, criteria, name, timeout=60, terminate_on_failure=False): - self.behavior = behavior - self.test_criteria = criteria - self.timeout = timeout - self.name = name - - if self.test_criteria is not None and not isinstance(self.test_criteria, py_trees.composites.Parallel): - # list of nodes - for criterion in self.test_criteria: - criterion.terminate_on_failure = terminate_on_failure - - # Create py_tree for test criteria - self.criteria_tree = py_trees.composites.Parallel( - name="Test Criteria", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE - ) - self.criteria_tree.add_children(self.test_criteria) - self.criteria_tree.setup(timeout=1) - else: - self.criteria_tree = criteria + criteria = [] + if not self.criteria_tree: + return criteria - # Create node for timeout - self.timeout_node = TimeOut(self.timeout, name="TimeOut") - - # Create overall py_tree - self.scenario_tree = py_trees.composites.Parallel(name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - if behavior is not None: - self.scenario_tree.add_child(self.behavior) - self.scenario_tree.add_child(self.timeout_node) - self.scenario_tree.add_child(WeatherBehavior()) - self.scenario_tree.add_child(UpdateAllActorControls()) + criteria_nodes = self._extract_nodes_from_tree(self.criteria_tree) + for criterion in criteria_nodes: + if isinstance(criterion, Criterion): + criteria.append(criterion) - if criteria is not None: - self.scenario_tree.add_child(self.criteria_tree) - self.scenario_tree.setup(timeout=1) + return criteria def _extract_nodes_from_tree(self, tree): # pylint: disable=no-self-use """ @@ -291,13 +248,6 @@ def _extract_nodes_from_tree(self, tree): # pylint: disable=no-self-use return node_list - def get_criteria(self): - """ - Return the list of test criteria (all leave nodes) - """ - criteria_list = self._extract_nodes_from_tree(self.criteria_tree) - return criteria_list - def terminate(self): """ This function sets the status of all leaves in the scenario tree to INVALID @@ -319,3 +269,14 @@ def terminate(self): for actor_id in actor_dict: actor_dict[actor_id].reset() py_trees.blackboard.Blackboard().set("ActorsWithController", {}, overwrite=True) + + def remove_all_actors(self): + """ + Remove all actors + """ + for i, _ in enumerate(self.other_actors): + if self.other_actors[i] is not None: + if CarlaDataProvider.actor_id_exists(self.other_actors[i].id): + CarlaDataProvider.remove_actor_by_id(self.other_actors[i].id) + self.other_actors[i] = None + self.other_actors = [] diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index 11e0fff0f..fc72d8d95 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -160,22 +160,23 @@ def _create_behavior(self): Only behavior here is to wait """ root = py_trees.composites.Sequence() - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: root.add_child(HandleStartAccidentScenario(self.construction_wp, self._distance_to_construction)) root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: root.add_child(HandleEndAccidentScenario()) root.add_child(Idle(15)) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) return root - def _create_test_criteria(self): """ A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): diff --git a/srunner/scenarios/control_loss.py b/srunner/scenarios/control_loss.py index f7d302658..2dff7e95b 100644 --- a/srunner/scenarios/control_loss.py +++ b/srunner/scenarios/control_loss.py @@ -113,9 +113,9 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - criteria = [] - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ diff --git a/srunner/scenarios/follow_leading_vehicle.py b/srunner/scenarios/follow_leading_vehicle.py index dc3a7af57..38d9b0044 100644 --- a/srunner/scenarios/follow_leading_vehicle.py +++ b/srunner/scenarios/follow_leading_vehicle.py @@ -360,13 +360,9 @@ def _create_behavior(self): def _create_test_criteria(self): """ - A list of all test criteria will be created that is later used - in parallel behavior tree. + Empty, the route already has a collision criteria """ - criteria = [] - criteria.append(CollisionTest(self.ego_vehicles[0])) - - return criteria + return [] def __del__(self): """ diff --git a/srunner/scenarios/junction_crossing_route.py b/srunner/scenarios/junction_crossing_route.py index d8529d884..8ffb9cd26 100644 --- a/srunner/scenarios/junction_crossing_route.py +++ b/srunner/scenarios/junction_crossing_route.py @@ -180,9 +180,7 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - criteria = [] - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria + return [] def __del__(self): """ diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 9ad74a61c..08df067da 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -74,7 +74,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._wmap = CarlaDataProvider.get_map() self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._ego_route = CarlaDataProvider.get_ego_vehicle_route() + self._ego_route = config.route self._start_distance = 10 self._spawn_dist = self._start_distance @@ -155,14 +155,14 @@ def _create_behavior(self): continue driving after the road is clear.If this does not happen within 90 seconds, a timeout stops the scenario. """ - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="CrossingActorIntersection") collision_location = self._collision_wp.transform.location collision_distance = collision_location.distance(self._adversary_transform.location) collision_duration = collision_distance / self._adversary_speed collision_time_trigger = collision_duration + self._reaction_time # On trigger behavior - if self._ego_route is not None: + if self.route_mode: sequence.add_child(HandleCrossingActor(self._spawn_dist)) # First waiting behavior @@ -201,11 +201,7 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - criteria = [] - collision_criterion = CollisionTest(self.ego_vehicles[0]) - - criteria.append(collision_criterion) - return criteria + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ @@ -260,3 +256,9 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._subtype = 'route' super(VehicleTurningRoute, self).__init__( world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout, "VehicleTurningRoute") + + def _create_test_criteria(self): + """ + Empty, the route already has a collision criteria + """ + return [] diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 14316d511..14c5e5057 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -95,6 +95,7 @@ def _create_behavior(self): # non leaf nodes root = py_trees.composites.Parallel( + name="StaticObstacle", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) scenario_sequence = py_trees.composites.Sequence() @@ -257,7 +258,7 @@ def _create_behavior(self): the cyclist starts crossing the road once the condition meets, then after 60 seconds, a timeout stops the scenario """ - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="CrossingActor") collision_location = self._collision_wp.transform.location collision_distance = collision_location.distance(self._adversary_transform.location) collision_duration = collision_distance / self._adversary_speed @@ -292,12 +293,9 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - criteria = [] - - collision_criterion = CollisionTest(self.ego_vehicles[0]) - criteria.append(collision_criterion) - - return criteria + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 0e4ff40da..982c25abb 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -163,7 +163,7 @@ def _create_behavior(self): Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="OppositeVehicleTakingPriority") # Wait until ego is close to the adversary trigger_adversary = py_trees.composites.Parallel( @@ -183,7 +183,7 @@ def _create_behavior(self): main_behavior.add_child(sequence) root = py_trees.composites.Sequence() - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: root.add_child(JunctionScenarioManager(self._direction, True, True, True)) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(main_behavior) @@ -197,7 +197,9 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - return [CollisionTest(self.ego_vehicles[0])] + if self.route_mode: + return [] + return CollisionTest(self.ego_vehicles[0]) def __del__(self): """ diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 04acbde3c..74bca4b28 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -130,10 +130,10 @@ def _create_behavior(self): The vehicle has to drive the whole predetermined distance. """ root = py_trees.composites.Sequence() - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: root.add_child(HandleStartAccidentScenario(self._accident_wp, self._distance_to_accident)) root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if CarlaDataProvider.get_ego_vehicle_route(): + if self.route_mode: root.add_child(HandleEndAccidentScenario()) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(ActorDestroy(self.other_actors[1])) @@ -146,6 +146,8 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index b81aceee9..2968ee009 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -22,9 +22,10 @@ from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle, ScenarioTriggerer +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ScenarioTriggerer +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitForBlackboardVariable from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.route_parser import RouteParser +from srunner.tools.route_parser import RouteParser, TRIGGER_THRESHOLD from srunner.tools.route_manipulation import interpolate_trajectory from srunner.tools.py_trees_port import oneshot_behavior @@ -46,7 +47,7 @@ OutsideRouteLanesTest, RunningRedLightTest, RunningStopTest, - ActorSpeedAboveThresholdTest) + ActorBlockedTest) SECONDS_GIVEN_PER_METERS = 0.4 @@ -64,17 +65,6 @@ } -def convert_transform_to_location(transform_vec): - """ - Convert a vector of transforms to a vector of locations - """ - location_vec = [] - for transform_tuple in transform_vec: - location_vec.append((transform_tuple[0].location, transform_tuple[1])) - - return location_vec - - class RouteScenario(BasicScenario): """ @@ -96,26 +86,13 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou if debug_mode: self._draw_waypoints(world, self.route, vertical_shift=0.1, size=0.1, persistency=self.timeout) - self.list_scenarios = self._build_scenarios(world, - ego_vehicle, - sampled_scenario_definitions, - scenarios_per_tick=5, - timeout=self.timeout, - debug_mode=debug_mode) - - self.list_scenarios.append(BackgroundActivity( - world, ego_vehicle, self.config, self.route, timeout=self.timeout)) - - self.list_scenarios.append(BackgroundActivity( - world, ego_vehicle, self.config, self.route, timeout=self.timeout)) + self._build_scenarios( + world, ego_vehicle, sampled_scenario_definitions, 5, self.timeout, debug_mode > 0 + ) - super(RouteScenario, self).__init__(name=config.name, - ego_vehicles=[ego_vehicle], - config=config, - world=world, - debug_mode=False, - terminate_on_failure=False, - criteria_enable=criteria_enable) + super(RouteScenario, self).__init__( + config.name, [ego_vehicle], config, world, debug_mode > 1, False, criteria_enable + ) def _get_route(self, config): """ @@ -129,7 +106,6 @@ def _get_route(self, config): """ # prepare route's trajectory (interpolate and add the GPS route) gps_route, route = interpolate_trajectory(config.trajectory) - CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(route)) if config.agent is not None: config.agent.set_global_plan(gps_route, route) @@ -220,7 +196,7 @@ def _build_scenarios(self, world, ego_vehicle, scenario_definitions, Initializes the class of all the scenarios that will be present in the route. If a class fails to be initialized, a warning is printed but the route execution isn't stopped """ - scenario_instance_vec = [] + self.list_scenarios = [] if debug_mode: tmap = CarlaDataProvider.get_map() @@ -255,9 +231,7 @@ def _build_scenarios(self, world, ego_vehicle, scenario_definitions, print("Skipping scenario '{}' due to setup error: {}".format(scenario_config.type, e)) continue - scenario_instance_vec.append(scenario_instance) - - return scenario_instance_vec + self.list_scenarios.append(scenario_instance) # pylint: enable=no-self-use def _initialize_actors(self, config): @@ -270,85 +244,104 @@ def _initialize_actors(self, config): def _create_behavior(self): """ - Basic behavior do nothing, i.e. Idle - """ - scenario_trigger_distance = 1.5 # Max trigger distance between route and scenario + Creates a parallel behavior that runs all of the scenarios part of the route. + These subbehaviors have had a trigger condition added so that they wait until + the agent is close to their trigger point before activating. - behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + It also adds the BackgroundActivity scenario, which will be active throughout the whole route. + This behavior never ends and the end condition is given by the RouteCompletionTest criterion. + """ + scenario_trigger_distance = TRIGGER_THRESHOLD # Max trigger distance between route and scenario - subbehavior = py_trees.composites.Parallel(name="Behavior", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + behavior = py_trees.composites.Parallel(name="Route Behavior", + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) scenario_behaviors = [] blackboard_list = [] - for i, scenario in enumerate(self.list_scenarios): - if scenario.scenario.behavior is not None: - route_var_name = scenario.config.route_var_name - if route_var_name is not None: - scenario_behaviors.append(scenario.scenario.behavior) - blackboard_list.append([scenario.config.route_var_name, - scenario.config.trigger_points[0].location]) - else: - name = "{} - {}".format(i, scenario.scenario.behavior.name) - oneshot_idiom = oneshot_behavior(name, - behaviour=scenario.scenario.behavior, - name=name) - scenario_behaviors.append(oneshot_idiom) - - # Add behavior that manages the scenarios trigger conditions + for scenario in self.list_scenarios: + if scenario.behavior_tree is not None: + scenario_behaviors.append(scenario.behavior_tree) + blackboard_list.append([scenario.config.route_var_name, + scenario.config.trigger_points[0].location]) + + # Add the behavior that manages the scenario trigger conditions scenario_triggerer = ScenarioTriggerer( - self.ego_vehicles[0], - self.route, - blackboard_list, - scenario_trigger_distance, - repeat_scenarios=False + self.ego_vehicles[0], self.route, blackboard_list, scenario_trigger_distance, repeat_scenarios=False ) + behavior.add_child(scenario_triggerer) # Tick the ScenarioTriggerer before the scenarios - subbehavior.add_child(scenario_triggerer) # make ScenarioTriggerer the first thing to be checked - subbehavior.add_children(scenario_behaviors) - subbehavior.add_child(Idle()) # The behaviours cannot make the route scenario stop - behavior.add_child(subbehavior) + # Add the Background Activity + background_activity = BackgroundActivity( + self.world, self.ego_vehicles[0], self.config, self.route, self.night_mode, timeout=self.timeout + ) + behavior.add_child(background_activity.behavior_tree) + behavior.add_children(scenario_behaviors) return behavior def _create_test_criteria(self): """ + Create the criteria tree. It starts with some route criteria (which are always active), + and adds the scenario specific ones, which will only be active during their scenario """ + criteria = py_trees.composites.Parallel(name="Criteria", + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + + # End condition + criteria.add_child(RouteCompletionTest(self.ego_vehicles[0], route=self.route)) + + # 'Normal' criteria + criteria.add_child(OutsideRouteLanesTest(self.ego_vehicles[0], route=self.route)) + criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='vehicle', name="CollisionVehicleTest")) + criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='miscellaneous', name="CollisionLayoutTest")) + criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='walker', name="CollisionPedestrianTest")) + criteria.add_child(RunningRedLightTest(self.ego_vehicles[0])) + criteria.add_child(RunningStopTest(self.ego_vehicles[0])) + + # These stop the route early to save computational time + criteria.add_child(InRouteTest( + self.ego_vehicles[0], route=self.route, offroad_max=30, terminate_on_failure=True)) + criteria.add_child(ActorBlockedTest( + self.ego_vehicles[0], min_speed=0.1, max_time=180.0, terminate_on_failure=True, name="AgentBlockedTest") + ) - criteria = [] - - route = convert_transform_to_location(self.route) - - collision_criterion = CollisionTest(self.ego_vehicles[0], terminate_on_failure=False) + for scenario in self.list_scenarios: - route_criterion = InRouteTest(self.ego_vehicles[0], - route=route, - offroad_max=30, - terminate_on_failure=True) + scenario_criteria = scenario.get_criteria() + if len(scenario_criteria) == 0: + continue # No need to create anything - completion_criterion = RouteCompletionTest(self.ego_vehicles[0], route=route) + criteria_tree = self._create_criterion_tree(scenario, + scenario_criteria, + ) + criteria.add_child(criteria_tree) - outsidelane_criterion = OutsideRouteLanesTest(self.ego_vehicles[0], route=route) + return criteria - red_light_criterion = RunningRedLightTest(self.ego_vehicles[0]) + def _create_criterion_tree(self, scenario, criteria): + """ + We can make use of the blackboard variables used by the behaviors themselves, + as we already have an atomic that handles their (de)activation. + The criteria will wait until that variable is active (the scenario has started), + and will automatically stop when it deactivates (as the scenario has finished) + """ + scenario_name = scenario.name, + var_name = scenario.config.route_var_name + check_name = "WaitForBlackboardVariable: {}".format(var_name) - stop_criterion = RunningStopTest(self.ego_vehicles[0]) + criteria_tree = py_trees.composites.Sequence(name=scenario_name) + criteria_tree.add_child(WaitForBlackboardVariable(var_name, True, False, name=check_name)) - blocked_criterion = ActorSpeedAboveThresholdTest(self.ego_vehicles[0], - speed_threshold=0.1, - below_threshold_max_time=90.0, - terminate_on_failure=True) + scenario_criteria = py_trees.composites.Parallel(name=scenario_name, + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + for criterion in criteria: + scenario_criteria.add_child(criterion) + scenario_criteria.add_child(WaitForBlackboardVariable(var_name, False, None, name=check_name)) - criteria.append(completion_criterion) - criteria.append(collision_criterion) - criteria.append(route_criterion) - criteria.append(outsidelane_criterion) - criteria.append(red_light_criterion) - criteria.append(stop_criterion) - criteria.append(blocked_criterion) + criteria_tree.add_child(scenario_criteria) + return criteria_tree - return criteria def __del__(self): """ diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index a05aa783a..8c0ef0280 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -142,8 +142,8 @@ def _create_behavior(self): tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) root.add_child(tl_freezer_sequence) - sequence = py_trees.composites.Sequence("Sequence Behavior") - if CarlaDataProvider.get_ego_vehicle_route(): + sequence = py_trees.composites.Sequence(name="SignalizedJunctionLeftTurn") + if self.route_mode: sequence.add_child(JunctionScenarioManager(self._direction, True, True, True)) sequence.add_child(root) @@ -154,7 +154,9 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - return [CollisionTest(self.ego_vehicles[0])] + if self.route_mode: + return [] + return CollisionTest(self.ego_vehicles[0]) def __del__(self): """ diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index 631541212..1ca7aaf9b 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -142,8 +142,8 @@ def _create_behavior(self): tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) root.add_child(tl_freezer_sequence) - sequence = py_trees.composites.Sequence("Sequence") - if CarlaDataProvider.get_ego_vehicle_route(): + sequence = py_trees.composites.Sequence(name="SignalizedJunctionRightTurn") + if self.route_mode: sequence.add_child(JunctionScenarioManager(self._direction, True, False, True)) sequence.add_child(root) @@ -154,7 +154,9 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - return [CollisionTest(self.ego_vehicles[0])] + if self.route_mode: + return [] + return CollisionTest(self.ego_vehicles[0]) def __del__(self): """ diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 055540be3..22351d4ac 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -153,7 +153,7 @@ class JunctionScenarioManager(AtomicBehavior): something like 'left', 'right' or 'opposite' """ - def __init__(self, entry_direction, remove_entry, remove_exit, remove_middle, name="Scenario7Manager"): + def __init__(self, entry_direction, remove_entry, remove_exit, remove_middle, name="JunctionScenarioManager"): self._entry_direction = entry_direction self._remove_entry = remove_entry self._remove_exit = remove_exit diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index d07f63490..7d80c2474 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -270,6 +270,7 @@ def scan_route_for_scenarios(route_name, trajectory, world_annotations): scenario_config.type = scenario_name scenario_config.subtype = subtype scenario_config.trigger_points = [trigger_point] + scenario_config.route = trajectory for other in scenario.pop('other_actors', []): scenario_config.other_actors.append(ActorConfigurationData.parse_from_dict(other, 'scenario')) scenario_config.other_parameters.update(scenario) diff --git a/srunner/tools/scenario_helper.py b/srunner/tools/scenario_helper.py index eb24583fb..d278deaac 100644 --- a/srunner/tools/scenario_helper.py +++ b/srunner/tools/scenario_helper.py @@ -378,19 +378,19 @@ def generate_target_waypoint_in_route(waypoint, route): # Get the route location shortest_distance = float('inf') for index, route_pos in enumerate(route): - wp = route_pos[0] + route_location = route_pos[0].location trigger_location = waypoint.transform.location - dist_to_route = trigger_location.distance(wp) + dist_to_route = trigger_location.distance(route_location) if dist_to_route <= shortest_distance: closest_index = index shortest_distance = dist_to_route - route_location = route[closest_index][0] + route_location = route[closest_index][0].location index = closest_index for i in range(index, len(route)): - route_location = route[i][0] + route_location = route[i][0].location road_option = route[i][1] # Enter the junction From 2c87574fda77767783cb06b40f4f71dd9f460ba0 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 14 Mar 2022 11:52:35 +0100 Subject: [PATCH 06/86] Scenarios are now route specific --- scenario_runner.py | 9 +- .../route_scenario_configuration.py | 4 +- srunner/scenarios/basic_scenario.py | 4 +- srunner/scenarios/bycicle_group.py | 16 -- .../scenarios/construction_crash_vehicle.py | 16 -- srunner/scenarios/junction_crossing_route.py | 189 -------------- .../scenarios/no_signal_junction_crossing.py | 50 +++- .../opposite_vehicle_taking_priority.py | 2 +- srunner/scenarios/route_obstacles.py | 16 -- srunner/scenarios/route_scenario.py | 142 ++++++----- .../signalized_junction_left_turn.py | 2 +- .../signalized_junction_right_turn.py | 2 +- srunner/tools/route_parser.py | 239 ++++-------------- 13 files changed, 189 insertions(+), 502 deletions(-) delete mode 100644 srunner/scenarios/junction_crossing_route.py diff --git a/scenario_runner.py b/scenario_runner.py index 740877a5b..84b1448a6 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -464,13 +464,10 @@ def _run_route(self): if self._args.route: routes = self._args.route[0] - scenario_file = self._args.route[1] - single_route = None - if len(self._args.route) > 2: - single_route = self._args.route[2] + single_route = self._args.route[1] if len(self._args.route) > 1 else None # retrieve routes - route_configurations = RouteParser.parse_routes_file(routes, scenario_file, single_route) + route_configurations = RouteParser.parse_routes_file(routes, single_route) for config in route_configurations: for _ in range(self._args.repetitions): @@ -547,7 +544,7 @@ def main(): parser.add_argument('--openscenario', help='Provide an OpenSCENARIO definition') parser.add_argument('--openscenarioparams', help='Overwrited for OpenSCENARIO ParameterDeclaration') parser.add_argument( - '--route', help='Run a route as a scenario (input: (route_file,scenario_file,[route id]))', nargs='+', type=str) + '--route', help='Run a route as a scenario (input: (route_file, [route id]))', nargs='+', type=str) parser.add_argument( '--agent', help="Agent used to execute the scenario. Currently only compatible with route-based scenarios.") diff --git a/srunner/scenarioconfigs/route_scenario_configuration.py b/srunner/scenarioconfigs/route_scenario_configuration.py index 55da841bb..6cdf82e4b 100644 --- a/srunner/scenarioconfigs/route_scenario_configuration.py +++ b/srunner/scenarioconfigs/route_scenario_configuration.py @@ -48,5 +48,5 @@ class RouteScenarioConfiguration(ScenarioConfiguration): def __init__(self): super(RouteScenarioConfiguration, self).__init__() - self.trajectory = None - self.scenario_file = None + self.keypoints = None + self.scenario_configs = [] diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index 0be76bd08..a49c73fa0 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -10,7 +10,6 @@ """ from __future__ import print_function -from ast import NameConstant import operator import py_trees @@ -104,7 +103,8 @@ def __init__(self, name, ego_vehicles, config, world, self.criteria_tree.setup(timeout=1) else: - raise ValueError("The scenario criteria is neither a list nor a py_trees.composites.Composite") + raise ValueError("WARNING: Scenario {} couldn't be setup, make sure the criteria is either " + "a list or a py_trees.composites.Composite".format(self.name)) self.scenario_tree.add_child(self.criteria_tree) diff --git a/srunner/scenarios/bycicle_group.py b/srunner/scenarios/bycicle_group.py index b8c064875..f1c8e3854 100644 --- a/srunner/scenarios/bycicle_group.py +++ b/srunner/scenarios/bycicle_group.py @@ -21,22 +21,6 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTimeToArrivalToVehicle from srunner.scenarios.basic_scenario import BasicScenario -def convert_dict_to_transform(actor_dict): - """ - Convert a JSON string to a CARLA transform - """ - return carla.Transform( - carla.Location( - x=float(actor_dict['x']), - y=float(actor_dict['y']), - z=float(actor_dict['z']) - ), - carla.Rotation( - roll=0.0, - pitch=0.0, - yaw=float(actor_dict['yaw']) - ) - ) class BycicleGroup(BasicScenario): """ diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index fc72d8d95..8342f39ca 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -21,22 +21,6 @@ from srunner.scenarios.object_crash_vehicle import StationaryObjectCrossing from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario -def convert_dict_to_transform(actor_dict): - """ - Convert a JSON string to a CARLA transform - """ - return carla.Transform( - carla.Location( - x=float(actor_dict['x']), - y=float(actor_dict['y']), - z=float(actor_dict['z']) - ), - carla.Rotation( - roll=0.0, - pitch=0.0, - yaw=float(actor_dict['yaw']) - ) - ) class ConstructionSetupCrossing(StationaryObjectCrossing): diff --git a/srunner/scenarios/junction_crossing_route.py b/srunner/scenarios/junction_crossing_route.py deleted file mode 100644 index 8ffb9cd26..000000000 --- a/srunner/scenarios/junction_crossing_route.py +++ /dev/null @@ -1,189 +0,0 @@ -#!/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_criteria import CollisionTest, DrivenDistanceTest, MaxVelocityTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, WaitEndIntersection -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, - name="DriveDistance") - - # 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 = [] - - max_velocity_criterion = MaxVelocityTest( - self.ego_vehicles[0], - self._ego_max_velocity_allowed, - optional=True) - collision_criterion = CollisionTest(self.ego_vehicles[0]) - driven_distance_criterion = DrivenDistanceTest( - self.ego_vehicles[0], - self._ego_expected_driven_distance) - - criteria.append(max_velocity_criterion) - criteria.append(collision_criterion) - criteria.append(driven_distance_criterion) - - return criteria - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self._traffic_light = None - self.remove_all_actors() - - -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. - """ - - # 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 - - 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 - - super(NoSignalJunctionCrossingRoute, self).__init__("NoSignalJunctionCrossingRoute", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - pass - - 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. - - If this does not happen within 120 seconds, a timeout stops the scenario - """ - # finally wait that ego vehicle drove a specific distance - wait = WaitEndIntersection( - self.ego_vehicles[0], - name="WaitEndIntersection") - end_condition = DriveDistance( - self.ego_vehicles[0], - self._ego_distance_to_drive, - name="DriveDistance") - - # Build behavior tree - sequence = py_trees.composites.Sequence("NoSignalJunctionCrossingRoute") - sequence.add_child(wait) - sequence.add_child(end_condition) - - return sequence - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - return [] - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/no_signal_junction_crossing.py b/srunner/scenarios/no_signal_junction_crossing.py index 0c2496571..0add82905 100644 --- a/srunner/scenarios/no_signal_junction_crossing.py +++ b/srunner/scenarios/no_signal_junction_crossing.py @@ -21,7 +21,7 @@ KeepVelocity, StopVehicle) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerRegion +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerRegion, DriveDistance, WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario @@ -162,3 +162,51 @@ def __del__(self): Remove all actors upon deletion """ self.remove_all_actors() + + +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): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + # Timeout of scenario in seconds + self.timeout = timeout + self._end_distance = 50 + + super(NoSignalJunctionCrossingRoute, self).__init__("NoSignalJunctionCrossingRoute", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Just wait for the ego to exit the junction, for route the BackgroundActivity already does all the job + """ + sequence = py_trees.composites.Sequence("UnSignalizedJunctionCrossingRoute") + sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 982c25abb..54f0c4989 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -199,7 +199,7 @@ def _create_test_criteria(self): """ if self.route_mode: return [] - return CollisionTest(self.ego_vehicles[0]) + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 74bca4b28..a76452ed5 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -23,22 +23,6 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario -def convert_dict_to_transform(actor_dict): - """ - Convert a JSON string to a CARLA transform - """ - return carla.Transform( - carla.Location( - x=float(actor_dict['x']), - y=float(actor_dict['y']), - z=float(actor_dict['z']) - ), - carla.Rotation( - roll=0.0, - pitch=0.0, - yaw=float(actor_dict['yaw']) - ) - ) class Accident(BasicScenario): """ diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 2968ee009..78efddd69 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -11,8 +11,12 @@ from __future__ import print_function +import glob +import os +import sys +import importlib +import inspect import traceback - import py_trees from numpy import random @@ -22,25 +26,9 @@ from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ScenarioTriggerer from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitForBlackboardVariable -from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.route_parser import RouteParser, TRIGGER_THRESHOLD -from srunner.tools.route_manipulation import interpolate_trajectory -from srunner.tools.py_trees_port import oneshot_behavior - -from srunner.scenarios.control_loss import ControlLoss -from srunner.scenarios.follow_leading_vehicle import FollowLeadingVehicleRoute -from srunner.scenarios.object_crash_vehicle import DynamicObjectCrossing -from srunner.scenarios.object_crash_intersection import VehicleTurningRoute -from srunner.scenarios.other_leading_vehicle import OtherLeadingVehicle -from srunner.scenarios.maneuver_opposite_direction import ManeuverOppositeDirection -from srunner.scenarios.junction_crossing_route import NoSignalJunctionCrossingRoute -from srunner.scenarios.signalized_junction_left_turn import SignalizedJunctionLeftTurn -from srunner.scenarios.signalized_junction_right_turn import SignalizedJunctionRightTurn -from srunner.scenarios.opposite_vehicle_taking_priority import OppositeVehicleRunningRedLight -from srunner.scenarios.background_activity import BackgroundActivity - from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest, InRouteTest, RouteCompletionTest, @@ -49,20 +37,14 @@ RunningStopTest, ActorBlockedTest) -SECONDS_GIVEN_PER_METERS = 0.4 +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.scenarios.background_activity import BackgroundActivity + +from srunner.tools.route_parser import RouteParser, DIST_THRESHOLD +from srunner.tools.route_manipulation import interpolate_trajectory -NUMBER_CLASS_TRANSLATION = { - "Scenario1": ControlLoss, - "Scenario2": FollowLeadingVehicleRoute, - "Scenario3": DynamicObjectCrossing, - "Scenario4": VehicleTurningRoute, - "Scenario5": OtherLeadingVehicle, - "Scenario6": ManeuverOppositeDirection, - "Scenario7": OppositeVehicleRunningRedLight, - "Scenario8": SignalizedJunctionLeftTurn, - "Scenario9": SignalizedJunctionRightTurn, - "Scenario10": NoSignalJunctionCrossingRoute -} + +SECONDS_GIVEN_PER_METERS = 0.4 class RouteScenario(BasicScenario): @@ -79,7 +61,8 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou self.config = config self.route = self._get_route(config) - sampled_scenario_definitions = self._get_scenarios(config) + sampled_scenario_definitions = self._filter_scenarios(config.scenario_configs) + ego_vehicle = self._spawn_ego_vehicle() self.timeout = self._estimate_route_timeout() @@ -87,7 +70,7 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou self._draw_waypoints(world, self.route, vertical_shift=0.1, size=0.1, persistency=self.timeout) self._build_scenarios( - world, ego_vehicle, sampled_scenario_definitions, 5, self.timeout, debug_mode > 0 + world, ego_vehicle, sampled_scenario_definitions, timeout=self.timeout, debug=debug_mode > 0 ) super(RouteScenario, self).__init__( @@ -105,25 +88,30 @@ def _get_route(self, config): - debug_mode: boolean to decide whether or not the route poitns are printed """ # prepare route's trajectory (interpolate and add the GPS route) - gps_route, route = interpolate_trajectory(config.trajectory) + gps_route, route = interpolate_trajectory(config.keypoints) if config.agent is not None: config.agent.set_global_plan(gps_route, route) return route - def _get_scenarios(self, config): + def _filter_scenarios(self, scenario_configs): """ - Gets the scenarios that will be part of the route. Automatically filters the scenarios - that affect the route and, if there are two or more scenarios with very similar triggering positions, - one of those is randomly chosen + Given a list of scenarios, filters out does that don't make sense to be triggered, + as they are either too far from the route or don't fit with the route shape Parameters: - - config: Scenario configuration (RouteConfiguration) + - scenario_configs: list of ScenarioConfiguration """ - scenario_dict = RouteParser.parse_scenario_file_to_dict(config.scenario_file) - potential_scenarios = RouteParser.scan_route_for_scenarios(config.town, self.route, scenario_dict) - sampled_scenarios = self._scenario_sampling(potential_scenarios) - return sampled_scenarios + new_scenarios_config = [] + for scenario_config in scenario_configs: + trigger_point = scenario_config.trigger_points[0] + if not RouteParser.is_scenario_at_route(trigger_point, self.route): + print("WARNING: Ignoring scenario '{}' as it is too far from the route".format(scenario_config.name)) + continue + + new_scenarios_config.append(scenario_config) + + return new_scenarios_config def _spawn_ego_vehicle(self): """Spawn the ego vehicle at the first waypoint of the route""" @@ -176,7 +164,7 @@ def _draw_waypoints(self, world, waypoints, vertical_shift, size, persistency=-1 world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=2*size, color=carla.Color(0, 0, 128), life_time=persistency) world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=2*size, - color=carla.Color(128, 0, 0), life_time=persistency) + color=carla.Color(128, 128, 128), life_time=persistency) def _scenario_sampling(self, potential_scenarios, random_seed=0): """Sample the scenarios that are going to happen for this route.""" @@ -190,49 +178,68 @@ def _scenario_sampling(self, potential_scenarios, random_seed=0): return sampled_scenarios - def _build_scenarios(self, world, ego_vehicle, scenario_definitions, - scenarios_per_tick=5, timeout=300, debug_mode=False): + def get_all_scenario_classes(self): + + # Path of all scenario at "srunner/scenarios" folder + scenarios_list = glob.glob("{}/srunner/scenarios/*.py".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) + + all_scenario_classes = {} + + for scenario_file in scenarios_list: + + # Get their module + module_name = os.path.basename(scenario_file).split('.')[0] + sys.path.insert(0, os.path.dirname(scenario_file)) + scenario_module = importlib.import_module(module_name) + + # And their members of type class + for member in inspect.getmembers(scenario_module, inspect.isclass): + # TODO: Filter out any class that isn't a child of BasicScenario + all_scenario_classes[member[0]] = member[1] + + return all_scenario_classes + + def _build_scenarios(self, world, ego_vehicle, scenario_definitions, scenarios_per_tick=5, timeout=300, debug=False): """ Initializes the class of all the scenarios that will be present in the route. If a class fails to be initialized, a warning is printed but the route execution isn't stopped """ + all_scenario_classes = self.get_all_scenario_classes() self.list_scenarios = [] + ego_data = ActorConfigurationData(ego_vehicle.type_id, ego_vehicle.get_transform(), 'hero') - if debug_mode: + if debug: tmap = CarlaDataProvider.get_map() for scenario_config in scenario_definitions: scenario_loc = scenario_config.trigger_points[0].location debug_loc = tmap.get_waypoint(scenario_loc).transform.location + carla.Location(z=0.2) world.debug.draw_point(debug_loc, size=0.2, color=carla.Color(128, 0, 0), life_time=timeout) - world.debug.draw_string(debug_loc, str(scenario_config.type), draw_shadow=False, + world.debug.draw_string(debug_loc, str(scenario_config.name), draw_shadow=False, color=carla.Color(0, 0, 128), life_time=timeout, persistent_lines=True) for scenario_number, scenario_config in enumerate(scenario_definitions): - scenario_config.ego_vehicles = [ActorConfigurationData(ego_vehicle.type_id, - ego_vehicle.get_transform(), - 'hero')] + scenario_config.ego_vehicles = [ego_data] scenario_config.route_var_name = "ScenarioRouteNumber{}".format(scenario_number) + scenario_config.route = self.route try: - scenario_class = NUMBER_CLASS_TRANSLATION[scenario_config.type] - scenario_instance = scenario_class(world, [ego_vehicle], scenario_config, - criteria_enable=False, timeout=timeout) + scenario_class = all_scenario_classes[scenario_config.type] + scenario_instance = scenario_class(world, [ego_vehicle], scenario_config, timeout=timeout) # Do a tick every once in a while to avoid spawning everything at the same time if scenario_number % scenarios_per_tick == 0: - if CarlaDataProvider.is_sync_mode(): - world.tick() - else: - world.wait_for_tick() + world.tick() - except Exception as e: # pylint: disable=broad-except - if debug_mode: + except Exception as e: + if not debug: + print("Skipping scenario '{}' due to setup error: {}".format(scenario_config.type, e)) + else: traceback.print_exc() - print("Skipping scenario '{}' due to setup error: {}".format(scenario_config.type, e)) continue self.list_scenarios.append(scenario_instance) + # pylint: enable=no-self-use def _initialize_actors(self, config): """ @@ -251,7 +258,7 @@ def _create_behavior(self): It also adds the BackgroundActivity scenario, which will be active throughout the whole route. This behavior never ends and the end condition is given by the RouteCompletionTest criterion. """ - scenario_trigger_distance = TRIGGER_THRESHOLD # Max trigger distance between route and scenario + scenario_trigger_distance = DIST_THRESHOLD # Max trigger distance between route and scenario behavior = py_trees.composites.Parallel(name="Route Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) @@ -273,7 +280,7 @@ def _create_behavior(self): # Add the Background Activity background_activity = BackgroundActivity( - self.world, self.ego_vehicles[0], self.config, self.route, self.night_mode, timeout=self.timeout + self.world, self.ego_vehicles[0], self.config, self.route, timeout=self.timeout ) behavior.add_child(background_activity.behavior_tree) @@ -307,18 +314,17 @@ def _create_test_criteria(self): ) for scenario in self.list_scenarios: - scenario_criteria = scenario.get_criteria() if len(scenario_criteria) == 0: continue # No need to create anything - criteria_tree = self._create_criterion_tree(scenario, - scenario_criteria, + criteria.add_child( + self._create_criterion_tree(scenario, scenario_criteria) ) - criteria.add_child(criteria_tree) return criteria + def _create_criterion_tree(self, scenario, criteria): """ We can make use of the blackboard variables used by the behaviors themselves, @@ -326,7 +332,7 @@ def _create_criterion_tree(self, scenario, criteria): The criteria will wait until that variable is active (the scenario has started), and will automatically stop when it deactivates (as the scenario has finished) """ - scenario_name = scenario.name, + scenario_name = scenario.name var_name = scenario.config.route_var_name check_name = "WaitForBlackboardVariable: {}".format(var_name) diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index 8c0ef0280..6f275f5b0 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -156,7 +156,7 @@ def _create_test_criteria(self): """ if self.route_mode: return [] - return CollisionTest(self.ego_vehicles[0]) + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index 1ca7aaf9b..f354d3ba3 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -156,7 +156,7 @@ def _create_test_criteria(self): """ if self.route_mode: return [] - return CollisionTest(self.ego_vehicles[0]) + return [CollisionTest(self.ego_vehicles[0])] def __del__(self): """ diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index 7d80c2474..c5b1defb2 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -16,16 +16,24 @@ from srunner.scenarioconfigs.route_scenario_configuration import RouteScenarioConfiguration from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData -# TODO check this threshold, it could be a bit larger but not so large that we cluster scenarios. -TRIGGER_THRESHOLD = 2.0 # Threshold to say if a trigger position is new or repeated, works for matching positions -TRIGGER_ANGLE_THRESHOLD = 10 # Threshold to say if two angles can be considering matching when matching transforms. +# Threshold to say if a scenarios trigger position is part of the route +DIST_THRESHOLD = 2.0 +ANGLE_THRESHOLD = 10 -def convert_dict_to_transform(scenario_dict): - """Convert a JSON dict to a CARLA transform""" +def convert_elem_to_transform(elem): + """Convert an ElementTree.Element to a CARLA transform""" return carla.Transform( - carla.Location(float(scenario_dict['x']), float(scenario_dict['y']), float(scenario_dict['z'])), - carla.Rotation(roll=0.0, pitch=0.0, yaw=float(scenario_dict['yaw'])) + carla.Location( + float(elem.attrib.get('x')), + float(elem.attrib.get('y')), + float(elem.attrib.get('z')) + ), + carla.Rotation( + roll=0.0, + pitch=0.0, + yaw=float(elem.attrib.get('yaw')) + ) ) @@ -36,25 +44,7 @@ class RouteParser(object): """ @staticmethod - def parse_scenario_file_to_dict(scenario_file): - """ - Parses and returns the scenario file into a dictionary - :param scenario_file: the filename for the scenario file - :return: - """ - - with open(scenario_file, 'r', encoding='utf-8') as f: - scenario_dict = json.loads(f.read()) - - final_dict = {} - - for town_dict in scenario_dict['available_scenarios']: - final_dict.update(town_dict) - - return final_dict # the file has a current maps name that is an one element vec - - @staticmethod - def parse_routes_file(route_filename, scenario_file, single_route=None): + def parse_routes_file(route_filename, single_route_id=''): """ Returns a list of route configuration elements. :param route_filename: the path to a set of routes. @@ -62,31 +52,48 @@ def parse_routes_file(route_filename, scenario_file, single_route=None): :return: List of dicts containing the waypoints, id and town of the routes """ - list_route_descriptions = [] + route_configs = [] tree = ET.parse(route_filename) for route in tree.iter("route"): route_id = route.attrib['id'] - if single_route and route_id != single_route: + if single_route_id and route_id != single_route_id: continue - new_config = RouteScenarioConfiguration() - new_config.town = route.attrib['town'] - new_config.name = "RouteScenario_{}".format(route_id) - new_config.weather = RouteParser.parse_weather(route) - new_config.scenario_file = scenario_file - - waypoint_list = [] # the list of waypoints that can be found on this route - for waypoint in route.iter('waypoint'): - waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']), - y=float(waypoint.attrib['y']), - z=float(waypoint.attrib['z']))) + route_config = RouteScenarioConfiguration() + route_config.town = route.attrib['town'] + route_config.name = "RouteScenario_{}".format(route_id) + route_config.weather = RouteParser.parse_weather(route) + + # The list of carla.Location that serve as keypoints on this route + positions = [] + for position in route.find('waypoints').iter('position'): + positions.append(carla.Location(x=float(position.attrib['x']), + y=float(position.attrib['y']), + z=float(position.attrib['z']))) + route_config.keypoints = positions + + # The list of ScenarioConfigurations that store the scenario's data + scenario_configs = [] + for scenario in route.find('scenarios').iter('scenario'): + scenario_config = ScenarioConfiguration() + scenario_config.name = scenario.attrib.get('name') + scenario_config.type = scenario.attrib.get('type') + + for elem in scenario.getchildren(): + if elem.tag == 'trigger_point': + scenario_config.trigger_points.append(convert_elem_to_transform(elem)) + elif elem.tag == 'other_actor': + scenario_config.other_actors.append(ActorConfigurationData.parse_from_node(elem, 'scenario')) + else: + scenario_config.other_parameters[elem.tag] = elem.attrib - new_config.trajectory = waypoint_list + scenario_configs.append(scenario_config) + route_config.scenario_configs = scenario_configs - list_route_descriptions.append(new_config) + route_configs.append(route_config) - return list_route_descriptions + return route_configs @staticmethod def parse_weather(route): @@ -97,7 +104,6 @@ def parse_weather(route): route_weather = route.find("weather") if route_weather is None: - weather = carla.WeatherParameters(sun_altitude_angle=70) else: @@ -132,154 +138,21 @@ def parse_weather(route): return weather @staticmethod - def get_trigger_position(scenario_trigger, existing_triggers): - """ - Check if this trigger position already exists or if it is a new one. - :param scenario_trigger: position to be checked - :param existing_triggers: list with all the already found position - :return: - """ - for trigger in existing_triggers: - dx = trigger.location.x - scenario_trigger.location.x - dy = trigger.location.y - scenario_trigger.location.y - distance = math.sqrt(dx * dx + dy * dy) - - dyaw = (trigger.rotation.yaw - scenario_trigger.rotation.yaw) % 360 - if distance < TRIGGER_THRESHOLD \ - and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)): - return trigger - - return None - - @staticmethod - def match_trigger_to_route(trigger_transform, route): + def is_scenario_at_route(trigger_transform, route): """ Check if the scenario is affecting the route. This is true if the trigger position is very close to any route point """ def is_trigger_close(trigger_transform, route_transform): """Check if the two transforms are similar""" - dx = trigger_transform.location.x - route_transform.location.x - dy = trigger_transform.location.y - route_transform.location.y - dz = trigger_transform.location.z - route_transform.location.z - dpos = math.sqrt(dx * dx + dy * dy + dz * dz) + dist = trigger_transform.location.distance(route_transform.location) + angle_dist = (trigger_transform.rotation.yaw - route_transform.rotation.yaw) % 360 - dyaw = (float(trigger_transform.rotation.yaw) - route_transform.rotation.yaw) % 360 + return dist < DIST_THRESHOLD \ + and (angle_dist < ANGLE_THRESHOLD or angle_dist > (360 - ANGLE_THRESHOLD)) - return dpos < TRIGGER_THRESHOLD \ - and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)) - - for position, [route_transform, _] in enumerate(route): + for route_transform, _ in route: if is_trigger_close(trigger_transform, route_transform): - return position + return True return None - - @staticmethod - def get_scenario_subtype(scenario, route): - """ - Some scenarios have subtypes depending on the route trajectory, - even being invalid if there isn't a valid one. As an example, - some scenarios need the route to turn in a specific direction, - and if this isn't the case, the scenario should not be considered valid. - This is currently only used for validity purposes. - - :param scenario: the scenario name - :param route: route starting at the triggering point of the scenario - :return: tag representing this subtype - """ - - def is_junction_option(option): - """Whether or not an option is part of a junction""" - if option in (RoadOption.LANEFOLLOW, RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT): - return False - return True - - subtype = None - - if scenario == 'Scenario4': # Only if the route turns - for _, option in route: - if is_junction_option(option): - if option == RoadOption.LEFT: - subtype = 'S4left' - elif option == RoadOption.RIGHT: - subtype = 'S4right' - else: - subtype = None - break # Avoid checking all of them - subtype = None - - if scenario == 'Scenario7': - for _, option in route: - if is_junction_option(option): - if RoadOption.STRAIGHT == option: - subtype = 'S7opposite' - break - elif scenario == 'Scenario8': # Only if the route turns left - for _, option in route: - if is_junction_option(option): - if option == RoadOption.LEFT: - subtype = 'S8left' - break - elif scenario == 'Scenario9': # Only if the route turns right - for _, option in route: - if is_junction_option(option): - if option == RoadOption.RIGHT: - subtype = 'S9right' - break - else: - subtype = 'valid' - - return subtype - - @staticmethod - def scan_route_for_scenarios(route_name, trajectory, world_annotations): - """ - Filters all the scenarios that are affecting the route. - Returns a dictionary where each item is a list of all the scenarios that are close to each other - """ - possible_scenarios = {} - - for town_name in world_annotations.keys(): - if town_name != route_name: - continue - - town_scenarios = world_annotations[town_name] - for scenario_data in town_scenarios: - - if "scenario_type" not in scenario_data: - break - scenario_name = scenario_data["scenario_type"] - - for scenario in scenario_data["available_event_configurations"]: - # Get the trigger point of the scenario - trigger_point = convert_dict_to_transform(scenario.pop('transform')) - - # Check if the route passes through the scenario - match_position = RouteParser.match_trigger_to_route(trigger_point, trajectory) - if match_position is None: - continue - - # Check the route has the correct topology - subtype = RouteParser.get_scenario_subtype(scenario_name, trajectory[match_position:]) - if subtype is None: - continue - - # Parse the scenario data - scenario_config = ScenarioConfiguration() - scenario_config.type = scenario_name - scenario_config.subtype = subtype - scenario_config.trigger_points = [trigger_point] - scenario_config.route = trajectory - for other in scenario.pop('other_actors', []): - scenario_config.other_actors.append(ActorConfigurationData.parse_from_dict(other, 'scenario')) - scenario_config.other_parameters.update(scenario) - - # Check if its location overlaps with other scenarios - existing_trigger = RouteParser.get_trigger_position(trigger_point, possible_scenarios.keys()) - if existing_trigger: - possible_scenarios[existing_trigger].append(scenario_config) - else: - possible_scenarios.update({trigger_point: [scenario_config]}) - - return possible_scenarios From 723878fcb0d7c69d62b014f997061d437ce6191c Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 16 Mar 2022 09:05:33 +0100 Subject: [PATCH 07/86] Added urban bicycle flow prototype --- .../scenariomanager/carla_data_provider.py | 28 ++---- .../scenarioatomics/atomic_behaviors.py | 5 +- srunner/scenarios/actor_flow.py | 93 +++++++++++++++++++ srunner/scenarios/background_activity.py | 4 +- 4 files changed, 108 insertions(+), 22 deletions(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 0c5b15413..fba152007 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -415,7 +415,7 @@ def generate_spawn_points(): CarlaDataProvider._spawn_index = 0 @staticmethod - def create_blueprint(model, rolename='scenario', color=None, actor_category="car", safe=False): + def create_blueprint(model, rolename='scenario', color=None, actor_category="car", filter_type=""): """ Function to setup the blueprint of an actor given its model and other relevant parameters """ @@ -438,16 +438,8 @@ def create_blueprint(model, rolename='scenario', color=None, actor_category="car try: blueprints = CarlaDataProvider._blueprint_library.filter(model) blueprints_ = [] - if safe: - # Safe vehicles are those that can move through all roads. - # This means that big vehicles such as ambulance and trucks are removed, - # as well as 2-wheeled ones as they can go through highways - for bp in blueprints: - if bp.id.endswith('firetruck') or bp.id.endswith('ambulance') \ - or bp.id.endswith('carlacola') or bp.id.endswith('cybertruck') \ - or int(bp.get_attribute('number_of_wheels')) != 4: - continue - blueprints_.append(bp) + if filter_type: + blueprints = [x for x in blueprints if x.get_attribute('type') == filter_type] else: blueprints_ = blueprints @@ -525,11 +517,11 @@ def handle_actor_batch(batch, tick=True): @staticmethod def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, random_location=False, color=None, actor_category="car", - safe_blueprint=False, tick=True): + filter_type="", 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, safe_blueprint) + blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category, filter_type) if random_location: actor = None @@ -552,7 +544,7 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, # De/activate the autopilot of the actor if it belongs to vehicle if autopilot: - if actor.type_id.startswith('vehicle.'): + if isinstance(actor, carla.Vehicle): actor.set_autopilot(autopilot, CarlaDataProvider._traffic_manager_port) else: print("WARNING: Tried to set the autopilot of a non vehicle actor") @@ -570,7 +562,7 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, return actor @staticmethod - def request_new_actors(actor_list, safe_blueprint=False, tick=True): + def request_new_actors(actor_list, filter_type="", tick=True): """ This method tries to series of actor in batch. If this was successful, the new actors are returned, None otherwise. @@ -594,7 +586,7 @@ def request_new_actors(actor_list, safe_blueprint=False, tick=True): # Get the blueprint blueprint = CarlaDataProvider.create_blueprint( - actor.model, actor.rolename, actor.color, actor.category, safe_blueprint) + actor.model, actor.rolename, actor.color, actor.category, filter_type) # Get the spawn point transform = actor.transform @@ -647,7 +639,7 @@ def request_new_actors(actor_list, safe_blueprint=False, tick=True): @staticmethod def request_new_batch_actors(model, amount, spawn_points, autopilot=False, random_location=False, rolename='scenario', - safe_blueprint=False, tick=True): + filter_type="", tick=True): """ Simplified version of "request_new_actors". This method also create several actors in batch. @@ -668,7 +660,7 @@ 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, safe=safe_blueprint) + blueprint = CarlaDataProvider.create_blueprint(model, rolename, filter_type=filter_type) if random_location: if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 8290f29e4..084b583fb 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2507,7 +2507,7 @@ class ActorFlow(AtomicBehavior): """ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, - actor_speed=20 / 3.6, name="ActorFlow"): + actor_speed=20 / 3.6, actor_type="car", name="ActorFlow"): """ Setup class members """ @@ -2528,6 +2528,7 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._sink_dist = sink_dist self._speed = actor_speed + self._actor_type = actor_type self._min_spawn_dist = spawn_dist_interval[0] self._max_spawn_dist = spawn_dist_interval[1] @@ -2554,7 +2555,7 @@ def update(self): if distance > self._spawn_dist: actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', safe_blueprint=True, tick=False + 'vehicle.*', self._source_transform, rolename='scenario', filter_type=self._actor_type, tick=False ) if actor is None: return py_trees.common.Status.RUNNING diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 999724f4a..21107d7ad 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -229,3 +229,96 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class CrossingBycicleFlow(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._sink_distance = 2 + + self._end_distance = 40 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + super(CrossActorFlow, self).__init__("CrossActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + self.world.debug.draw_point(source_wp.transform.location + carla.Location(z=3)) + self.world.debug.draw_point(sink_wp.transform.location + carla.Location(z=3)) + + grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + junction_id = None + for wp, _ in route: + if wp.is_junction: + junction_id = wp.get_junction().id + break + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, "bycicle")) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) + + root.add_child(end_condition) + + sequence = py_trees.composites.Sequence() + if self.route_mode: + sequence.add_child(JunctionScenarioManager(remove_middle=True)) + sequence.add_child(StopEntries()) + sequence.add_child(root) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 9ac88d8c7..f75bcee3b 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1918,7 +1918,7 @@ def _spawn_actors(self, spawn_wps): actors = CarlaDataProvider.request_new_batch_actors( 'vehicle.*', len(spawn_transforms), spawn_transforms, True, False, 'background', - safe_blueprint=True, tick=False) + filter_type="car", tick=False) if not actors: return actors @@ -1946,7 +1946,7 @@ def _spawn_source_actor(self, source, ego_dist=0): ) actor = CarlaDataProvider.request_new_actor( 'vehicle.*', new_transform, rolename='background', - autopilot=True, random_location=False, safe_blueprint=True, tick=False) + autopilot=True, random_location=False, filter_type="car", tick=False) if not actor: return actor From 1fb6b9945f7b1ce459e1f441ebbd209dbd5dfaff Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 16 Mar 2022 11:31:01 +0100 Subject: [PATCH 08/86] Added CHANGELOG (#870) Co-authored-by: Guillermo --- Docs/CHANGELOG.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 751848ac1..a4f4383c7 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -17,9 +17,25 @@ * Actor Flows are now more consistent * Scenarios can now parse and use all parameters present at the configuration file. * Improved overall parsing of routes and scenarios. +* Added new scenarios: + - Accident: the ego has to lane change in order to avoid an accident + - CrossBycicleFlow: the ego has to do aturn at an intersection but it has to cross a bycicle lane full of incoming traffic +* Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. +* `NoSignalJunctionCrossingRoute` scenario has been moved from `junction_crossing_route.py` to `no_signal_junction_crossing.py` * The BackgroundActivity has been readded to the routes, which the objective of creating the sensation of traffic around the ego +* BasicScenario and Scenario class have been merged into one, simplifying its usage. +* Actor Flows are now more consistent. +* Scenarios are no logner fixed to a location but instead are now part of the route. +* Routes now automatically import all scenarios. +* Routes will now also take into account the criteria of their specific scenarios, only being active when they are running. The ResultWriter now automaically adds these criteria, grouping them if more than one scenario of the same type is triggered. + +* Scenarios can now parse and use all parameters present at the configuration file. +* Renamed RouteScenarioConfiguration's 'trajectory' to 'keypoints' and 'scenarios' to 'scenario_configs` +* Improved the handling of the route information. Added the route information to their configuration (config.route). As such,`CarlaDataProvider.get_ego_vehicle_route()` and `CarlaDataProvider.set_ego_vehicle_route()` functions have been replaced. +* The CarlaDataProvider's spawning functions can now filter vehicles by type. +* atomic criteria now have the `units` attribute, which is used by the ResultWriter for better output readibility. ### :bug: Bug Fixes * Fixed bug at OtherLeadingVehicle scenario causing the vehicles to move faster than intended From b82a681edb15790baf51b0e1ed79c4a025b09272 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Fri, 18 Mar 2022 11:14:57 +0100 Subject: [PATCH 09/86] Added VehicleOpensDoor scenario --- Docs/CHANGELOG.md | 4 +- scenario_runner.py | 23 +- srunner/examples/VehicleOpensDoor.xml | 7 + .../scenariomanager/carla_data_provider.py | 42 +++- .../scenarioatomics/atomic_behaviors.py | 52 +++- srunner/scenarios/background_activity.py | 234 ++++-------------- srunner/scenarios/basic_scenario.py | 5 +- .../scenarios/object_crash_intersection.py | 9 +- srunner/scenarios/object_crash_vehicle.py | 5 + srunner/scenarios/vehicle_opens_door.py | 140 +++++++++++ srunner/tools/background_manager.py | 53 ++-- 11 files changed, 331 insertions(+), 243 deletions(-) create mode 100644 srunner/examples/VehicleOpensDoor.xml create mode 100644 srunner/scenarios/vehicle_opens_door.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index a4f4383c7..43ecd79b1 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -19,7 +19,8 @@ * Improved overall parsing of routes and scenarios. * Added new scenarios: - Accident: the ego has to lane change in order to avoid an accident - - CrossBycicleFlow: the ego has to do aturn at an intersection but it has to cross a bycicle lane full of incoming traffic + - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic + - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. After a while, the door automatically closes. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. @@ -30,6 +31,7 @@ * Scenarios are no logner fixed to a location but instead are now part of the route. * Routes now automatically import all scenarios. * Routes will now also take into account the criteria of their specific scenarios, only being active when they are running. The ResultWriter now automaically adds these criteria, grouping them if more than one scenario of the same type is triggered. +* Separated the route argument into two, `route` for the file path, and `route-id`, for the name of route. the functionaility remains unchanged. * Scenarios can now parse and use all parameters present at the configuration file. * Renamed RouteScenarioConfiguration's 'trajectory' to 'keypoints' and 'scenarios' to 'scenario_configs` diff --git a/scenario_runner.py b/scenario_runner.py index 84b1448a6..a99707c1e 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -392,11 +392,11 @@ def _load_and_run_scenario(self, config): debug_mode=self._args.debug) else: scenario_class = self._get_scenario_class_or_fail(config.type) - scenario = scenario_class(self.world, - self.ego_vehicles, - config, - self._args.randomize, - self._args.debug) + scenario = scenario_class(world=self.world, + ego_vehicles=self.ego_vehicles, + config=config, + randomize=self._args.randomize, + debug_mode=self._args.debug) except Exception as exception: # pylint: disable=broad-except print("The scenario cannot be loaded") traceback.print_exc() @@ -462,12 +462,8 @@ def _run_route(self): """ result = False - if self._args.route: - routes = self._args.route[0] - single_route = self._args.route[1] if len(self._args.route) > 1 else None - # retrieve routes - route_configurations = RouteParser.parse_routes_file(routes, single_route) + route_configurations = RouteParser.parse_routes_file(self._args.route, self._args.route_id) for config in route_configurations: for _ in range(self._args.repetitions): @@ -543,11 +539,10 @@ def main(): '--scenario', help='Name of the scenario to be executed. Use the preposition \'group:\' to run all scenarios of one class, e.g. ControlLoss or FollowLeadingVehicle') parser.add_argument('--openscenario', help='Provide an OpenSCENARIO definition') parser.add_argument('--openscenarioparams', help='Overwrited for OpenSCENARIO ParameterDeclaration') + parser.add_argument('--route', help='Run a route as a scenario', type=str) + parser.add_argument('--route-id', help='Run a specific route inside that \'route\' file', default='', type=str) parser.add_argument( - '--route', help='Run a route as a scenario (input: (route_file, [route id]))', nargs='+', type=str) - - parser.add_argument( - '--agent', help="Agent used to execute the scenario. Currently only compatible with route-based scenarios.") + '--agent', help="Agent used to execute the route. Not compatible with non-route-based scenarios.") parser.add_argument('--agentConfig', type=str, help="Path to Agent's configuration file", default="") parser.add_argument('--output', action="store_true", help='Provide results on stdout') diff --git a/srunner/examples/VehicleOpensDoor.xml b/srunner/examples/VehicleOpensDoor.xml new file mode 100644 index 000000000..2d4f1c354 --- /dev/null +++ b/srunner/examples/VehicleOpensDoor.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index fba152007..ae3b7521e 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -415,10 +415,28 @@ def generate_spawn_points(): CarlaDataProvider._spawn_index = 0 @staticmethod - def create_blueprint(model, rolename='scenario', color=None, actor_category="car", filter_type=""): + 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 + """ + if not blueprint.has_attribute(name): + return False + + attribute_type = blueprint.get_attribute(key).type + if attribute_type == carla.ActorAttributeType.Bool: + return blueprint.get_attribute(name).as_bool() == value + elif attribute_type == carla.ActorAttributeType.Int: + return blueprint.get_attribute(name).as_int() == value + elif attribute_type == carla.ActorAttributeType.Float: + return blueprint.get_attribute(name).as_float() == value + elif attribute_type == carla.ActorAttributeType.String: + return blueprint.get_attribute(name).as_str() == value + + return False _actor_blueprint_categories = { 'car': 'vehicle.tesla.model3', @@ -437,13 +455,11 @@ def create_blueprint(model, rolename='scenario', color=None, actor_category="car # Set the model try: blueprints = CarlaDataProvider._blueprint_library.filter(model) - blueprints_ = [] - if filter_type: - blueprints = [x for x in blueprints if x.get_attribute('type') == filter_type] - else: - blueprints_ = blueprints + 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)] - blueprint = CarlaDataProvider._rng.choice(blueprints_) + 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.*" @@ -517,11 +533,11 @@ def handle_actor_batch(batch, tick=True): @staticmethod def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, random_location=False, color=None, actor_category="car", - filter_type="", tick=True): + 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, filter_type) + blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category, attribute_filter) if random_location: actor = None @@ -562,7 +578,7 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, return actor @staticmethod - def request_new_actors(actor_list, filter_type="", tick=True): + def request_new_actors(actor_list, attribute_filter=None, tick=True): """ This method tries to series of actor in batch. If this was successful, the new actors are returned, None otherwise. @@ -586,7 +602,7 @@ def request_new_actors(actor_list, filter_type="", tick=True): # Get the blueprint blueprint = CarlaDataProvider.create_blueprint( - actor.model, actor.rolename, actor.color, actor.category, filter_type) + actor.model, actor.rolename, actor.color, actor.category, attribute_filter) # Get the spawn point transform = actor.transform @@ -639,7 +655,7 @@ def request_new_actors(actor_list, filter_type="", tick=True): @staticmethod def request_new_batch_actors(model, amount, spawn_points, autopilot=False, random_location=False, rolename='scenario', - filter_type="", tick=True): + attribute_filter=None, tick=True): """ Simplified version of "request_new_actors". This method also create several actors in batch. @@ -660,7 +676,7 @@ 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, filter_type=filter_type) + blueprint = CarlaDataProvider.create_blueprint(model, rolename, attribute_filter=attribute_filter) if random_location: if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 084b583fb..ca8f967a4 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2555,7 +2555,8 @@ def update(self): if distance > self._spawn_dist: actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', filter_type=self._actor_type, tick=False + 'vehicle.*', self._source_transform, rolename='scenario', + attribute_filter={'base_type': self._actor_type}, tick=False ) if actor is None: return py_trees.common.Status.RUNNING @@ -2595,6 +2596,55 @@ def terminate(self, new_status): pass # Actor was already destroyed +class OpenVehicleDoor(AtomicBehavior): + + """ + Implementation for a behavior that will open the door of a vehicle, + then close it after a while. + + Important parameters: + - actor: Type of CARLA actors to be spawned + - vehicle_door: The specific door that will be opened + - duration: Duration of the open door + """ + + def __init__(self, actor, vehicle_door, duration, name="OpenVehicleDoor"): + """ + Setup class members + """ + super(OpenVehicleDoor, self).__init__(name, actor) + self._vehicle_door = vehicle_door + self._duration = duration + self._start_time = 0 + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def initialise(self): + """ + Set start time + """ + self._start_time = GameTime.get_time() + self._actor.open_door(self._vehicle_door) + super().initialise() + + def update(self): + """ + Keep running until termination condition is satisfied + """ + new_status = py_trees.common.Status.RUNNING + + if GameTime.get_time() - self._start_time > self._duration: + new_status = py_trees.common.Status.SUCCESS + + return new_status + + def terminate(self, new_status): + """ + Close the open door + """ + self._actor.close_door(self._vehicle_door) + super().terminate(new_status) + + class TrafficLightFreezer(AtomicBehavior): """ Behavior that freezes a group of traffic lights for a specific amount of time, diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index f75bcee3b..e55ecc404 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -115,7 +115,6 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind self.id = junction_id # pylint: disable=invalid-name self.route_entry_index = route_entry_index self.route_exit_index = route_exit_index - self.exit_road_length = 0 self.route_entry_keys = [] self.route_exit_keys = [] self.opposite_entry_keys = [] @@ -277,23 +276,8 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._opposite_spawn_dist = 20 # Initial distance between spawned opposite vehicles [m] self._opposite_sources_max_actors = 8 # Maximum vehicles alive at the same time per source - # Scenario 2 variables - self._stopped_road_actors = [] - - # Scenario 4 variables - self._is_crossing_scenario_active = False - self._crossing_scenario_actors = [] - self._ego_exitted_junction = False - self._crossing_dist = None # Distance between the crossing object and the junction exit - self._start_ego_wp = None - - # Junction scenario variables - self.scenario_info = { - 'direction': None, - 'remove_entries': False, - 'remove_middle': False, - 'remove_exits': False, - } # Same as the Junction.scenario_info, but this stores the data in case no junctions are active + # Scenario variables: + self._stopped_road_actors = [] # Actors stopped by a hard break scenario self._route_sources_active = True @@ -369,8 +353,6 @@ def update(self): # Update non junction sources. self._update_opposite_actors() - self._monitor_crossing_scenario_end() - return py_trees.common.Status.RUNNING def terminate(self, new_status): @@ -437,7 +419,6 @@ def _get_junctions_data(self): if prev_junction: start_dist = self._accum_dist[i] prev_end_dist = self._accum_dist[prev_junction.route_exit_index] - prev_junction.exit_road_length = start_dist - prev_end_dist # Same junction as the prev one and closer than 2 meters if prev_junction and prev_junction.junctions[-1].id == junction_id: @@ -452,14 +433,6 @@ def _get_junctions_data(self): junction_data.append(Junction(next_wp.get_junction(), junction_num, i)) junction_num += 1 - if len(junction_data) > 0: - road_end_dist = self._accum_dist[self._route_length - 1] - if junction_data[-1].route_exit_index: - route_start_dist = self._accum_dist[junction_data[-1].route_exit_index] - else: - route_start_dist = self._accum_dist[self._route_length - 1] - junction_data[-1].exit_road_length = road_end_dist - route_start_dist - return junction_data def _filter_fake_junctions(self, data): @@ -925,128 +898,6 @@ def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, 'remove_exits': remove_exits, } - def _handle_junction_scenario_end(self, junction): - """Ends the junction scenario interaction. This is pretty much useless as the junction - scenario ends at the same time as the active junction, but in the future it might not""" - junction.scenario_info = { - 'direction': None, - 'remove_entries': False, - 'remove_middle': False, - 'remove_exits': False, - } - - def _monitor_crossing_scenario_end(self): - """Monitors the ego distance to the junction to know if the scenario 4 has ended""" - if self._ego_exitted_junction: - ref_location = self._start_ego_wp.transform.location - ego_location = self._ego_wp.transform.location - if ego_location.distance(ref_location) > self._crossing_dist: - for actor in self._crossing_scenario_actors: - self._tm.vehicle_percentage_speed_difference(actor, 0) - self._is_crossing_scenario_active = False - self._crossing_scenario_actors.clear() - self._ego_exitted_junction = False - self._crossing_dist = None - - def _handle_crossing_scenario_interaction(self, junction, ego_wp): - """ - Handles the interation between the scenario 4 of the Leaderboard and the background activity. - This removes all vehicles near the bycicle path, and stops the others so that they don't interfere - """ - if not self._is_crossing_scenario_active: - return - - self._ego_exitted_junction = True - self._start_ego_wp = ego_wp - min_crossing_space = 2 - - # Actor exitting the junction - exit_dict = junction.exit_dict - for exit_key in exit_dict: - if exit_key not in junction.route_exit_keys: - continue - actors = exit_dict[exit_key]['actors'] - exit_lane_wp = exit_dict[exit_key]['ref_wp'] - exit_lane_location = exit_lane_wp.transform.location - for actor in list(actors): - actor_location = CarlaDataProvider.get_location(actor) - if not actor_location: - self._destroy_actor(actor) - continue - - dist_to_scenario = exit_lane_location.distance(actor_location) - self._crossing_dist - actor_length = actor.bounding_box.extent.x - if abs(dist_to_scenario) < actor_length + min_crossing_space: - self._destroy_actor(actor) - continue - - if dist_to_scenario > 0: - continue # Don't stop the actors that have already passed the scenario - - if get_lane_key(ego_wp) == get_lane_key(exit_lane_wp): - self._destroy_actor(actor) - continue # Actor at the ego lane and between the ego and scenario - - self._crossing_scenario_actors.append(actor) - - # Actor entering the junction - for entry_source in junction.entry_sources: - entry_lane_wp = entry_source.entry_lane_wp - if get_lane_key(entry_lane_wp) in junction.opposite_entry_keys: - # Source is affected - actors = entry_source.actors - entry_lane_location = entry_lane_wp.transform.location - for actor in list(actors): - actor_location = CarlaDataProvider.get_location(actor) - if not actor_location: - self._destroy_actor(actor) - continue - - crossing_space = abs(entry_lane_location.distance(actor_location) - self._crossing_dist) - actor_length = actor.bounding_box.extent.x - if crossing_space < actor_length + min_crossing_space: - self._destroy_actor(actor) - continue # Actors blocking the path of the crossing obstacle - - self._crossing_scenario_actors.append(actor) - - # Actors entering the next junction - if len(self._active_junctions) > 1: - next_junction = self._active_junctions[1] - actors_dict = next_junction.actor_dict - for actor in list(actors_dict): - if actors_dict[actor]['state'] != JUNCTION_ENTRY: - continue - - actor_location = CarlaDataProvider.get_location(actor) - if not actor_location: - self._destroy_actor(actor) - continue - - dist_to_scenario = exit_lane_location.distance(actor_location) - self._crossing_dist - actor_length = actor.bounding_box.extent.x - if abs(dist_to_scenario) < actor_length + min_crossing_space: - self._destroy_actor(actor) - continue - - if dist_to_scenario > 0: - continue # Don't stop the actors that have already passed the scenario - - actor_wp = self._map.get_waypoint(actor_location) - if get_lane_key(ego_wp) == get_lane_key(actor_wp): - self._destroy_actor(actor) - continue # Actor at the ego lane and between the ego and scenario - - self._crossing_scenario_actors.append(actor) - - # Immediately freeze the actors - for actor in self._crossing_scenario_actors: - try: - actor.set_target_velocity(carla.Vector3D(0, 0, 0)) - self._tm.vehicle_percentage_speed_difference(actor, 100) - except RuntimeError: - pass # Just in case the actor is not alive - def _end_junction_behavior(self, junction): """ Destroys unneeded actors (those that aren't part of the route's road), @@ -1103,9 +954,6 @@ def _end_junction_behavior(self, junction): # Destroy the rest self._destroy_actor(actor) - self._handle_crossing_scenario_interaction(junction, self._ego_wp) # TODO: Do a better scenario case - self._handle_junction_scenario_end(junction) # TODO: See if this can be removed (as it changes a popped element) - if not self._active_junctions: self._ego_state = EGO_ROAD self._initialise_opposite_sources() @@ -1745,13 +1593,6 @@ def _update_parameters(self): self._start_road_front_vehicles() py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) - # Handles crossing actor scenarios. This currently only works for Scenario4 - crossing_dist = py_trees.blackboard.Blackboard().get('BA_HandleCrossingActor') - if crossing_dist is not None: - self._is_crossing_scenario_active = True - self._crossing_dist = crossing_dist - py_trees.blackboard.Blackboard().set('BA_HandleCrossingActor', None, True) - # Handles junction scenarios (scenarios 7 to 10) junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_JunctionScenario') if junction_scenario_data is not None: @@ -1778,6 +1619,12 @@ def _update_parameters(self): self._stop_non_route_entries() py_trees.blackboard.Blackboard().set('BA_StopEntries', None, True) + # Leave space in front + leave_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveSpaceInFront') + if leave_space_data is not None: + self._leave_space_in_front(leave_space_data) + py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) + self._compute_parameters() def _compute_parameters(self): @@ -1843,23 +1690,26 @@ def _extent_road_exit_space(self, space, direction): if exit_lane_key in direction_lane_keys: exit_dict[exit_lane_key]['max_distance'] += space - # For all the actors present, teleport them a bit to the front and activate them - for actor in exit_dict[exit_lane_key]['actors']: - location = CarlaDataProvider.get_location(actor) - if not location: - continue - - actor_wp = self._map.get_waypoint(location) - new_actor_wps = actor_wp.next(space) - if len(new_actor_wps) > 0: - new_transform = new_actor_wps[0].transform - new_transform.location.z += 0.2 - actor.set_transform(new_transform) - + actors = exit_dict[exit_lane_key]['actors'] + self._move_actors_forward(actors, space) + for actor in actors: if junction.actor_dict[actor]['state'] == JUNCTION_INACTIVE: self._tm.vehicle_percentage_speed_difference(actor, 0) junction.actor_dict[actor]['state'] = JUNCTION_EXIT + def _move_actors_forward(self, actors, space): + """Teleports the actors forward a set distance""" + for actor in actors: + location = CarlaDataProvider.get_location(actor) + if not location: + continue + + actor_wp = self._map.get_waypoint(location) + new_actor_wps = actor_wp.next(space) + if len(new_actor_wps) > 0: + new_transform = new_actor_wps[0].transform + new_transform.location.z += 0.2 + actor.set_transform(new_transform) def _stop_non_route_entries(self): if len(self._active_junctions) > 0: @@ -1903,6 +1753,32 @@ def _switch_route_sources(self, enabled): if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: source.active = enabled + def _leave_space_in_front(self, space): + """Teleports all the vehicles in front of the ego forward""" + all_actors = [] + if not self._active_junctions: + for lane in self._road_dict: + all_actors.extend(self._road_dict[lane].actors) + + else: + junction = self._active_junctions[0] + if self._is_junction(self._ego_wp): # We care about the exit + for actor, info in junction.actor_dict.items(): + if info['exit_lane_key'] in junction.route_exit_keys: + all_actors.append(actor) + else: # We care about the entry + for source in junction.entry_sources: + if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: + all_actors.extend(source.actors) + + front_actors = [] + for actor in all_actors: + location = CarlaDataProvider.get_location(actor) + if location and not self._is_location_behind_ego(location): + front_actors.append(actor) + + self._move_actors_forward(front_actors, space) + ############################# ## Actor functions ## ############################# @@ -1918,7 +1794,7 @@ def _spawn_actors(self, spawn_wps): actors = CarlaDataProvider.request_new_batch_actors( 'vehicle.*', len(spawn_transforms), spawn_transforms, True, False, 'background', - filter_type="car", tick=False) + attribute_filter={'base_type': 'car'}, tick=False) if not actors: return actors @@ -1946,7 +1822,7 @@ def _spawn_source_actor(self, source, ego_dist=0): ) actor = CarlaDataProvider.request_new_actor( 'vehicle.*', new_transform, rolename='background', - autopilot=True, random_location=False, filter_type="car", tick=False) + autopilot=True, random_location=False, attribute_filter={'base_type':'car'}, tick=False) if not actor: return actor @@ -1999,7 +1875,7 @@ def _update_road_actors(self): """ # Updates their speed route_wp = self._route[self._route_index] - scenario_actors = self._crossing_scenario_actors + self._stopped_road_actors + scenario_actors = self._stopped_road_actors for lane in self._road_dict: for i, actor in enumerate(self._road_dict[lane].actors): location = CarlaDataProvider.get_location(actor) @@ -2134,7 +2010,7 @@ def _update_junction_actors(self): state, exit_lane_key, _ = actor_dict[actor].values() if self.debug: - string = 'J' + str(i+1) + '_' + state[:2] + '(' + str(j) + ')' + string = 'J' + str(i+1) + '_' + state[:2] draw_string(self._world, location, string, DEBUG_JUNCTION, False) # Monitor its entry @@ -2215,8 +2091,6 @@ def _remove_actor_info(self, actor): self._opposite_actors.remove(actor) if actor in self._stopped_road_actors: self._stopped_road_actors.remove(actor) - if actor in self._crossing_scenario_actors: - self._crossing_scenario_actors.remove(actor) for opposite_source in self._opposite_sources: if actor in opposite_source.actors: diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index a49c73fa0..dd44f1b9f 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -50,8 +50,9 @@ def __init__(self, name, ego_vehicles, config, world, self.behavior_tree = None self.criteria_tree = None - if not self.timeout: - self.timeout = 60 # If no timeout was provided, set it to 60 seconds + # If no timeout was provided, set it to 60 seconds + if not hasattr(self, 'timeout'): + self.timeout = 60 if debug_mode: py_trees.logging.level = py_trees.logging.Level.DEBUG diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 08df067da..9808c543d 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -27,7 +27,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route -from srunner.tools.background_manager import HandleCrossingActor +from srunner.tools.background_manager import LeaveSpaceInFront def get_sidewalk_transform(waypoint): @@ -161,10 +161,6 @@ def _create_behavior(self): collision_duration = collision_distance / self._adversary_speed collision_time_trigger = collision_duration + self._reaction_time - # On trigger behavior - if self.route_mode: - sequence.add_child(HandleCrossingActor(self._spawn_dist)) - # First waiting behavior sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) @@ -178,6 +174,9 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) sequence.add_child(HandBrakeVehicle(self.other_actors[0], False)) + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._spawn_dist)) + # Move the adversary. speed_duration = 2.0 * collision_duration speed_distance = 2.0 * collision_distance diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 14c5e5057..a85add43b 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -25,6 +25,8 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_location_in_distance_from_wp +from srunner.tools.background_manager import LeaveSpaceInFront + class StationaryObjectCrossing(BasicScenario): @@ -259,6 +261,9 @@ def _create_behavior(self): then after 60 seconds, a timeout stops the scenario """ sequence = py_trees.composites.Sequence(name="CrossingActor") + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._start_distance)) + collision_location = self._collision_wp.transform.location collision_distance = collision_location.distance(self._adversary_transform.location) collision_duration = collision_distance / self._adversary_speed diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py new file mode 100644 index 000000000..ebe8d1713 --- /dev/null +++ b/srunner/scenarios/vehicle_opens_door.py @@ -0,0 +1,140 @@ +#!/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 . + +""" +Scenarios in which another (opposite) vehicle 'illegally' takes +priority, e.g. by running a red traffic light. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest + +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import OpenVehicleDoor +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + InTimeToArrivalToLocation, + DriveDistance) +from srunner.scenarios.basic_scenario import BasicScenario + +from srunner.tools.background_manager import LeaveSpaceInFront + + + +class VehicleOpensDoor(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + + self.timeout = timeout + self._parked_distance = 20 + self._wait_duration = 15 + self._end_distance = 40 + self._min_trigger_dist = 5 + self._reaction_time = 1.5 + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = 'right' + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + + super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Creates a parked vehicle on the side of the road + """ + trigger_location = config.trigger_points[0].location + starting_wp = self._map.get_waypoint(trigger_location) + front_wps = starting_wp.next(self._parked_distance) + if len(front_wps) == 0: + raise ValueError("Couldn't find a spot to place the adversary vehicle") + elif len(front_wps) > 1: + print("WARNING: Found a diverging lane. Choosing one at random") + self._front_wp = front_wps[0] + + if self._direction == 'left': + parked_wp = self._front_wp.get_left_lane() + else: + parked_wp = self._front_wp.get_right_lane() + + if parked_wp is None: + raise ValueError("Couldn't find a spot to place the adversary vehicle") + + self.parked_actor = CarlaDataProvider.request_new_actor( + "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True}) + self.other_actors.append(self.parked_actor) + + # Move the actor to the edge of the lane, or the open door might not reach the ego vehicle + displacement = (parked_wp.lane_width - self.parked_actor.bounding_box.extent.y) / 2 + displacement_vector = parked_wp.transform.get_right_vector() + if self._direction == 'right': + displacement_vector *= -1 + + new_location = parked_wp.transform.location + carla.Location(x=displacement*displacement_vector.x, + y=displacement*displacement_vector.y) + self.parked_actor.set_location(new_location) + + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + sequence = py_trees.composites.Sequence(name="CrossingActor") + + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._parked_distance)) + + collision_location = self._front_wp.transform.location + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) + + door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL + + sequence.add_child(OpenVehicleDoor(self.parked_actor, door, self._wait_duration)) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 22351d4ac..11ff43e51 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -32,7 +32,7 @@ def __init__(self, num_front_vehicles=None, num_back_vehicles=None, self._num_back_vehicles = num_back_vehicles self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist - super(ChangeRoadBehavior, self).__init__(name) + super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( @@ -61,7 +61,7 @@ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist self._max_actors = max_actors - super(ChangeOppositeBehavior, self).__init__(name) + super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( @@ -91,7 +91,7 @@ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, self._vehicle_dist = vehicle_dist self._spawn_dist = spawn_dist self._max_actors = max_actors - super(ChangeJunctionBehavior, self).__init__(name) + super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( @@ -109,7 +109,7 @@ class StopFrontVehicles(AtomicBehavior): """ def __init__(self, name="StopFrontVehicles"): - super(StopFrontVehicles, self).__init__(name) + super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set("BA_StopFrontVehicles", True, overwrite=True) @@ -123,29 +123,13 @@ class StartFrontVehicles(AtomicBehavior): """ def __init__(self, name="StartFrontVehicles"): - super(StartFrontVehicles, self).__init__(name) + super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", True, overwrite=True) return py_trees.common.Status.SUCCESS -class HandleCrossingActor(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that a crossing actor has been triggered. - 'crossing_dist' is the distance between the crossing actor and the junction - """ - - def __init__(self, crossing_dist=10, name="HandleCrossingActor"): - self._crossing_dist = crossing_dist - super(HandleCrossingActor, self).__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_HandleCrossingActor", self._crossing_dist, overwrite=True) - return py_trees.common.Status.SUCCESS - - class JunctionScenarioManager(AtomicBehavior): """ Updates the blackboard to tell the background activity that a JunctionScenarioManager has been triggered @@ -158,7 +142,7 @@ def __init__(self, entry_direction, remove_entry, remove_exit, remove_middle, na self._remove_entry = remove_entry self._remove_exit = remove_exit self._remove_middle = remove_middle - super(JunctionScenarioManager, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" @@ -177,7 +161,7 @@ class ExtentExitRoadSpace(AtomicBehavior): def __init__(self, distance, direction, name="ExtentExitRoadSpace"): self._distance = distance self._direction = direction - super(ExtentExitRoadSpace, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" @@ -185,12 +169,27 @@ def update(self): return py_trees.common.Status.SUCCESS +class LeaveSpaceInFront(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that the ego needs more space in front. + This only works at roads, not junctions. + """ + def __init__(self, space, name="LeaveSpaceInFront"): + self._space = space + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_LeaveSpaceInFront", self._space, overwrite=True) + return py_trees.common.Status.SUCCESS + + class StopEntries(AtomicBehavior): """ Updates the blackboard to tell the background activity to stop all junction entries """ def __init__(self, name="StopEntries"): - super(StopEntries, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" @@ -204,7 +203,7 @@ class SwitchRouteSources(AtomicBehavior): """ def __init__(self, enabled=True, name="SwitchRouteSources"): self._enabled = enabled - super(SwitchRouteSources, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" @@ -219,7 +218,7 @@ class HandleStartAccidentScenario(AtomicBehavior): def __init__(self, accident_wp, distance, name="HandleStartAccidentScenario"): self._accident_wp = accident_wp self._distance = distance - super(HandleStartAccidentScenario, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" @@ -232,7 +231,7 @@ class HandleEndAccidentScenario(AtomicBehavior): Updates the blackboard to tell the background activity that the road behavior has to be initialized """ def __init__(self, name="HandleEndAccidentScenario"): - super(HandleEndAccidentScenario, self).__init__(name) + super().__init__(name) def update(self): """Updates the blackboard and succeds""" From e86b4928964633171b10407679c811b89aaa74d6 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 23 Mar 2022 11:22:47 +0100 Subject: [PATCH 10/86] Minor changes --- srunner/scenarios/actor_flow.py | 3 --- srunner/scenarios/background_activity.py | 6 +++--- srunner/tools/background_manager.py | 6 ++---- srunner/tools/route_parser.py | 2 +- 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 21107d7ad..7ac6542b2 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -189,9 +189,6 @@ def _create_behavior(self): source_wp = self._map.get_waypoint(self._start_actor_flow) sink_wp = self._map.get_waypoint(self._end_actor_flow) - self.world.debug.draw_point(source_wp.transform.location + carla.Location(z=3)) - self.world.debug.draw_point(sink_wp.transform.location + carla.Location(z=3)) - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) junction_id = None diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index e55ecc404..03be01445 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -853,7 +853,7 @@ def _switch_to_junction_mode(self, junction): self._road_num_front_vehicles = self._road_front_vehicles self._opposite_sources.clear() - def _initialise_junction_scenario(self, direction, remove_entries, remove_exits, remove_middle): + def _initialise_junction_scenario(self, direction, remove_exits, remove_entries, remove_middle): """ Removes all vehicles in a particular 'direction' as well as all actors inside the junction. Additionally, activates some flags to ensure the junction is empty at all times @@ -1596,8 +1596,8 @@ def _update_parameters(self): # Handles junction scenarios (scenarios 7 to 10) junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_JunctionScenario') if junction_scenario_data is not None: - direction, remove_entries, remove_exits, remove_middle = junction_scenario_data - self._initialise_junction_scenario(direction, remove_entries, remove_exits, remove_middle) + entry_direction, remove_exits = junction_scenario_data + self._initialise_junction_scenario(entry_direction, remove_exits, True, True) py_trees.blackboard.Blackboard().set('BA_JunctionScenario', None, True) # Handles road accident scenario (Accident and Construction) diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 11ff43e51..4e8acf4f7 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -137,18 +137,16 @@ class JunctionScenarioManager(AtomicBehavior): something like 'left', 'right' or 'opposite' """ - def __init__(self, entry_direction, remove_entry, remove_exit, remove_middle, name="JunctionScenarioManager"): + def __init__(self, entry_direction, remove_exit=True, name="JunctionScenarioManager"): self._entry_direction = entry_direction - self._remove_entry = remove_entry self._remove_exit = remove_exit - self._remove_middle = remove_middle super().__init__(name) def update(self): """Updates the blackboard and succeds""" py_trees.blackboard.Blackboard().set( "BA_JunctionScenario", - [self._entry_direction, self._remove_entry, self._remove_exit, self._remove_middle], + [self._entry_direction, self._remove_exit], overwrite=True ) return py_trees.common.Status.SUCCESS diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index c5b1defb2..977076f0f 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -155,4 +155,4 @@ def is_trigger_close(trigger_transform, route_transform): if is_trigger_close(trigger_transform, route_transform): return True - return None + return False From 38cec5aeffc41567eeb31712a5ab709174c05768 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 23 Mar 2022 13:23:34 +0100 Subject: [PATCH 11/86] Fixed bug with new JucntionScenarioManager (#873) Co-authored-by: Guillermo --- srunner/scenarios/actor_flow.py | 4 ++-- srunner/scenarios/opposite_vehicle_taking_priority.py | 2 +- srunner/scenarios/signalized_junction_left_turn.py | 2 +- srunner/scenarios/signalized_junction_right_turn.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 7ac6542b2..43c7df9a0 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -103,7 +103,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(JunctionScenarioManager('left', True, False, True)) + sequence.add_child(JunctionScenarioManager('left', False)) grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) @@ -208,7 +208,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(JunctionScenarioManager('opposite', True, True, True)) + sequence.add_child(JunctionScenarioManager('opposite', True)) sequence.add_child(StopEntries()) sequence.add_child(root) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 54f0c4989..6670227c5 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -184,7 +184,7 @@ def _create_behavior(self): root = py_trees.composites.Sequence() if self.route_mode: - root.add_child(JunctionScenarioManager(self._direction, True, True, True)) + root.add_child(JunctionScenarioManager(self._direction, True)) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(main_behavior) root.add_child(ActorDestroy(self.other_actors[0])) diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index 6f275f5b0..c62597cc7 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -144,7 +144,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence(name="SignalizedJunctionLeftTurn") if self.route_mode: - sequence.add_child(JunctionScenarioManager(self._direction, True, True, True)) + sequence.add_child(JunctionScenarioManager(self._direction, True)) sequence.add_child(root) return sequence diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index f354d3ba3..b115ff2ee 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -144,7 +144,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence(name="SignalizedJunctionRightTurn") if self.route_mode: - sequence.add_child(JunctionScenarioManager(self._direction, True, False, True)) + sequence.add_child(JunctionScenarioManager(self._direction, False)) sequence.add_child(root) return sequence From 3f63408ad3bc7a77ff488887b43963384647de87 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Fri, 25 Mar 2022 17:10:27 +0100 Subject: [PATCH 12/86] Added dynamic weathers to routes --- Docs/CHANGELOG.md | 1 + .../scenarioatomics/atomic_behaviors.py | 2 +- srunner/scenariomanager/weather_sim.py | 133 +++++++++++++++++- srunner/scenarios/background_activity.py | 6 + srunner/scenarios/basic_scenario.py | 13 +- srunner/scenarios/open_scenario.py | 7 + srunner/scenarios/route_scenario.py | 18 ++- srunner/tools/route_parser.py | 57 +++----- srunner/tools/scenario_parser.py | 48 +------ 9 files changed, 200 insertions(+), 85 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 43ecd79b1..9a705ed72 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -31,6 +31,7 @@ * Scenarios are no logner fixed to a location but instead are now part of the route. * Routes now automatically import all scenarios. * Routes will now also take into account the criteria of their specific scenarios, only being active when they are running. The ResultWriter now automaically adds these criteria, grouping them if more than one scenario of the same type is triggered. +* Routes can now have dynamic weather. These are set by keypoints at a routes percentage, and all values between them are interpolated. * Separated the route argument into two, `route` for the file path, and `route-id`, for the name of route. the functionaility remains unchanged. * Scenarios can now parse and use all parameters present at the configuration file. diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index ca8f967a4..742c9dacd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -229,7 +229,7 @@ class ChangeWeather(AtomicBehavior): """ Atomic to write a new weather configuration into the blackboard. - Used in combination with WeatherBehavior() to have a continuous weather simulation. + Used in combination with OSCWeatherBehavior() to have a continuous weather simulation. The behavior immediately terminates with SUCCESS after updating the blackboard. diff --git a/srunner/scenariomanager/weather_sim.py b/srunner/scenariomanager/weather_sim.py index 62362b174..0160e5a8d 100644 --- a/srunner/scenariomanager/weather_sim.py +++ b/srunner/scenariomanager/weather_sim.py @@ -88,7 +88,7 @@ def update(self, delta_time=0): self.carla_weather.sun_azimuth_angle = math.degrees(self._sun.az) -class WeatherBehavior(py_trees.behaviour.Behaviour): +class OSCWeatherBehavior(py_trees.behaviour.Behaviour): """ Atomic to read weather settings from the blackboard and apply these in CARLA. @@ -112,7 +112,7 @@ def __init__(self, name="WeatherBehavior"): """ Setup parameters """ - super(WeatherBehavior, self).__init__(name) + super(OSCWeatherBehavior, self).__init__(name) self._weather = None self._current_time = None @@ -164,3 +164,132 @@ def update(self): py_trees.blackboard.Blackboard().set("Datetime", self._weather.datetime, overwrite=True) return py_trees.common.Status.RUNNING + + +class RouteWeatherBehavior(py_trees.behaviour.Behaviour): + + """ + Given a set of route weathers ([position, carla.WeatherParameters]), + monitors the ego vehicle to dynamically change the weather as the ego advanced through the route. + + This behavior interpolates the desired weather between two weather keypoints and if the extreme cases + (0% and 100%) aren't defined, the closest one will be chosen + (i.e, if the route weather is at 90%, all weathers from 90% to 100% will be the one defined at 90%) + + Use the debug argument to print what is the route's percentage of each route position. + """ + + def __init__(self, ego_vehicle, route, weathers, debug=False, name="RouteWeatherBehavior"): + """ + Setup parameters + """ + super().__init__(name) + self._world = CarlaDataProvider.get_world() + self._ego_vehicle = ego_vehicle + self._route = route + + self._weathers = weathers + if self._weathers[0][0] != 0: # Make sure the weather is defined at 0% + self._weathers.insert(0, [0, self._weathers[0]]) + if self._weathers[-1][0] != 100: # Make sure the weather is defined at 100% + self._weathers.append([100, self._weathers[-1]]) + + self._wsize = 3 + + self._current_index = 0 + self._route_length = len(self._route) + self._route_transforms, _ = zip(*self._route) + self._route_perc = self._get_route_percentages() + if debug: + debug_perc = -1 + for transform, perc in zip(self._route_transforms, self._route_perc): + location = transform.location + new_perc = int(perc) + if new_perc > debug_perc: + self._world.debug.draw_string( + location + carla.Location(z=1), + str(new_perc), + color=carla.Color(50, 50, 50), + life_time=100000 + ) + debug_perc = new_perc + self._route_weathers = self.get_route_weathers() + + def _get_route_percentages(self): + """ + Calculate the accumulated distance percentage at each point in the route + """ + accum_m = [] + prev_loc = self._route_transforms[0].location + for i, tran in enumerate(self._route_transforms): + new_dist = tran.location.distance(prev_loc) + added_dist = 0 if i == 0 else accum_m[i - 1] + accum_m.append(new_dist + added_dist) + prev_loc = tran.location + + max_dist = accum_m[-1] + return [x / max_dist * 100 for x in accum_m] + + def get_route_weathers(self): + """Calculate the desired weather at each point in the route""" + def interpolate(prev_w, next_w, perc, name): + x0 = prev_w[0] + x1 = next_w[0] + y0 = getattr(prev_w[1], name) + y1 = getattr(next_w[1], name) + return y0 + (y1 - y0) * (perc - x0) / (x1 - x0) + + route_weathers = [] + + weather_index = 0 + prev_w = self._weathers[weather_index] + next_w = self._weathers[weather_index + 1] + + for perc in self._route_perc: + if perc > next_w[0]: # Must be strictly less, or an IndexError will occur at 100% + weather_index += 1 + prev_w = self._weathers[weather_index] + next_w = self._weathers[weather_index + 1] + + weather = carla.WeatherParameters() + weather.cloudiness = interpolate(prev_w, next_w, perc, 'cloudiness') + weather.precipitation = interpolate(prev_w, next_w, perc, 'precipitation') + weather.precipitation_deposits = interpolate(prev_w, next_w, perc, 'precipitation_deposits') + weather.wind_intensity = interpolate(prev_w, next_w, perc, 'wind_intensity') + weather.sun_azimuth_angle = interpolate(prev_w, next_w, perc, 'sun_azimuth_angle') + weather.sun_altitude_angle = interpolate(prev_w, next_w, perc, 'sun_altitude_angle') + weather.wetness = interpolate(prev_w, next_w, perc, 'wetness') + weather.fog_distance = interpolate(prev_w, next_w, perc, 'fog_distance') + weather.fog_density = interpolate(prev_w, next_w, perc, 'fog_density') + weather.fog_falloff = interpolate(prev_w, next_w, perc, 'fog_falloff') + weather.scattering_intensity = interpolate(prev_w, next_w, perc, 'scattering_intensity') + weather.mie_scattering_scale = interpolate(prev_w, next_w, perc, 'mie_scattering_scale') + weather.rayleigh_scattering_scale = interpolate(prev_w, next_w, perc, 'rayleigh_scattering_scale') + + route_weathers.append(weather) + + return route_weathers + + def update(self): + """ + Check the location of the ego vehicle, updating the weather if it has advanced through the route + """ + new_status = py_trees.common.Status.RUNNING + + ego_location = CarlaDataProvider.get_location(self._ego_vehicle) + if ego_location is None: + return new_status + + new_index = self._current_index + + 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: + new_index = index + + if new_index > self._current_index: + self._world.set_weather(self._route_weathers[new_index]) + self._current_index = new_index + + return new_status diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 03be01445..2c2c810e1 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -190,6 +190,12 @@ def _create_test_criteria(self): """ return [] + def _initialize_environment(self, world): + """ + Nothing to do here, avoid settign the weather + """ + pass + def __del__(self): """ Remove all actors upon deletion diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index dd44f1b9f..b87469d90 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -20,7 +20,6 @@ InTimeToArrivalToLocation) from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.timer import TimeOut -from srunner.scenariomanager.weather_sim import WeatherBehavior from srunner.scenariomanager.scenarioatomics.atomic_behaviors import UpdateAllActorControls from srunner.scenariomanager.scenarioatomics.atomic_criteria import Criterion @@ -112,9 +111,12 @@ def __init__(self, name, ego_vehicles, config, world, # Add other nodes self.timeout_node = TimeOut(self.timeout, name="TimeOut") # Timeout node self.scenario_tree.add_child(self.timeout_node) - self.scenario_tree.add_child(WeatherBehavior()) self.scenario_tree.add_child(UpdateAllActorControls()) + weather = self._create_weather_behavior() + if weather: + self.scenario_tree.add_child(weather) + self.scenario_tree.setup(timeout=1) def _initialize_environment(self, world): @@ -203,6 +205,13 @@ def _create_test_criteria(self): "This function is re-implemented by all scenarios" "If this error becomes visible the class hierarchy is somehow broken") + def _create_weather_behavior(self): + """ + Default initialization of the weather behavior. + Override this method in child class to provide custom initialization. + """ + pass + def change_control(self, control): # pylint: disable=no-self-use """ This is a function that changes the control based on the scenario determination diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 4236d4425..f3194753d 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -18,6 +18,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeWeather, ChangeRoadFriction, ChangeParameter 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 @@ -439,6 +440,12 @@ def _create_behavior(self): 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): """ diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 78efddd69..99231cc8b 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -39,6 +39,8 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.scenarios.background_activity import BackgroundActivity +from srunner.scenariomanager.weather_sim import RouteWeatherBehavior + from srunner.tools.route_parser import RouteParser, DIST_THRESHOLD from srunner.tools.route_manipulation import interpolate_trajectory @@ -293,7 +295,7 @@ def _create_test_criteria(self): and adds the scenario specific ones, which will only be active during their scenario """ criteria = py_trees.composites.Parallel(name="Criteria", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # End condition criteria.add_child(RouteCompletionTest(self.ego_vehicles[0], route=self.route)) @@ -324,6 +326,20 @@ def _create_test_criteria(self): return criteria + def _create_weather_behavior(self): + """ + If needed, add the dynamic weather behavior to the route + """ + if len(self.config.weather) == 1: + return + return RouteWeatherBehavior(self.ego_vehicles[0], self.route, self.config.weather) + + def _initialize_environment(self, world): + """ + Set the weather + """ + # Set the appropriate weather conditions + world.set_weather(self.config.weather[0][1]) def _create_criterion_tree(self, scenario, criteria): """ diff --git a/srunner/tools/route_parser.py b/srunner/tools/route_parser.py index 977076f0f..89f1995f6 100644 --- a/srunner/tools/route_parser.py +++ b/srunner/tools/route_parser.py @@ -98,44 +98,29 @@ def parse_routes_file(route_filename, single_route_id=''): @staticmethod def parse_weather(route): """ - Returns a carla.WeatherParameters with the corresponding weather for that route. If the route - has no weather attribute, the default one is triggered. + Parses all the weather information as a list of [position, carla.WeatherParameters], + where the position represents a % of the route. """ + weathers = [] - route_weather = route.find("weather") - if route_weather is None: - weather = carla.WeatherParameters(sun_altitude_angle=70) - - else: - weather = carla.WeatherParameters() - for weather_attrib in route.iter("weather"): - - if 'cloudiness' in weather_attrib.attrib: - weather.cloudiness = float(weather_attrib.attrib['cloudiness']) - if 'precipitation' in weather_attrib.attrib: - weather.precipitation = float(weather_attrib.attrib['precipitation']) - if 'precipitation_deposits' in weather_attrib.attrib: - weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits']) - if 'wind_intensity' in weather_attrib.attrib: - weather.wind_intensity = float(weather_attrib.attrib['wind_intensity']) - if 'sun_azimuth_angle' in weather_attrib.attrib: - weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle']) - if 'sun_altitude_angle' in weather_attrib.attrib: - weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle']) - if 'wetness' in weather_attrib.attrib: - weather.wetness = float(weather_attrib.attrib['wetness']) - if 'fog_distance' in weather_attrib.attrib: - weather.fog_distance = float(weather_attrib.attrib['fog_distance']) - if 'fog_density' in weather_attrib.attrib: - weather.fog_density = float(weather_attrib.attrib['fog_density']) - if 'scattering_intensity' in weather_attrib.attrib: - weather.scattering_intensity = float(weather_attrib.attrib['scattering_intensity']) - if 'mie_scattering_scale' in weather_attrib.attrib: - weather.mie_scattering_scale = float(weather_attrib.attrib['mie_scattering_scale']) - if 'rayleigh_scattering_scale' in weather_attrib.attrib: - weather.rayleigh_scattering_scale = float(weather_attrib.attrib['rayleigh_scattering_scale']) - - return weather + weathers_elem = route.find("weathers") + if weathers_elem is None: + return [[0, carla.WeatherParameters(sun_altitude_angle=70, cloudiness=50)]] + + for weather_elem in weathers_elem.iter('weather'): + route_percentage = float(weather_elem.attrib['route_percentage']) + + weather = carla.WeatherParameters(sun_altitude_angle=70, cloudiness=50) # Base weather + for weather_attrib in weather_elem.attrib: + if hasattr(weather, weather_attrib): + setattr(weather, weather_attrib, float(weather_elem.attrib[weather_attrib])) + elif weather_attrib != 'route_percentage': + print(f"WARNING: Ignoring '{weather_attrib}', as it isn't a weather parameter") + + weathers.append([route_percentage, weather]) + + weathers.sort(key=lambda x: x[0]) + return weathers @staticmethod def is_scenario_at_route(trigger_transform, route): diff --git a/srunner/tools/scenario_parser.py b/srunner/tools/scenario_parser.py index 8ea64b08c..0de98fa6c 100644 --- a/srunner/tools/scenario_parser.py +++ b/srunner/tools/scenario_parser.py @@ -76,20 +76,11 @@ def parse_scenario_configuration(scenario_name, additional_config_file_name): elif elem.tag == 'other_actor': config.other_actors.append(ActorConfigurationData.parse_from_node(elem, 'scenario')) elif elem.tag == 'weather': - config.weather.cloudiness = float(elem.attrib.get("cloudiness", 50)) - config.weather.precipitation = float(elem.attrib.get("precipitation", 10)) - config.weather.precipitation_deposits = float(elem.attrib.get("precipitation_deposits", 10)) - config.weather.wind_intensity = float(elem.attrib.get("wind_intensity", 0.35)) - config.weather.sun_azimuth_angle = float(elem.attrib.get("sun_azimuth_angle", 0.0)) - config.weather.sun_altitude_angle = float(elem.attrib.get("sun_altitude_angle", 70.0)) - config.weather.fog_density = float(elem.attrib.get("fog_density", 0.0)) - config.weather.fog_distance = float(elem.attrib.get("fog_distance", 0.0)) - config.weather.wetness = float(elem.attrib.get("wetness", 0.0)) - config.weather.fog_falloff = float(elem.attrib.get("fog_falloff", 0.0)) - config.weather.scattering_intensity = float(elem.attrib.get("scattering_intensity", 0.0)) - config.weather.mie_scattering_scale = float(elem.attrib.get("mie_scattering_scale", 0.0)) - config.weather.rayleigh_scattering_scale = float( - elem.attrib.get("rayleigh_scattering_scale", 0.0331)) + for weather_attrib in elem.attrib: + if hasattr(config.weather, weather_attrib): + setattr(config.weather, weather_attrib, float(elem.attrib[weather_attrib])) + else: + print(f"WARNING: Ignoring '{weather_attrib}', as it isn't a weather parameter") elif elem.tag == 'route': route_conf = RouteConfiguration() @@ -103,35 +94,6 @@ def parse_scenario_configuration(scenario_name, additional_config_file_name): scenario_configurations.append(config) return scenario_configurations - @staticmethod - def parse_weather(weather_element): - """Parses the weather element into a carla.WeatherParameters. - """ - weather = carla.WeatherParameters() - - for weather_attrib in weather_element: - - if 'cloudiness' in weather_attrib.attrib: - weather.cloudiness = float(weather_attrib.attrib['cloudiness']) - if 'precipitation' in weather_attrib.attrib: - weather.precipitation = float(weather_attrib.attrib['precipitation']) - if 'precipitation_deposits' in weather_attrib.attrib: - weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits']) - if 'wind_intensity' in weather_attrib.attrib: - weather.wind_intensity = float(weather_attrib.attrib['wind_intensity']) - if 'sun_azimuth_angle' in weather_attrib.attrib: - weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle']) - if 'sun_altitude_angle' in weather_attrib.attrib: - weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle']) - if 'wetness' in weather_attrib.attrib: - weather.wetness = float(weather_attrib.attrib['wetness']) - if 'fog_distance' in weather_attrib.attrib: - weather.fog_distance = float(weather_attrib.attrib['fog_distance']) - if 'fog_density' in weather_attrib.attrib: - weather.fog_density = float(weather_attrib.attrib['fog_density']) - - return weather - @staticmethod def get_list_of_scenarios(additional_config_file_name): """ From 92ec277907f41f222974e732b07bc08fbd6da2f6 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Thu, 7 Apr 2022 11:06:31 +0200 Subject: [PATCH 13/86] Improved traffic event handling (#877) Co-authored-by: Guillermo --- .../scenarioatomics/atomic_criteria.py | 54 ++++++++++--------- srunner/scenarios/background_activity.py | 4 +- 2 files changed, 30 insertions(+), 28 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 851bdf745..d3af607cd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1507,9 +1507,12 @@ class RouteCompletionTest(Criterion): - route: Route to be checked - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ - DISTANCE_THRESHOLD = 10.0 # meters 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): """ """ @@ -1519,24 +1522,29 @@ def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failur self._route = route self._map = CarlaDataProvider.get_map() - self._wsize = self.WINDOWS_SIZE - self._current_index = 0 + self._index = 0 self._route_length = len(self._route) self._route_transforms, _ = zip(*self._route) + self._route_accum_perc = self._get_acummulated_percentages() + self.target_location = self._route_transforms[-1].location - self._accum_meters = [] + self._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION) + self.events.append(self._traffic_event) + + def _get_acummulated_percentages(self): + """Gets the accumulated percentage of each of the route transforms""" + accum_meters = [] prev_loc = self._route_transforms[0].location for i, tran in enumerate(self._route_transforms): - loc = tran.location - d = loc.distance(prev_loc) - accum = 0 if i == 0 else self._accum_meters[i - 1] + d = tran.location.distance(prev_loc) + new_d = 0 if i == 0 else accum_meters[i - 1] - self._accum_meters.append(d + accum) - prev_loc = loc + accum_meters.append(d + new_d) + prev_loc = tran.location - self._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION) - self.events.append(self._traffic_event) + max_dist = accum_meters[-1] + return [x / max_dist * 100 for x in accum_meters] def update(self): """ @@ -1553,7 +1561,7 @@ def update(self): elif self.test_status in ('RUNNING', 'INIT'): - for index in range(self._current_index, min(self._current_index + self._wsize + 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 ref_location = self._route_transforms[index].location wp = self._map.get_waypoint(ref_location) @@ -1561,20 +1569,11 @@ def update(self): wp_veh = location - ref_location # vector waypoint - vehicle if wp_veh.dot(wp_dir) > 0: - # good! segment completed! - self._current_index = index - self.actual_value = 100.0 * float(self._accum_meters[self._current_index]) \ - / float(self._accum_meters[-1]) - self._traffic_event.set_dict({ - 'route_completed': self.actual_value}) - self._traffic_event.set_message( - "Agent has completed > {:.2f}% of the route".format( - self.actual_value)) - - if self.actual_value > 99.0 and location.distance(self.target_location) < self.DISTANCE_THRESHOLD: - route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED) - route_completion_event.set_message("Destination was successfully reached") - self.events.append(route_completion_event) + self._index = index + self.actual_value = self._route_accum_perc[self._index] + + if self.actual_value > self.PERCENTAGE_THRESHOLD \ + and location.distance(self.target_location) < self.DISTANCE_THRESHOLD: self.test_status = "SUCCESS" self.actual_value = 100 @@ -1591,6 +1590,9 @@ 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)) + if self.test_status == "INIT": self.test_status = "FAILURE" super(RouteCompletionTest, self).terminate(new_status) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 2c2c810e1..9291dc65f 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1800,7 +1800,7 @@ def _spawn_actors(self, spawn_wps): actors = CarlaDataProvider.request_new_batch_actors( 'vehicle.*', len(spawn_transforms), spawn_transforms, True, False, 'background', - attribute_filter={'base_type': 'car'}, tick=False) + attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False) if not actors: return actors @@ -1828,7 +1828,7 @@ def _spawn_source_actor(self, source, ego_dist=0): ) actor = CarlaDataProvider.request_new_actor( 'vehicle.*', new_transform, rolename='background', - autopilot=True, random_location=False, attribute_filter={'base_type':'car'}, tick=False) + autopilot=True, random_location=False, attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False) if not actor: return actor From b7843263e5b3e04c1afa024edee5ba36fb3f478c Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Thu, 21 Apr 2022 17:16:15 +0800 Subject: [PATCH 14/86] Added HighwayExit scenario --- srunner/routes/FastExit.xml | 29 +++++++ srunner/scenarios/actor_flow.py | 97 +++++++++++++++++++++++- srunner/scenarios/background_activity.py | 18 +++++ srunner/tools/background_manager.py | 18 +++++ 4 files changed, 161 insertions(+), 1 deletion(-) create mode 100644 srunner/routes/FastExit.xml diff --git a/srunner/routes/FastExit.xml b/srunner/routes/FastExit.xml new file mode 100644 index 000000000..6cabe6269 --- /dev/null +++ b/srunner/routes/FastExit.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 43c7df9a0..4d1423f9c 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -26,7 +26,8 @@ from srunner.tools.background_manager import (SwitchRouteSources, JunctionScenarioManager, ExtentExitRoadSpace, - StopEntries) + StopEntries, + RemoveLane) from srunner.tools.scenario_helper import get_same_dir_lanes def convert_dict_to_location(actor_dict): @@ -207,6 +208,7 @@ def _create_behavior(self): root.add_child(end_condition) sequence = py_trees.composites.Sequence() + if self.route_mode: sequence.add_child(JunctionScenarioManager('opposite', True)) sequence.add_child(StopEntries()) @@ -319,3 +321,96 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + +class HighwayExit(BasicScenario): + """ + This scenario is similar to CrossActorFlow + It will remove the BackgroundActivity from the lane where ActorFlow starts. + Then vehicles (cars) will start driving from start_actor_flow location to end_actor_flow location + in a relatively high speed, forcing the ego to accelerate to cut in the actor flow + then exit from the highway. + This scenario works when Background Activity is running in route mode. And there should be no junctions in front of the ego. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._sink_distance = 2 + + self._end_distance = 40 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + + super(HighwayExit, self).__init__("HighwayExit", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Vehicles run from the start to the end continuously. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + junction_id = None + for wp, _ in route: + if wp.is_junction: + junction_id = wp.get_junction().id + break + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) + + root.add_child(end_condition) + + sequence = py_trees.composites.Sequence() + + if self.route_mode: + sequence.add_child(RemoveLane(source_wp.lane_id)) + sequence.add_child(JunctionScenarioManager('opposite', True)) + sequence.add_child(root) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 9291dc65f..75a079cdd 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1631,6 +1631,12 @@ def _update_parameters(self): self._leave_space_in_front(leave_space_data) py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) + # Remove Lane + remove_lane_data = py_trees.blackboard.Blackboard().get('BA_RemoveLane') + if remove_lane_data is not None: + self._remove_lane(remove_lane_data) + py_trees.blackboard.Blackboard().set('BA_RemoveLanes', None, True) + self._compute_parameters() def _compute_parameters(self): @@ -1785,6 +1791,18 @@ def _leave_space_in_front(self, space): self._move_actors_forward(front_actors, space) + def _remove_lane(self, lane_id): + """Remove BA actors from this lane, and BA would never generate new actors on this lane""" + lane_id = str(lane_id) + lane_id_list = [x.split('*')[-1] for x in list(self._road_dict.keys())] + if lane_id in lane_id_list: + lane_index = lane_id_list.index(lane_id) + lane_key = list(self._road_dict.keys())[lane_index] + for actor in list(self._road_dict[lane_key].actors): + self._destroy_actor(actor) + self._road_dict.pop(lane_key) + + ############################# ## Actor functions ## ############################# diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 4e8acf4f7..c890aa47d 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -235,3 +235,21 @@ def update(self): """Updates the blackboard and succeds""" py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", True, overwrite=True) return py_trees.common.Status.SUCCESS + +class RemoveLane(AtomicBehavior): + """ + Updates the blackboard to tell the background activity to remove its actors from the given lane, + and stop generating new ones on this lane. + + Args: + lane_id (str): A carla.Waypoint.lane_id + """ + def __init__(self, lane=None, name="RemoveLane"): + self._lane = lane + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_RemoveLane", self._lane, overwrite=True) + return py_trees.common.Status.SUCCESS + \ No newline at end of file From b424bea363fcd0eb444a494b6a5d242752458aa6 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 25 Apr 2022 15:34:00 +0200 Subject: [PATCH 15/86] Added minimum speed criteria --- Docs/CHANGELOG.md | 1 + .../scenarioatomics/atomic_behaviors.py | 19 + .../scenarioatomics/atomic_criteria.py | 118 +++++- srunner/scenariomanager/traffic_events.py | 1 + srunner/scenarios/actor_flow.py | 23 +- srunner/scenarios/background_activity.py | 391 +++++++++++------- srunner/scenarios/route_scenario.py | 4 +- srunner/tools/background_manager.py | 35 +- srunner/tools/route_manipulation.py | 16 +- 9 files changed, 420 insertions(+), 188 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 9a705ed72..d32253da0 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -33,6 +33,7 @@ * Routes will now also take into account the criteria of their specific scenarios, only being active when they are running. The ResultWriter now automaically adds these criteria, grouping them if more than one scenario of the same type is triggered. * Routes can now have dynamic weather. These are set by keypoints at a routes percentage, and all values between them are interpolated. * Separated the route argument into two, `route` for the file path, and `route-id`, for the name of route. the functionaility remains unchanged. +* Added a new criteria for routes, `CheckMinSpeed`, that checks the ego's speed and compares it with teh rest of the traffic * Scenarios can now parse and use all parameters present at the configuration file. * Renamed RouteScenarioConfiguration's 'trajectory' to 'keypoints' and 'scenarios' to 'scenario_configs` diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 742c9dacd..7893c23dd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3233,3 +3233,22 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status + + +class SwitchMinSpeedCriteria(AtomicBehavior): + + def __init__(self, active, name="ChangeMinSpeed"): + """ + Setup parameters + """ + super().__init__(name) + self._active = active + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + 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) + return new_status \ No newline at end of file diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index d3af607cd..10551baa4 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1031,26 +1031,24 @@ def update(self): 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 - index_location = self._route_transforms[index].location - index_waypoint = self._map.get_waypoint(index_location) + route_transform = self._route_transforms[index] + route_location = route_transform.location - wp_dir = index_waypoint.transform.get_forward_vector() # Waypoint's forward vector - wp_veh = location - index_location # vector waypoint - vehicle + wp_dir = route_transform.get_forward_vector() # Waypoint's forward vector + wp_veh = location - route_location # vector waypoint - vehicle if wp_veh.dot(wp_dir) > 0: - # Get the distance traveled - index_location = self._route_transforms[index].location - current_index_location = self._route_transforms[self._current_index].location - new_dist = current_index_location.distance(index_location) - - # Add it to the total distance - self._current_index = index + # Get the distance traveled and add it to the total distance + 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_lane_active: self._wrong_distance += new_dist + self._current_index = index + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status @@ -1563,10 +1561,10 @@ def update(self): 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 - ref_location = self._route_transforms[index].location - wp = self._map.get_waypoint(ref_location) - wp_dir = wp.transform.get_forward_vector() # Waypoint's forward vector - wp_veh = location - ref_location # vector waypoint - vehicle + 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 if wp_veh.dot(wp_dir) > 0: self._index = index @@ -1940,3 +1938,93 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status + + +class CheckMinSpeed(Criterion): + + """ + Check at which stage of the route is the actor at each tick + + Important parameters: + - actor: CARLA actor to be used for this test + - 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 + MULTIPLIER = 1.5 # % + + def __init__(self, actor, name="CheckMinSpeed", terminate_on_failure=False): + """ + """ + super().__init__(name, actor, terminate_on_failure=terminate_on_failure) + self.units = "%" + self.success_value = 100 + self._world = CarlaDataProvider.get_world() + self._mean_speed = 0 + self._actor_speed = 0 + self._speed_points = 0 + + self._active = True + + self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION) + self.events.append(self._traffic_event) + + def update(self): + """ + Check if the actor location is within trigger region + """ + new_status = py_trees.common.Status.RUNNING + + # Get the actor speed + velocity = CarlaDataProvider.get_velocity(self.actor) + if velocity is None: + return new_status + + set_speed_data = py_trees.blackboard.Blackboard().get('BA_ClearJunction') + if set_speed_data is not None: + self._active = set_speed_data + py_trees.blackboard.Blackboard().set('BA_ClearJunction', None, True) + + if self._active: + # Get the speed of the surrounding Background Activity + all_vehicles = self._world.get_actors().filter('vehicle*') + background_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'background'] + + if background_vehicles: + frame_mean_speed = 0 + for vehicle in background_vehicles: + frame_mean_speed += CarlaDataProvider.get_velocity(vehicle) + frame_mean_speed /= len(background_vehicles) + + # Record the data + self._mean_speed += frame_mean_speed + self._actor_speed += velocity + self._speed_points += 1 + + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + return new_status + + def terminate(self, new_status): + """ + Set the actual value as a percentage of the two mean speeds, + the test status to failure if not successful and terminate + """ + if self._speed_points > 0: + self._mean_speed /= self._speed_points + self._actor_speed /= self._speed_points + self.actual_value = round(self._actor_speed / self._mean_speed * 100, 2) + else: + self.actual_value = 100 + + if self.actual_value >= self.success_value: + self.test_status = "SUCCESS" + else: + self.test_status = "FAILURE" + + self._traffic_event.set_dict({'percentage': self.actual_value}) + self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) + + super().terminate(new_status) diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index 0de653d2a..47df56558 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -30,6 +30,7 @@ class TrafficEventType(Enum): OUTSIDE_LANE_INFRACTION = 11 OUTSIDE_ROUTE_LANES_INFRACTION = 12 VEHICLE_BLOCKED = 13 + MIN_SPEED_INFRACTION = 14 class TrafficEvent(object): diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 4d1423f9c..592a9da09 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -27,7 +27,9 @@ JunctionScenarioManager, ExtentExitRoadSpace, StopEntries, - RemoveLane) + RemoveLane, + RemoveJunctionEntry, + ClearJunction) from srunner.tools.scenario_helper import get_same_dir_lanes def convert_dict_to_location(actor_dict): @@ -75,6 +77,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m + if 'clear_junction' in config.other_parameters: + self._clear_junction = config.other_parameters['clear_junction'] + else: + self._clear_junction = True + super(EnterActorFlow, self).__init__("EnterActorFlow", ego_vehicles, config, @@ -104,7 +111,9 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(JunctionScenarioManager('left', False)) + sequence.add_child(RemoveJunctionEntry(source_wp, True)) + if self._clear_junction: + sequence.add_child(ClearJunction()) grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) @@ -114,6 +123,7 @@ def _create_behavior(self): extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) if current_wp.is_junction: sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='left')) + sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='ref')) break sequence.add_child(SwitchRouteSources(False)) @@ -210,7 +220,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(JunctionScenarioManager('opposite', True)) + sequence.add_child(RemoveJunctionEntry(source_wp, True)) sequence.add_child(StopEntries()) sequence.add_child(root) @@ -221,6 +231,8 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): @@ -390,14 +402,13 @@ def _create_behavior(self): source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) end_condition = py_trees.composites.Sequence() end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) - + root.add_child(end_condition) sequence = py_trees.composites.Sequence() if self.route_mode: sequence.add_child(RemoveLane(source_wp.lane_id)) - sequence.add_child(JunctionScenarioManager('opposite', True)) sequence.add_child(root) return sequence @@ -407,6 +418,8 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 75a079cdd..afa1f663a 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -23,11 +23,27 @@ JUNCTION_ENTRY = 'entry' JUNCTION_MIDDLE = 'middle' JUNCTION_EXIT = 'exit' -JUNCTION_INACTIVE = 'inactive' +JUNCTION_ROAD = 'road' EGO_JUNCTION = 'junction' EGO_ROAD = 'road' +def get_lane_key(waypoint): + """Returns a key corresponding to the waypoint lane. Equivalent to a 'Lane' + object and used to compare waypoint lanes""" + return '' if waypoint is None else get_road_key(waypoint) + '*' + str(waypoint.lane_id) + + +def get_road_key(waypoint): + """Returns a key corresponding to the waypoint road. Equivalent to a 'Road' + object and used to compare waypoint roads""" + return '' if waypoint is None else str(waypoint.road_id) + +def is_lane_at_road(lane_key, road_key): + """Returns whther or not a lane is part of a road""" + return lane_key.startswith(road_key) + +# Debug variables DEBUG_ROAD = 'road' DEBUG_OPPOSITE = 'opposite' DEBUG_JUNCTION = 'junction' @@ -42,7 +58,7 @@ DEBUG_COLORS = { DEBUG_ROAD: carla.Color(0, 0, 255), # Blue DEBUG_OPPOSITE: carla.Color(255, 0, 0), # Red - DEBUG_JUNCTION: carla.Color(0, 0, 0), # Black + DEBUG_JUNCTION: carla.Color(0, 0, 0), # Black DEBUG_ENTRY: carla.Color(255, 255, 0), # Yellow DEBUG_EXIT: carla.Color(0, 255, 255), # Teal DEBUG_CONNECT: carla.Color(0, 255, 0), # Green @@ -52,7 +68,7 @@ DEBUG_SMALL: [0.8, 0.1], DEBUG_MEDIUM: [0.5, 0.15], DEBUG_LARGE: [0.2, 0.2], -} +} # Size, height def draw_string(world, location, string='', debug_type=DEBUG_ROAD, persistent=False): """Utility function to draw debugging strings""" @@ -62,7 +78,6 @@ def draw_string(world, location, string='', debug_type=DEBUG_ROAD, persistent=Fa life_time = 0.07 if not persistent else 100000 world.debug.draw_string(location + l_shift, string, False, color, life_time) - def draw_point(world, location, point_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, persistent=False): """Utility function to draw debugging points""" v_shift, size = DEBUG_TYPE.get(point_type, DEBUG_SMALL) @@ -72,18 +87,6 @@ def draw_point(world, location, point_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, p world.debug.draw_point(location + l_shift, size, color, life_time) -def get_lane_key(waypoint): - """Returns a key corresponding to the waypoint lane. Equivalent to a 'Lane' - object and used to compare waypoint lanes""" - return '' if waypoint is None else get_road_key(waypoint) + '*' + str(waypoint.lane_id) - - -def get_road_key(waypoint): - """Returns a key corresponding to the waypoint road. Equivalent to a 'Road' - object and used to compare waypoint roads""" - return '' if waypoint is None else str(waypoint.road_id) - - class Source(object): """ @@ -115,6 +118,8 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind self.id = junction_id # pylint: disable=invalid-name self.route_entry_index = route_entry_index self.route_exit_index = route_exit_index + self.entry_lane_keys = [] + self.exit_lane_keys = [] self.route_entry_keys = [] self.route_exit_keys = [] self.opposite_entry_keys = [] @@ -137,6 +142,7 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind 'remove_exits': False, } self.stop_entries = False + self.inactive_entry_keys = [] def contains(self, other_junction): """Checks whether or not a carla.Junction is part of the class""" @@ -208,6 +214,8 @@ class BackgroundBehavior(AtomicBehavior): Handles the background activity """ + VELOCITY_MULTIPLIER = 2.5 # TODO: Remove it when the map has speed limits + def __init__(self, ego_actor, route, night_mode=False, debug=False, name="BackgroundBehavior"): """ Setup class members @@ -234,6 +242,9 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._fake_junction_ids = [] self._fake_lane_pair_keys = [] + # Vehicle variables + self._spawn_dist = 15 # Distance between spawned vehicles [m] + # Road variables self._road_dict = {} # Dictionary lane key -> actor source self._road_checker_index = 0 @@ -241,10 +252,7 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._road_front_vehicles = 3 # Amount of vehicles in front of the ego self._road_back_vehicles = 3 # Amount of vehicles behind the ego - self._road_vehicle_dist = 8 # Distance road vehicles leave betweeen each other[m] - self._road_spawn_dist = 11 # Initial distance between spawned road vehicles [m] - self._road_new_sources_dist = 20 # Distance of the source to the start of the new lanes - self._radius_increase_ratio = 1.8 # Meters the radius increases per m/s of the ego + self._radius_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego # TODO: Remove these 3 and refine road radiuses self._extra_radius = 0.0 # Extra distance to avoid the road behavior from blocking. @@ -267,8 +275,6 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._active_junctions = [] self._junction_sources_dist = 40 # Distance from the entry sources to the junction [m] - self._junction_vehicle_dist = 8 # Distance junction vehicles leave betweeen each other[m] - self._junction_spawn_dist = 10 # Initial distance between spawned junction vehicles [m] self._junction_sources_max_actors = 5 # Maximum vehicles alive at the same time per source # Opposite lane variables @@ -278,9 +284,8 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._opposite_removal_dist = 30 # Distance at which actors are destroyed self._opposite_sources_dist = 60 # Distance from the ego to the opposite sources [m] - self._opposite_vehicle_dist = 10 # Distance opposite vehicles leave betweeen each other[m] - self._opposite_spawn_dist = 20 # Initial distance between spawned opposite vehicles [m] self._opposite_sources_max_actors = 8 # Maximum vehicles alive at the same time per source + self._opposite_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego # Scenario variables: self._stopped_road_actors = [] # Actors stopped by a hard break scenario @@ -310,8 +315,8 @@ def _get_road_radius(self): Computes the min and max radius of the road behaviorm which will determine the speed of the vehicles. Vehicles closer than the min radius maintain full speed, while those further than max radius are stopped. Between the two, the velocity decreases linearly""" - self._base_min_radius = (self._road_num_front_vehicles + self._road_extra_front_actors) * self._road_spawn_dist - self._base_max_radius = (self._road_num_front_vehicles + self._road_extra_front_actors + 1) * self._road_spawn_dist + self._base_min_radius = (self._road_num_front_vehicles + self._road_extra_front_actors) * self._spawn_dist + self._base_max_radius = (self._road_num_front_vehicles + self._road_extra_front_actors + 1) * self._spawn_dist self._min_radius = self._base_min_radius self._max_radius = self._base_max_radius @@ -351,7 +356,7 @@ def update(self): else: self._update_road_actors() self._update_road_sources(prev_ego_index) - self._move_road_checker(prev_ego_index) + self._check_lane_merges(prev_ego_index) self._move_opposite_sources(prev_ego_index) self._monitor_road_changes(prev_ego_index) self._update_opposite_sources() @@ -719,6 +724,8 @@ def _add_junctions_topology(self, route_data): junction_data.entry_wps = entry_lane_wps junction_data.exit_wps = exit_lane_wps + junction_data.entry_lane_keys = entry_lane_keys + junction_data.exit_lane_keys = exit_lane_keys for exit_wp in exit_lane_wps: junction_data.exit_dict[get_lane_key(exit_wp)] = { 'actors': [], @@ -845,7 +852,7 @@ def _switch_to_junction_mode(self, junction): # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' self._add_actor_dict_element(junction.actor_dict, actor) if actor not in self._stopped_road_actors: - self._tm.vehicle_percentage_speed_difference(actor, 0) + self._set_actor_speed_percentage(actor, 100) for lane_key in self._road_dict: source = self._road_dict[lane_key] @@ -937,7 +944,7 @@ def _end_junction_behavior(self, junction): # Don't destroy those that are on the route's road opposite lane. # Instead, let them move freely until they are automatically destroyed. - self._tm.vehicle_percentage_speed_difference(actor, 0) + self._set_actor_speed_percentage(actor, 100) if actor_dict[actor]['at_oppo_entry_lane']: self._opposite_actors.append(actor) self._tm.ignore_lights_percentage(actor, 100) @@ -992,7 +999,7 @@ def _initialise_connecting_lanes(self, junction): for actor in list(exit_actors): self._remove_actor_info(actor) self._add_actor_dict_element(junction.actor_dict, actor) - self._tm.vehicle_percentage_speed_difference(actor, 0) + self._set_actor_speed_percentage(actor, 100) def _monitor_incoming_junctions(self): """ @@ -1041,7 +1048,7 @@ def _add_incoming_actors(self, junction, source): if get_lane_key(actor_wp) not in source.previous_lane_keys: continue # Don't use actors that won't pass through the source - self._tm.vehicle_percentage_speed_difference(actor, 0) + self._set_actor_speed_percentage(actor, 100) self._remove_actor_info(actor) source.actors.append(actor) @@ -1062,22 +1069,27 @@ def _update_road_sources(self, prev_ego_index): prev_route_wp = self._route[prev_ego_index] added_dist = route_wp.transform.location.distance(prev_route_wp.transform.location) - min_distance = self._road_back_vehicles * self._road_spawn_dist + min_distance = self._road_back_vehicles * self._spawn_dist for lane_key in self._road_dict: source = self._road_dict[lane_key] - if source.dist_to_ego < min_distance: - source.dist_to_ego += added_dist - continue # Don't move the source that is too close to the ego, - - distance = source.wp.transform.location.distance(source.actors[-1].get_location()) - if distance <= self._road_spawn_dist: - continue # or is it is too close to the last vehicle - added_dist += distance - self._road_spawn_dist # But don't let it stagger behind - new_source_wps = source.wp.next(added_dist) - if not new_source_wps: + last_location = CarlaDataProvider.get_location(source.actors[-1]) + if last_location is None: continue - source.wp = new_source_wps[0] + source_location = source.wp.transform.location + ego_location = self._ego_wp.transform.location + + # Stop the source from being too close to the ego or last lane vehicle + actor_dist = max(0, last_location.distance(source_location) - self._spawn_dist) + ego_dist = max(0, ego_location.distance(source_location) - min_distance) + move_dist = min(actor_dist, ego_dist) + + # Move the source forward if needed + if move_dist > 0: + new_source_wps = source.wp.next(added_dist) + if not new_source_wps: + continue + source.wp = new_source_wps[0] for lane_key in self._road_dict: source = self._road_dict[lane_key] @@ -1111,12 +1123,12 @@ def _update_road_sources(self, prev_ego_index): distance = location.distance(source.wp.transform.location) # Spawn a new actor if the last one is far enough - if distance > self._road_spawn_dist: - actor = self._spawn_source_actor(source, ego_dist=self._road_vehicle_dist) + if distance > self._spawn_dist: + actor = self._spawn_source_actor(source, ego_dist=self._spawn_dist) if actor is None: continue - self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) + self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) source.actors.append(actor) ################################ @@ -1136,7 +1148,7 @@ def _initialise_road_behavior(self, road_wps): # Front spawn points next_wp = wp for _ in range(self._road_front_vehicles): - next_wps = next_wp.next(self._road_spawn_dist) + next_wps = next_wp.next(self._spawn_dist) if len(next_wps) != 1 or self._is_junction(next_wps[0]): break # Stop when there's no next or found a junction next_wp = next_wps[0] @@ -1146,17 +1158,17 @@ def _initialise_road_behavior(self, road_wps): source_dist = 0 prev_wp = wp for _ in range(self._road_back_vehicles): - prev_wps = prev_wp.previous(self._road_spawn_dist) + prev_wps = prev_wp.previous(self._spawn_dist) if len(prev_wps) != 1 or self._is_junction(prev_wps[0]): break # Stop when there's no next or found a junction prev_wp = prev_wps[0] spawn_wps.append(prev_wp) - source_dist += self._road_spawn_dist + source_dist += self._spawn_dist # Spawn actors actors = self._spawn_actors(spawn_wps) for actor in actors: - self._tm.distance_to_leading_vehicle(actor, self._road_vehicle_dist) + self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._road_dict[get_lane_key(wp)] = Source( prev_wp, actors, dist_to_ego=source_dist, active=self._route_sources_active @@ -1175,9 +1187,12 @@ def _initialise_opposite_sources(self): else: next_junction_index = self._junctions[0].route_entry_index + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) + source_dist = self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio + ego_accum_dist = self._accum_dist[self._route_index] for i in range(self._route_index, next_junction_index): - if self._accum_dist[i] - ego_accum_dist > self._opposite_sources_dist: + if self._accum_dist[i] - ego_accum_dist > source_dist: self._opposite_route_index = i break if not self._opposite_route_index: @@ -1236,7 +1251,13 @@ def _initialise_junction_sources(self, junction): prev_wp = prev_wps[0] moved_dist += 5 - junction.entry_sources.append(Source(prev_wp, [], entry_lane_wp=wp)) + source = Source(prev_wp, [], entry_lane_wp=wp) + entry_lane_key = get_lane_key(wp) + if entry_lane_key in junction.inactive_entry_keys: + source.active = False + junction.inactive_entry_keys.remove(entry_lane_key) + junction.entry_sources.append(source) + def _initialise_junction_exits(self, junction): """ @@ -1267,9 +1288,9 @@ def _initialise_junction_exits(self, junction): # Get the moving distance (first jump is higher to allow space for another vehicle) if i == 0: - move_dist = 2 * self._junction_spawn_dist + move_dist = 2 * self._spawn_dist else: - move_dist = self._junction_spawn_dist + move_dist = self._spawn_dist # And move such distance next_wps = next_wp.next(move_dist) @@ -1294,7 +1315,7 @@ def _initialise_junction_exits(self, junction): if exit_lane_key in route_exit_keys: actors = self._spawn_actors(exiting_wps) for actor in actors: - self._tm.distance_to_leading_vehicle(actor, self._junction_vehicle_dist) + self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(junction.actor_dict, actor, exit_lane_key=exit_lane_key) junction.exit_dict[exit_lane_key]['actors'] = actors @@ -1337,37 +1358,33 @@ def _update_junction_sources(self): distance = actor_location.distance(source.wp.transform.location) # Spawn a new actor if the last one is far enough - if distance > self._junction_spawn_dist: + if distance > self._spawn_dist: actor = self._spawn_source_actor(source) if not actor: continue if junction.stop_entries: - self._tm.vehicle_percentage_speed_difference(actor, 100) - self._tm.distance_to_leading_vehicle(actor, self._junction_vehicle_dist) + self._set_actor_speed_percentage(actor, 0) + self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(actor_dict, actor, at_oppo_entry_lane=at_oppo_entry_lane) source.actors.append(actor) - def _found_a_road_change(self, old_index, new_index, ignore_false_junctions=True): + def _found_a_road_change(self, old_index, new_index): """Checks if the new route waypoint is part of a new road (excluding fake junctions)""" if new_index == old_index: return False + # Check if the road has changed (either in its ID or the amount of lanes) new_wp = self._route[new_index] old_wp = self._route[old_index] if get_road_key(new_wp) == get_road_key(old_wp): return False - if ignore_false_junctions: - new_wp_junction = new_wp.get_junction() - if new_wp_junction and new_wp_junction.id in self._fake_junction_ids: - return False - return True - def _move_road_checker(self, prev_index): + def _check_lane_merges(self, prev_index): """ Continually check the road in front to see if it has changed its topology. - If so and the number of lanes have reduced, remove the actor of the lane that merges into others + If the number of lanes have been reduced, remove the actors part of the merging lane. """ if self.debug: checker_wp = self._route[self._road_checker_index] @@ -1388,33 +1405,25 @@ def _move_road_checker(self, prev_index): if not checker_index: checker_index = last_index - if self._found_a_road_change(self._road_checker_index, checker_index): - new_wps = get_same_dir_lanes(self._route[checker_index]) - old_wps = get_same_dir_lanes(self._route[self._road_checker_index]) - - if len(new_wps) >= len(old_wps): - pass - else: - new_accum_dist = self._accum_dist[checker_index] - prev_accum_dist = self._accum_dist[self._road_checker_index] - route_move_dist = new_accum_dist - prev_accum_dist - unmapped_lane_keys = [] + if checker_index == self._road_checker_index: + return - for old_wp in list(old_wps): - location = old_wp.transform.location - mapped_wp = None - for new_wp in new_wps: - if location.distance(new_wp.transform.location) < 1.1 * route_move_dist: - mapped_wp = new_wp - break + new_wps = get_same_dir_lanes(self._route[checker_index]) + old_wps = get_same_dir_lanes(self._route[self._road_checker_index]) - if not mapped_wp: - unmapped_lane_keys.append(get_lane_key(old_wp)) + if len(new_wps) >= len(old_wps): + pass + else: + new_accum_dist = self._accum_dist[checker_index] + prev_accum_dist = self._accum_dist[self._road_checker_index] + route_move_dist = new_accum_dist - prev_accum_dist - for lane in self._road_dict: - if lane in unmapped_lane_keys: - for actor in list(self._road_dict[lane].actors): - self._destroy_actor(actor) + for old_wp in list(old_wps): + next_wps = old_wp.next(route_move_dist) + if not next_wps: + for actor in list(self._road_dict[get_lane_key(old_wp)].actors): + self._destroy_actor(actor) + self._road_dict.pop(get_lane_key(old_wp), None) self._road_checker_index = checker_index @@ -1433,22 +1442,25 @@ def _move_opposite_sources(self, prev_index): if prev_index == self._route_index: return + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) + source_dist = self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio + # Get the new route tracking wp oppo_route_index = None last_index = self._junctions[0].route_entry_index if self._junctions else self._route_length - 1 current_accum_dist = self._accum_dist[self._route_index] for i in range(self._opposite_route_index, last_index): accum_dist = self._accum_dist[i] - if accum_dist - current_accum_dist >= self._opposite_sources_dist: + if accum_dist - current_accum_dist >= source_dist: oppo_route_index = i break if not oppo_route_index: oppo_route_index = last_index - if self._found_a_road_change(self._opposite_route_index, oppo_route_index): - # Recheck the left lanes as the topology might have changed + new_opposite_wps = get_opposite_dir_lanes(self._route[oppo_route_index]) + if len(new_opposite_wps) != len(self._opposite_sources): + # The topology has changed. Remap the new lanes to the sources new_opposite_sources = [] - new_opposite_wps = get_opposite_dir_lanes(self._route[oppo_route_index]) # Map the old sources to the new wps, and add new ones / remove uneeded ones new_accum_dist = self._accum_dist[oppo_route_index] @@ -1464,8 +1476,8 @@ def _move_opposite_sources(self, prev_index): if new_source: new_source.wp = wp - new_opposite_sources.append(source) - self._opposite_sources.remove(source) + new_opposite_sources.append(new_source) + self._opposite_sources.remove(new_source) else: new_opposite_sources.append(Source(wp, [])) @@ -1497,7 +1509,7 @@ def _update_opposite_sources(self): # Calculate distance to the last created actor if len(source.actors) == 0: - distance = self._opposite_spawn_dist + 1 + distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(source.actors[-1]) if not actor_location: @@ -1505,12 +1517,12 @@ def _update_opposite_sources(self): distance = source.wp.transform.location.distance(actor_location) # Spawn a new actor if the last one is far enough - if distance > self._opposite_spawn_dist: + if distance > self._spawn_dist: actor = self._spawn_source_actor(source) if actor is None: continue - self._tm.distance_to_leading_vehicle(actor, self._opposite_vehicle_dist) + self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._opposite_actors.append(actor) source.actors.append(actor) @@ -1530,9 +1542,9 @@ def _update_parameters(self): if num_back_vehicles is not None: self._road_back_vehicles = num_back_vehicles if vehicle_dist is not None: - self._road_vehicle_dist = vehicle_dist + self._spawn_dist = vehicle_dist if spawn_dist is not None: - self._road_spawn_dist = spawn_dist + self._spawn_dist = spawn_dist if sources_active is not None: for lane_key in self._road_dict: self._road_dict[lane_key].active = sources_active @@ -1563,9 +1575,9 @@ def _update_parameters(self): else: self._opposite_sources_dist = source_dist if vehicle_dist is not None: - self._opposite_vehicle_dist = vehicle_dist + self._spawn_dist = vehicle_dist if spawn_dist is not None: - self._opposite_spawn_dist = spawn_dist + self._spawn_dist = spawn_dist if max_actors is not None: self._opposite_sources_max_actors = max_actors py_trees.blackboard.Blackboard().set('BA_ChangeOppositeBehavior', None, True) @@ -1580,9 +1592,9 @@ def _update_parameters(self): else: self._junction_sources_dist = source_dist if vehicle_dist is not None: - self._junction_vehicle_dist = vehicle_dist + self._spawn_dist = vehicle_dist if spawn_dist is not None: - self._junction_spawn_dist = spawn_dist + self._spawn_dist = spawn_dist if max_actors is not None: self._junction_sources_max_actors = max_actors py_trees.blackboard.Blackboard().set('BA_ChangeJunctionBehavior', None, True) @@ -1637,6 +1649,19 @@ def _update_parameters(self): self._remove_lane(remove_lane_data) py_trees.blackboard.Blackboard().set('BA_RemoveLanes', None, True) + # Remove junction entry + remove_junction_entry_data = py_trees.blackboard.Blackboard().get('BA_RemoveJunctionEntry') + if remove_junction_entry_data is not None: + wp, all_entries = remove_junction_entry_data + self._remove_junction_entry(wp, all_entries) + py_trees.blackboard.Blackboard().set('BA_RemoveJunctionEntry', None, True) + + # Clear jucntion + clear_junction_data = py_trees.blackboard.Blackboard().get('BA_ClearJunction') + if clear_junction_data is not None: + self._clear_junction() + py_trees.blackboard.Blackboard().set('BA_ClearJunction', None, True) + self._compute_parameters() def _compute_parameters(self): @@ -1670,7 +1695,7 @@ def _stop_road_front_vehicles(self): location = CarlaDataProvider.get_location(actor) if location and not self._is_location_behind_ego(location): self._stopped_road_actors.append(actor) - self._tm.vehicle_percentage_speed_difference(actor, 100) + self._set_actor_speed_percentage(actor, 0) lights = actor.get_light_state() lights |= carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) @@ -1681,7 +1706,7 @@ def _start_road_front_vehicles(self): wait for a bit, and start moving again. This will never trigger unless done so from outside. """ for actor in self._stopped_road_actors: - self._tm.vehicle_percentage_speed_difference(actor, 0) + self._set_actor_speed_percentage(actor, 100) lights = actor.get_light_state() lights &= ~carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) @@ -1705,8 +1730,8 @@ def _extent_road_exit_space(self, space, direction): actors = exit_dict[exit_lane_key]['actors'] self._move_actors_forward(actors, space) for actor in actors: - if junction.actor_dict[actor]['state'] == JUNCTION_INACTIVE: - self._tm.vehicle_percentage_speed_difference(actor, 0) + if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: + self._set_actor_speed_percentage(actor, 100) junction.actor_dict[actor]['state'] = JUNCTION_EXIT def _move_actors_forward(self, actors, space): @@ -1728,7 +1753,7 @@ def _stop_non_route_entries(self): entry_sources = self._active_junctions[0].entry_sources for source in entry_sources: for actor in source.actors: - self._tm.vehicle_percentage_speed_difference(actor, 100) + self._set_actor_speed_percentage(actor, 0) elif len(self._junctions) > 0: self._junctions[0].stop_entries = True @@ -1750,7 +1775,7 @@ def _handle_lanechange_scenario(self, accident_wp, distance): ## maybe check here to activate lane changing lights self._road_extra_front_actors += 1 else: - self._tm.vehicle_percentage_speed_difference(actor, 100) + self._set_actor_speed_percentage(actor, 0) def _switch_route_sources(self, enabled): """ @@ -1802,7 +1827,54 @@ def _remove_lane(self, lane_id): self._destroy_actor(actor) self._road_dict.pop(lane_key) - + def _remove_junction_entry(self, wp, all_entries): + """Removes a specific entry (or all the entries at the same road) of the closest junction""" + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return + + mapped_wp = None + mapped_dist = float('inf') + ref_loc = wp.transform.location + for entry_wp in junction.entry_wps: + distance = ref_loc.distance(entry_wp.transform.location) + if distance < mapped_dist: + mapped_wp = entry_wp + mapped_dist = distance + + if all_entries: + mapped_road_key = get_road_key(mapped_wp) + mapped_lane_keys = [key for key in junction.entry_lane_keys if is_lane_at_road(key, mapped_road_key)] + else: + mapped_lane_keys = [get_lane_key(mapped_wp)] + + if len(self._active_junctions) > 0: + for source in junction.entry_sources: + if get_lane_key(source.wp) in mapped_lane_keys: + for actor in list(source.actors): + self._destroy_actor(actor) + source.active = False + else: + junction.inactive_entry_keys = mapped_lane_keys + + def _clear_junction(self): + """Clears the junction, and all subsequent actors that enter it""" + if self._active_junctions: + junction = self._active_junctions[0] + actor_dict = junction.actor_dict + for actor in list(actor_dict): + if actor_dict[actor]['state'] == JUNCTION_MIDDLE: + self._destroy_actor(actor) + + junction.scenario_info['remove_middle'] = True + + elif self._junctions: + self._junctions[0].scenario_info['remove_middle'] = True + + ############################# ## Actor functions ## ############################# @@ -1825,6 +1897,7 @@ def _spawn_actors(self, spawn_wps): for actor in actors: self._tm.auto_lane_change(actor, False) + self._set_actor_speed_percentage(actor, 100) if self._night_mode: for actor in actors: @@ -1852,6 +1925,7 @@ def _spawn_source_actor(self, source, ego_dist=0): return actor self._tm.auto_lane_change(actor, False) + self._set_actor_speed_percentage(actor, 100) if self._night_mode: actor.set_light_state(carla.VehicleLightState( carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam)) @@ -1898,7 +1972,6 @@ def _update_road_actors(self): Not applied to those behind it so that they can catch up it """ # Updates their speed - route_wp = self._route[self._route_index] scenario_actors = self._stopped_road_actors for lane in self._road_dict: for i, actor in enumerate(self._road_dict[lane].actors): @@ -1910,15 +1983,13 @@ def _update_road_actors(self): string += 'B' if self._is_location_behind_ego(location) else 'F' string += '(' + str(i) + ')' draw_string(self._world, location, string, DEBUG_ROAD, False) + if actor in scenario_actors or self._is_location_behind_ego(location): continue - - distance = location.distance(route_wp.transform.location) - speed_red = (distance - self._min_radius) / (self._max_radius - self._min_radius) * 100 - speed_red = np.clip(speed_red, 0, 100) - self._tm.vehicle_percentage_speed_difference(actor, speed_red) + self._set_road_actor_speed(location, actor) if not self._road_ego_key in self._road_dict: + self._road_num_front_vehicles = self._road_front_vehicles return front_actors = 0 @@ -1930,50 +2001,50 @@ def _update_road_actors(self): self._get_road_radius() self._compute_parameters() + def _set_road_actor_speed(self, location, actor, multiplier=1): + """Changes the speed of the vehicle depending on its distance to the ego""" + distance = location.distance(self._ego_wp.transform.location) + percentage = (self._max_radius - distance) / (self._max_radius - self._min_radius) * 100 + percentage *= multiplier + percentage = min(percentage, 100) + self._set_actor_speed_percentage(actor, percentage) + def _monitor_road_changes(self, prev_route_index): """ - Checks if the ego changes road, remapping the route keys, and creating new sources (if needed) + Checks if the ego changes road, remapping the route keys. """ + def get_lane_waypoints(reference_wp, index=0): + if not reference_wp.is_junction: + wps = get_same_dir_lanes(reference_wp) + else: # Handle fake junction by using its entry / exit wps + wps = [] + for junction_wps in reference_wp.get_junction().get_waypoints(carla.LaneType.Driving): + if get_road_key(junction_wps[index]) == get_road_key(reference_wp): + wps.append(junction_wps[index]) + return wps + route_wp = self._route[ self._route_index] prev_route_wp = self._route[prev_route_index] check_dist = 1.1 * route_wp.transform.location.distance(prev_route_wp.transform.location) if prev_route_index != self._route_index: - road_change = self._found_a_road_change(prev_route_index, self._route_index, ignore_false_junctions=False) + road_change = self._found_a_road_change(prev_route_index, self._route_index) + if not self._is_junction(prev_route_wp) and road_change: - # Get all the wps of the new road - if not route_wp.is_junction: - new_wps = get_same_dir_lanes(route_wp) - else: # Entering a false junction - new_wps = [] - for enter_wp, _ in route_wp.get_junction().get_waypoints(carla.LaneType.Driving): - if get_road_key(enter_wp) == get_road_key(route_wp): - new_wps.append(enter_wp) - - # Get all the wps of the old road - if not prev_route_wp.is_junction: - old_wps = get_same_dir_lanes(prev_route_wp) - else: # Exitting a false junction - old_wps = [] - for _, exit_wp in prev_route_wp.get_junction().get_waypoints(carla.LaneType.Driving): - if get_road_key(exit_wp) == get_road_key(prev_route_wp): - old_wps.append(exit_wp) + old_wps = get_lane_waypoints(prev_route_wp, 1) + new_wps = get_lane_waypoints(route_wp, 0) # Map the new lanes to the old ones mapped_keys = {} + unmapped_wps = new_wps - for old_wp in list(old_wps): + for old_wp in old_wps: location = old_wp.transform.location - mapped_wp = None for new_wp in unmapped_wps: if location.distance(new_wp.transform.location) < check_dist: - mapped_wp = new_wp + unmapped_wps.remove(new_wp) + mapped_keys[get_lane_key(old_wp)] = get_lane_key(new_wp) break - if mapped_wp: - unmapped_wps.remove(mapped_wp) - old_wps.remove(old_wp) - mapped_keys[get_lane_key(old_wp)] = get_lane_key(mapped_wp) - # Remake the road back actors dictionary new_road_dict = {} for lane_key in self._road_dict: @@ -1984,14 +2055,6 @@ def _monitor_road_changes(self, prev_route_index): self._road_dict = new_road_dict - # New lanes, add new sources - for unmapped_wp in unmapped_wps: - source_wps = unmapped_wp.next(self._road_new_sources_dist) - if len(source_wps) != 1: - continue - new_lane_wp = source_wps[0] - self._road_dict[get_lane_key(new_lane_wp)] = Source(new_lane_wp, []) - if not self._road_ego_key in mapped_keys: # Return the default. This might happen when the route lane ends and should be fixed next frame self._road_ego_key = get_lane_key(route_wp) @@ -2073,18 +2136,18 @@ def _update_junction_actors(self): actors = exit_dict[actor_lane_key]['actors'] if len(actors) > 0 and len(actors) >= exit_dict[actor_lane_key]['max_actors']: - self._destroy_actor(actors[-1]) # This is always the front most vehicle + self._destroy_actor(actors[0]) # This is always the front most vehicle actors.append(actor) - # Deactivate them when far from the junction + # Change them to "road mode" when far enough from the junction elif state == JUNCTION_EXIT: distance = location.distance(exit_dict[exit_lane_key]['ref_wp'].transform.location) if distance > exit_dict[exit_lane_key]['max_distance']: - self._tm.vehicle_percentage_speed_difference(actor, 100) - actor_dict[actor]['state'] = JUNCTION_INACTIVE + actor_dict[actor]['state'] = JUNCTION_ROAD - # Wait for something to happen - elif state == JUNCTION_INACTIVE: + # Set them ready to move so that the ego can smoothly cross the junction + elif state == JUNCTION_ROAD: + self._set_road_actor_speed(location, actor, multiplier=1.5) pass def _update_opposite_actors(self): @@ -2092,7 +2155,8 @@ def _update_opposite_actors(self): Updates the opposite actors. This involves tracking their position, removing them if too far behind the ego. """ - max_dist = max(self._opposite_removal_dist, self._opposite_spawn_dist) + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) + max_dist = max(self._opposite_removal_dist + ego_speed * self._opposite_increase_ratio, self._spawn_dist) for actor in list(self._opposite_actors): location = CarlaDataProvider.get_location(actor) if not location: @@ -2103,6 +2167,12 @@ def _update_opposite_actors(self): if distance > max_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) + def _set_actor_speed_percentage(self, actor, percentage): + percentage *= self.VELOCITY_MULTIPLIER + speed_red = (100 - percentage) + speed_red = min(speed_red, 100) + self._tm.vehicle_percentage_speed_difference(actor, speed_red) + def _remove_actor_info(self, actor): """Removes all the references of the actor""" @@ -2138,6 +2208,7 @@ def _remove_actor_info(self, actor): def _destroy_actor(self, actor): """Destroy the actor and all its references""" self._remove_actor_info(actor) + actor.set_autopilot(False) try: actor.destroy() except RuntimeError: diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 99231cc8b..938411249 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -35,7 +35,8 @@ OutsideRouteLanesTest, RunningRedLightTest, RunningStopTest, - ActorBlockedTest) + ActorBlockedTest, + CheckMinSpeed) from srunner.scenarios.basic_scenario import BasicScenario from srunner.scenarios.background_activity import BackgroundActivity @@ -307,6 +308,7 @@ def _create_test_criteria(self): criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='walker', name="CollisionPedestrianTest")) criteria.add_child(RunningRedLightTest(self.ego_vehicles[0])) criteria.add_child(RunningStopTest(self.ego_vehicles[0])) + criteria.add_child(CheckMinSpeed(self.ego_vehicles[0], name="MinSpeedTest")) # These stop the route early to save computational time criteria.add_child(InRouteTest( diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index c890aa47d..a668c97a3 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -236,6 +236,7 @@ def update(self): py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", True, overwrite=True) return py_trees.common.Status.SUCCESS + class RemoveLane(AtomicBehavior): """ Updates the blackboard to tell the background activity to remove its actors from the given lane, @@ -252,4 +253,36 @@ def update(self): """Updates the blackboard and succeds""" py_trees.blackboard.Blackboard().set("BA_RemoveLane", self._lane, overwrite=True) return py_trees.common.Status.SUCCESS - \ No newline at end of file + + +class RemoveJunctionEntry(AtomicBehavior): + """ + Updates the blackboard to tell the background activity to remove its actors from the given lane, + and stop generating new ones on this lane. + + Args: + wp (carla.Waypoint): A waypoint used as reference to the entry lane + all_road_entries (bool): Boolean to remove all entries part of the same road, or just one + """ + def __init__(self, wp, all_road_entries=False, name="RemoveJunctionEntry"): + self._wp = wp + self._all_road_entries = all_road_entries + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_RemoveJunctionEntry", [self._wp, self._all_road_entries], overwrite=True) + return py_trees.common.Status.SUCCESS + + +class ClearJunction(AtomicBehavior): + """ + """ + + def __init__(self, name="ClearJunction"): + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_ClearJunction", True, overwrite=True) + return py_trees.common.Status.SUCCESS diff --git a/srunner/tools/route_manipulation.py b/srunner/tools/route_manipulation.py index b7207c836..9efd4eb6b 100644 --- a/srunner/tools/route_manipulation.py +++ b/srunner/tools/route_manipulation.py @@ -134,7 +134,7 @@ def interpolate_trajectory(waypoints_trajectory, hop_resolution=1.0): """ Given some raw keypoints interpolate a full dense trajectory to be used by the user. returns the full interpolated route both in GPS coordinates and also in its original form. - + Args: - waypoints_trajectory: the current coarse trajectory - hop_resolution: distance between the trajectory's waypoints @@ -142,15 +142,19 @@ def interpolate_trajectory(waypoints_trajectory, hop_resolution=1.0): grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), hop_resolution) # Obtain route plan + lat_ref, lon_ref = _get_latlon_ref(CarlaDataProvider.get_world()) + route = [] + gps_route = [] + for i in range(len(waypoints_trajectory) - 1): waypoint = waypoints_trajectory[i] waypoint_next = waypoints_trajectory[i + 1] interpolated_trace = grp.trace_route(waypoint, waypoint_next) - for wp_tuple in interpolated_trace: - route.append((wp_tuple[0].transform, wp_tuple[1])) - - lat_ref, lon_ref = _get_latlon_ref(CarlaDataProvider.get_world()) + for wp, connection in interpolated_trace: + route.append((wp.transform, connection)) + gps_coord = _location_to_gps(lat_ref, lon_ref, wp.transform.location) + gps_route.append((gps_coord, connection)) - return location_route_to_gps(route, lat_ref, lon_ref), route + return gps_route, route \ No newline at end of file From e42c9501f6f95887ec4ea8a60b6b71e18edc69e6 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 25 Apr 2022 15:36:48 +0200 Subject: [PATCH 16/86] Pretty code --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 2 +- srunner/tools/route_manipulation.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 7893c23dd..23e351265 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3251,4 +3251,4 @@ def update(self): """ new_status = py_trees.common.Status.SUCCESS py_trees.blackboard.Blackboard().set("SwitchMinSpeedCriteria", self._active, overwrite=True) - return new_status \ No newline at end of file + return new_status diff --git a/srunner/tools/route_manipulation.py b/srunner/tools/route_manipulation.py index 9efd4eb6b..c657c1b0f 100644 --- a/srunner/tools/route_manipulation.py +++ b/srunner/tools/route_manipulation.py @@ -157,4 +157,4 @@ def interpolate_trajectory(waypoints_trajectory, hop_resolution=1.0): gps_coord = _location_to_gps(lat_ref, lon_ref, wp.transform.location) gps_route.append((gps_coord, connection)) - return gps_route, route \ No newline at end of file + return gps_route, route From 1d6138b66bf6284b6b3a6e363a52ebf4a4861691 Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Wed, 27 Apr 2022 22:41:34 +0800 Subject: [PATCH 17/86] New Parking Exit scenario --- srunner/routes/ParkingExit.xml | 20 ++ .../scenarioatomics/atomic_behaviors.py | 20 ++ .../scenarioatomics/atomic_criteria.py | 13 +- srunner/scenarios/parking_exit.py | 174 ++++++++++++++++++ 4 files changed, 224 insertions(+), 3 deletions(-) create mode 100644 srunner/routes/ParkingExit.xml create mode 100644 srunner/scenarios/parking_exit.py diff --git a/srunner/routes/ParkingExit.xml b/srunner/routes/ParkingExit.xml new file mode 100644 index 000000000..c8f5057dc --- /dev/null +++ b/srunner/routes/ParkingExit.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 23e351265..71a171849 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3234,6 +3234,26 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status +class SwitchOutsideRouteLanesTest(AtomicBehavior): + + """ + Atomic that switch the OutsideRouteLanesTest criterion. + + Args: + activate (bool): True: activate; False: deactivate + name (str): name of the behavior + """ + + def __init__(self, activate, name="SwitchOutsideRouteLanesTest"): + """ + Setup class members + """ + super(SwitchOutsideRouteLanesTest, self).__init__(name) + self._activate = activate + + def update(self): + py_trees.blackboard.Blackboard().set("AC_SwitchOutsideRouteLanesTest", self._activate, overwrite=True) + return py_trees.common.Status.SUCCESS class SwitchMinSpeedCriteria(AtomicBehavior): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 10551baa4..21e5a466f 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1003,6 +1003,7 @@ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): self._last_lane_id = None self._total_distance = 0 self._wrong_distance = 0 + self._active = True def update(self): """ @@ -1020,11 +1021,16 @@ def update(self): if location is None: return new_status - # Check if outside route lanes + # Deactivate/Activate checking by blackboard message + active = py_trees.blackboard.Blackboard().get('AC_SwitchOutsideRouteLanesTest') + if active is not None: + self._active = active + py_trees.blackboard.Blackboard().set("AC_SwitchOutsideRouteLanesTest", None, overwrite=True) + self._is_outside_driving_lanes(location) self._is_at_wrong_lane(location) - if self._outside_lane_active or self._wrong_lane_active: + if self._active and (self._outside_lane_active or self._wrong_lane_active): self.test_status = "FAILURE" # Get the traveled distance @@ -1044,11 +1050,12 @@ def update(self): self._total_distance += new_dist # And to the wrong one if outside route lanes - if self._outside_lane_active or self._wrong_lane_active: + if self._active and (self._outside_lane_active or self._wrong_lane_active): self._wrong_distance += new_dist self._current_index = index + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py new file mode 100644 index 000000000..f860762fd --- /dev/null +++ b/srunner/scenarios/parking_exit.py @@ -0,0 +1,174 @@ +#!/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 . + +""" +Scenario in which the ego is parked between two vehicles and has to maneuver to start the route. +""" + +from __future__ import print_function + +import py_trees +import carla + + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import SwitchOutsideRouteLanesTest, ActorTransformSetter, ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenarios.basic_scenario import BasicScenario + + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + + +class ParkingExit(BasicScenario): + """ + This class holds everything required for a scenario in which the ego would be teleported to the parking lane. + Once the scenario is triggered, the OutsideRouteLanesTest will be deactivated since the ego is out of the driving lane. + Then blocking vehicles will be generated in front of and behind the parking point. + The ego need to exit from the parking lane and then merge into the driving lane. + After the ego is {end_distance} meters away from the parking point, the OutsideRouteLanesTest will be activated and the scenario ends. + + Note 1: For route mode, this shall be the first scenario of the route. The trigger point shall be the first point of the route waypoints. + + Note 2: Make sure there are enough space for spawning blocking vehicles. + """ + + def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, + timeout=180): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + if 'front_vehicle_distance' in config.other_parameters: + self._front_vehicle_distance = float( + config.other_parameters['front_vehicle_distance']['value']) + else: + self._front_vehicle_distance = 20 # m + + if 'behind_vehicle_distance' in config.other_parameters: + self._behind_vehicle_distance = float( + config.other_parameters['behind_vehicle_distance']['value']) + else: + self._behind_vehicle_distance = 5 # m + + self._end_distance = self._front_vehicle_distance + 15 + + if 'parking_lane_side' in config.other_parameters: + self._parking_lane_side = config.other_parameters['parking_lane_side']['value'] + else: + self._parking_lane_side = "right" + + # Get parking_waypoint based on trigger_point + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._map.get_waypoint(self._trigger_location) + if self._parking_lane_side == "left": + self._parking_waypoint = self._reference_waypoint.get_left_lane() + else: + self._parking_waypoint = self._reference_waypoint.get_right_lane() + + if self._parking_waypoint is None: + raise Exception( + "Couldn't find parking point on the {} side".format(self._parking_lane_side)) + + super(ParkingExit, self).__init__("ParkingExit", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Put blocking vehicles + front_points = self._parking_waypoint.next( + self._front_vehicle_distance) + if front_points: + actor_front = CarlaDataProvider.request_new_actor( + 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + if actor_front is None: + raise Exception( + "Couldn't spawn the vehicle in front of the parking point") + self.other_actors.append(actor_front) + else: + raise Exception( + "Couldn't find viable position for the vehicle in front of the parking point") + + behind_points = self._parking_waypoint.previous( + self._behind_vehicle_distance) + if behind_points: + actor_behind = CarlaDataProvider.request_new_actor( + 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + if actor_behind is None: + raise Exception( + "Couldn't spawn the vehicle behind the parking point") + self.other_actors.append(actor_behind) + else: + raise Exception( + "Couldn't find viable position for the vehicle behind the parking point") + + def _create_behavior(self): + """ + Deactivate OutsideRouteLanesTest, then move ego to the parking point, generate blocking vehicles in front of and behind the ego. + After ego drives away, actviate OutsideRouteLanesTest, end scenario. + """ + + sequence = py_trees.composites.Sequence() + + # Deactivate OutsideRouteLanesTest + sequence.add_child(SwitchOutsideRouteLanesTest(False)) + + # Teleport ego to the parking point + sequence.add_child(ActorTransformSetter( + self.ego_vehicles[0], self._parking_waypoint.transform)) + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + + end_condition = py_trees.composites.Sequence() + end_condition.add_child(DriveDistance( + self.ego_vehicles[0], self._end_distance)) + root.add_child(end_condition) + sequence.add_child(root) + + sequence.add_child(SwitchOutsideRouteLanesTest(True)) + + for actor in self.other_actors: + sequence.add_child(ActorDestroy(actor)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return[] + + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() From 5835f0976544fc6152eaa5aac19f47f3f5b8edd6 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 2 May 2022 08:59:49 +0200 Subject: [PATCH 18/86] General improvements (#884) Co-authored-by: Guillermo --- srunner/scenarios/actor_flow.py | 11 +- srunner/scenarios/background_activity.py | 162 ++++++++++------------- srunner/scenarios/vehicle_opens_door.py | 26 ++-- srunner/tools/background_manager.py | 55 ++++---- 4 files changed, 113 insertions(+), 141 deletions(-) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 592a9da09..d77b82875 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -77,11 +77,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m - if 'clear_junction' in config.other_parameters: - self._clear_junction = config.other_parameters['clear_junction'] - else: - self._clear_junction = True - super(EnterActorFlow, self).__init__("EnterActorFlow", ego_vehicles, config, @@ -112,8 +107,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: sequence.add_child(RemoveJunctionEntry(source_wp, True)) - if self._clear_junction: - sequence.add_child(ClearJunction()) + sequence.add_child(ClearJunction()) grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) @@ -122,8 +116,7 @@ def _create_behavior(self): current_wp = route[i][0] extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) if current_wp.is_junction: - sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='left')) - sequence.add_child(ExtentExitRoadSpace(distance=extra_space, direction='ref')) + sequence.add_child(ExtentExitRoadSpace(extra_space)) break sequence.add_child(SwitchRouteSources(False)) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index afa1f663a..bf1ce6422 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -254,11 +254,6 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._road_back_vehicles = 3 # Amount of vehicles behind the ego self._radius_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego - # TODO: Remove these 3 and refine road radiuses - self._extra_radius = 0.0 # Extra distance to avoid the road behavior from blocking. - self._extra_radius_increase_ratio = 0.5 * timestep # Distance the radius increases per tick (0.5 m/s) - self._max_extra_radius = 10 # Max extra distance - self._road_num_front_vehicles = self._road_front_vehicles # Checks the real amount of actors in the front of the ego self._road_extra_front_actors = 0 # For cases where we want more space but not more vehicles self._road_time_delay = 3 @@ -271,8 +266,8 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._get_road_radius() # Junction variables - self._junctions = [] - self._active_junctions = [] + self._junctions = [] # List with all the junctions part of the route, in order of appearance + self._active_junctions = [] # List of all the active junctions self._junction_sources_dist = 40 # Distance from the entry sources to the junction [m] self._junction_sources_max_actors = 5 # Maximum vehicles alive at the same time per source @@ -1532,52 +1527,33 @@ def _update_parameters(self): This is done using py_trees' Blackboard variables and all behaviors should be at `background_manager.py`. The blackboard variable is reset to None to avoid changing them back again next time. """ + # General behavior + general_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeGeneralBehavior') + if general_behavior_data is not None: + self._spawn_dist = general_behavior_data + self._get_road_radius() + py_trees.blackboard.Blackboard().set('BA_ChangeGeneralBehavior', None, True) # Road behavior road_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeRoadBehavior') if road_behavior_data is not None: - num_front_vehicles, num_back_vehicles, vehicle_dist, spawn_dist, sources_active = road_behavior_data + num_front_vehicles, num_back_vehicles = road_behavior_data if num_front_vehicles is not None: self._road_front_vehicles = num_front_vehicles if num_back_vehicles is not None: self._road_back_vehicles = num_back_vehicles - if vehicle_dist is not None: - self._spawn_dist = vehicle_dist - if spawn_dist is not None: - self._spawn_dist = spawn_dist - if sources_active is not None: - for lane_key in self._road_dict: - self._road_dict[lane_key].active = sources_active - self._get_road_radius() py_trees.blackboard.Blackboard().set('BA_ChangeRoadBehavior', None, True) - # Extend the space of a specific exit lane - road_exit_data = py_trees.blackboard.Blackboard().get('BA_ExtentExitRoadSpace') - if road_exit_data is not None: - space, direction = road_exit_data - self._extent_road_exit_space(space, direction) - py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", None, True) - - # Switch route sources - switch_sources_data = py_trees.blackboard.Blackboard().get('BA_SwitchRouteSources') - if switch_sources_data is not None: - self._switch_route_sources(switch_sources_data) - py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", None, True) - # Opposite behavior opposite_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeOppositeBehavior') if opposite_behavior_data is not None: - source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data + source_dist, max_actors = road_behavior_data if source_dist is not None: if source_dist < self._junction_sources_dist: print('WARNING: Opposite sources distance is lower than the junction ones. Ignoring it') else: self._opposite_sources_dist = source_dist - if vehicle_dist is not None: - self._spawn_dist = vehicle_dist - if spawn_dist is not None: - self._spawn_dist = spawn_dist if max_actors is not None: self._opposite_sources_max_actors = max_actors py_trees.blackboard.Blackboard().set('BA_ChangeOppositeBehavior', None, True) @@ -1585,20 +1561,28 @@ def _update_parameters(self): # Junction behavior junction_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeJunctionBehavior') if junction_behavior_data is not None: - source_dist, vehicle_dist, spawn_dist, max_actors = road_behavior_data + source_dist, max_actors = road_behavior_data if source_dist is not None: if source_dist > self._opposite_sources_dist: print('WARNING: Junction sources distance is higher than the opposite ones. Ignoring it') else: self._junction_sources_dist = source_dist - if vehicle_dist is not None: - self._spawn_dist = vehicle_dist - if spawn_dist is not None: - self._spawn_dist = spawn_dist if max_actors is not None: self._junction_sources_max_actors = max_actors py_trees.blackboard.Blackboard().set('BA_ChangeJunctionBehavior', None, True) + # Extend the space of a specific exit lane + road_exit_data = py_trees.blackboard.Blackboard().get('BA_ExtentExitRoadSpace') + if road_exit_data is not None: + self._extent_road_exit_space(road_exit_data) + py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", None, True) + + # Switch route sources + switch_sources_data = py_trees.blackboard.Blackboard().get('BA_SwitchRouteSources') + if switch_sources_data is not None: + self._switch_route_sources(switch_sources_data) + py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", None, True) + # Stop front vehicles stop_data = py_trees.blackboard.Blackboard().get('BA_StopFrontVehicles') if stop_data is not None: @@ -1656,7 +1640,7 @@ def _update_parameters(self): self._remove_junction_entry(wp, all_entries) py_trees.blackboard.Blackboard().set('BA_RemoveJunctionEntry', None, True) - # Clear jucntion + # Clear junction clear_junction_data = py_trees.blackboard.Blackboard().get('BA_ClearJunction') if clear_junction_data is not None: self._clear_junction() @@ -1667,22 +1651,8 @@ def _update_parameters(self): def _compute_parameters(self): """Computes the parameters that are dependent on the speed of the ego. """ ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - - # As the vehicles don't move if the agent doesn't, some agents might get blocked forever. - # Partially avoid this by adding an extra distance to the radius when the vehicle is stopped - # in the middle of the road and unaffected by any object such as traffic lights or stops. - if ego_speed == 0 \ - and len(self._stopped_road_actors) == 0 \ - and not self._ego_actor.is_at_traffic_light() \ - and len(self._active_junctions) <= 0: - self._extra_radius = min(self._extra_radius + self._extra_radius_increase_ratio, self._max_extra_radius) - - # At all cases, reduce it if the agent is moving - if ego_speed > 0 and self._extra_radius > 0: - self._extra_radius = max(self._extra_radius - self._extra_radius_increase_ratio, 0) - - self._min_radius = self._base_min_radius + self._radius_increase_ratio * ego_speed + self._extra_radius - self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed + self._extra_radius + self._min_radius = self._base_min_radius + self._radius_increase_ratio * ego_speed + self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed self._junction_detection_dist = self._max_radius def _stop_road_front_vehicles(self): @@ -1712,7 +1682,7 @@ def _start_road_front_vehicles(self): actor.set_light_state(carla.VehicleLightState(lights)) self._stopped_road_actors = [] - def _extent_road_exit_space(self, space, direction): + def _extent_road_exit_space(self, space): """Increases the space left by the exit vehicles at a specific road""" if len(self._active_junctions) > 0: junction = self._active_junctions[0] @@ -1721,22 +1691,20 @@ def _extent_road_exit_space(self, space, direction): else: return - direction_lane_keys = junction.exit_directions[direction] - exit_dict = junction.exit_dict - for exit_lane_key in exit_dict: - if exit_lane_key in direction_lane_keys: - exit_dict[exit_lane_key]['max_distance'] += space + route_lane_keys = junction.route_exit_keys - actors = exit_dict[exit_lane_key]['actors'] - self._move_actors_forward(actors, space) - for actor in actors: - if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: - self._set_actor_speed_percentage(actor, 100) - junction.actor_dict[actor]['state'] = JUNCTION_EXIT + for exit_lane_key in route_lane_keys: + junction.exit_dict[exit_lane_key]['max_distance'] += space + actors = junction.exit_dict[exit_lane_key]['actors'] + self._move_actors_forward(actors, space) + for actor in actors: + if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: + self._set_actor_speed_percentage(actor, 100) + junction.actor_dict[actor]['state'] = JUNCTION_EXIT def _move_actors_forward(self, actors, space): """Teleports the actors forward a set distance""" - for actor in actors: + for actor in list(actors): location = CarlaDataProvider.get_location(actor) if not location: continue @@ -1747,6 +1715,8 @@ def _move_actors_forward(self, actors, space): new_transform = new_actor_wps[0].transform new_transform.location.z += 0.2 actor.set_transform(new_transform) + else: + self._destroy_actor(actor) def _stop_non_route_entries(self): if len(self._active_junctions) > 0: @@ -1792,29 +1762,41 @@ def _switch_route_sources(self, enabled): def _leave_space_in_front(self, space): """Teleports all the vehicles in front of the ego forward""" - all_actors = [] + # all_actors = [] if not self._active_junctions: for lane in self._road_dict: - all_actors.extend(self._road_dict[lane].actors) - - else: - junction = self._active_junctions[0] - if self._is_junction(self._ego_wp): # We care about the exit - for actor, info in junction.actor_dict.items(): - if info['exit_lane_key'] in junction.route_exit_keys: - all_actors.append(actor) - else: # We care about the entry - for source in junction.entry_sources: - if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: - all_actors.extend(source.actors) - - front_actors = [] - for actor in all_actors: - location = CarlaDataProvider.get_location(actor) - if location and not self._is_location_behind_ego(location): - front_actors.append(actor) - - self._move_actors_forward(front_actors, space) + lane_actors = self._road_dict[lane].actors + front_actors = [] + for actor in lane_actors: + location = CarlaDataProvider.get_location(actor) + if location and not self._is_location_behind_ego(location): + front_actors.append(actor) + + last_actor_location = CarlaDataProvider.get_location(front_actors[-1]) + distance = last_actor_location.distance(self._ego_wp.transform.location) + step = space - distance + if step > 0: # Only move the needed distance + self._move_actors_forward(front_actors, step) + + # else: + # junction = self._active_junctions[0] + # if self._is_junction(self._ego_wp): # We care about the exit + # for actor, info in junction.actor_dict.items(): + # if info['exit_lane_key'] in junction.route_exit_keys: + # all_actors.append(actor) + # else: # We care about the entry + # for source in junction.entry_sources: + # if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: + # all_actors.extend(source.actors) + + # front_actors = [] + # for actor in all_actors: + # location = CarlaDataProvider.get_location(actor) + # if location and not self._is_location_behind_ego(location): + # front_actors.append(actor) + + + # self._move_actors_forward(front_actors, space) def _remove_lane(self, lane_id): """Remove BA actors from this lane, and BA would never generate new actors on this lane""" diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index ebe8d1713..baded66f3 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -18,7 +18,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import OpenVehicleDoor +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import OpenVehicleDoor, HandBrakeVehicle from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, DriveDistance) @@ -45,11 +45,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._parked_distance = 20 self._wait_duration = 15 self._end_distance = 40 self._min_trigger_dist = 5 - self._reaction_time = 1.5 + self._reaction_time = 2.0 + + if 'distance' in config.other_parameters: + self._parked_distance = config.other_parameters['distance']['value'] + else: + self._parked_distance = 50 if 'direction' in config.other_parameters: self._direction = config.other_parameters['direction']['value'] @@ -82,26 +86,18 @@ def _initialize_actors(self, config): raise ValueError("Couldn't find a spot to place the adversary vehicle") self.parked_actor = CarlaDataProvider.request_new_actor( - "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True}) + "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) + if not self.parked_actor: + raise ValueError("Couldn't spawn the parked vehicle") self.other_actors.append(self.parked_actor) - # Move the actor to the edge of the lane, or the open door might not reach the ego vehicle - displacement = (parked_wp.lane_width - self.parked_actor.bounding_box.extent.y) / 2 - displacement_vector = parked_wp.transform.get_right_vector() - if self._direction == 'right': - displacement_vector *= -1 - - new_location = parked_wp.transform.location + carla.Location(x=displacement*displacement_vector.x, - y=displacement*displacement_vector.y) - self.parked_actor.set_location(new_location) - - def _create_behavior(self): """ Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ sequence = py_trees.composites.Sequence(name="CrossingActor") + sequence.add_child(HandBrakeVehicle(self.parked_actor, True)) if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._parked_distance)) diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index a668c97a3..e8877d80d 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -13,6 +13,28 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior +class ChangeGeneralBehavior(AtomicBehavior): + """ + Updates the blackboard to change the general parameters. + None values imply that these values won't be changed. + + Args: + spawn_dist (float): Minimum distance between spawned vehicles. Must be positive + target_speed (float): Target speed of all BA vehicles + """ + + def __init__(self, spawn_dist=None, target_speed=None, name="ChangeGeneralBehavior"): + self._spawn_dist = spawn_dist + self._target_speed = target_speed + super().__init__(name) + + def update(self): + py_trees.blackboard.Blackboard().set( + "BA_ChangeGeneralBehavior", [self._spawn_dist, self._target_speed], overwrite=True + ) + return py_trees.common.Status.SUCCESS + + class ChangeRoadBehavior(AtomicBehavior): """ Updates the blackboard to change the parameters of the road behavior. @@ -21,24 +43,17 @@ class ChangeRoadBehavior(AtomicBehavior): Args: num_front_vehicles (int): Amount of vehicles in front of the ego. Can't be negative num_back_vehicles (int): Amount of vehicles behind it. Can't be negative - vehicle_dist (float): Minimum distance between the road vehicles. Must between 0 and 'spawn_dist' - spawn_dist (float): Minimum distance between spawned vehicles. Must be positive switch_source (bool): (De)activatea the road sources. """ - def __init__(self, num_front_vehicles=None, num_back_vehicles=None, - vehicle_dist=None, spawn_dist=None, name="ChangeRoadBehavior"): + def __init__(self, num_front_vehicles=None, num_back_vehicles=None, name="ChangeRoadBehavior"): self._num_front_vehicles = num_front_vehicles self._num_back_vehicles = num_back_vehicles - self._vehicle_dist = vehicle_dist - self._spawn_dist = spawn_dist super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeRoadBehavior", - [self._num_front_vehicles, self._num_back_vehicles, self._vehicle_dist, self._spawn_dist], - overwrite=True + "BA_ChangeRoadBehavior", [self._num_front_vehicles, self._num_back_vehicles], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -50,24 +65,18 @@ class ChangeOppositeBehavior(AtomicBehavior): Args: source_dist (float): Distance between the opposite sources and the ego vehicle. Must be positive - vehicle_dist (float) Minimum distance between the opposite vehicles. Must between 0 and 'spawn_dist' - spawn_dist (float): Minimum distance between spawned vehicles. Must be positive max_actors (int): Max amount of concurrent alive actors spawned by the same source. Can't be negative """ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, max_actors=None, name="ChangeOppositeBehavior"): self._source_dist = source_dist - self._vehicle_dist = vehicle_dist - self._spawn_dist = spawn_dist self._max_actors = max_actors super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", - [self._source_dist, self._vehicle_dist, self._spawn_dist, self._max_actors], - overwrite=True + "BA_ChangeOppositeBehavior", [self._source_dist, self._max_actors], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -79,25 +88,18 @@ class ChangeJunctionBehavior(AtomicBehavior): Args: source_dist (float): Distance between the junctiob sources and the junction entry. Must be positive - vehicle_dist (float) Minimum distance between the junction vehicles. Must between 0 and 'spawn_dist' - spawn_dist (float): Minimum distance between spawned vehicles. Must be positive max_actors (int): Max amount of concurrent alive actors spawned by the same source. Can't be negative - """ def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, max_actors=None, name="ChangeJunctionBehavior"): self._source_dist = source_dist - self._vehicle_dist = vehicle_dist - self._spawn_dist = spawn_dist self._max_actors = max_actors super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeJunctionBehavior", - [self._source_dist, self._vehicle_dist, self._spawn_dist, self._max_actors], - overwrite=True + "BA_ChangeJunctionBehavior", [self._source_dist, self._max_actors], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -156,14 +158,13 @@ class ExtentExitRoadSpace(AtomicBehavior): """ Updates the blackboard to tell the background activity that an exit road needs more space """ - def __init__(self, distance, direction, name="ExtentExitRoadSpace"): + def __init__(self, distance, name="ExtentExitRoadSpace"): self._distance = distance - self._direction = direction super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", [self._distance, self._direction], overwrite=True) + py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", self._distance, overwrite=True) return py_trees.common.Status.SUCCESS From d467860d3c0174301e6c18bb3f2242820c3b1209 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Wed, 4 May 2022 09:57:41 +0200 Subject: [PATCH 19/86] Improve bycicle flow scenario --- srunner/routes/FastExit.xml | 29 --- srunner/routes/ParkingExit.xml | 20 -- srunner/routes/leaderboard_2.0_testing.xml | 90 +++++++++ .../scenarioatomics/atomic_behaviors.py | 86 +++++++++ .../scenarioatomics/atomic_criteria.py | 4 +- srunner/scenarios/actor_flow.py | 96 +--------- srunner/scenarios/background_activity.py | 32 ++-- srunner/scenarios/cross_bicycle_flow.py | 175 ++++++++++++++++++ srunner/tools/background_manager.py | 15 +- 9 files changed, 370 insertions(+), 177 deletions(-) delete mode 100644 srunner/routes/FastExit.xml delete mode 100644 srunner/routes/ParkingExit.xml create mode 100644 srunner/routes/leaderboard_2.0_testing.xml create mode 100644 srunner/scenarios/cross_bicycle_flow.py diff --git a/srunner/routes/FastExit.xml b/srunner/routes/FastExit.xml deleted file mode 100644 index 6cabe6269..000000000 --- a/srunner/routes/FastExit.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/srunner/routes/ParkingExit.xml b/srunner/routes/ParkingExit.xml deleted file mode 100644 index c8f5057dc..000000000 --- a/srunner/routes/ParkingExit.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml new file mode 100644 index 000000000..af0ad7020 --- /dev/null +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 71a171849..7921f21d1 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2596,6 +2596,92 @@ def terminate(self, new_status): pass # Actor was already destroyed +class BicycleFlow(AtomicBehavior): + """ + Behavior that indefinitely creates bicycles at a location, + controls them until another location, and then destroys them. + Therefore, a parallel termination behavior has to be used. + + Important parameters: + - plan (list(carla.Waypoint)): plan used by the bicycles. + - spawn_distance_interval (list(float, float)): Distance between spawned actors + - sink_distance: Actors at this distance from the sink will be deleted + - actors_speed: Speed of the actors part of the flow [m/s] + """ + + def __init__(self, plan, spawn_dist_interval, sink_dist=2, + actor_speed=20 / 3.6, name="BicycleFlow"): + """ + Setup class members + """ + super().__init__(name) + self._rng = CarlaDataProvider.get_random_seed() + self._world = CarlaDataProvider.get_world() + + self._plan = plan + self._sink_dist = sink_dist + self._speed = actor_speed + + self._source_transform = self._plan[0][0].transform + self._source_location = self._source_transform.location + self._source_vector = self._source_transform.get_forward_vector() + self._sink_location = self._plan[-1][0].transform.location + + self._min_spawn_dist = spawn_dist_interval[0] + self._max_spawn_dist = spawn_dist_interval[1] + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + self._actor_data = [] + self._grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + + def update(self): + """Controls the created actors and creaes / removes other when needed""" + # Control the vehicles, removing them when needed + for actor_data in list(self._actor_data): + actor, controller = actor_data + sink_distance = self._sink_location.distance(CarlaDataProvider.get_location(actor)) + if sink_distance < self._sink_dist: + actor.destroy() + self._actor_data.remove(actor_data) + else: + controller.run_step() + + # Spawn new actors if needed + if len(self._actor_data) == 0: + distance = self._spawn_dist + 1 + else: + actor_location = CarlaDataProvider.get_location(self._actor_data[-1][0]) + distance = self._source_location.distance(actor_location) + + if distance > self._spawn_dist: + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._source_transform, rolename='scenario', + attribute_filter={'base_type': 'bicycle'}, tick=False + ) + if actor is None: + return py_trees.common.Status.RUNNING + + actor.set_target_velocity(self._speed * self._source_vector) + controller = BasicAgent(actor, 3.6 * self._speed, + map_inst=CarlaDataProvider.get_map(), grp_inst=self._grp) + controller.set_global_plan(self._plan) + + self._actor_data.append([actor, controller]) + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + """ + Default terminate. Can be extended in derived class + """ + for actor, _ in self._actor_data: + try: + actor.destroy() + except RuntimeError: + pass # Actor was already destroyed + + class OpenVehicleDoor(AtomicBehavior): """ diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 21e5a466f..9406a9cc1 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1989,10 +1989,10 @@ def update(self): if velocity is None: return new_status - set_speed_data = py_trees.blackboard.Blackboard().get('BA_ClearJunction') + set_speed_data = py_trees.blackboard.Blackboard().get('BA_CheckMinSpeed') if set_speed_data is not None: self._active = set_speed_data - py_trees.blackboard.Blackboard().set('BA_ClearJunction', None, True) + py_trees.blackboard.Blackboard().set('BA_CheckMinSpeed', None, True) if self._active: # Get the speed of the surrounding Background Activity diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index d77b82875..cd5ed27b6 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -24,9 +24,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import (SwitchRouteSources, - JunctionScenarioManager, ExtentExitRoadSpace, - StopEntries, RemoveLane, RemoveJunctionEntry, ClearJunction) @@ -214,7 +212,7 @@ def _create_behavior(self): if self.route_mode: sequence.add_child(RemoveJunctionEntry(source_wp, True)) - sequence.add_child(StopEntries()) + sequence.add_child(ClearJunction()) sequence.add_child(root) return sequence @@ -235,98 +233,6 @@ def __del__(self): self.remove_all_actors() -class CrossingBycicleFlow(BasicScenario): - """ - This class holds everything required for a scenario in which another vehicle runs a red light - in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, - ambulances or firetrucks. - """ - - 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 - """ - self._world = world - self._map = CarlaDataProvider.get_map() - self.timeout = timeout - - self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) - self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._sink_distance = 2 - - self._end_distance = 40 - - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - - super(CrossActorFlow, self).__init__("CrossActorFlow", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _create_behavior(self): - """ - Hero vehicle is entering a junction in an urban area, at a signalized intersection, - while another actor runs a red lift, forcing the ego to break. - """ - source_wp = self._map.get_waypoint(self._start_actor_flow) - sink_wp = self._map.get_waypoint(self._end_actor_flow) - - self.world.debug.draw_point(source_wp.transform.location + carla.Location(z=3)) - self.world.debug.draw_point(sink_wp.transform.location + carla.Location(z=3)) - - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) - route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) - junction_id = None - for wp, _ in route: - if wp.is_junction: - junction_id = wp.get_junction().id - break - - root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, "bycicle")) - end_condition = py_trees.composites.Sequence() - end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) - - root.add_child(end_condition) - - sequence = py_trees.composites.Sequence() - if self.route_mode: - sequence.add_child(JunctionScenarioManager(remove_middle=True)) - sequence.add_child(StopEntries()) - sequence.add_child(root) - - return sequence - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - return [CollisionTest(self.ego_vehicles[0])] - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() - class HighwayExit(BasicScenario): """ This scenario is similar to CrossActorFlow diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index bf1ce6422..f2904c56d 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1357,7 +1357,7 @@ def _update_junction_sources(self): actor = self._spawn_source_actor(source) if not actor: continue - if junction.stop_entries: + if junction.stop_entries and get_lane_key(source.entry_lane_wp) not in junction.route_entry_keys: self._set_actor_speed_percentage(actor, 0) self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(actor_dict, actor, at_oppo_entry_lane=at_oppo_entry_lane) @@ -1615,12 +1615,6 @@ def _update_parameters(self): self._road_extra_front_actors = 0 py_trees.blackboard.Blackboard().set('BA_HandleEndAccidentScenario', None, True) - # Stops non route entries - stop_entry_data = py_trees.blackboard.Blackboard().get('BA_StopEntries') - if stop_entry_data is not None: - self._stop_non_route_entries() - py_trees.blackboard.Blackboard().set('BA_StopEntries', None, True) - # Leave space in front leave_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveSpaceInFront') if leave_space_data is not None: @@ -1718,15 +1712,6 @@ def _move_actors_forward(self, actors, space): else: self._destroy_actor(actor) - def _stop_non_route_entries(self): - if len(self._active_junctions) > 0: - entry_sources = self._active_junctions[0].entry_sources - for source in entry_sources: - for actor in source.actors: - self._set_actor_speed_percentage(actor, 0) - elif len(self._junctions) > 0: - self._junctions[0].stop_entries = True - def _handle_lanechange_scenario(self, accident_wp, distance): """ Handles the scenario in which the BA has to change lane. @@ -1845,17 +1830,28 @@ def _remove_junction_entry(self, wp, all_entries): def _clear_junction(self): """Clears the junction, and all subsequent actors that enter it""" if self._active_junctions: + + # Remove all actors in the middle junction = self._active_junctions[0] actor_dict = junction.actor_dict for actor in list(actor_dict): if actor_dict[actor]['state'] == JUNCTION_MIDDLE: self._destroy_actor(actor) - junction.scenario_info['remove_middle'] = True + # Stop all entry actors + entry_sources = junction.entry_sources + route_entry_keys = junction.route_entry_keys + for source in entry_sources: + + if get_lane_key(source.entry_lane_wp) not in route_entry_keys: + for actor in source.actors: + if actor_dict[actor]['state'] == JUNCTION_ENTRY: + self._set_actor_speed_percentage(actor, 0) + elif self._junctions: self._junctions[0].scenario_info['remove_middle'] = True - + self._junctions[0].stop_entries = True ############################# ## Actor functions ## diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py new file mode 100644 index 000000000..2f736f397 --- /dev/null +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python + +# Copyright (c) 2018-2022 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Scenarios in which the ego has to cross a flow of bycicles +""" + +from __future__ import print_function + +import py_trees +import carla + +from agents.navigation.global_route_planner import GlobalRoutePlanner + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import BicycleFlow, TrafficLightFreezer +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenarios.basic_scenario import BasicScenario + +from srunner.tools.scenario_helper import get_closest_traffic_light +from srunner.tools.background_manager import ClearJunction +from agents.navigation.local_planner import RoadOption + + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + + +class CrossingBycicleFlow(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle runs a red light + in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, + ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_dist_flow = 40 # m + self._sink_distance = 2 + + self._end_distance = 40 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + if 'green_light_delay' in config.other_parameters: + self._green_light_delay = float(config.other_parameters['green_light_delay']['value']) + else: + self._green_light_delay = 3 # s + + self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) + + super().__init__("CrossingBycicleFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + source_wp = self._map.get_waypoint(self._start_flow, lane_type=carla.LaneType.Biking) + if not source_wp: + raise ValueError("Couldn't find a biking lane") + elif source_wp.transform.location.distance(self._start_flow) > 10: + raise ValueError("Couldn't find a biking lane at the specified location") + + # Get the plan + plan = [] + junction_id = -1 + plan_step = 0 + wp = source_wp + while True: + next_wps = wp.next(1) + if not next_wps: + raise ValueError("Couldn't find a proper plan for the bicycle flow") + next_wp = next_wps + wp = next_wp[0] + plan.append([next_wp[0], RoadOption.LANEFOLLOW]) + + if plan_step == 0 and wp.is_junction: + junction_id = wp.get_junction().id + plan_step += 1 + elif plan_step == 1 and not wp.is_junction: + plan_step += 1 + exit_loc = wp.transform.location + elif plan_step == 2 and exit_loc.distance(wp.transform.location) > self._end_dist_flow: + break + + # Get the relevant traffic lights + tls = self._world.get_traffic_lights_in_junction(junction_id) + ego_tl = get_closest_traffic_light(self._reference_waypoint, tls) + self._flow_tl_dict = {} + self._init_tl_dict = {} + for tl in tls: + if tl == ego_tl: + self._flow_tl_dict[tl] = carla.TrafficLightState.Green + self._init_tl_dict[tl] = carla.TrafficLightState.Red + else: + self._flow_tl_dict[tl] = carla.TrafficLightState.Red + self._init_tl_dict[tl] = carla.TrafficLightState.Red + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(BicycleFlow(plan, self._source_dist_interval, self._sink_distance, self._flow_speed)) + + # End condition, when the ego exits the junction + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) + root.add_child(end_condition) + + # Freeze the traffic lights to allow the flow to populate the junction + tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") + tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) + tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) + root.add_child(tl_freezer_sequence) + + # Add the BackgroundActivity behaviors + if not self.route_mode: + return root + + sequence = py_trees.composites.Sequence() + sequence.add_child(ClearJunction()) + sequence.add_child(root) + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index e8877d80d..ec75cb141 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -183,19 +183,6 @@ def update(self): return py_trees.common.Status.SUCCESS -class StopEntries(AtomicBehavior): - """ - Updates the blackboard to tell the background activity to stop all junction entries - """ - def __init__(self, name="StopEntries"): - super().__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_StopEntries", True, overwrite=True) - return py_trees.common.Status.SUCCESS - - class SwitchRouteSources(AtomicBehavior): """ Updates the blackboard to tell the background activity to (de)activate all route sources @@ -278,6 +265,8 @@ def update(self): class ClearJunction(AtomicBehavior): """ + Updates the blackboard to tell the background activity to remove all actors inside the junction, + and stop the ones that are about to enter it, leaving an empty space inside the junction. """ def __init__(self, name="ClearJunction"): From 8b223b6d4061006f12c7fcc3029455d2d95ee9e2 Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Fri, 6 May 2022 22:48:12 +0800 Subject: [PATCH 20/86] Changed BA 'RemoveLane' for 'SwitchLane' --- srunner/scenarios/actor_flow.py | 7 +++-- srunner/scenarios/background_activity.py | 35 ++++++++++++++++-------- srunner/tools/background_manager.py | 16 +++++------ 3 files changed, 36 insertions(+), 22 deletions(-) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index cd5ed27b6..586400997 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -25,7 +25,7 @@ from srunner.tools.background_manager import (SwitchRouteSources, ExtentExitRoadSpace, - RemoveLane, + SwitchLane, RemoveJunctionEntry, ClearJunction) from srunner.tools.scenario_helper import get_same_dir_lanes @@ -307,9 +307,12 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveLane(source_wp.lane_id)) + sequence.add_child(SwitchLane(source_wp.lane_id, False)) sequence.add_child(root) + if self.route_mode: + sequence.add_child(SwitchLane(source_wp.lane_id, True)) + return sequence def _create_test_criteria(self): diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index f2904c56d..95d7ce3d0 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1068,7 +1068,13 @@ def _update_road_sources(self, prev_ego_index): for lane_key in self._road_dict: source = self._road_dict[lane_key] - last_location = CarlaDataProvider.get_location(source.actors[-1]) + # If no actors are found, let the last_location be ego's location + # to keep moving the source waypoint forward + if len(source.actors) == 0: + last_location = self._ego_wp.transform.location + else: + last_location = CarlaDataProvider.get_location(source.actors[-1]) + if last_location is None: continue source_location = source.wp.transform.location @@ -1620,12 +1626,13 @@ def _update_parameters(self): if leave_space_data is not None: self._leave_space_in_front(leave_space_data) py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) - - # Remove Lane - remove_lane_data = py_trees.blackboard.Blackboard().get('BA_RemoveLane') - if remove_lane_data is not None: - self._remove_lane(remove_lane_data) - py_trees.blackboard.Blackboard().set('BA_RemoveLanes', None, True) + + # Switch lane + switch_lane_data = py_trees.blackboard.Blackboard().get('BA_SwitchLane') + if switch_lane_data is not None: + lane_id, active = switch_lane_data + self._switch_lane(lane_id, active) + py_trees.blackboard.Blackboard().set('BA_SwitchLane', None, True) # Remove junction entry remove_junction_entry_data = py_trees.blackboard.Blackboard().get('BA_RemoveJunctionEntry') @@ -1783,16 +1790,20 @@ def _leave_space_in_front(self, space): # self._move_actors_forward(front_actors, space) - def _remove_lane(self, lane_id): - """Remove BA actors from this lane, and BA would never generate new actors on this lane""" + def _switch_lane(self, lane_id, active): + """ + active is False: remove BA actors from this lane, and BA would stop generating new actors on this lane. + active is True: recover BA on the lane. + """ lane_id = str(lane_id) lane_id_list = [x.split('*')[-1] for x in list(self._road_dict.keys())] if lane_id in lane_id_list: lane_index = lane_id_list.index(lane_id) lane_key = list(self._road_dict.keys())[lane_index] - for actor in list(self._road_dict[lane_key].actors): - self._destroy_actor(actor) - self._road_dict.pop(lane_key) + self._road_dict[lane_key].active = active + if not active: + for actor in list(self._road_dict[lane_key].actors): + self._destroy_actor(actor) def _remove_junction_entry(self, wp, all_entries): """Removes a specific entry (or all the entries at the same road) of the closest junction""" diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index ec75cb141..b341a7497 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -224,25 +224,25 @@ def update(self): py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", True, overwrite=True) return py_trees.common.Status.SUCCESS - -class RemoveLane(AtomicBehavior): +class SwitchLane(AtomicBehavior): """ - Updates the blackboard to tell the background activity to remove its actors from the given lane, - and stop generating new ones on this lane. + Updates the blackboard to tell the background activity to remove its actors from the given lane + and stop generating new ones on this lane, or recover from stopping. Args: lane_id (str): A carla.Waypoint.lane_id + active (bool) """ - def __init__(self, lane=None, name="RemoveLane"): - self._lane = lane + def __init__(self, lane_id=None, active=True, name="SwitchLane"): + self._lane_id = lane_id + self._active = active super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_RemoveLane", self._lane, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_SwitchLane", [self._lane_id, self._active], overwrite=True) return py_trees.common.Status.SUCCESS - class RemoveJunctionEntry(AtomicBehavior): """ Updates the blackboard to tell the background activity to remove its actors from the given lane, From da308d8701171102d37a75d911b3132e78067c5b Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 9 May 2022 10:09:17 +0200 Subject: [PATCH 21/86] New LB2.0 scenario improvements --- Docs/CHANGELOG.md | 5 +- srunner/routes/leaderboard_2.0_testing.xml | 36 +++-- .../scenarioatomics/atomic_behaviors.py | 19 +-- srunner/scenarios/background_activity.py | 21 ++- srunner/scenarios/parking_exit.py | 76 +++++++---- srunner/scenarios/route_obstacles.py | 129 +++++++++++++++++- srunner/scenarios/vehicle_opens_door.py | 52 +++++-- srunner/tools/background_manager.py | 7 +- 8 files changed, 272 insertions(+), 73 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index d32253da0..17d952169 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -18,9 +18,10 @@ * Scenarios can now parse and use all parameters present at the configuration file. * Improved overall parsing of routes and scenarios. * Added new scenarios: - - Accident: the ego has to lane change in order to avoid an accident + - Accident: the ego has to lane change in order to avoid an accident. Two variants, one with traffic in the same direction,a nd another one having to invade the opposite lane + - ParkingExit: the ego starts parked at the side and has to maneuver to properly enter the driving road. - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic - - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. After a while, the door automatically closes. + - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index af0ad7020..65b0df2c8 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -39,7 +39,7 @@ - + @@ -53,22 +53,20 @@ - - - - + - - + + - + + - + @@ -86,5 +84,23 @@ - + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 7921f21d1..97e106624 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2694,21 +2694,18 @@ class OpenVehicleDoor(AtomicBehavior): - duration: Duration of the open door """ - def __init__(self, actor, vehicle_door, duration, name="OpenVehicleDoor"): + def __init__(self, actor, vehicle_door, name="OpenVehicleDoor"): """ Setup class members """ super(OpenVehicleDoor, self).__init__(name, actor) self._vehicle_door = vehicle_door - self._duration = duration - self._start_time = 0 self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def initialise(self): """ Set start time """ - self._start_time = GameTime.get_time() self._actor.open_door(self._vehicle_door) super().initialise() @@ -2716,20 +2713,10 @@ def update(self): """ Keep running until termination condition is satisfied """ - new_status = py_trees.common.Status.RUNNING - - if GameTime.get_time() - self._start_time > self._duration: - new_status = py_trees.common.Status.SUCCESS - + new_status = py_trees.common.Status.SUCCESS + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status - def terminate(self, new_status): - """ - Close the open door - """ - self._actor.close_door(self._vehicle_door) - super().terminate(new_status) - class TrafficLightFreezer(AtomicBehavior): """ diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 95d7ce3d0..3697202af 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -281,6 +281,7 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._opposite_sources_dist = 60 # Distance from the ego to the opposite sources [m] self._opposite_sources_max_actors = 8 # Maximum vehicles alive at the same time per source self._opposite_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego + self._opposite_spawn_dist = self._spawn_dist # Distance between spawned vehicles [m] # Scenario variables: self._stopped_road_actors = [] # Actors stopped by a hard break scenario @@ -1508,9 +1509,12 @@ def _update_opposite_sources(self): if len(source.actors) >= self._opposite_sources_max_actors: continue + if not source.active: + continue + # Calculate distance to the last created actor if len(source.actors) == 0: - distance = self._spawn_dist + 1 + distance = self._opposite_spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(source.actors[-1]) if not actor_location: @@ -1518,12 +1522,14 @@ def _update_opposite_sources(self): distance = source.wp.transform.location.distance(actor_location) # Spawn a new actor if the last one is far enough - if distance > self._spawn_dist: + if distance > self._opposite_spawn_dist: actor = self._spawn_source_actor(source) if actor is None: continue - self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) + self._tm.distance_to_leading_vehicle(actor, self._opposite_spawn_dist) + self._tm.ignore_lights_percentage(actor, 100) + self._tm.ignore_signs_percentage(actor, 100) self._opposite_actors.append(actor) source.actors.append(actor) @@ -1554,7 +1560,7 @@ def _update_parameters(self): # Opposite behavior opposite_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeOppositeBehavior') if opposite_behavior_data is not None: - source_dist, max_actors = road_behavior_data + source_dist, max_actors, spawn_dist, active = opposite_behavior_data if source_dist is not None: if source_dist < self._junction_sources_dist: print('WARNING: Opposite sources distance is lower than the junction ones. Ignoring it') @@ -1562,6 +1568,11 @@ def _update_parameters(self): self._opposite_sources_dist = source_dist if max_actors is not None: self._opposite_sources_max_actors = max_actors + if spawn_dist is not None: + self._opposite_spawn_dist = spawn_dist + if active is not None: + for source in self._opposite_sources: + source.active = active py_trees.blackboard.Blackboard().set('BA_ChangeOppositeBehavior', None, True) # Junction behavior @@ -2145,7 +2156,7 @@ def _update_opposite_actors(self): removing them if too far behind the ego. """ ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - max_dist = max(self._opposite_removal_dist + ego_speed * self._opposite_increase_ratio, self._spawn_dist) + max_dist = max(self._opposite_removal_dist + ego_speed * self._opposite_increase_ratio, self._opposite_spawn_dist) for actor in list(self._opposite_actors): location = CarlaDataProvider.get_location(actor) if not location: diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index f860762fd..6e17f0225 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -99,37 +99,67 @@ def _initialize_actors(self, config): """ Custom initialization """ - # Put blocking vehicles + # Spawn the actor in front of the ego front_points = self._parking_waypoint.next( self._front_vehicle_distance) - if front_points: - actor_front = CarlaDataProvider.request_new_actor( - 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) - if actor_front is None: - raise Exception( - "Couldn't spawn the vehicle in front of the parking point") - self.other_actors.append(actor_front) - else: - raise Exception( + if not front_points: + raise ValueError( "Couldn't find viable position for the vehicle in front of the parking point") + actor_front = CarlaDataProvider.request_new_actor( + 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + if actor_front is None: + raise ValueError( + "Couldn't spawn the vehicle in front of the parking point") + self.other_actors.append(actor_front) + + # And move it to the side + side_transform = self._get_displaced_transform(actor_front, front_points[0]) + actor_front.set_location(side_transform) + + # Spawn the actor behind the ego behind_points = self._parking_waypoint.previous( self._behind_vehicle_distance) - if behind_points: - actor_behind = CarlaDataProvider.request_new_actor( - 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) - if actor_behind is None: - raise Exception( - "Couldn't spawn the vehicle behind the parking point") - self.other_actors.append(actor_behind) - else: - raise Exception( + if not behind_points: + raise ValueError( "Couldn't find viable position for the vehicle behind the parking point") + actor_behind = CarlaDataProvider.request_new_actor( + 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + if actor_behind is None: + raise ValueError( + "Couldn't spawn the vehicle behind the parking point") + self.other_actors.append(actor_behind) + + # And move it to the side + side_transform = self._get_displaced_transform(actor_behind, behind_points[0]) + actor_behind.set_location(side_transform) + + # Move the ego to its side position + self._ego_transform = self._get_displaced_transform(self.ego_vehicles[0], self._parking_waypoint) + self.ego_vehicles[0].set_location(self._ego_transform) + + def _get_displaced_transform(self, actor, wp): + """ + Calculates the transforming such that the actor is at the sidemost part of the lane + """ + # Move the actor to the edge of the lane near the sidewalk + displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 + displacement_vector = wp.transform.get_right_vector() + if self._parking_lane_side == 'left': + displacement_vector *= -1 + + new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, + y=displacement*displacement_vector.y, + z=displacement*displacement_vector.z) + new_location.z += 0.05 # Just in case, avoid collisions with the ground + return new_location + def _create_behavior(self): """ - Deactivate OutsideRouteLanesTest, then move ego to the parking point, generate blocking vehicles in front of and behind the ego. - After ego drives away, actviate OutsideRouteLanesTest, end scenario. + Deactivate OutsideRouteLanesTest, then move ego to the parking point, + generate blocking vehicles in front of and behind the ego. + After ego drives away, activate OutsideRouteLanesTest, end scenario. """ sequence = py_trees.composites.Sequence() @@ -137,10 +167,6 @@ def _create_behavior(self): # Deactivate OutsideRouteLanesTest sequence.add_child(SwitchOutsideRouteLanesTest(False)) - # Teleport ego to the parking point - sequence.add_child(ActorTransformSetter( - self.ego_vehicles[0], self._parking_waypoint.transform)) - root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index a76452ed5..4c7b3fbe6 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -11,17 +11,19 @@ """ from __future__ import print_function -from distutils.log import error import py_trees import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, SwitchOutsideRouteLanesTest from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario +from srunner.tools.background_manager import (HandleStartAccidentScenario, + HandleEndAccidentScenario, + LeaveSpaceInFront, + ChangeOppositeBehavior) class Accident(BasicScenario): @@ -139,3 +141,124 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class AccidentTwoWays(BasicScenario): + """ + This class holds everything required for a scenario in which there is an accident + in front of the ego, forcing it to react. A police vehicle is located before + two other cars that have been in an accident. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 120 + self._distance_to_accident = 100 + self._offset = 0.75 + self._first_distance = 10 + self._second_distance = 6 + self._accident_wp = None + + super().__init__("AccidentTwoWays", + ego_vehicles, + config, + world, + randomize, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + accident_wps = starting_wp.next(self._distance_to_accident) + pre_accident_wps = starting_wp.next(self._distance_to_accident/2) + if not accident_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + if not pre_accident_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + self._accident_wp = accident_wps[0] + + # Create the police vehicle + displacement = self._offset * self._accident_wp.lane_width / 2 + r_vec = self._accident_wp.transform.get_right_vector() + w_loc = self._accident_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) + police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) + police_lights = carla.VehicleLightState.Special1 + police_lights |= carla.VehicleLightState.Special2 + police_lights |= carla.VehicleLightState.Position + police_car.set_light_state(carla.VehicleLightState(police_lights)) + self.other_actors.append(police_car) + + # Create the first vehicle that has been in the accident + vehicle_wps = self._accident_wp.next(self._first_distance) + if not vehicle_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + vehicle_wp = vehicle_wps[0] + self._accident_wp = pre_accident_wps[0] + displacement = self._offset * vehicle_wp.lane_width / 2 + r_vec = vehicle_wp.transform.get_right_vector() + w_loc = vehicle_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + vehicle_1_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) + vehicle_1_car = CarlaDataProvider.request_new_actor('vehicle.tesla.model3', vehicle_1_transform) + self.other_actors.append(vehicle_1_car) + + # Create the second vehicle that has been in the accident + vehicle_wps = vehicle_wp.next(self._second_distance) + if not vehicle_wps: + raise ValueError("Couldn't find a viable position to set up the accident actors") + vehicle_wp = vehicle_wps[0] + + displacement = self._offset * vehicle_wp.lane_width / 2 + r_vec = vehicle_wp.transform.get_right_vector() + w_loc = vehicle_wp.transform.location + w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + vehicle_2_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) + vehicle_2_car = CarlaDataProvider.request_new_actor('vehicle.mercedes.coupe_2020', vehicle_2_transform) + self.other_actors.append(vehicle_2_car) + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. + """ + root = py_trees.composites.Sequence() + if self.route_mode: + total_dist = self._distance_to_accident + self._first_distance + self._second_distance + 20 + root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(SwitchOutsideRouteLanesTest(False)) + root.add_child(ChangeOppositeBehavior(active=False)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if self.route_mode: + root.add_child(SwitchOutsideRouteLanesTest(True)) + root.add_child(ChangeOppositeBehavior(active=True)) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(ActorDestroy(self.other_actors[2])) + + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index baded66f3..07e15a1c9 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -18,13 +18,15 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import OpenVehicleDoor, HandBrakeVehicle +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + OpenVehicleDoor, + SwitchOutsideRouteLanesTest) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior @@ -47,8 +49,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._wait_duration = 15 self._end_distance = 40 - self._min_trigger_dist = 5 - self._reaction_time = 2.0 + self._min_trigger_dist = 10 + self._reaction_time = 3.0 if 'distance' in config.other_parameters: self._parked_distance = config.other_parameters['distance']['value'] @@ -62,6 +64,12 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + if 'cross_onto_opposite_lane' in config.other_parameters: + self._cross_opposite_lane = True + self._opposite_frequency = 70 + else: + self._cross_opposite_lane = False + super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): @@ -85,11 +93,16 @@ def _initialize_actors(self, config): if parked_wp is None: raise ValueError("Couldn't find a spot to place the adversary vehicle") - self.parked_actor = CarlaDataProvider.request_new_actor( + self._parked_actor = CarlaDataProvider.request_new_actor( "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) - if not self.parked_actor: + if not self._parked_actor: raise ValueError("Couldn't spawn the parked vehicle") - self.other_actors.append(self.parked_actor) + self.other_actors.append(self._parked_actor) + + # And move it to the side + side_transform = self._get_displaced_transform(self._parked_actor, parked_wp) + self._parked_actor.set_location(side_transform) + self._parked_actor.apply_control(carla.VehicleControl(hand_brake=True)) def _create_behavior(self): """ @@ -97,7 +110,6 @@ def _create_behavior(self): while another actor runs a red lift, forcing the ego to break. """ sequence = py_trees.composites.Sequence(name="CrossingActor") - sequence.add_child(HandBrakeVehicle(self.parked_actor, True)) if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._parked_distance)) @@ -115,11 +127,33 @@ def _create_behavior(self): door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL - sequence.add_child(OpenVehicleDoor(self.parked_actor, door, self._wait_duration)) + sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) + if self._cross_opposite_lane: + sequence.add_child(SwitchOutsideRouteLanesTest(False)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + if self._cross_opposite_lane: + sequence.add_child(SwitchOutsideRouteLanesTest(True)) + sequence.add_child(ActorDestroy(self._parked_actor)) return sequence + def _get_displaced_transform(self, actor, wp): + """ + Calculates the transforming such that the actor is at the sidemost part of the lane + """ + # Move the actor to the edge of the lane, or the open door might not reach the ego vehicle + displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 + displacement_vector = wp.transform.get_right_vector() + if self._direction == 'right': + displacement_vector *= -1 + + new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, + y=displacement*displacement_vector.y, + z=displacement*displacement_vector.z) + new_location.z += 0.05 # Just in case, avoid collisions with the ground + return new_location def _create_test_criteria(self): """ A list of all test criteria will be created that is later used diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index b341a7497..0cd817c4c 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -68,15 +68,16 @@ class ChangeOppositeBehavior(AtomicBehavior): max_actors (int): Max amount of concurrent alive actors spawned by the same source. Can't be negative """ - def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, - max_actors=None, name="ChangeOppositeBehavior"): + def __init__(self, source_dist=None, max_actors=None, spawn_dist=None, active=None, name="ChangeOppositeBehavior"): self._source_dist = source_dist self._max_actors = max_actors + self._spawn_dist = spawn_dist + self._active = active super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", [self._source_dist, self._max_actors], overwrite=True + "BA_ChangeOppositeBehavior", [self._source_dist, self._max_actors, self._spawn_dist, self._active], overwrite=True ) return py_trees.common.Status.SUCCESS From 7a4f9a85764516b44c4fbfb68fc3982d93fa9a01 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Tue, 10 May 2022 08:14:53 +0200 Subject: [PATCH 22/86] Refined BA's target speed Co-authored-by: Guillermo --- srunner/scenarios/background_activity.py | 93 +++++++++++++----------- 1 file changed, 50 insertions(+), 43 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 3697202af..0db73e1d4 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -214,8 +214,6 @@ class BackgroundBehavior(AtomicBehavior): Handles the background activity """ - VELOCITY_MULTIPLIER = 2.5 # TODO: Remove it when the map has speed limits - def __init__(self, ego_actor, route, night_mode=False, debug=False, name="BackgroundBehavior"): """ Setup class members @@ -224,7 +222,6 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self.debug = debug self._map = CarlaDataProvider.get_map() self._world = CarlaDataProvider.get_world() - timestep = self._world.get_snapshot().timestamp.delta_seconds self._tm = CarlaDataProvider.get_client().get_trafficmanager( CarlaDataProvider.get_traffic_manager_port()) self._tm.global_percentage_speed_difference(0.0) @@ -235,6 +232,8 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._ego_state = EGO_ROAD self._route_index = 0 self._get_route_data(route) + self._actors_speed_perc = {} # Dictionary actor - percentage + self._all_actors = [] self._spawn_vertical_shift = 0.2 self._reuse_dist = 10 # When spawning actors, might reuse actors closer to this distance @@ -331,16 +330,16 @@ def update(self): prev_ego_index = self._route_index # Check if the TM destroyed an actor - if self._route_index > 0: # TODO: This check is due to intialization problem. + if self._route_index > 0: # TODO: This check is due to intialization problem self._check_background_actors() # Update ego's route position. For robustness, the route point is used for most calculus - self._update_ego_route_location() + self._update_ego_data() # Parameters and scenarios self._update_parameters() - # Update ego state. + # Update ego state if self._ego_state == EGO_JUNCTION: self._monitor_ego_junction_exit() self._monitor_incoming_junctions() @@ -357,32 +356,25 @@ def update(self): self._monitor_road_changes(prev_ego_index) self._update_opposite_sources() - # Update non junction sources. + # Update non junction sources self._update_opposite_actors() + # Update the speed of all vehicles + self._set_actors_speed() + return py_trees.common.Status.RUNNING def terminate(self, new_status): """Destroy all actors""" - all_actors = self._get_actors() + all_actors = list(self._actors_speed_perc) for actor in list(all_actors): self._destroy_actor(actor) super(BackgroundBehavior, self).terminate(new_status) - def _get_actors(self): - """Returns a list of all actors part of the background activity""" - actors = list(self._opposite_actors) - for lane in self._road_dict: - actors.extend(self._road_dict[lane].actors) - for junction in self._active_junctions: - actors.extend(list(junction.actor_dict)) - return actors - def _check_background_actors(self): """Checks if the Traffic Manager has removed a backgroudn actor""" - background_actors = self._get_actors() alive_ids = [actor.id for actor in self._world.get_actors().filter('vehicle*')] - for actor in background_actors: + for actor in list(self._all_actors): if actor.id not in alive_ids: self._remove_actor_info(actor) @@ -848,7 +840,7 @@ def _switch_to_junction_mode(self, junction): # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' self._add_actor_dict_element(junction.actor_dict, actor) if actor not in self._stopped_road_actors: - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 for lane_key in self._road_dict: source = self._road_dict[lane_key] @@ -940,7 +932,7 @@ def _end_junction_behavior(self, junction): # Don't destroy those that are on the route's road opposite lane. # Instead, let them move freely until they are automatically destroyed. - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 if actor_dict[actor]['at_oppo_entry_lane']: self._opposite_actors.append(actor) self._tm.ignore_lights_percentage(actor, 100) @@ -995,7 +987,7 @@ def _initialise_connecting_lanes(self, junction): for actor in list(exit_actors): self._remove_actor_info(actor) self._add_actor_dict_element(junction.actor_dict, actor) - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 def _monitor_incoming_junctions(self): """ @@ -1030,7 +1022,7 @@ def _add_incoming_actors(self, junction, source): source.previous_lane_keys = [get_lane_key(prev_wp) for prev_wp in source.wp.previous(self._reuse_dist)] source.previous_lane_keys.append(get_lane_key(source.wp)) - for actor in self._get_actors(): + for actor in self._all_actors: if actor in source.actors: continue # Don't use actors already part of the source @@ -1044,7 +1036,7 @@ def _add_incoming_actors(self, junction, source): if get_lane_key(actor_wp) not in source.previous_lane_keys: continue # Don't use actors that won't pass through the source - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 self._remove_actor_info(actor) source.actors.append(actor) @@ -1365,7 +1357,7 @@ def _update_junction_sources(self): if not actor: continue if junction.stop_entries and get_lane_key(source.entry_lane_wp) not in junction.route_entry_keys: - self._set_actor_speed_percentage(actor, 0) + self._actors_speed_perc[actor] = 0 self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(actor_dict, actor, at_oppo_entry_lane=at_oppo_entry_lane) source.actors.append(actor) @@ -1677,7 +1669,7 @@ def _stop_road_front_vehicles(self): location = CarlaDataProvider.get_location(actor) if location and not self._is_location_behind_ego(location): self._stopped_road_actors.append(actor) - self._set_actor_speed_percentage(actor, 0) + self._actors_speed_perc[actor] = 0 lights = actor.get_light_state() lights |= carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) @@ -1688,7 +1680,7 @@ def _start_road_front_vehicles(self): wait for a bit, and start moving again. This will never trigger unless done so from outside. """ for actor in self._stopped_road_actors: - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 lights = actor.get_light_state() lights &= ~carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) @@ -1711,7 +1703,7 @@ def _extent_road_exit_space(self, space): self._move_actors_forward(actors, space) for actor in actors: if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: - self._set_actor_speed_percentage(actor, 100) + self._actors_speed_perc[actor] = 100 junction.actor_dict[actor]['state'] = JUNCTION_EXIT def _move_actors_forward(self, actors, space): @@ -1748,7 +1740,7 @@ def _handle_lanechange_scenario(self, accident_wp, distance): ## maybe check here to activate lane changing lights self._road_extra_front_actors += 1 else: - self._set_actor_speed_percentage(actor, 0) + self._actors_speed_perc[actor] = 0 def _switch_route_sources(self, enabled): """ @@ -1869,7 +1861,7 @@ def _clear_junction(self): if get_lane_key(source.entry_lane_wp) not in route_entry_keys: for actor in source.actors: if actor_dict[actor]['state'] == JUNCTION_ENTRY: - self._set_actor_speed_percentage(actor, 0) + self._actors_speed_perc[actor] = 0 elif self._junctions: self._junctions[0].scenario_info['remove_middle'] = True @@ -1878,6 +1870,11 @@ def _clear_junction(self): ############################# ## Actor functions ## ############################# + def _initialize_actor(self, actor): + """Save the actor into the needed structures and disable its lane changes""" + self._tm.auto_lane_change(actor, False) + self._actors_speed_perc[actor] = 100 + self._all_actors.append(actor) def _spawn_actors(self, spawn_wps): """Spawns several actors in batch""" @@ -1896,8 +1893,7 @@ def _spawn_actors(self, spawn_wps): return actors for actor in actors: - self._tm.auto_lane_change(actor, False) - self._set_actor_speed_percentage(actor, 100) + self._initialize_actor(actor) if self._night_mode: for actor in actors: @@ -1924,8 +1920,7 @@ def _spawn_source_actor(self, source, ego_dist=0): if not actor: return actor - self._tm.auto_lane_change(actor, False) - self._set_actor_speed_percentage(actor, 100) + self._initialize_actor(actor) if self._night_mode: actor.set_light_state(carla.VehicleLightState( carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam)) @@ -2007,7 +2002,7 @@ def _set_road_actor_speed(self, location, actor, multiplier=1): percentage = (self._max_radius - distance) / (self._max_radius - self._min_radius) * 100 percentage *= multiplier percentage = min(percentage, 100) - self._set_actor_speed_percentage(actor, percentage) + self._actors_speed_perc[actor] = percentage def _monitor_road_changes(self, prev_route_index): """ @@ -2167,11 +2162,16 @@ def _update_opposite_actors(self): if distance > max_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) - def _set_actor_speed_percentage(self, actor, percentage): - percentage *= self.VELOCITY_MULTIPLIER - speed_red = (100 - percentage) - speed_red = min(speed_red, 100) - self._tm.vehicle_percentage_speed_difference(actor, speed_red) + def _set_actors_speed(self): + """ + Sets the speed of all the BA actors. Done by changing a given percentage it the TM value. + Additionaly, avoid issues with speed limits trigger boxes by using the ego's target speed as reference. + """ + for actor, percentage in self._actors_speed_perc.items(): + percentage = min(100, max(0, percentage)) + real_percentage = self._ego_target_speed / actor.get_speed_limit() * percentage + speed_red = (100 - real_percentage) + self._tm.vehicle_percentage_speed_difference(actor, speed_red) def _remove_actor_info(self, actor): """Removes all the references of the actor""" @@ -2205,6 +2205,10 @@ def _remove_actor_info(self, actor): exit_actors.remove(actor) break + self._actors_speed_perc.pop(actor, None) + if actor in self._all_actors: + self._all_actors.remove(actor) + def _destroy_actor(self, actor): """Destroy the actor and all its references""" self._remove_actor_info(actor) @@ -2214,10 +2218,11 @@ def _destroy_actor(self, actor): except RuntimeError: pass - def _update_ego_route_location(self): + def _update_ego_data(self): """ - Checks the ego location to see if i has moved closer to the next route waypoint, - updating its information. This never checks for backwards movements to avoid unnedded confusion + Checks the ego location to see if it has moved closer to the next route waypoint, + updating its information. This never checks for backwards movements to avoid unnedded confusion. + It also saves its max speed, used as a baseline for all BA vehicles. """ location = CarlaDataProvider.get_location(self._ego_actor) @@ -2237,3 +2242,5 @@ def _update_ego_route_location(self): string = 'EGO_' + self._ego_state[0].upper() debug_name = DEBUG_ROAD if self._ego_state == EGO_ROAD else DEBUG_JUNCTION draw_string(self._world, location, string, debug_name, False) + + self._ego_target_speed = self._ego_actor.get_speed_limit() \ No newline at end of file From dc6367a1715978ff55b2fd3fcdff3a03809d524a Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Tue, 10 May 2022 09:01:40 +0200 Subject: [PATCH 23/86] Added HighwayCutIn scenario * Improved HighwayCutIn * CHANGELOG Co-authored-by: Guillermo --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 18 ++- .../scenarioatomics/atomic_behaviors.py | 144 ++++++++++++++++++ srunner/scenarios/background_activity.py | 1 - srunner/scenarios/highway_cut_in.py | 125 +++++++++------ 5 files changed, 239 insertions(+), 50 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 17d952169..bb08c9b99 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -22,6 +22,7 @@ - ParkingExit: the ego starts parked at the side and has to maneuver to properly enter the driving road. - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. + - HighwayCutIn: The ego is met with a vehicle that tries to enter the highway by cutting in front of it. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index 65b0df2c8..c101f4018 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -103,4 +103,20 @@ - \ No newline at end of file + + + + + + + + + + + + + + + + + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 97e106624..4e09b8c81 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1681,6 +1681,150 @@ def terminate(self, new_status): super(SyncArrival, self).terminate(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. + + The behavior is in RUNNING state until the "main" actor has reached its destination. + + Args: + actor (carla.Actor): Controlled actor. + reference_actor (carla.Actor): Reference actor to sync up to. + actor_target (carla.Transform): Endpoint of the actor after the behavior finishes. + reference_target (carla.Transform): Endpoint of the reference_actor after the behavior finishes. + delay (float): Time difference between the actors synchronization. + end_dist (float): Minimum distance from the target to finish the behavior. + name (string): Name of the behavior. + Defaults to 'SyncArrivalWithAgent'. + """ + + def __init__(self, actor, reference_actor, actor_target, reference_target, end_dist=1, + name="SyncArrivalWithAgent"): + """ + Setup required parameters + """ + super().__init__(name, actor) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + self._actor = actor + self._actor_target = actor_target + self._reference_actor = reference_actor + self._reference_target = reference_target + self._end_dist = end_dist + self._agent = None + + self._map = CarlaDataProvider.get_map() + self._grp = GlobalRoutePlanner(self._map, 100) + + def initialise(self): + """Initialises the agent""" + self._agent = ConstantVelocityAgent(self._actor, map_inst=self._map, grp_inst=self._grp) + + def update(self): + """ + Dynamic control update for actor velocity to ensure that both actors reach their target + positions at the same time. + """ + 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) + + # 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() + 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) + + velocity_reference = CarlaDataProvider.get_velocity(self._reference_actor) + if velocity_reference > 0: + time_reference = distance_reference / velocity_reference + else: + time_reference = float('inf') + + # Get the required velocity of the actor + desired_velocity = distance / time_reference + + 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)) + return new_status + + def terminate(self, new_status): + """Destroy the collision sensor of the agent""" + if self._agent: + self._agent.destroy_sensor() + return super().terminate(new_status) + + +class CutIn(AtomicBehavior): + + """ + Atomic to make an actor lane change using a Python API agent, cutting in front of another one + + The behavior creates a lane change path and is in RUNNING state until the "main" actor has finsihes it. + + Args: + actor (carla.Actor): Controlled actor. + reference_actor (carla.Actor): Reference actor to cut in. + direction (string): Side from which the cut in happens. Either 'left' or 'right'. + speed_perc (float): Percentage of the reference actor speed on which the cut in is performed. + same_lane_time (float): Amount of time spent at the same lane before cutting in. + other_lane_time (float): Amount of time spent at the other lane after cutting in. + change_time (float): Amount of time spent changing into the other + name (string): Name of the behavior. + 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"): + """ + Setup required parameters + """ + super().__init__(name, actor) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + self._reference_actor = reference_actor + self._direction = direction + self._speed_perc = speed_perc + self._same_lane_time = same_lane_time + self._other_lane_time = other_lane_time + self._change_time = change_time + + self._map = CarlaDataProvider.get_map() + self._grp = GlobalRoutePlanner(self._map, 100) + + def initialise(self): + """Initialises the agent""" + speed = CarlaDataProvider.get_velocity(self._reference_actor) + self._agent = BasicAgent(self._actor, 3.6 * speed * self._speed_perc / 100, map_inst=self._map, grp_inst=self._grp) + self._agent.lane_change(self._direction, self._same_lane_time, self._other_lane_time, self._change_time) + + def update(self): + """ + Dynamic control update for actor velocity to ensure that both actors reach their target + positions at the same time. + """ + new_status = py_trees.common.Status.RUNNING + if self._agent.done(): + return py_trees.common.Status.SUCCESS + + self._actor.apply_control(self._agent.run_step()) + + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + class AddNoiseToVehicle(AtomicBehavior): """ diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 0db73e1d4..1fe7ee024 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -11,7 +11,6 @@ import math from collections import OrderedDict import py_trees -import numpy as np import carla diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index 22f2586e9..0b5a26ad8 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -16,12 +16,32 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, WaypointFollower, LaneChange, AccelerateToVelocity, SetInitSpeed +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + ActorTransformSetter, + SyncArrivalWithAgent, + CutIn) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, WaitUntilInFront from srunner.scenarios.basic_scenario import BasicScenario -class HighwayEntryCutIn(BasicScenario): +from srunner.tools.background_manager import (SwitchRouteSources, + ExtentExitRoadSpace, + RemoveJunctionEntry, + ClearJunction) + +from srunner.tools.scenario_helper import generate_target_waypoint + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + +class HighwayCutInRoute(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, @@ -37,75 +57,84 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._drive_distance = 100 - self._other_init_speed = 100 / 3.6 - self._other_end_speed = 70 / 3.6 - self._factor = 0 - - super(HighwayEntryCutIn, self).__init__("HighwayEntryCutIn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + + self._same_lane_time = 1 + self._other_lane_time = 3 + self._change_time = 2 + self._speed_perc = 80 + self._cut_in_distance = 8 + self._extra_space = 170 + + self._start_location = convert_dict_to_location(config.other_parameters['other_actor_location']) + + super().__init__("HighwayEntryCutIn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ - starting_location = config.other_actors[0].transform.location - starting_waypoint = self._map.get_waypoint(starting_location) + self._other_waypoint = self._map.get_waypoint(self._start_location) + self._other_transform = self._other_waypoint.transform + + self._cut_in_vehicle = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._other_transform, rolename='scenario', + attribute_filter={'base_type': 'car', 'has_lights': True} + ) + self.other_actors.append(self._cut_in_vehicle) + + # Move below ground + self._cut_in_vehicle.set_location(self._other_transform.location - carla.Location(z=100)) + self._cut_in_vehicle.set_simulate_physics(False) - other_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, starting_waypoint.transform) - self.other_actors.append(other_vehicle) def _create_behavior(self): """ Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ + # behavior.add_child(SwitchRouteSources(False)) - behaviour = py_trees.composites.Sequence("ExitFreewayCutIn") - - # Make the vehicle "sync" with the ego_vehicle - sync_arrival = py_trees.composites.Parallel( - "Sync Arrival", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - sync_arrival.add_child(WaitUntilInFront(self.other_actors[0], self.ego_vehicles[0], factor=self._factor)) - - sync_behavior = py_trees.composites.Sequence() - sync_behavior.add_child(SetInitSpeed(self.other_actors[0], self._other_init_speed)) - sync_behavior.add_child(WaypointFollower(self.other_actors[0], self._other_init_speed * 1000)) - sync_arrival.add_child(sync_behavior) + behavior = py_trees.composites.Sequence("HighwayCutInRoute") - # Force a lane change - lane_change = LaneChange( - self.other_actors[0], - speed=self._other_end_speed, - direction='left', - distance_same_lane=1, - distance_other_lane=30, - distance_lane_change=60) + if self.route_mode: + behavior.add_child(RemoveJunctionEntry(self._other_waypoint, True)) + behavior.add_child(ClearJunction()) + behavior.add_child(ExtentExitRoadSpace(self._extra_space)) - # End of the scenario - end_condition = py_trees.composites.Parallel( - "End condition", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + behavior.add_child(ActorTransformSetter(self._cut_in_vehicle, self._other_transform)) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(WaypointFollower(self.other_actors[0], self._other_end_speed)) + # Sync behavior + target_wp = generate_target_waypoint(self._other_waypoint) + front_wps = target_wp.next(self._cut_in_distance) + if not front_wps: + raise ValueError("Couldn't find a waypoint to perform the cut in") + target_wp = front_wps[0] - # behaviour.add_child(init_speed) - behaviour.add_child(sync_arrival) - behaviour.add_child(lane_change) - behaviour.add_child(end_condition) - behaviour.add_child(ActorDestroy(self.other_actors[0])) + trigger_wp = self._map.get_waypoint(self.config.trigger_points[0].location) + reference_wp = generate_target_waypoint(trigger_wp) + behavior.add_child(SyncArrivalWithAgent( + self._cut_in_vehicle, self.ego_vehicles[0], target_wp.transform, reference_wp.transform, 5)) - return behaviour + # Cut in + behavior.add_child(CutIn( + self._cut_in_vehicle, self.ego_vehicles[0], 'left', self._speed_perc, + self._same_lane_time, self._other_lane_time, self._change_time, name="Cut_in") + ) + behavior.add_child(ActorDestroy(self._cut_in_vehicle)) + return behavior def _create_test_criteria(self): """ A list of all test criteria will be created that is later used in parallel behavior tree. """ + if self.route_mode: + return [] return [CollisionTest(self.ego_vehicles[0])] def __del__(self): From f8c79fac5da0b4078c5b5673173ef375f5eecd9d Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Fri, 20 May 2022 09:36:46 +0200 Subject: [PATCH 24/86] Added Idle (#898) Co-authored-by: Guillermo --- srunner/scenarios/route_scenario.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 938411249..41c035599 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -27,7 +27,7 @@ from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ScenarioTriggerer +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ScenarioTriggerer, Idle from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitForBlackboardVariable from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest, InRouteTest, @@ -364,6 +364,7 @@ def _create_criterion_tree(self, scenario, criteria): scenario_criteria.add_child(WaitForBlackboardVariable(var_name, False, None, name=check_name)) criteria_tree.add_child(scenario_criteria) + criteria_tree.add_child(Idle()) # Avoid the indivual criteria stopping the simulation return criteria_tree From 69c2ef75a7742f5834ae8190af61c24cd854fe58 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 23 May 2022 12:08:49 +0200 Subject: [PATCH 25/86] Added ParkingCutIn scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 21 ++- srunner/scenarios/parking_cut_in.py | 169 +++++++++++++++++++++ srunner/scenarios/parking_exit.py | 12 +- 4 files changed, 196 insertions(+), 7 deletions(-) create mode 100644 srunner/scenarios/parking_cut_in.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index bb08c9b99..2ae73b33f 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -23,6 +23,7 @@ - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. - HighwayCutIn: The ego is met with a vehicle that tries to enter the highway by cutting in front of it. + - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index c101f4018..6c1f8726f 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -47,7 +47,7 @@ - + @@ -119,4 +119,23 @@ + + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py new file mode 100644 index 000000000..269b35f1f --- /dev/null +++ b/srunner/scenarios/parking_cut_in.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Parking cut in scenario synchronizes a vehicle that is parked at a side lane +to cut in in front of the ego vehicle, forcing it to break +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + WaypointFollower) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + InTimeToArrivalToLocation, + DriveDistance) +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import LeaveSpaceInFront + + +class ParkingCutIn(BasicScenario): + + """ + Parking cut in scenario synchronizes a vehicle that is parked at a side lane + to cut in in front of the ego vehicle, forcing it to break + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): + """ + Setup all relevant parameters and create scenario + """ + self._wmap = CarlaDataProvider.get_map() + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + + self._cut_in_distance = 25 + self._blocker_distance = 18 + + self._adversary_speed = 10.0 # Speed of the adversary [m/s] + self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 10.0 # Min distance to the collision location that triggers the adversary [m] + self._end_distance = 40 + self.timeout = timeout + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = "right" + + super().__init__("ParkingCutIn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Spawn the blocker vehicle + blocker_wps = self._reference_waypoint.next(self._blocker_distance) + if not blocker_wps: + raise ValueError("Couldn't find a proper position for the cut in vehicle") + self._blocker_wp = blocker_wps[0] + + if self._direction == 'left': + parking_wp = self._blocker_wp.get_left_lane() + else: + parking_wp = self._blocker_wp.get_right_lane() + + self._blocker_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + if not self._blocker_actor: + raise ValueError("Couldn't spawn the parked actor") + self._blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(self._blocker_actor) + + side_transform = self._get_displaced_transform(self._blocker_actor, parking_wp) + self._blocker_actor.set_location(side_transform) + + collision_wps = self._reference_waypoint.next(self._cut_in_distance) + if not collision_wps: + raise ValueError("Couldn't find a proper position for the cut in vehicle") + self._collision_wp = collision_wps[0] + + # Get the parking direction + if self._direction == 'left': + parking_wp = self._collision_wp.get_left_lane() + else: + parking_wp = self._collision_wp.get_right_lane() + + self._parked_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + if not self._parked_actor: + raise ValueError("Couldn't spawn the parked actor") + self.other_actors.append(self._parked_actor) + + side_transform = self._get_displaced_transform(self._parked_actor, parking_wp) + self._parked_actor.set_location(side_transform) + + def _get_displaced_transform(self, actor, wp): + """ + Calculates the transforming such that the actor is at the sidemost part of the lane + """ + # Move the actor to the edge of the lane near the sidewalk + displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 + displacement_vector = wp.transform.get_right_vector() + if self._direction == 'left': + displacement_vector *= -1 + + new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, + y=displacement*displacement_vector.y, + z=displacement*displacement_vector.z) + new_location.z += 0.05 # Just in case, avoid collisions with the ground + return new_location + + def _create_behavior(self): + """ + After invoking this scenario, a parked vehicle will wait for the ego to + be close-by, merging into its lane, forcing it to break. + """ + sequence = py_trees.composites.Sequence(name="CrossingActor") + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._cut_in_distance)) + + collision_location = self._collision_wp.transform.location + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) + + # Move the adversary + cut_in = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") + cut_in.add_child(WaypointFollower(self.other_actors[1], self._adversary_speed)) + cut_in.add_child(DriveDistance(self.other_actors[1], self._end_distance)) + sequence.add_child(cut_in) + + # Remove everything + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) + sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 6e17f0225..f399a007e 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -71,22 +71,22 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._end_distance = self._front_vehicle_distance + 15 - if 'parking_lane_side' in config.other_parameters: - self._parking_lane_side = config.other_parameters['parking_lane_side']['value'] + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] else: - self._parking_lane_side = "right" + self._direction = "right" # Get parking_waypoint based on trigger_point self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint(self._trigger_location) - if self._parking_lane_side == "left": + if self._direction == "left": self._parking_waypoint = self._reference_waypoint.get_left_lane() else: self._parking_waypoint = self._reference_waypoint.get_right_lane() if self._parking_waypoint is None: raise Exception( - "Couldn't find parking point on the {} side".format(self._parking_lane_side)) + "Couldn't find parking point on the {} side".format(self._direction)) super(ParkingExit, self).__init__("ParkingExit", ego_vehicles, @@ -146,7 +146,7 @@ def _get_displaced_transform(self, actor, wp): # Move the actor to the edge of the lane near the sidewalk displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 displacement_vector = wp.transform.get_right_vector() - if self._parking_lane_side == 'left': + if self._direction == 'left': displacement_vector *= -1 new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, From a40f593bd358e6f67fa581f8a3d5ae9d8bc1ec8c Mon Sep 17 00:00:00 2001 From: yuting <88497445+13370724230@users.noreply.github.com> Date: Wed, 25 May 2022 19:28:27 +0800 Subject: [PATCH 26/86] New MergerIntoSlowTraffic scenario * Updated _remove_junction_entry * Update highway_cut_in.py * Added MergerIntoSlowTraffic scenario * Update background_manager.py * Added MergerIntoSlowTraffic route * Added MergerIntoSlowTraffic scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 18 ++++ srunner/scenarios/actor_flow.py | 105 ++++++++++++++++++++- srunner/scenarios/background_activity.py | 67 ++++++------- srunner/scenarios/highway_cut_in.py | 2 +- srunner/tools/background_manager.py | 8 +- 6 files changed, 161 insertions(+), 40 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 2ae73b33f..12d1c779f 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -24,6 +24,7 @@ - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. - HighwayCutIn: The ego is met with a vehicle that tries to enter the highway by cutting in front of it. - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. + - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index 6c1f8726f..d32e775ae 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -138,4 +138,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 586400997..45cec10ab 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -104,7 +104,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry(source_wp, True)) + sequence.add_child(RemoveJunctionEntry([source_wp], True)) sequence.add_child(ClearJunction()) grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) @@ -211,7 +211,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry(source_wp, True)) + sequence.add_child(RemoveJunctionEntry([source_wp], True)) sequence.add_child(ClearJunction()) sequence.add_child(root) @@ -329,3 +329,104 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class MergerIntoSlowTraffic(BasicScenario): + """ + This scenario is similar to EnterActorFlow + It will remove the BackgroundActivity from the lane where ActorFlow starts. + Then vehicles (cars) will start driving from start_actor_flow location to end_actor_flow location + in a relatively low speed, ego car must merger into this slow traffic flow. + This scenario works when Background Activity is running in route mode. And applies to a confluence + area at a highway intersection. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._trigger_point=config.trigger_points[0].location + + self._sink_distance = 2 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + super(MergerIntoSlowTraffic, self).__init__("MergerIntoSlowTraffic", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + the ego vehicle mergers into a slow traffic flow from the freeway entrance. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + trigger_wp=self._map.get_waypoint(self._trigger_point) + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) + + end_condition = py_trees.composites.Sequence() + end_condition.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + root.add_child(end_condition) + + sequence = py_trees.composites.Sequence() + if self.route_mode: + + sequence.add_child(RemoveJunctionEntry([trigger_wp,source_wp], False)) + + grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + extra_space = 0 + for i in range(-2, -len(route)-1, -1): + current_wp = route[i][0] + extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + if current_wp.is_junction: + sequence.add_child(ExtentExitRoadSpace(extra_space)) + break + + sequence.add_child(SwitchRouteSources(False)) + sequence.add_child(root) + if self.route_mode: + sequence.add_child(SwitchRouteSources(True)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 1fe7ee024..cbd889801 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1639,8 +1639,8 @@ def _update_parameters(self): # Remove junction entry remove_junction_entry_data = py_trees.blackboard.Blackboard().get('BA_RemoveJunctionEntry') if remove_junction_entry_data is not None: - wp, all_entries = remove_junction_entry_data - self._remove_junction_entry(wp, all_entries) + wps, all_entries = remove_junction_entry_data + self._remove_junction_entry(wps, all_entries) py_trees.blackboard.Blackboard().set('BA_RemoveJunctionEntry', None, True) # Clear junction @@ -1807,38 +1807,39 @@ def _switch_lane(self, lane_id, active): for actor in list(self._road_dict[lane_key].actors): self._destroy_actor(actor) - def _remove_junction_entry(self, wp, all_entries): + def _remove_junction_entry(self, wps, all_entries): """Removes a specific entry (or all the entries at the same road) of the closest junction""" - if len(self._active_junctions) > 0: - junction = self._active_junctions[0] - elif len(self._junctions) > 0: - junction = self._junctions[0] - else: - return - - mapped_wp = None - mapped_dist = float('inf') - ref_loc = wp.transform.location - for entry_wp in junction.entry_wps: - distance = ref_loc.distance(entry_wp.transform.location) - if distance < mapped_dist: - mapped_wp = entry_wp - mapped_dist = distance - - if all_entries: - mapped_road_key = get_road_key(mapped_wp) - mapped_lane_keys = [key for key in junction.entry_lane_keys if is_lane_at_road(key, mapped_road_key)] - else: - mapped_lane_keys = [get_lane_key(mapped_wp)] + for wp in wps: + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return - if len(self._active_junctions) > 0: - for source in junction.entry_sources: - if get_lane_key(source.wp) in mapped_lane_keys: - for actor in list(source.actors): - self._destroy_actor(actor) - source.active = False - else: - junction.inactive_entry_keys = mapped_lane_keys + mapped_wp = None + mapped_dist = float('inf') + ref_loc = wp.transform.location + for entry_wp in junction.entry_wps: + distance = ref_loc.distance(entry_wp.transform.location) + if distance < mapped_dist: + mapped_wp = entry_wp + mapped_dist = distance + + if all_entries: + mapped_road_key = get_road_key(mapped_wp) + mapped_lane_keys = [key for key in junction.entry_lane_keys if is_lane_at_road(key, mapped_road_key)] + else: + mapped_lane_keys = [get_lane_key(mapped_wp)] + + if len(self._active_junctions) > 0: + for source in junction.entry_sources: + if get_lane_key(source.wp) in mapped_lane_keys: + for actor in list(source.actors): + self._destroy_actor(actor) + source.active = False + else: + junction.inactive_entry_keys = mapped_lane_keys def _clear_junction(self): """Clears the junction, and all subsequent actors that enter it""" @@ -2242,4 +2243,4 @@ def _update_ego_data(self): debug_name = DEBUG_ROAD if self._ego_state == EGO_ROAD else DEBUG_JUNCTION draw_string(self._world, location, string, debug_name, False) - self._ego_target_speed = self._ego_actor.get_speed_limit() \ No newline at end of file + self._ego_target_speed = self._ego_actor.get_speed_limit() diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index 0b5a26ad8..0167edb22 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -102,7 +102,7 @@ def _create_behavior(self): behavior = py_trees.composites.Sequence("HighwayCutInRoute") if self.route_mode: - behavior.add_child(RemoveJunctionEntry(self._other_waypoint, True)) + behavior.add_child(RemoveJunctionEntry([self._other_waypoint], True)) behavior.add_child(ClearJunction()) behavior.add_child(ExtentExitRoadSpace(self._extra_space)) diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 0cd817c4c..0f485a9a6 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -250,17 +250,17 @@ class RemoveJunctionEntry(AtomicBehavior): and stop generating new ones on this lane. Args: - wp (carla.Waypoint): A waypoint used as reference to the entry lane + wp (carla.Waypoint): A list of waypoint used as reference to the entry lane all_road_entries (bool): Boolean to remove all entries part of the same road, or just one """ - def __init__(self, wp, all_road_entries=False, name="RemoveJunctionEntry"): - self._wp = wp + def __init__(self, wps, all_road_entries=False, name="RemoveJunctionEntry"): + self._wps = wps self._all_road_entries = all_road_entries super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_RemoveJunctionEntry", [self._wp, self._all_road_entries], overwrite=True) + py_trees.blackboard.Blackboard().set("BA_RemoveJunctionEntry", [self._wps, self._all_road_entries], overwrite=True) return py_trees.common.Status.SUCCESS From 26f6f7c01cff60e8974f98ee670e5bfe4b0c31e0 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Fri, 27 May 2022 12:41:32 +0200 Subject: [PATCH 27/86] Improve night mode (#902) * Added lights beahvior to rotues * Improved lights and automatic TM lights Co-authored-by: Guillermo --- srunner/scenariomanager/lights_sim.py | 120 ++++++++++++++++++ .../scenarioatomics/atomic_behaviors.py | 2 +- srunner/scenarios/background_activity.py | 17 +-- srunner/scenarios/basic_scenario.py | 13 +- .../opposite_vehicle_taking_priority.py | 7 +- srunner/scenarios/route_obstacles.py | 9 +- srunner/scenarios/route_scenario.py | 8 +- 7 files changed, 154 insertions(+), 22 deletions(-) create mode 100644 srunner/scenariomanager/lights_sim.py diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py new file mode 100644 index 000000000..bdc47c455 --- /dev/null +++ b/srunner/scenariomanager/lights_sim.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +This module provides a weather class and py_trees behavior +to simulate weather in CARLA according to the astronomic +behavior of the sun. +""" + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + + +class RouteLightsBehavior(py_trees.behaviour.Behaviour): + + """ + """ + + def __init__(self, ego_vehicle, radius=50, name="LightsBehavior"): + """ + Setup parameters + """ + super().__init__(name) + self._ego_vehicle = ego_vehicle + self._radius = radius + 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._prev_night_mode = False + + def update(self): + """ + Turns on / off all the lghts around a radius of the ego vehicle + """ + new_status = py_trees.common.Status.RUNNING + + location = CarlaDataProvider.get_location(self._ego_vehicle) + if not location: + return new_status + + night_mode = self._world.get_weather().sun_altitude_angle < 0 + if night_mode: + self.turn_close_lights_on(location) + elif self._prev_night_mode: + self.turn_all_lights_off() + + self._prev_night_mode = night_mode + return new_status + + def turn_close_lights_on(self, location): + """Turns on the lights of all the objects close to the ego vehicle""" + ego_speed = CarlaDataProvider.get_velocity(self._ego_vehicle) + radius = max(self._radius, 5 * ego_speed) + + # Street lights + on_lights = [] + off_lights = [] + + all_lights = self._light_manager.get_all_lights() + for light in all_lights: + if light.location.distance(location) > radius: + if light.is_on: + off_lights.append(light) + else: + if not light.is_on: + on_lights.append(light) + + self._light_manager.turn_on(on_lights) + self._light_manager.turn_off(off_lights) + + # Vehicles + all_vehicles = self._world.get_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: + lights = vehicle.get_light_state() + lights &= ~self._vehicle_lights # Remove those lights + vehicle.set_light_state(carla.VehicleLightState(lights)) + else: + lights = vehicle.get_light_state() + lights |= self._vehicle_lights # Add those lights + vehicle.set_light_state(carla.VehicleLightState(lights)) + + # Ego vehicle + lights = self._ego_vehicle.get_light_state() + lights |= self._vehicle_lights + self._ego_vehicle.set_light_state(carla.VehicleLightState(lights)) + + def turn_all_lights_off(self): + """Turns off the lights of all object""" + all_lights = self._light_manager.get_all_lights() + off_lights = [l for l in all_lights if l.is_on] + self._light_manager.turn_off(off_lights) + + # Vehicles + all_vehicles = self._world.get_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() + lights &= ~self._vehicle_lights # Remove those lights + vehicle.set_light_state(carla.VehicleLightState(lights)) + + # Ego vehicle + lights = self._ego_vehicle.get_light_state() + lights &= ~self._vehicle_lights # Remove those lights + self._ego_vehicle.set_light_state(carla.VehicleLightState(lights)) + + def terminate(self, new_status): + self._light_manager.set_day_night_cycle(True) + return super().terminate(new_status) \ No newline at end of file diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 4e09b8c81..cd470b48a 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2700,7 +2700,7 @@ def update(self): if distance > self._spawn_dist: actor = CarlaDataProvider.request_new_actor( 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter={'base_type': self._actor_type}, tick=False + attribute_filter={'base_type': self._actor_type, 'has_lights': True}, tick=False ) if actor is None: return py_trees.common.Status.RUNNING diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index cbd889801..95fb59c67 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -161,7 +161,7 @@ class BackgroundActivity(BasicScenario): This is a single ego vehicle scenario """ - def __init__(self, world, ego_vehicle, config, route, night_mode=False, debug_mode=False, timeout=0): + def __init__(self, world, ego_vehicle, config, route, debug_mode=False, timeout=0): """ Setup all relevant parameters and create scenario """ @@ -169,7 +169,6 @@ def __init__(self, world, ego_vehicle, config, route, night_mode=False, debug_mo self.ego_vehicle = ego_vehicle self.route = route self.config = config - self._night_mode = night_mode self.debug = debug_mode self.timeout = timeout # Timeout of scenario in seconds @@ -186,7 +185,7 @@ def _create_behavior(self): Basic behavior do nothing, i.e. Idle """ # Check if a vehicle is further than X, destroy it if necessary and respawn it - return BackgroundBehavior(self.ego_vehicle, self.route, self._night_mode) + return BackgroundBehavior(self.ego_vehicle, self.route) def _create_test_criteria(self): """ @@ -213,7 +212,7 @@ class BackgroundBehavior(AtomicBehavior): Handles the background activity """ - def __init__(self, ego_actor, route, night_mode=False, debug=False, name="BackgroundBehavior"): + def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): """ Setup class members """ @@ -224,7 +223,6 @@ def __init__(self, ego_actor, route, night_mode=False, debug=False, name="Backgr self._tm = CarlaDataProvider.get_client().get_trafficmanager( CarlaDataProvider.get_traffic_manager_port()) self._tm.global_percentage_speed_difference(0.0) - self._night_mode = night_mode # Global variables self._ego_actor = ego_actor @@ -1873,6 +1871,7 @@ def _clear_junction(self): def _initialize_actor(self, actor): """Save the actor into the needed structures and disable its lane changes""" self._tm.auto_lane_change(actor, False) + self._tm.update_vehicle_lights(actor, True) self._actors_speed_perc[actor] = 100 self._all_actors.append(actor) @@ -1895,11 +1894,6 @@ def _spawn_actors(self, spawn_wps): for actor in actors: self._initialize_actor(actor) - if self._night_mode: - for actor in actors: - actor.set_light_state(carla.VehicleLightState( - carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam)) - return actors def _spawn_source_actor(self, source, ego_dist=0): @@ -1921,9 +1915,6 @@ def _spawn_source_actor(self, source, ego_dist=0): return actor self._initialize_actor(actor) - if self._night_mode: - actor.set_light_state(carla.VehicleLightState( - carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam)) return actor diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index b87469d90..34feb51fd 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -81,6 +81,10 @@ def __init__(self, name, ego_vehicles, config, world, if end_behavior: self.behavior_tree.add_child(end_behavior) + lights = self._create_lights_behavior() + if lights: + self.scenario_tree.add_child(lights) + # And then add it to the main tree self.scenario_tree.add_child(self.behavior_tree) @@ -207,7 +211,14 @@ def _create_test_criteria(self): def _create_weather_behavior(self): """ - Default initialization of the weather behavior. + Default empty initialization of the weather behavior. + Override this method in child class to provide custom initialization. + """ + pass + + def _create_lights_behavior(self): + """ + Default empty initialization of the lights behavior. Override this method in child class to provide custom initialization. """ pass diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 6670227c5..173ef7f39 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -61,6 +61,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._speed_duration_ratio = 2.0 self._speed_distance_ratio = 1.5 + self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 + # Get the CDP seed or at routes, all copies of the scenario will have the same configuration self._rng = CarlaDataProvider.get_random_seed() @@ -123,8 +125,9 @@ def _initialize_actors(self, config): opposite_actor = CarlaDataProvider.request_new_actor(opposite_bp_wildcard, self._spawn_location) if not opposite_actor: raise Exception("Couldn't spawn the actor") - opposite_actor.set_light_state(carla.VehicleLightState( - carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) + lights = opposite_actor.get_light_state() + lights |= self._lights + opposite_actor.set_light_state(carla.VehicleLightState(lights)) self.other_actors.append(opposite_actor) opposite_transform = carla.Transform( diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 4c7b3fbe6..51fb75078 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -49,6 +49,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._second_distance = 6 self._accident_wp = None + self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 + super(Accident, self).__init__("Accident", ego_vehicles, config, @@ -77,10 +79,9 @@ def _initialize_actors(self, config): w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) - police_lights = carla.VehicleLightState.Special1 - police_lights |= carla.VehicleLightState.Special2 - police_lights |= carla.VehicleLightState.Position - police_car.set_light_state(carla.VehicleLightState(police_lights)) + lights = police_car.get_light_state() + lights |= self._lights + police_car.set_light_state(carla.VehicleLightState(lights)) self.other_actors.append(police_car) # Create the first vehicle that has been in the accident diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 41c035599..96eaba080 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -41,7 +41,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.scenarios.background_activity import BackgroundActivity from srunner.scenariomanager.weather_sim import RouteWeatherBehavior - +from srunner.scenariomanager.lights_sim import RouteLightsBehavior from srunner.tools.route_parser import RouteParser, DIST_THRESHOLD from srunner.tools.route_manipulation import interpolate_trajectory @@ -336,6 +336,12 @@ def _create_weather_behavior(self): return return RouteWeatherBehavior(self.ego_vehicles[0], self.route, self.config.weather) + def _create_lights_behavior(self): + """ + Create the light behavior + """ + return RouteLightsBehavior(self.ego_vehicles[0], 100) + def _initialize_environment(self, world): """ Set the weather From 621115062b4b63a29d0071c178db6e40f5742599 Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Thu, 2 Jun 2022 14:15:35 +0800 Subject: [PATCH 28/86] Add yield to emergency vehicle scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 12 ++ .../scenarioatomics/atomic_behaviors.py | 70 +++++++- .../scenarioatomics/atomic_criteria.py | 71 ++++++++ .../scenarios/yield_to_emergency_vehicle.py | 155 ++++++++++++++++++ 5 files changed, 308 insertions(+), 1 deletion(-) create mode 100644 srunner/scenarios/yield_to_emergency_vehicle.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 12d1c779f..8cc9a93ef 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -23,6 +23,7 @@ - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. - HighwayCutIn: The ego is met with a vehicle that tries to enter the highway by cutting in front of it. + - YieldToEmergencyVehicle: The ego has to yield its lane to emergency vehicle. - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. * Added new functions to the BackgroundManager diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index d32e775ae..53f41de72 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -156,4 +156,16 @@ + + + + + + + + + + + + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index cd470b48a..4d6a28eab 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -33,7 +33,7 @@ from agents.navigation.constant_velocity_agent import ConstantVelocityAgent from agents.navigation.local_planner import RoadOption, LocalPlanner from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.tools.misc import is_within_distance +from agents.tools.misc import is_within_distance, get_speed from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.actorcontrols.actor_control import ActorControl @@ -2012,6 +2012,74 @@ def terminate(self, new_status): self._actor.apply_control(self._control) super(ConstantVelocityAgentBehavior, self).terminate(new_status) +class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): + + """ + This class contains an atomic behavior, which uses the + constant_velocity_agent from CARLA to control the actor until + reaching a target location. + Important parameters: + - actor: CARLA actor to execute the behavior. + - reference_actor: Reference CARLA actor to get target speed. + - 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. + 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="ConstantVelocityAgentBehavior"): + """ + Set up actor and local planner + """ + super(AdaptiveConstantVelocityAgentBehavior, self).__init__(name, actor) + self._speed_increment = speed_increment + self._reference_actor = reference_actor + self._map = CarlaDataProvider.get_map() + self._target_location = target_location + self._opt_dict = opt_dict if opt_dict else {} + self._control = carla.VehicleControl() + self._agent = None + self._plan = None + + def initialise(self): + """Initialises the agent""" + # Get target speed + self._target_speed = get_speed(self._reference_actor) + self._speed_increment*3.6 + py_trees.blackboard.Blackboard().set( + "ACVAB_speed_{}".format(self._reference_actor.id), self._target_speed, overwrite=True) + + self._agent = ConstantVelocityAgent(self._actor, self._target_speed, opt_dict=self._opt_dict) + + 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._agent.set_global_plan(self._plan) + + def update(self): + """Moves the actor and waits for it to finish the plan""" + new_status = py_trees.common.Status.RUNNING + + if self._agent.done(): + new_status = py_trees.common.Status.SUCCESS + + 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)) + + return new_status + + def terminate(self, new_status): + """Resets the control""" + self._control.throttle = 0.0 + self._control.brake = 0.0 + self._actor.apply_control(self._control) + super(AdaptiveConstantVelocityAgentBehavior, self).terminate(new_status) class Idle(AtomicBehavior): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 9406a9cc1..861a070b7 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -21,6 +21,7 @@ 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 @@ -2035,3 +2036,73 @@ def terminate(self, new_status): self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) super().terminate(new_status) + +class YieldToEmergencyVehicleTest(Criterion): + + """ + Atomic Criterion to detect if the actor yields its lane to the emergency vehicle behind it + + Args: + actor (carla.ACtor): CARLA actor to be used for this test + ev (carla.ACtor): The emergency vehicle + 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 + + def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest"): + """ + Constructor + """ + super(YieldToEmergencyVehicleTest, self).__init__(name, actor, ev, optional) + self.units = "%" + self.success_value = 70 + self.actual_value = 0 + # self._last_time = 0 + self._ev = ev + self._target_speed = None + self._ev_speed_log = [] + self._map = CarlaDataProvider.get_map() + + def initialise(self): + # self._last_time = GameTime.get_time() + super(YieldToEmergencyVehicleTest, self).initialise() + + def update(self): + """ + Collect ev's actual speed on each time-step + + returns: + py_trees.common.Status.RUNNING + """ + new_status = py_trees.common.Status.RUNNING + + # Get target speed from Blackboard + # The value is expected to be set by AdaptiveConstantVelocityAgentBehavior + if self._target_speed is None: + target_speed = py_trees.blackboard.Blackboard().get("ACVAB_speed_{}".format(self.actor.id)) + if target_speed is not None: + self._target_speed = target_speed + py_trees.blackboard.Blackboard().set("ACVAB_speed_{}".format(self.actor.id), None, overwrite=True) + else: + return new_status + + if self._ev.is_alive: + ev_speed = get_speed(self._ev) + # Record ev's speed in this moment + self._ev_speed_log.append(ev_speed) + + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + return new_status + + def terminate(self, new_status): + mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) + self.actual_value = mean_speed / self._target_speed *100 + self.actual_value = round(self.actual_value, 2) + + if self.actual_value >= self.success_value: + self.test_status = "SUCCESS" + else: + self.test_status = "FAILURE" + + super().terminate(new_status) diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py new file mode 100644 index 000000000..fb1ca3762 --- /dev/null +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -0,0 +1,155 @@ +#!/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 . + +""" +Scenario in which the ego has to yield its lane to emergency vehicle. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, Idle, AdaptiveConstantVelocityAgentBehavior +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, YieldToEmergencyVehicleTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import SwitchLane + + +class YieldToEmergencyVehicle(BasicScenario): + """ + This class holds everything required for a scenario in which the ego has to yield its lane to emergency vehicle. + The background activity will be removed from the lane the emergency vehicle will pass through, + and will be recreated once the scenario is over. + + Should be on the highway which is long enough and has no junctions. + There should be at least two lanes on the highway. + """ + + def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, + timeout=180): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._ev_drive_time = 12 # seconds + + # m/s. How much the EV is expected to be faster than the EGO + self._speed_increment = 15 + + if 'distance' in config.other_parameters: + self._distance = float( + config.other_parameters['distance']['value']) + else: + self._distance = 15 # m + + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._map.get_waypoint( + self._trigger_location) + self._ev_start_transform = None + + super(YieldToEmergencyVehicle, self).__init__("YieldToEmergencyVehicle", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Spawn emergency vehicle + ev_points = self._reference_waypoint.previous( + self._distance) + if ev_points: + self._ev_start_transform = ev_points[0].transform + else: + raise Exception( + "Couldn't find viable position for the emergency vehicle") + + actor = CarlaDataProvider.request_new_actor( + "vehicle.*.*", self._ev_start_transform, rolename='scenario', attribute_filter={'special_type': 'emergency'}) + if actor is None: + raise Exception( + "Couldn't spawn the emergency vehicle") + # Remove its physics so that it doesn't fall + actor.set_simulate_physics(False) + # Move the actor underground + new_location = actor.get_location() + new_location.z -= 500 + actor.set_location(new_location) + # Turn on special lights + actor.set_light_state(carla.VehicleLightState( + carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) + self.other_actors.append(actor) + + def _create_behavior(self): + """ + - Remove BA from current lane + - Teleport Emergency Vehicle(EV) behind the ego + - [Parallel SUCCESS_ON_ONE] + - Idle(20 seconds) + - AdaptiveConstantVelocityAgentBehavior + - Destroy EV + - [Parallel SUCCESS_ON_ONE] + - DriveDistance(ego, 30) + - Recover BA + """ + + sequence = py_trees.composites.Sequence() + + if self.route_mode: + sequence.add_child(SwitchLane( + self._reference_waypoint.lane_id, False)) + + # Teleport EV behind the ego + sequence.add_child(ActorTransformSetter( + self.other_actors[0], self._ev_start_transform)) + + # Emergency Vehicle runs for self._ev_drive_time seconds + ev_end_condition = py_trees.composites.Parallel("Waiting for emergency vehicle driving for a certein distance", + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + + ev_end_condition.add_child(Idle(self._ev_drive_time)) + + ev_end_condition.add_child(AdaptiveConstantVelocityAgentBehavior( + self.other_actors[0], self.ego_vehicles[0], speed_increment=self._speed_increment)) + + sequence.add_child(ev_end_condition) + + sequence.add_child(ActorDestroy(self.other_actors[0])) + + if self.route_mode: + sequence.add_child(SwitchLane( + self._reference_waypoint.lane_id, True)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + criterias = [] + criterias.append(YieldToEmergencyVehicleTest( + self.ego_vehicles[0], self.other_actors[0])) + if not self.route_mode: + criterias.append(CollisionTest(self.ego_vehicles[0])) + + return criterias + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() From 492ac3b93a313a32865a4546c207e0ac508d407c Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Wed, 15 Jun 2022 14:52:51 +0800 Subject: [PATCH 29/86] Add new scenario PedestrianCrossing --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 15 ++ .../scenarioatomics/atomic_behaviors.py | 178 ++++++++++++++++++ srunner/scenarios/object_crash_vehicle.py | 2 +- srunner/scenarios/pedestrian_crossing.py | 138 ++++++++++++++ 5 files changed, 333 insertions(+), 1 deletion(-) create mode 100644 srunner/scenarios/pedestrian_crossing.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 8cc9a93ef..da748a761 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -23,6 +23,7 @@ - CrossBycicleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of incoming traffic - VehicleOpensDoor: a parked vehicle next to the ego suddenly opens the door, forcing the ego to brake. The ego has to maneuver to an adjacent lane to surpass the obstacle. - HighwayCutIn: The ego is met with a vehicle that tries to enter the highway by cutting in front of it. + - PedestrianCrossing: A group of natual pedestrians crossing the road. - YieldToEmergencyVehicle: The ego has to yield its lane to emergency vehicle. - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index 53f41de72..ff40c911b 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -168,4 +168,19 @@ + + + + + + + + + + + + + + + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 4d6a28eab..7bfe45f30 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3557,3 +3557,181 @@ def update(self): new_status = py_trees.common.Status.SUCCESS py_trees.blackboard.Blackboard().set("SwitchMinSpeedCriteria", self._active, overwrite=True) return new_status + +class WalkerFlow(AtomicBehavior): + """ + Behavior that indefinitely creates walkers at a location, + controls them until another location, and then destroys them. + Therefore, a parallel termination behavior has to be used. + + There can be more than one target location. + + Important parameters: + - source_location (carla.Location): Location at which actors will be spawned + - sink_locations (list(carla.Location)): Locations at which actors will be deleted + - 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. + 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"): + """ + Setup class members + """ + super(WalkerFlow, self).__init__(name) + + if random_seed is not None: + self._rng = random.RandomState(random_seed) + else: + self._rng = CarlaDataProvider.get_random_seed() + self._world = CarlaDataProvider.get_world() + + self._controller_bp = self._world.get_blueprint_library().find('controller.ai.walker') + + self._source_location = source_location + + self._sink_locations = sink_locations + self._sink_locations_prob = sink_locations_prob + self._sink_dist = sink_dist + + self._min_spawn_dist = spawn_dist_interval[0] + 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._walkers = [] + + def update(self): + """Controls the created actors and creates / removes other when needed""" + # Remove walkers when needed + for item in self._walkers: + walker, controller, sink_location = item + loc = CarlaDataProvider.get_location(walker) + if loc.distance(sink_location) < self._sink_dist: + self._destroy_walker(walker, controller) + self._walkers.remove(item) + + # Spawn new walkers + if len(self._walkers) == 0: + distance = self._spawn_dist + 1 + else: + actor_location = CarlaDataProvider.get_location(self._walkers[-1][0]) + distance = self._source_location.distance(actor_location) + + if distance > self._spawn_dist: + # spawn new walkers + walker_amount = self._rng.choice(self._batch_size_list) + for i in range(walker_amount): + spawn_tran = carla.Transform(self._source_location) + spawn_tran.location.y -= i + walker = CarlaDataProvider.request_new_actor( + '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.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) + + return py_trees.common.Status.RUNNING + + def _destroy_walker(self, walker, controller): + controller.stop() + controller.destroy() + walker.destroy() + + def terminate(self, new_status): + """ + Default terminate. Can be extended in derived class + """ + for walker, controller, _ in self._walkers: + try: + self._destroy_walker(walker, controller) + except RuntimeError: + pass # Actor was already destroyed + +class AIWalkerBehavior(AtomicBehavior): + """ + Behavior that creates a walker controlled by AI Walker controller. + The walker go from source location to sink location. + A parallel termination behavior has to be used. + + Important parameters: + - source_location (carla.Location): Location at which the actor will be spawned + - sink_location (carla.Location): Location at which the actor will be deleted + """ + + 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._source_location = source_location + + self._sink_location = sink_location + self._sink_dist = 2 + + self._walker = None + self._controller = None + + def initialise(self): + """ + Spawn the walker at source location. + Setup the AI controller. + + May throw RuntimeError if the walker can not be + spawned at given location. + """ + spawn_tran = carla.Transform(self._source_location) + self._walker = CarlaDataProvider.request_new_actor( + '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.start() + self._controller.go_to_location(self._sink_location) + + super(AIWalkerBehavior, self).initialise() + + def update(self): + """Controls the created walker""" + # Remove walkers when needed + if self._walker is not None: + loc = CarlaDataProvider.get_location(self._walker) + # At the very beginning of the scenario, the get_location may return None + if loc is not None: + if loc.distance(self._sink_location) < self._sink_dist: + self.terminate(py_trees.common.Status.SUCCESS) + + return py_trees.common.Status.RUNNING + + def _destroy_walker(self, walker, controller): + controller.stop() + controller.destroy() + walker.destroy() + + def terminate(self, new_status): + """ + Default terminate. Can be extended in derived class + """ + try: + self._destroy_walker(self._walker, self._controller) + except RuntimeError: + pass # Actor was already destroyed diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index a85add43b..4b1176e13 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -25,7 +25,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_location_in_distance_from_wp -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import LeaveSpaceInFront, RemoveJunctionEntry class StationaryObjectCrossing(BasicScenario): diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py new file mode 100644 index 000000000..d8fabe378 --- /dev/null +++ b/srunner/scenarios/pedestrian_crossing.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Pedestrians crossing through the middle of the lane. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import WalkerFlow, AIWalkerBehavior +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenarios.basic_scenario import BasicScenario + +from srunner.tools.background_manager import RemoveJunctionEntry + + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + + +class PedestrianCrossing(BasicScenario): + + """ + This class holds everything required for a group of natual pedestrians crossing the road. + The ego vehicle is passing through a road, + And encounters a group of pedestrians crossing the road. + + This is a single ego vehicle scenario. + + Notice that the initial pedestrian will walk from the start of the junction ahead to end_walker_flow_1. + """ + + def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, timeout=60): + """ + Setup all relevant parameters and create scenario + """ + self._wmap = CarlaDataProvider.get_map() + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._wmap.get_waypoint( + self._trigger_location) + + # Get the start point of the initial pedestrian + sidewalk_waypoint = self._reference_waypoint + while sidewalk_waypoint.lane_type != carla.LaneType.Sidewalk: + right_wp = sidewalk_waypoint.get_right_lane() + if right_wp is None: + raise RuntimeError("Can't find sidewalk to spawn pedestrian") + sidewalk_waypoint = right_wp + self._init_walker_start = sidewalk_waypoint.next_until_lane_end( + 1)[-1].transform.location + self._init_walker_start.z += 1 + + # The initial pedestrian will walk to end_walker_flow_1 + self._init_walker_end = convert_dict_to_location( + config.other_parameters['end_walker_flow_1']) + + self._start_walker_flow = convert_dict_to_location( + config.other_parameters['start_walker_flow']) + self._sink_locations = [] + self._sink_locations_prob = [] + + end_walker_flow_list = [ + v for k, v in config.other_parameters.items() if 'end_walker_flow' in k] + + for item in end_walker_flow_list: + self._sink_locations.append(convert_dict_to_location(item)) + self._sink_locations_prob.append(float(item['p'])) + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [2, 8] # m + + self.timeout = timeout + + super(PedestrianCrossing, self).__init__("PedestrianCrossing", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + After invoking this scenario, pedestrians will start to walk to two different destinations randomly, and some of them will cross the road. + Pedestrians will be removed when they arrive at their destinations. + + Ego is expected to cross the junction when there is enough space to go through. + Ego is not expected to wait for pedestrians crossing the road for a long time. + """ + sequence = py_trees.composites.Sequence(name="CrossingPedestrian") + if self.route_mode: + sequence.add_child(RemoveJunctionEntry([self._reference_waypoint])) + + # Move the adversary + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + + root.add_child(AIWalkerBehavior( + self._init_walker_start, self._init_walker_end)) + + walker_root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="PedestrianMove") + walker_root.add_child(WalkerFlow( + self._start_walker_flow, self._sink_locations, self._sink_locations_prob, self._source_dist_interval)) + walker_root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + + root.add_child(walker_root) + + sequence.add_child(root) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] From 5b4fb7712893c655b2eff55f0cb9d69ec5068dd4 Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Fri, 17 Jun 2022 14:50:19 +0800 Subject: [PATCH 30/86] New Blocked intersection scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 17 ++ srunner/scenarios/blocked_intersection.py | 176 +++++++++++++++++++++ 3 files changed, 194 insertions(+) create mode 100644 srunner/scenarios/blocked_intersection.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index da748a761..5f82347a6 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -26,6 +26,7 @@ - PedestrianCrossing: A group of natual pedestrians crossing the road. - YieldToEmergencyVehicle: The ego has to yield its lane to emergency vehicle. - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. + - BlockedIntersection: With low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index ff40c911b..ead5bb5ae 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -183,4 +183,21 @@ + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py new file mode 100644 index 000000000..16af87599 --- /dev/null +++ b/srunner/scenarios/blocked_intersection.py @@ -0,0 +1,176 @@ +#!/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 . + +""" +Scenario with low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle. +""" + +from __future__ import print_function + +import carla +import py_trees +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ( + ActorDestroy, Idle) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import \ + CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import \ + InTriggerDistanceToVehicle +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import RemoveJunctionEntry + + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + + +class BlockedIntersection(BasicScenario): + """ + This class holds everything required for a scenario in which with low visibility, + the ego performs a turn only to find out that the end is blocked by another vehicle. + The ego is expected to not see the blockage until far into the junction, resulting in a hard brake. + + User needs to specify the location of the blocker. + This scenario is expected to spawn obstacles on the sidewalk. + """ + + def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, + timeout=180): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._map.get_waypoint( + self._trigger_location) + + self._blocker_location = convert_dict_to_location( + config.other_parameters['blocker_point']) + self._blocker_waypoint = self._map.get_waypoint(self._blocker_location) + self._block_distance = 10 # m. Will stop blocking when ego is within this distance + self._block_time = 5 # s + + if 'obstacle_model' in config.other_parameters: + self._obstacle_model = config.other_parameters['obstacle_model']['value'] + else: + self._obstacle_model = "static.prop.trampoline" + + if 'obstacle_gap' in config.other_parameters: + self._obstacle_gap = int( + config.other_parameters['obstacle_gap']['value']) + else: + self._obstacle_gap = 2 + + # Extra obstacles are not included. One obstacle by default. + self._obstacle_amount = 1 + + # The amount of obstacles that invade the road + if 'extra_obstacle' in config.other_parameters: + self._extra_obstacle = int( + config.other_parameters['extra_obstacle']['value']) + else: + self._extra_obstacle = 2 + + super(BlockedIntersection, self).__init__("BlockedIntersection", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Spawn obstacles to block the view + + # Move to the right sidewalk + sidewalk_waypoint = self._reference_waypoint + + while sidewalk_waypoint.lane_type != carla.LaneType.Sidewalk: + right_wp = sidewalk_waypoint.get_right_lane() + if right_wp is None: + raise RuntimeError("Can't find sidewalk to spawn obstacles") + sidewalk_waypoint = right_wp + + sidewalk_points = sidewalk_waypoint.next_until_lane_end( + self._obstacle_gap) + sidewalk_transforms = [wp.transform for wp in sidewalk_points] + + # Add some obstacles to invade the road + for _ in range(self._extra_obstacle): + end_transform_1 = sidewalk_transforms[-1] + end_transform_2 = sidewalk_transforms[-2] + delta_location = carla.Location(x=end_transform_1.location.x-end_transform_2.location.x, + y=end_transform_1.location.y-end_transform_2.location.y, + z=end_transform_1.location.z-end_transform_2.location.z) + extra_location = end_transform_1.location + delta_location + extra_transform = carla.Transform(extra_location, carla.Rotation()) + sidewalk_transforms.append(extra_transform) + + obs_transforms = sidewalk_transforms[-1 * + min(len(sidewalk_transforms), self._obstacle_amount + self._extra_obstacle):] + + # Spawn obstacles + actors = CarlaDataProvider.request_new_batch_actors( + self._obstacle_model, len(obs_transforms), obs_transforms, rolename='scenario') + self.other_actors += actors + + # Spawn the blocker vehicle + actor = CarlaDataProvider.request_new_actor( + "vehicle.*.*", self._blocker_waypoint.transform, rolename='scenario') + if actor is None: + raise Exception( + "Couldn't spawn the blocker vehicle") + self.other_actors.append(actor) + + def _create_behavior(self): + """ + When ego arrives behind the blocker, idel for a while, then clear the blocker. + """ + + sequence = py_trees.composites.Sequence() + + if self.route_mode: + sequence.add_child(RemoveJunctionEntry( + [self._reference_waypoint, self._blocker_waypoint], all_road_entries=True)) + + # Ego go behind the blocker + sequence.add_child(InTriggerDistanceToVehicle( + self.other_actors[-1], self.ego_vehicles[0], self._block_distance)) + sequence.add_child(Idle(self._block_time)) + sequence.add_child(ActorDestroy(self.other_actors[-1])) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return[] + + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() From 14ae69e9bd502ccdfb81bcc0b9bf8a9ccfe6fb36 Mon Sep 17 00:00:00 2001 From: Tay Yim Date: Thu, 30 Jun 2022 15:55:02 +0800 Subject: [PATCH 31/86] Added InvadingTurn scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 13 ++ .../scenarioatomics/atomic_behaviors.py | 5 +- srunner/scenarios/invading_turn.py | 153 ++++++++++++++++++ 4 files changed, 170 insertions(+), 2 deletions(-) create mode 100644 srunner/scenarios/invading_turn.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 5f82347a6..69bea87f6 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -28,6 +28,7 @@ - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. - BlockedIntersection: With low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. + - InvadingTurn: the ego is about to turn right when a vehicle coming from the opposite lane invades the ego's lane, forcing the ego to move right to avoid a possible collision. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index ead5bb5ae..45c040db0 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -200,4 +200,17 @@ + + + + + + + + + + + + + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 7bfe45f30..464c033a5 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1916,20 +1916,21 @@ class BasicAgentBehavior(AtomicBehavior): The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, target_location, name="BasicAgentBehavior"): + def __init__(self, actor, target_location, opt_dict=None, name="BasicAgentBehavior"): """ Setup actor and maximum steer value """ super(BasicAgentBehavior, self).__init__(name, actor) self._map = CarlaDataProvider.get_map() self._target_location = target_location + self._opt_dict = opt_dict if opt_dict else {} self._control = carla.VehicleControl() self._agent = None self._plan = None def initialise(self): """Initialises the agent""" - self._agent = BasicAgent(self._actor) + self._agent = BasicAgent(self._actor, opt_dict=self._opt_dict) self._plan = self._agent.trace_route( self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), self._map.get_waypoint(self._target_location)) diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py new file mode 100644 index 000000000..7d48e84d4 --- /dev/null +++ b/srunner/scenarios/invading_turn.py @@ -0,0 +1,153 @@ +#!/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 . + +""" +Scenario in which the ego is about to turn right +when a vehicle coming from the opposite lane invades the ego's lane, forcing the ego to move right to avoid a possible collision. +""" + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, BasicAgentBehavior +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import LeaveSpaceInFront + + +def convert_dict_to_location(actor_dict): + """ + Convert a JSON string to a Carla.Location + """ + location = carla.Location( + x=float(actor_dict['x']), + y=float(actor_dict['y']), + z=float(actor_dict['z']) + ) + return location + + +class InvadingTurn(BasicScenario): + """ + This class holds everything required for a scenario in which the ego is about to turn right + when a vehicle coming from the opposite lane invades the ego's lane, + forcing the ego to move right to avoid a possible collision. + + This scenario is expected to take place on a road that has only one lane in each direction. + """ + + def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, + timeout=180): + """ + Setup all relevant parameters and create scenario + and instantiate scenario manager + """ + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._map.get_waypoint( + self._trigger_location) + + # Distance between the trigger point and the start location of the adversary + if 'distance' in config.other_parameters: + self._distance = float( + config.other_parameters['distance']['value']) + else: + self._distance = 120 # m + + self._adversary_end = self._reference_waypoint.get_left_lane().transform.location + + # The width (m) of the vehicle invading the opposite lane. + if 'offset' in config.other_parameters: + self._offset = float( + config.other_parameters['offset']['value']) + else: + self._offset = 0.5 + + super(InvadingTurn, self).__init__("InvadingTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Spawn adversary actor + self._adversary_start_waypoint = self._reference_waypoint.next(self._distance)[ + 0] + self._adversary_start_waypoint = self._adversary_start_waypoint.get_left_lane() + if self._adversary_start_waypoint: + self._adversary_start_transform = self._adversary_start_waypoint.transform + else: + raise Exception( + "Couldn't find viable position for the adversary vehicle") + spawn_transform = carla.Transform( + self._adversary_start_transform.location, self._adversary_start_transform.rotation) + + spawn_transform.location.z += 5 # Avoid colliding with BA actors + actor = CarlaDataProvider.request_new_actor( + "vehicle.*", spawn_transform, rolename='scenario', attribute_filter={'base_type': 'car', 'has_lights': True}) + if actor is None: + raise Exception( + "Couldn't spawn the adversary vehicle") + + # Remove its physics so that it doesn't fall + actor.set_simulate_physics(False) + # Move the actor underground + new_location = actor.get_location() + new_location.z -= 500 + actor.set_location(new_location) + + self.other_actors.append(actor) + + # Calculate the real offset + lane_width = self._adversary_start_waypoint.lane_width + car_width = actor.bounding_box.extent.y*2 + self._offset = -1*(self._offset + 0.5*lane_width - 0.5*car_width) + + def _create_behavior(self): + """ + The adversary vehicle will go to the target place while invading another lane. + """ + + sequence = py_trees.composites.Sequence() + + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._distance)) + + # Teleport adversary + sequence.add_child(ActorTransformSetter( + self.other_actors[0], self._adversary_start_transform)) + + sequence.add_child(BasicAgentBehavior( + self.other_actors[0], self._adversary_end, opt_dict={'offset': self._offset})) + + sequence.add_child(ActorDestroy(self.other_actors[0])) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() From 441033a18896e201b2fc6b4db1a473b5f05d6f8b Mon Sep 17 00:00:00 2001 From: dudu_fang <108377816+May-fang@users.noreply.github.com> Date: Mon, 4 Jul 2022 14:18:23 +0800 Subject: [PATCH 32/86] Added Static cut in scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 42 +++-- .../scenarios/cut_in_with_static_vehicle.py | 154 ++++++++++++++++++ 3 files changed, 186 insertions(+), 11 deletions(-) create mode 100644 srunner/scenarios/cut_in_with_static_vehicle.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 69bea87f6..14d2fb871 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -482,3 +482,4 @@ - WaitForTrafficLightState: wait for the traffic light to have a given state - SyncArrival: sync the arrival of two vehicles to a given target - AddNoiseToVehicle: Add noise to steer as well as throttle of the vehicle + - CutInWithStaticVehicle:Based on the code of ParkingCutIn,realized the cutin function of a static vehicle on the highway diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index 45c040db0..d9f43439b 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -201,16 +201,36 @@ - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py new file mode 100644 index 000000000..16080f15e --- /dev/null +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Intel Corporation +# Copyright (c) 2019-2022 Intel Corporation + +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +from __future__ import print_function + +import py_trees +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + WaypointFollower, + BasicAgentBehavior + ) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + InTimeToArrivalToLocation, + DriveDistance) +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import SwitchLane + +class CutInWithStaticVehicle(BasicScenario): + + """ + Cut in(with static vehicle) scenario synchronizes a vehicle that is parked at a side lane + to cut in in front of the ego vehicle, forcing it to break + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): + """ + Setup all relevant parameters and create scenario + """ + self._wmap = CarlaDataProvider.get_map() + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + self._cut_in_distance = 25 + self._blocker_distance = 16 + self._front_distance = 30 + self._adversary_speed = 10.0 # Speed of the adversary [m/s] + self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 18.0 # Min distance to the collision location that triggers the adversary [m] + self.timeout = timeout + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = "right" + super().__init__("CutInWithStaticVehicle", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Spawn the blocker vehicle + blocker_wps = self._reference_waypoint.next(self._blocker_distance) + if not blocker_wps: + raise ValueError("Couldn't find a proper position for the cut in vehicle") + self._blocker_wp = blocker_wps[0] + if self._direction == 'left': + sideblocker_wp = self._blocker_wp.get_left_lane() + else: + sideblocker_wp = self._blocker_wp.get_right_lane() + + self._blocker_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', sideblocker_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + if not self._blocker_actor: + raise ValueError("Couldn't spawn the parked actor") + self._blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(self._blocker_actor) + + collision_wps = self._reference_waypoint.next(self._cut_in_distance) + if not collision_wps: + raise ValueError("Couldn't find a proper position for the cut in vehicle") + self._collision_wp = collision_wps[0] + + # GGet the parking direction of the car that will be cut in + if self._direction == 'left': + cutin_wp = self._collision_wp.get_left_lane() + else: + cutin_wp = self._collision_wp.get_right_lane() + + self._parked_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', cutin_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + if not self._parked_actor: + raise ValueError("Couldn't spawn the parked actor") + self.other_actors.append(self._parked_actor) + + self._front_wps = self._collision_wp.next(self._front_distance) + self._front_wp = self._front_wps[0].transform.location + + def _create_behavior(self): + """ + After invoking this scenario, a parked vehicle will wait for the ego to + be close-by, merging into its lane, forcing it to break. + """ + sequence = py_trees.composites.Sequence(name="CrossingActor") + if self.route_mode: + other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) + sequence.add_child(SwitchLane(other_car.lane_id, False)) + + collision_location = self._collision_wp.transform.location + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + + sequence.add_child(trigger_adversary) + + # The adversary change the lane + sequence.add_child(BasicAgentBehavior(self.other_actors[1],target_location=self._front_wp)) + + # Move the adversary + cut_in = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") + other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) + sequence.add_child(SwitchLane(other_car.lane_id, True)) + cut_in.add_child(WaypointFollower(self.other_actors[1], self._adversary_speed)) + + sequence.add_child(cut_in) + + # Remove everything + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) + sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + + From 9a7b909b55c2ddd8b4be5d3cdb58aa5c9be45746 Mon Sep 17 00:00:00 2001 From: threewater-wang <60544237+threewater-wang@users.noreply.github.com> Date: Mon, 4 Jul 2022 15:00:19 +0800 Subject: [PATCH 33/86] Added BicycleAtSideLane scenario --- Docs/CHANGELOG.md | 1 + srunner/routes/leaderboard_2.0_testing.xml | 20 ++++ srunner/scenarios/route_obstacles.py | 116 ++++++++++++++++++++- 3 files changed, 136 insertions(+), 1 deletion(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 14d2fb871..9686d70e7 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -28,6 +28,7 @@ - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. - BlockedIntersection: With low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. + - BicycleFlowAtSideLane:Added the dangerous scene of ego vehicles driving on roads without sidewalks, with three bicycles encroaching on some roads in front. - InvadingTurn: the ego is about to turn right when a vehicle coming from the opposite lane invades the ego's lane, forcing the ego to move right to avoid a possible collision. * Added new functions to the BackgroundManager * Minor improvements to some example scenarios. These include FollowLeadingVehicle, VehicleTurning, DynamicObjectCrossing and SignalizedJunctionRightTurn and RunningRedLight. Their behaviors are now more smooth, robust and some outdated mechanics have been removed diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index d9f43439b..f8b65f8d1 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -234,3 +234,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 51fb75078..926e9e65b 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -16,10 +16,12 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, SwitchOutsideRouteLanesTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, SwitchOutsideRouteLanesTest, \ + BasicAgentBehavior, BicycleFlow, ConstantVelocityAgentBehavior from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tests.carla_mocks.agents.navigation.local_planner import RoadOption from srunner.tools.background_manager import (HandleStartAccidentScenario, HandleEndAccidentScenario, LeaveSpaceInFront, @@ -142,7 +144,119 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + +class BicycleFlowAtSideLane(BasicScenario): + """ + Added the dangerous scene of ego vehicles driving on roads without sidewalks, + with three bicycles encroaching on some roads in front. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 100 + self._offset = [0.6,0.75,0.9] + self._bicycle_wp = [] + self._target_location=None + self._plan=[] + + if 'distance' in config.other_parameters: + self._distance_to_Trigger = [ + float(config.other_parameters['distance']['first']), + float(config.other_parameters['distance']['second']), + float(config.other_parameters['distance']['third']) + ] + else: + self._distance_to_Trigger = [74,76,88] # m + + super().__init__("Hazard", + ego_vehicles, + config, + world, + randomize, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + + starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + if 'end_bycicle_distance' in config.other_parameters: + self._end_bycicle_distance = float( + config.other_parameters['end_bycicle_distance']['value']) + else: + self._end_bycicle_distance = 150 + self._target_location = starting_wp.next(self._end_bycicle_distance)[0].transform.location + for offset,distance in zip(self._offset,self._distance_to_Trigger): + + bicycle_wps = starting_wp.next(distance) + + if not bicycle_wps: + raise ValueError("Couldn't find a viable position to set up the bicycle actors") + self._bicycle_wp.append(bicycle_wps[0]) + displacement = offset* bicycle_wps[0].lane_width / 2 + r_vec = bicycle_wps[0].transform.get_right_vector() + w_loc = bicycle_wps[0].transform.location + w_loc = w_loc + carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + bycicle_transform = carla.Transform(w_loc, bicycle_wps[0].transform.rotation) + bycicle = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', bycicle_transform) + self.other_actors.append(bycicle) + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. + """ + root = py_trees.composites.Sequence() + if self.route_mode: + total_dist = self._distance_to_Trigger[2] + 30 + root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(SwitchOutsideRouteLanesTest(False)) + root.add_child(ChangeOppositeBehavior(active=False)) + bycicle = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[2], self._target_location,target_speed=3.1,opt_dict={'offset':self._offset[2]* self._bicycle_wp[2].lane_width / 2})) + + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[1], self._target_location, target_speed=3, + opt_dict={'offset': self._offset[1] * self._bicycle_wp[1].lane_width / 2})) + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[0], self._target_location, target_speed=3, + opt_dict={'offset': self._offset[0] * self._bicycle_wp[0].lane_width / 2})) + root.add_child(bycicle) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if self.route_mode: + root.add_child(SwitchOutsideRouteLanesTest(True)) + root.add_child(ChangeOppositeBehavior(active=True)) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(ActorDestroy(self.other_actors[2])) + + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() class AccidentTwoWays(BasicScenario): """ From 84a8c525cb83ee1d99240435fc129cb965730e67 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 4 Jul 2022 09:54:55 +0200 Subject: [PATCH 34/86] Improved route's timeout --- Docs/CHANGELOG.md | 1 + srunner/scenariomanager/timer.py | 87 +++++++++++++++++++++++++++++ srunner/scenarios/basic_scenario.py | 30 +++++++--- srunner/scenarios/route_scenario.py | 13 ++++- 4 files changed, 120 insertions(+), 11 deletions(-) diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 9686d70e7..07661773f 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -14,6 +14,7 @@ ## Latest changes ### :rocket: New Features +* Improved the timeout at routes * Actor Flows are now more consistent * Scenarios can now parse and use all parameters present at the configuration file. * Improved overall parsing of routes and scenarios. diff --git a/srunner/scenariomanager/timer.py b/srunner/scenariomanager/timer.py index ae380b3ea..696c208e9 100644 --- a/srunner/scenariomanager/timer.py +++ b/srunner/scenariomanager/timer.py @@ -14,6 +14,8 @@ import operator import py_trees +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + class GameTime(object): @@ -155,3 +157,88 @@ def update(self): self.timeout = True return new_status + + +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 = 180 + TIMEOUT_ROUTE_PERC = 25 + + def __init__(self, ego_vehicle, route, debug=False, name="RouteTimeoutBehavior"): + """ + Setup timeout + """ + super().__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + self._ego_vehicle = ego_vehicle + self._route = route + self._debug = debug + + self._start_time = None + self._timeout_value = self.MIN_TIMEOUT + self.timeout = False + + # Route variables + self._wsize = 3 + self._current_index = 0 + + self._route_length = len(self._route) + self._route_transforms, _ = zip(*self._route) + + self._route_accum_meters = [] + prev_loc = self._route_transforms[0].location + for i, tran in enumerate(self._route_transforms): + loc = tran.location + d = loc.distance(prev_loc) + accum = 0 if i == 0 else self._route_accum_meters[i - 1] + + self._route_accum_meters.append(d + accum) + prev_loc = loc + + def initialise(self): + """ + Set start_time to current GameTime + """ + self._start_time = GameTime.get_time() + self.logger.debug("%s.initialise()" % (self.__class__.__name__)) + + def update(self): + """ + Get current game time, and compare it to the timeout value + Upon successfully comparison using the provided comparison_operator, + the status changes to SUCCESS + """ + new_status = py_trees.common.Status.RUNNING + + ego_location = CarlaDataProvider.get_location(self._ego_vehicle) + if ego_location is None: + return new_status + + new_index = self._current_index + + 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: + new_index = index + + # Update the timeout value + if new_index > 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 + self._current_index = new_index + + elapsed_time = GameTime.get_time() - self._start_time + print(f"Timeout node: {round(elapsed_time, 2)}/{round(self._timeout_value, 2)}s") + if elapsed_time > self._timeout_value: + new_status = py_trees.common.Status.SUCCESS + self.timeout = True + + self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) + + return new_status diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index 34feb51fd..2c512d4d0 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -81,10 +81,16 @@ def __init__(self, name, ego_vehicles, config, world, if end_behavior: self.behavior_tree.add_child(end_behavior) + # Create the lights behavior lights = self._create_lights_behavior() if lights: self.scenario_tree.add_child(lights) + # Create the weather behavior + weather = self._create_weather_behavior() + if weather: + self.scenario_tree.add_child(weather) + # And then add it to the main tree self.scenario_tree.add_child(self.behavior_tree) @@ -112,15 +118,14 @@ def __init__(self, name, ego_vehicles, config, world, self.scenario_tree.add_child(self.criteria_tree) + # Create the timeout behavior + self.timeout_node = self._create_timeout_behavior() + if self.timeout_node: + self.scenario_tree.add_child(self.timeout_node) + # Add other nodes - self.timeout_node = TimeOut(self.timeout, name="TimeOut") # Timeout node - self.scenario_tree.add_child(self.timeout_node) self.scenario_tree.add_child(UpdateAllActorControls()) - weather = self._create_weather_behavior() - if weather: - self.scenario_tree.add_child(weather) - self.scenario_tree.setup(timeout=1) def _initialize_environment(self, world): @@ -211,18 +216,27 @@ def _create_test_criteria(self): def _create_weather_behavior(self): """ - Default empty initialization of the weather behavior. + Default empty initialization of the weather behavior, + responsible of controlling the weather during the simulation. Override this method in child class to provide custom initialization. """ pass def _create_lights_behavior(self): """ - Default empty initialization of the lights behavior. + Default empty initialization of the lights behavior, + responsible of controlling the street lights during the simulation. Override this method in child class to provide custom initialization. """ pass + def _create_timeout_behavior(self): + """ + Default initialization of the timeout behavior. + Override this method in child class to provide custom initialization. + """ + return TimeOut(self.timeout, name="TimeOut") # Timeout node + def change_control(self, control): # pylint: disable=no-self-use """ This is a function that changes the control based on the scenario determination diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 96eaba080..ded78eaa7 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -42,6 +42,7 @@ from srunner.scenarios.background_activity import BackgroundActivity from srunner.scenariomanager.weather_sim import RouteWeatherBehavior from srunner.scenariomanager.lights_sim import RouteLightsBehavior +from srunner.scenariomanager.timer import RouteTimeoutBehavior from srunner.tools.route_parser import RouteParser, DIST_THRESHOLD from srunner.tools.route_manipulation import interpolate_trajectory @@ -330,18 +331,24 @@ def _create_test_criteria(self): def _create_weather_behavior(self): """ - If needed, add the dynamic weather behavior to the route + Create the weather behavior """ if len(self.config.weather) == 1: - return + return # Just set the weather at the beginning and done return RouteWeatherBehavior(self.ego_vehicles[0], self.route, self.config.weather) def _create_lights_behavior(self): """ - Create the light behavior + Create the street lights behavior """ return RouteLightsBehavior(self.ego_vehicles[0], 100) + def _create_timeout_behavior(self): + """ + Create the timeout behavior + """ + return RouteTimeoutBehavior(self.ego_vehicles[0], self.route) + def _initialize_environment(self, world): """ Set the weather From 716195e6b045f6a3fc9fe90b7bbb4b5ab5ab46e2 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Mon, 4 Jul 2022 09:59:23 +0200 Subject: [PATCH 35/86] Leaderboard 2.0 scenario improvements --- Docs/CHANGELOG.md | 8 + srunner/examples/ObjectCrossing.xml | 2 +- srunner/examples/RouteObstacles.xml | 2 +- srunner/routes/leaderboard_2.0_testing.xml | 8 +- .../scenariomanager/carla_data_provider.py | 19 +- srunner/scenariomanager/lights_sim.py | 29 +- .../scenarioatomics/atomic_behaviors.py | 247 ++-- .../scenarioatomics/atomic_criteria.py | 58 +- srunner/scenariomanager/traffic_events.py | 1 + srunner/scenariomanager/weather_sim.py | 2 + srunner/scenarios/actor_flow.py | 402 ++++-- srunner/scenarios/background_activity.py | 1219 ++++++++++------- .../background_activity_parametrizer.py | 105 ++ srunner/scenarios/blocked_intersection.py | 13 +- .../scenarios/construction_crash_vehicle.py | 148 +- srunner/scenarios/control_loss.py | 36 +- srunner/scenarios/cross_bicycle_flow.py | 79 +- srunner/scenarios/follow_leading_vehicle.py | 64 +- srunner/scenarios/green_traffic_light.py | 98 ++ srunner/scenarios/hard_break.py | 80 ++ srunner/scenarios/highway_cut_in.py | 25 +- .../scenarios/object_crash_intersection.py | 25 +- srunner/scenarios/object_crash_vehicle.py | 253 +++- .../opposite_vehicle_taking_priority.py | 278 +++- srunner/scenarios/parking_cut_in.py | 24 +- srunner/scenarios/parking_exit.py | 35 +- srunner/scenarios/pedestrian_crossing.py | 14 +- srunner/scenarios/route_obstacles.py | 240 ++-- srunner/scenarios/route_scenario.py | 14 +- .../signalized_junction_left_turn.py | 236 +++- .../signalized_junction_right_turn.py | 239 +++- srunner/scenarios/vehicle_opens_door.py | 89 +- .../scenarios/yield_to_emergency_vehicle.py | 21 +- srunner/tools/background_manager.py | 167 +-- srunner/tools/scenario_helper.py | 22 +- 35 files changed, 2956 insertions(+), 1346 deletions(-) create mode 100644 srunner/scenarios/background_activity_parametrizer.py create mode 100644 srunner/scenarios/green_traffic_light.py create mode 100644 srunner/scenarios/hard_break.py diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 07661773f..5999dd57b 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -29,6 +29,14 @@ - ParkingCutIn: A vehicle parked at the side cuts in front of the ego. - BlockedIntersection: With low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle. - MergerIntoSlowTraffic: the ego has to enter a lane that is filled with slow traffic. + - InterurbanActorFlow and InterurbanAdvancedActorFlow: actor flow scenarios for the new interurban intersections with dedicated lanes. + - ParkingCrossingPedestrian: similar to DynamicObjectCrossing, but the occluder is a vehicle stopped at a parking lane. + - ParkedObstacle: very similar to the Accident one but with a different blocker. + - NonSignalizedJunctionLeftTurn: non signalized version of SignalizedJunctionLeftTurn. + - NonSignalizedJunctionRightTurn: non signalized version of SignalizedJunctionRightTurn. + - OppositeVehicleTakingPriority: non signalized version of OppositeVehicleRunningRedLight. + - BackgroundActivityParametrizer: allows the customization of the BackgroundActivity parameters. + - PriorityAtJunction: sets the traffic light of the incoming junction to green. Improves route smoothness by letting the vehicle cross the junction without stopping. - BicycleFlowAtSideLane:Added the dangerous scene of ego vehicles driving on roads without sidewalks, with three bicycles encroaching on some roads in front. - InvadingTurn: the ego is about to turn right when a vehicle coming from the opposite lane invades the ego's lane, forcing the ego to move right to avoid a possible collision. * Added new functions to the BackgroundManager diff --git a/srunner/examples/ObjectCrossing.xml b/srunner/examples/ObjectCrossing.xml index f255403df..ba0b5f03f 100644 --- a/srunner/examples/ObjectCrossing.xml +++ b/srunner/examples/ObjectCrossing.xml @@ -51,7 +51,7 @@ - + diff --git a/srunner/examples/RouteObstacles.xml b/srunner/examples/RouteObstacles.xml index 71b4a6d32..7981d2915 100644 --- a/srunner/examples/RouteObstacles.xml +++ b/srunner/examples/RouteObstacles.xml @@ -9,7 +9,7 @@ - + \ No newline at end of file diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index f8b65f8d1..ee313d5d1 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -59,10 +59,9 @@ - + - @@ -76,7 +75,7 @@ - + @@ -84,7 +83,7 @@ - + @@ -100,6 +99,7 @@ + diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index ae3b7521e..dcc25af9e 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -18,6 +18,7 @@ from six import iteritems import carla +from agents.navigation.global_route_planner import GlobalRoutePlanner def calculate_velocity(actor): @@ -63,6 +64,7 @@ class CarlaDataProvider(object): # pylint: disable=too-many-public-methods _traffic_manager_port = 8000 _random_seed = 2000 _rng = random.RandomState(_random_seed) + _grp = None @staticmethod def register_actor(actor): @@ -196,6 +198,7 @@ def set_world(world): CarlaDataProvider._sync_flag = world.get_settings().synchronous_mode CarlaDataProvider._map = world.get_map() CarlaDataProvider._blueprint_library = world.get_blueprint_library() + CarlaDataProvider._grp = GlobalRoutePlanner(CarlaDataProvider._map, 2.0) CarlaDataProvider.generate_spawn_points() CarlaDataProvider.prepare_map() @@ -225,10 +228,17 @@ def get_map(world=None): @staticmethod def get_random_seed(): """ - @return true if syncronuous mode is used + @return the random seed. """ return CarlaDataProvider._rng + @staticmethod + def get_global_route_planner(): + """ + @return the global route planner + """ + return CarlaDataProvider._grp + @staticmethod def is_sync_mode(): """ @@ -546,12 +556,14 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point) else: - # slightly lift the actor to avoid collisions with ground when spawning the actor + # 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 + # DO NOT USE spawn_point directly, as this will modify spawn_point permanently _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation) _spawn_point.location.x = spawn_point.location.x _spawn_point.location.y = spawn_point.location.y - _spawn_point.location.z = spawn_point.location.z + 0.2 + _spawn_point.location.z = spawn_point.location.z + z_offset actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point) if actor is None: @@ -822,3 +834,4 @@ def cleanup(): CarlaDataProvider._spawn_points = None CarlaDataProvider._spawn_index = 0 CarlaDataProvider._rng = random.RandomState(CarlaDataProvider._random_seed) + CarlaDataProvider._grp = None diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py index bdc47c455..14182cca0 100644 --- a/srunner/scenariomanager/lights_sim.py +++ b/srunner/scenariomanager/lights_sim.py @@ -20,7 +20,15 @@ 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 + + # For higher fog and cloudness values, the amount of light in scene starts to rapidly decrease + CLOUDINESS_THRESHOLD = 95 + FOG_THRESHOLD = 40 def __init__(self, ego_vehicle, radius=50, name="LightsBehavior"): """ @@ -46,16 +54,27 @@ def update(self): if not location: return new_status - night_mode = self._world.get_weather().sun_altitude_angle < 0 + night_mode = self._get_night_mode(self._world.get_weather()) if night_mode: - self.turn_close_lights_on(location) + self._turn_close_lights_on(location) elif self._prev_night_mode: - self.turn_all_lights_off() + self._turn_all_lights_off() self._prev_night_mode = night_mode return new_status - def turn_close_lights_on(self, location): + def _get_night_mode(self, weather): + """Check wheather or not the street lights need to be turned on""" + if weather.sun_altitude_angle <= self.SUN_ALTITUDE_THRESHOLD_1 \ + or weather.sun_altitude_angle >= self.SUN_ALTITUDE_THRESHOLD_2: + return True + if weather.cloudiness >= self.CLOUDINESS_THRESHOLD: + return True + if weather.fog_density >= self.FOG_THRESHOLD: + return True + return False + + def _turn_close_lights_on(self, location): """Turns on the lights of all the objects close to the ego vehicle""" ego_speed = CarlaDataProvider.get_velocity(self._ego_vehicle) radius = max(self._radius, 5 * ego_speed) @@ -95,7 +114,7 @@ def turn_close_lights_on(self, location): lights |= self._vehicle_lights self._ego_vehicle.set_light_state(carla.VehicleLightState(lights)) - def turn_all_lights_off(self): + def _turn_all_lights_off(self): """Turns off the lights of all object""" all_lights = self._light_manager.get_all_lights() off_lights = [l for l in all_lights if l.is_on] diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 464c033a5..6d71f36c4 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -32,7 +32,6 @@ from agents.navigation.basic_agent import BasicAgent from agents.navigation.constant_velocity_agent import ConstantVelocityAgent from agents.navigation.local_planner import RoadOption, LocalPlanner -from agents.navigation.global_route_planner import GlobalRoutePlanner from agents.tools.misc import is_within_distance, get_speed from srunner.scenariomanager.carla_data_provider import CarlaDataProvider @@ -794,7 +793,7 @@ def initialise(self): # Obtain final route, considering the routing option # At the moment everything besides "shortest" will use the CARLA GlobalPlanner - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + grp = CarlaDataProvider.get_global_route_planner() route = [] for i, _ in enumerate(carla_route_elements): if carla_route_elements[i][1] == "shortest": @@ -1402,7 +1401,7 @@ class KeepVelocity(AtomicBehavior): Important parameters: - actor: CARLA actor to execute the behavior - target_velocity: The target velocity the actor should reach - - forced_speed: Whether or not to forcefully set the actors speed + - forced_speed: Whether or not to forcefully set the actors speed. This will ony be active until a collision happens - duration[optional]: Duration in seconds of this behavior - distance[optional]: Maximum distance in meters covered by the actor during this behavior @@ -1421,7 +1420,8 @@ def __init__(self, actor, target_velocity, force_speed=False, self._target_velocity = target_velocity self._control, self._type = get_actor_control(actor) - self._map = self._actor.get_world().get_map() + self._world = CarlaDataProvider.get_world() + self._map = CarlaDataProvider.get_map() self._waypoint = self._map.get_waypoint(self._actor.get_location()) self._forced_speed = force_speed @@ -1431,6 +1431,17 @@ def __init__(self, actor, target_velocity, force_speed=False, self._start_time = 0 self._location = None + self._collision_sensor = None + + def _set_collision_sensor(self): + blueprint = self._world.get_blueprint_library().find('sensor.other.collision') + self._collision_sensor = self._world.spawn_actor(blueprint, carla.Transform(), attach_to=self._actor) + self._collision_sensor.listen(lambda event: self._stop_constant_velocity(event)) + + def _stop_constant_velocity(self, event): + """Stops the constant velocity behavior""" + self._forced_speed = False + def initialise(self): self._location = CarlaDataProvider.get_location(self._actor) self._start_time = GameTime.get_time() @@ -1443,6 +1454,8 @@ def initialise(self): self._control.hand_brake = False self._actor.apply_control(self._control) + self._set_collision_sensor() + super(KeepVelocity, self).initialise() def update(self): @@ -1465,6 +1478,9 @@ def update(self): 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)) + new_location = CarlaDataProvider.get_location(self._actor) self._distance += calculate_distance(self._location, new_location) self._location = new_location @@ -1484,13 +1500,18 @@ def terminate(self, new_status): On termination of this behavior, the throttle should be set back to 0., to avoid further acceleration. """ - - if self._type == 'vehicle': - self._control.throttle = 0.0 - 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) + try: + if self._type == 'vehicle': + self._control.throttle = 0.0 + 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) + if self._collision_sensor: + self._collision_sensor.stop() + self._collision_sensor.destroy() + except RuntimeError: + pass super(KeepVelocity, self).terminate(new_status) @@ -1715,12 +1736,12 @@ def __init__(self, actor, reference_actor, actor_target, reference_target, end_d self._end_dist = end_dist self._agent = None - self._map = CarlaDataProvider.get_map() - self._grp = GlobalRoutePlanner(self._map, 100) - def initialise(self): """Initialises the agent""" - self._agent = ConstantVelocityAgent(self._actor, map_inst=self._map, grp_inst=self._grp) + self._agent = ConstantVelocityAgent( + self._actor, + map_inst=CarlaDataProvider.get_map(), + grp_inst=CarlaDataProvider.get_global_route_planner()) def update(self): """ @@ -1802,12 +1823,16 @@ def __init__(self, actor, reference_actor, direction, speed_perc=100, self._change_time = change_time self._map = CarlaDataProvider.get_map() - self._grp = GlobalRoutePlanner(self._map, 100) + self._grp = CarlaDataProvider.get_global_route_planner() def initialise(self): """Initialises the agent""" speed = CarlaDataProvider.get_velocity(self._reference_actor) - self._agent = BasicAgent(self._actor, 3.6 * speed * self._speed_perc / 100, map_inst=self._map, grp_inst=self._grp) + self._agent = BasicAgent( + 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) def update(self): @@ -1916,7 +1941,7 @@ class BasicAgentBehavior(AtomicBehavior): The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, target_location, opt_dict=None, name="BasicAgentBehavior"): + def __init__(self, actor, target_location=None, opt_dict=None, name="BasicAgentBehavior"): """ Setup actor and maximum steer value """ @@ -1930,11 +1955,12 @@ def __init__(self, actor, target_location, opt_dict=None, name="BasicAgentBehavi def initialise(self): """Initialises the agent""" - self._agent = BasicAgent(self._actor, opt_dict=self._opt_dict) - self._plan = self._agent.trace_route( - self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), - self._map.get_waypoint(self._target_location)) - self._agent.set_global_plan(self._plan) + self._agent = BasicAgent(self._actor, opt_dict=self._opt_dict, + map_inst=CarlaDataProvider.get_map(), grp_inst=CarlaDataProvider.get_global_route_planner()) + if self._target_location: + self._plan = self._agent.set_destination( + self._target_location, CarlaDataProvider.get_location(self._actor)) + self._agent.set_global_plan(self._plan) def update(self): new_status = py_trees.common.Status.RUNNING @@ -1984,9 +2010,14 @@ def __init__(self, actor, target_location, target_speed=None, self._agent = None self._plan = None + self._map = CarlaDataProvider.get_map() + self._grp = CarlaDataProvider.get_global_route_planner() + def initialise(self): """Initialises the agent""" - self._agent = ConstantVelocityAgent(self._actor, self._target_speed * 3.6, opt_dict=self._opt_dict) + 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._plan = self._agent.trace_route( self._map.get_waypoint(CarlaDataProvider.get_location(self._actor)), self._map.get_waypoint(self._target_location)) @@ -2046,6 +2077,8 @@ def __init__(self, actor, reference_actor, target_location=None, speed_incremen self._agent = None self._plan = None + self._grp = CarlaDataProvider.get_global_route_planner() + def initialise(self): """Initialises the agent""" # Get target speed @@ -2053,7 +2086,8 @@ def initialise(self): py_trees.blackboard.Blackboard().set( "ACVAB_speed_{}".format(self._reference_actor.id), self._target_speed, overwrite=True) - self._agent = ConstantVelocityAgent(self._actor, self._target_speed, opt_dict=self._opt_dict) + self._agent = ConstantVelocityAgent(self._actor, self._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( @@ -2720,7 +2754,7 @@ class ActorFlow(AtomicBehavior): """ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, - actor_speed=20 / 3.6, actor_type="car", name="ActorFlow"): + actor_speed=20 / 3.6, add_initial_actors=False, name="ActorFlow"): """ Setup class members """ @@ -2741,7 +2775,7 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._sink_dist = sink_dist self._speed = actor_speed - self._actor_type = actor_type + self._add_initial_actors = add_initial_actors self._min_spawn_dist = spawn_dist_interval[0] self._max_spawn_dist = spawn_dist_interval[1] @@ -2750,11 +2784,51 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._actor_list = [] self._collision_sensor_list = [] + def initialise(self): + if self._add_initial_actors: + grp = CarlaDataProvider.get_global_route_planner() + plan = grp.trace_route(self._source_location, self._sink_location) + + ref_loc = plan[0][0].transform.location + for wp, _ in plan: + if wp.transform.location.distance(ref_loc) < self._spawn_dist: + 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) + + def _spawn_actor(self, transform): + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', transform, rolename='scenario', + attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + ) + if actor is None: + return py_trees.common.Status.RUNNING + + actor.set_autopilot(True) + self._tm.set_path(actor, [self._source_transform.location, self._sink_location]) + + if self._is_constant_velocity_active: + self._tm.ignore_vehicles_percentage(actor, 100) + self._tm.auto_lane_change(actor, False) + self._tm.set_desired_speed(actor, 3.6 * self._speed) + self._tm.update_vehicle_lights(actor, True) + actor.enable_constant_velocity(carla.Vector3D(self._speed, 0, 0)) # For when physics are active + self._actor_list.append(actor) + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) + sensor.listen(lambda _: self.stop_constant_velocity()) + self._collision_sensor_list.append(sensor) + def update(self): """Controls the created actors and creaes / removes other when needed""" # Control the vehicles, removing them when needed for actor in list(self._actor_list): - sink_distance = self._sink_location.distance(CarlaDataProvider.get_location(actor)) + location = CarlaDataProvider.get_location(actor) + if not location: + continue + sink_distance = self._sink_location.distance(location) if sink_distance < self._sink_dist: actor.destroy() self._actor_list.remove(actor) @@ -2764,30 +2838,13 @@ def update(self): distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_list[-1]) - distance = self._source_transform.location.distance(actor_location) + if actor_location is None: + distance = 0 + else: + distance = self._source_location.distance(actor_location) if distance > self._spawn_dist: - actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter={'base_type': self._actor_type, 'has_lights': True}, tick=False - ) - if actor is None: - return py_trees.common.Status.RUNNING - - actor.set_autopilot(True) - self._tm.set_path(actor, [self._source_location, self._sink_location]) - - if self._is_constant_velocity_active: - self._tm.ignore_vehicles_percentage(actor, 100) - self._tm.auto_lane_change(actor, False) - self._tm.set_desired_speed(actor, 3.6 * self._speed) - actor.enable_constant_velocity(carla.Vector3D(self._speed, 0, 0)) # For when physics are active - self._actor_list.append(actor) - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - - sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) - sensor.listen(lambda _: self.stop_constant_velocity()) - self._collision_sensor_list.append(sensor) + self._spawn_actor(self._source_transform) return py_trees.common.Status.RUNNING @@ -2818,12 +2875,13 @@ class BicycleFlow(AtomicBehavior): Important parameters: - plan (list(carla.Waypoint)): plan used by the bicycles. - spawn_distance_interval (list(float, float)): Distance between spawned actors - - sink_distance: Actors at this distance from the sink will be deleted - - actors_speed: Speed of the actors part of the flow [m/s] + - sink_distance (float): Actors at this distance from the sink will be deleted + - actors_speed (float): Speed of the actors part of the flow [m/s] + - add_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, name="BicycleFlow"): + actor_speed=20 / 3.6, add_initial_actors=False, name="BicycleFlow"): """ Setup class members """ @@ -2844,15 +2902,46 @@ def __init__(self, plan, 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._add_initial_actors = add_initial_actors + self._actor_data = [] - self._grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + self._grp = CarlaDataProvider.get_global_route_planner() + + def initialise(self): + if self._add_initial_actors: + ref_loc = self._plan[0][0].transform.location + for wp, _ in self._plan: + if wp.transform.location.distance(ref_loc) < self._spawn_dist: + 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) + + def _spawn_actor(self, transform): + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', transform, rolename='scenario', + attribute_filter={'base_type': 'bicycle'}, tick=False + ) + if actor is None: + return + + actor.set_target_velocity(self._speed * self._source_vector) + controller = BasicAgent(actor, 3.6 * self._speed, + map_inst=CarlaDataProvider.get_map(), grp_inst=CarlaDataProvider.get_global_route_planner()) + controller.set_global_plan(self._plan) + + self._actor_data.append([actor, controller]) + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) def update(self): """Controls the created actors and creaes / removes other when needed""" # Control the vehicles, removing them when needed for actor_data in list(self._actor_data): actor, controller = actor_data - sink_distance = self._sink_location.distance(CarlaDataProvider.get_location(actor)) + location = CarlaDataProvider.get_location(actor) + if not location: + continue + sink_distance = self._sink_location.distance(location) if sink_distance < self._sink_dist: actor.destroy() self._actor_data.remove(actor_data) @@ -2864,23 +2953,13 @@ def update(self): distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_data[-1][0]) - distance = self._source_location.distance(actor_location) + if actor_location is None: + distance = 0 + else: + distance = self._source_location.distance(actor_location) if distance > self._spawn_dist: - actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter={'base_type': 'bicycle'}, tick=False - ) - if actor is None: - return py_trees.common.Status.RUNNING - - actor.set_target_velocity(self._speed * self._source_vector) - controller = BasicAgent(actor, 3.6 * self._speed, - map_inst=CarlaDataProvider.get_map(), grp_inst=self._grp) - controller.set_global_plan(self._plan) - - self._actor_data.append([actor, controller]) - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._spawn_actor(self._source_transform) return py_trees.common.Status.RUNNING @@ -3332,15 +3411,13 @@ class ScenarioTriggerer(AtomicBehavior): WINDOWS_SIZE = 5 - def __init__(self, actor, route, blackboard_list, distance, - repeat_scenarios=False, debug=False, name="ScenarioTriggerer"): + def __init__(self, actor, route, blackboard_list, distance, debug=False, name="ScenarioTriggerer"): """ Setup class members """ super(ScenarioTriggerer, self).__init__(name) self._world = CarlaDataProvider.get_world() self._map = CarlaDataProvider.get_map() - self._repeat = repeat_scenarios self._debug = debug self._actor = actor @@ -3396,7 +3473,7 @@ def update(self): condition2 = bool(not value) # Already done, if needed - condition3 = bool(self._repeat or black_var_name not in self._triggered_scenarios) + condition3 = bool(black_var_name not in self._triggered_scenarios) if condition1 and condition2 and condition3: _ = blackboard.set(black_var_name, True) @@ -3468,7 +3545,7 @@ def initialise(self): self._start_time = GameTime.get_time() actor_dict[self._actor.id].update_target_speed(self.max_speed, start_time=self._start_time) - self._global_rp = GlobalRoutePlanner(CarlaDataProvider.get_world().get_map(), 1.0) + self._global_rp = CarlaDataProvider.get_global_route_planner() super(KeepLongitudinalGap, self).initialise() @@ -3520,25 +3597,25 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status -class SwitchOutsideRouteLanesTest(AtomicBehavior): +class SwitchWrongDirectionTest(AtomicBehavior): """ Atomic that switch the OutsideRouteLanesTest criterion. Args: - activate (bool): True: activate; False: deactivate + active (bool): True: activated; False: deactivated name (str): name of the behavior """ - def __init__(self, activate, name="SwitchOutsideRouteLanesTest"): + def __init__(self, active, name="SwitchWrongDirectionTest"): """ Setup class members """ - super(SwitchOutsideRouteLanesTest, self).__init__(name) - self._activate = activate + self._active = active + super().__init__(name) def update(self): - py_trees.blackboard.Blackboard().set("AC_SwitchOutsideRouteLanesTest", self._activate, overwrite=True) + py_trees.blackboard.Blackboard().set("AC_SwitchWrongDirectionTest", self._active, overwrite=True) return py_trees.common.Status.SUCCESS class SwitchMinSpeedCriteria(AtomicBehavior): @@ -3724,9 +3801,11 @@ def update(self): return py_trees.common.Status.RUNNING def _destroy_walker(self, walker, controller): - controller.stop() - controller.destroy() - walker.destroy() + if controller: + controller.stop() + controller.destroy() + if walker: + walker.destroy() def terminate(self, new_status): """ diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 861a070b7..be22f8e71 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1004,7 +1004,7 @@ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): self._last_lane_id = None self._total_distance = 0 self._wrong_distance = 0 - self._active = True + self._wrong_direction_active = True def update(self): """ @@ -1023,15 +1023,15 @@ def update(self): return new_status # Deactivate/Activate checking by blackboard message - active = py_trees.blackboard.Blackboard().get('AC_SwitchOutsideRouteLanesTest') + active = py_trees.blackboard.Blackboard().get('AC_SwitchWrongDirectionTest') if active is not None: - self._active = active - py_trees.blackboard.Blackboard().set("AC_SwitchOutsideRouteLanesTest", None, overwrite=True) + self._wrong_direction_active = active + py_trees.blackboard.Blackboard().set("AC_SwitchWrongDirectionTest", None, overwrite=True) self._is_outside_driving_lanes(location) self._is_at_wrong_lane(location) - if self._active and (self._outside_lane_active or 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 @@ -1051,7 +1051,7 @@ def update(self): self._total_distance += new_dist # And to the wrong one if outside route lanes - if self._active and (self._outside_lane_active or self._wrong_lane_active): + if self._outside_lane_active or (self._wrong_direction_active and self._wrong_lane_active): self._wrong_distance += new_dist self._current_index = index @@ -1948,7 +1948,7 @@ def update(self): return new_status -class CheckMinSpeed(Criterion): +class MinSpeedRouteTest(Criterion): """ Check at which stage of the route is the actor at each tick @@ -1963,7 +1963,7 @@ class CheckMinSpeed(Criterion): # Thresholds to return that a route has been completed MULTIPLIER = 1.5 # % - def __init__(self, actor, name="CheckMinSpeed", terminate_on_failure=False): + def __init__(self, actor, name="MinSpeedRouteTest", terminate_on_failure=False): """ """ super().__init__(name, actor, terminate_on_failure=terminate_on_failure) @@ -1976,9 +1976,6 @@ def __init__(self, actor, name="CheckMinSpeed", terminate_on_failure=False): self._active = True - self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION) - self.events.append(self._traffic_event) - def update(self): """ Check if the actor location is within trigger region @@ -1990,10 +1987,10 @@ def update(self): if velocity is None: return new_status - set_speed_data = py_trees.blackboard.Blackboard().get('BA_CheckMinSpeed') + set_speed_data = py_trees.blackboard.Blackboard().get('BA_MinSpeedRouteTest') if set_speed_data is not None: self._active = set_speed_data - py_trees.blackboard.Blackboard().set('BA_CheckMinSpeed', None, True) + py_trees.blackboard.Blackboard().set('BA_MinSpeedRouteTest', None, True) if self._active: # Get the speed of the surrounding Background Activity @@ -2020,7 +2017,9 @@ def terminate(self, new_status): Set the actual value as a percentage of the two mean speeds, the test status to failure if not successful and terminate """ - if self._speed_points > 0: + if self._mean_speed == 0: + self.actual_value = 0 + elif self._speed_points > 0: self._mean_speed /= self._speed_points self._actor_speed /= self._speed_points self.actual_value = round(self._actor_speed / self._mean_speed * 100, 2) @@ -2032,8 +2031,11 @@ def terminate(self, new_status): else: self.test_status = "FAILURE" - self._traffic_event.set_dict({'percentage': self.actual_value}) - self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) + if self.test_status == "FAILURE": + self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION) + self._traffic_event.set_dict({'percentage': self.actual_value}) + self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) + self.events.append(self._traffic_event) super().terminate(new_status) @@ -2096,13 +2098,23 @@ def update(self): return new_status def terminate(self, new_status): - mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) - self.actual_value = mean_speed / self._target_speed *100 - self.actual_value = round(self.actual_value, 2) - - if self.actual_value >= self.success_value: - self.test_status = "SUCCESS" + if not len(self._ev_speed_log): + self.actual_value = 100 else: - self.test_status = "FAILURE" + mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) + self.actual_value = mean_speed / self._target_speed *100 + self.actual_value = round(self.actual_value, 2) + + if self.actual_value >= self.success_value: + self.test_status = "SUCCESS" + else: + self.test_status = "FAILURE" + + if self.test_status == "FAILURE": + traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE) + traffic_event.set_dict({'speed_percentage': self.actual_value}) + traffic_event.set_message( + f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") + self.events.append(traffic_event) super().terminate(new_status) diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index 47df56558..ee746e3bb 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -31,6 +31,7 @@ class TrafficEventType(Enum): OUTSIDE_ROUTE_LANES_INFRACTION = 12 VEHICLE_BLOCKED = 13 MIN_SPEED_INFRACTION = 14 + YIELD_TO_EMERGENCY_VEHICLE = 15 class TrafficEvent(object): diff --git a/srunner/scenariomanager/weather_sim.py b/srunner/scenariomanager/weather_sim.py index 0160e5a8d..2ad9713ed 100644 --- a/srunner/scenariomanager/weather_sim.py +++ b/srunner/scenariomanager/weather_sim.py @@ -235,6 +235,8 @@ def get_route_weathers(self): def interpolate(prev_w, next_w, perc, name): x0 = prev_w[0] x1 = next_w[0] + if x0 == x1: + raise ValueError("Two weather keypoints have the same route percentage") y0 = getattr(prev_w[1], name) y1 = getattr(next_w[1], name) return y0 + (y1 - y0) * (perc - x0) / (x1 - x0) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 45cec10ab..420b556d4 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -15,8 +15,6 @@ import py_trees import carla -from agents.navigation.global_route_planner import GlobalRoutePlanner - from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest @@ -24,11 +22,10 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import (SwitchRouteSources, - ExtentExitRoadSpace, - SwitchLane, - RemoveJunctionEntry, - ClearJunction) -from srunner.tools.scenario_helper import get_same_dir_lanes + ChangeOppositeBehavior, + HandleJunctionScenario, + RemoveRoadLane) +from srunner.tools.scenario_helper import get_same_dir_lanes, generate_target_waypoint_in_route def convert_dict_to_location(actor_dict): """ @@ -75,12 +72,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m - super(EnterActorFlow, self).__init__("EnterActorFlow", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + ego_location = config.trigger_points[0].location + self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) + + super().__init__("EnterActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _create_behavior(self): """ @@ -93,30 +93,34 @@ def _create_behavior(self): # Get all lanes source_wps = get_same_dir_lanes(source_wp) sink_wps = get_same_dir_lanes(sink_wp) - num_flows = min(len(source_wps), len(sink_wps)) root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - for i in range(num_flows): + + for source_wp, sink_wp in zip(source_wps, sink_wps): + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( - source_wps[i], sink_wps[i], self._source_dist_interval, self._sink_distance, self._flow_speed)) - root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wps[i].transform.location, self._sink_distance)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry([source_wp], True)) - sequence.add_child(ClearJunction()) - - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + grp = CarlaDataProvider.get_global_route_planner() route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) extra_space = 0 for i in range(-2, -len(route)-1, -1): current_wp = route[i][0] extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) if current_wp.is_junction: - sequence.add_child(ExtentExitRoadSpace(extra_space)) break + sequence.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=True, + remove_entries=source_wps, + remove_exits=[], + stop_entries=False, + extend_road_exit=extra_space + )) sequence.add_child(SwitchRouteSources(False)) sequence.add_child(root) if self.route_mode: @@ -140,11 +144,58 @@ def __del__(self): self.remove_all_actors() -class CrossActorFlow(BasicScenario): +class EnterActorFlowV2(EnterActorFlow): """ - This class holds everything required for a scenario in which another vehicle runs a red light - in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, - ambulances or firetrucks. + Variation of EnterActorFlow for special highway entry exits with dedicated lanes + """ + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + # Get all lanes + sink_wps = get_same_dir_lanes(sink_wp) + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + for sink_wp in sink_wps: + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + + exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) + exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match + + sequence = py_trees.composites.Sequence() + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=True, + remove_entries=[source_wp], + remove_exits= get_same_dir_lanes(exit_wp), + stop_entries=False, + extend_road_exit=0 + )) + sequence.add_child(SwitchRouteSources(False)) + + sequence.add_child(root) + if self.route_mode: + sequence.add_child(SwitchRouteSources(True)) + + return sequence + + +class HighwayExit(BasicScenario): + """ + This scenario is similar to CrossActorFlow + It will remove the BackgroundActivity from the lane where ActorFlow starts. + Then vehicles (cars) will start driving from start_actor_flow location to end_actor_flow location + in a relatively high speed, forcing the ego to accelerate to cut in the actor flow + then exit from the highway. + This scenario works when Background Activity is running in route mode. And there should be no junctions in front of the ego. """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -176,22 +227,21 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m - super(CrossActorFlow, self).__init__("CrossActorFlow", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("HighwayExit", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _create_behavior(self): """ - Hero vehicle is entering a junction in an urban area, at a signalized intersection, - while another actor runs a red lift, forcing the ego to break. + Vehicles run from the start to the end continuously. """ source_wp = self._map.get_waypoint(self._start_actor_flow) sink_wp = self._map.get_waypoint(self._end_actor_flow) - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) + grp = CarlaDataProvider.get_global_route_planner() route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) junction_id = None for wp, _ in route: @@ -202,17 +252,16 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) end_condition = py_trees.composites.Sequence() end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) - + root.add_child(end_condition) sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry([source_wp], True)) - sequence.add_child(ClearJunction()) + sequence.add_child(RemoveRoadLane(source_wp)) sequence.add_child(root) return sequence @@ -233,14 +282,162 @@ def __del__(self): self.remove_all_actors() -class HighwayExit(BasicScenario): +class MergerIntoSlowTraffic(BasicScenario): """ - This scenario is similar to CrossActorFlow + This scenario is similar to EnterActorFlow It will remove the BackgroundActivity from the lane where ActorFlow starts. Then vehicles (cars) will start driving from start_actor_flow location to end_actor_flow location - in a relatively high speed, forcing the ego to accelerate to cut in the actor flow - then exit from the highway. - This scenario works when Background Activity is running in route mode. And there should be no junctions in front of the ego. + in a relatively low speed, ego car must merger into this slow traffic flow. + This scenario works when Background Activity is running in route mode. And applies to a confluence + area at a highway intersection. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + self._trigger_point=config.trigger_points[0].location + + self._sink_distance = 2 + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 10 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [5, 7] # m + + ego_location = config.trigger_points[0].location + self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) + + super().__init__("MergerIntoSlowTraffic", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + the ego vehicle mergers into a slow traffic flow from the freeway entrance. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + # Get all lanes + source_wps = get_same_dir_lanes(source_wp) + sink_wps = get_same_dir_lanes(sink_wp) + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + for source_wp, sink_wp in zip(source_wps, sink_wps): + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + + sequence = py_trees.composites.Sequence() + if self.route_mode: + + grp = CarlaDataProvider.get_global_route_planner() + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + extra_space = 0 + for i in range(-2, -len(route)-1, -1): + current_wp = route[i][0] + extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + if current_wp.is_junction: + break + + sequence.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=True, + remove_entries=source_wp, + remove_exits=[], + stop_entries=False, + extend_road_exit=extra_space + )) + sequence.add_child(SwitchRouteSources(False)) + sequence.add_child(root) + if self.route_mode: + sequence.add_child(SwitchRouteSources(True)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() + + +class MergerIntoSlowTrafficV2(MergerIntoSlowTraffic): + """ + Variation of MergerIntoSlowTraffic + """ + + def _create_behavior(self): + """ + the ego vehicle mergers into a slow traffic flow from the freeway entrance. + """ + source_wp = self._map.get_waypoint(self._start_actor_flow) + sink_wp = self._map.get_waypoint(self._end_actor_flow) + + sink_wps = get_same_dir_lanes(sink_wp) + + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(ActorFlow( + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + for sink_wp in sink_wps: + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + + exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) + exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match + + sequence = py_trees.composites.Sequence() + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=True, + remove_entries=[source_wp], + remove_exits=get_same_dir_lanes(exit_wp), + stop_entries=False, + extend_road_exit=0 + )) + sequence.add_child(SwitchRouteSources(False)) + sequence.add_child(root) + if self.route_mode: + sequence.add_child(SwitchRouteSources(True)) + + return sequence + + +class InterurbanActorFlow(BasicScenario): + """ + Scenario specifically made for the interurban intersections, + where the ego leaves the interurban road by turning left, crossing an actor flow. """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -272,46 +469,52 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m + self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) + exit_wp = generate_target_waypoint_in_route(self._reference_wp, config.route) + exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match + self._other_entry_wp = exit_wp.get_left_lane() + if not self._other_entry_wp or self._other_entry_wp.lane_type != carla.LaneType.Driving: + raise ValueError("Couldn't find an end position") - super(HighwayExit, self).__init__("HighwayExit", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("InterurbanActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _create_behavior(self): """ - Vehicles run from the start to the end continuously. + Create an actor flow at the opposite lane which the ego has to cross """ source_wp = self._map.get_waypoint(self._start_actor_flow) sink_wp = self._map.get_waypoint(self._end_actor_flow) - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) - route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) - junction_id = None - for wp, _ in route: - if wp.is_junction: - junction_id = wp.get_junction().id - break root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) end_condition = py_trees.composites.Sequence() - end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(end_condition) sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(SwitchLane(source_wp.lane_id, False)) + sequence.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=False, + remove_entries=[source_wp, self._other_entry_wp], + remove_exits=[], + stop_entries=False, + extend_road_exit=0 + )) + sequence.add_child(ChangeOppositeBehavior(active=False)) sequence.add_child(root) - if self.route_mode: - sequence.add_child(SwitchLane(source_wp.lane_id, True)) + sequence.add_child(ChangeOppositeBehavior(active=True)) return sequence @@ -329,16 +532,13 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() - -class MergerIntoSlowTraffic(BasicScenario): + +class InterurbanAdvancedActorFlow(BasicScenario): """ - This scenario is similar to EnterActorFlow - It will remove the BackgroundActivity from the lane where ActorFlow starts. - Then vehicles (cars) will start driving from start_actor_flow location to end_actor_flow location - in a relatively low speed, ego car must merger into this slow traffic flow. - This scenario works when Background Activity is running in route mode. And applies to a confluence - area at a highway intersection. + Scenario specifically made for the interurban intersections, + where the ego incorportates into the interurban road by turning left, + first crossing an actor flow, and then merging into another one. """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -351,9 +551,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) - self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._trigger_point=config.trigger_points[0].location + self._start_actor_flow_1 = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow_1 = convert_dict_to_location(config.other_parameters['end_actor_flow']) self._sink_distance = 2 @@ -370,49 +569,66 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m - super(MergerIntoSlowTraffic, self).__init__("MergerIntoSlowTraffic", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) + self._exit_wp = generate_target_waypoint_in_route(self._reference_wp, config.route) + + super().__init__("InterurbanAdvancedActorFlow", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _create_behavior(self): """ the ego vehicle mergers into a slow traffic flow from the freeway entrance. """ - source_wp = self._map.get_waypoint(self._start_actor_flow) - sink_wp = self._map.get_waypoint(self._end_actor_flow) - trigger_wp=self._map.get_waypoint(self._trigger_point) + source_wp_1 = self._map.get_waypoint(self._start_actor_flow_1) + sink_wp_1 = self._map.get_waypoint(self._end_actor_flow_1) + + source_wp_2 = sink_wp_1.get_left_lane() + if not source_wp_2 or source_wp_2.lane_type != carla.LaneType.Driving: + raise ValueError("Couldn't find a position for the actor flow") + sink_wp_2 = source_wp_1.get_left_lane() + if not sink_wp_2 or sink_wp_2.lane_type != carla.LaneType.Driving: + raise ValueError("Couldn't find a position for the actor flow") + root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp_2.transform.location, self._sink_distance)) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) - - end_condition = py_trees.composites.Sequence() - end_condition.add_child(InTriggerDistanceToLocation( - self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) - root.add_child(end_condition) + source_wp_1, sink_wp_1, self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(ActorFlow( + source_wp_2, sink_wp_2, self._source_dist_interval, self._sink_distance, self._flow_speed)) sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry([trigger_wp,source_wp], False)) - - grp = GlobalRoutePlanner(CarlaDataProvider.get_map(), 2.0) - route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + grp = CarlaDataProvider.get_global_route_planner() + route = grp.trace_route(source_wp_2.transform.location, sink_wp_2.transform.location) extra_space = 0 for i in range(-2, -len(route)-1, -1): current_wp = route[i][0] extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) if current_wp.is_junction: - sequence.add_child(ExtentExitRoadSpace(extra_space)) break + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[source_wp_1, source_wp_2], + remove_exits=[self._exit_wp], + stop_entries=False, + extend_road_exit=extra_space + )) sequence.add_child(SwitchRouteSources(False)) + sequence.add_child(ChangeOppositeBehavior(active=False)) + sequence.add_child(root) + if self.route_mode: sequence.add_child(SwitchRouteSources(True)) + sequence.add_child(ChangeOppositeBehavior(active=True)) return sequence diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 95fb59c67..cc728ae56 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -8,7 +8,6 @@ Scenario spawning elements to make the town dynamic and interesting """ -import math from collections import OrderedDict import py_trees @@ -16,7 +15,6 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior -from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes JUNCTION_ENTRY = 'entry' @@ -74,7 +72,7 @@ def draw_string(world, location, string='', debug_type=DEBUG_ROAD, persistent=Fa v_shift, _ = DEBUG_TYPE.get(DEBUG_SMALL) l_shift = carla.Location(z=v_shift) color = DEBUG_COLORS.get(debug_type, DEBUG_ROAD) - life_time = 0.07 if not persistent else 100000 + life_time = 0.06 if not persistent else 100000 world.debug.draw_string(location + l_shift, string, False, color, life_time) def draw_point(world, location, point_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, persistent=False): @@ -82,9 +80,19 @@ def draw_point(world, location, point_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, p v_shift, size = DEBUG_TYPE.get(point_type, DEBUG_SMALL) l_shift = carla.Location(z=v_shift) color = DEBUG_COLORS.get(debug_type, DEBUG_ROAD) - life_time = 0.07 if not persistent else 100000 + life_time = 0.06 if not persistent else 100000 world.debug.draw_point(location + l_shift, size, color, life_time) +def draw_arrow(world, location1, location2, arrow_type=DEBUG_SMALL, debug_type=DEBUG_ROAD, persistent=False): + """Utility function to draw debugging points""" + if location1 == location2: + draw_point(world, location1, arrow_type, debug_type, persistent) + v_shift, thickness = DEBUG_TYPE.get(arrow_type, DEBUG_SMALL) + l_shift = carla.Location(z=v_shift) + color = DEBUG_COLORS.get(debug_type, DEBUG_ROAD) + life_time = 0.06 if not persistent else 100000 + world.debug.draw_arrow(location1 + l_shift, location2 + l_shift, thickness, thickness, color, life_time) + class Source(object): @@ -92,14 +100,11 @@ class Source(object): Source object to store its position and its responsible actors """ - def __init__(self, wp, actors, entry_lane_wp='', dist_to_ego=0, active=True): # pylint: disable=invalid-name + def __init__(self, wp, actors, entry_lane_wp='', active=True): # pylint: disable=invalid-name self.wp = wp # pylint: disable=invalid-name self.actors = actors self.active = active - # For road sources - self.dist_to_ego = dist_to_ego - # For junction sources self.entry_lane_wp = entry_lane_wp self.previous_lane_keys = [] # Source lane and connecting lanes of the previous junction @@ -133,15 +138,11 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind self.exit_dict = OrderedDict() self.actor_dict = OrderedDict() - # Scenario interactions - self.scenario_info = { - 'direction': None, - 'remove_entries': False, - 'remove_middle': False, - 'remove_exits': False, - } - self.stop_entries = False + # Junction scenario variables + self.stop_non_route_entries = False + self.clear_middle = False self.inactive_entry_keys = [] + self.inactive_exit_keys = [] def contains(self, other_junction): """Checks whether or not a carla.Junction is part of the class""" @@ -152,61 +153,6 @@ def contains(self, other_junction): return False -class BackgroundActivity(BasicScenario): - - """ - Implementation of a scenario to spawn a set of background actors, - and to remove traffic jams in background traffic - - This is a single ego vehicle scenario - """ - - def __init__(self, world, ego_vehicle, config, route, debug_mode=False, timeout=0): - """ - Setup all relevant parameters and create scenario - """ - self._map = CarlaDataProvider.get_map() - self.ego_vehicle = ego_vehicle - self.route = route - self.config = config - self.debug = debug_mode - self.timeout = timeout # Timeout of scenario in seconds - - super(BackgroundActivity, self).__init__("BackgroundActivity", - [ego_vehicle], - config, - world, - debug_mode, - terminate_on_failure=True, - criteria_enable=True) - - def _create_behavior(self): - """ - Basic behavior do nothing, i.e. Idle - """ - # Check if a vehicle is further than X, destroy it if necessary and respawn it - return BackgroundBehavior(self.ego_vehicle, self.route) - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - return [] - - def _initialize_environment(self, world): - """ - Nothing to do here, avoid settign the weather - """ - pass - - def __del__(self): - """ - Remove all actors upon deletion - """ - pass - - class BackgroundBehavior(AtomicBehavior): """ Handles the background activity @@ -223,14 +169,18 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._tm = CarlaDataProvider.get_client().get_trafficmanager( CarlaDataProvider.get_traffic_manager_port()) self._tm.global_percentage_speed_difference(0.0) + self._rng = CarlaDataProvider.get_random_seed() # Global variables self._ego_actor = ego_actor self._ego_state = EGO_ROAD + self._ego_wp = None + self._ego_key = "" self._route_index = 0 self._get_route_data(route) self._actors_speed_perc = {} # Dictionary actor - percentage self._all_actors = [] + self._lane_width_threshold = 2 # Used to stop some behaviors at narrow lanes to avoid problems [m] self._spawn_vertical_shift = 0.2 self._reuse_dist = 10 # When spawning actors, might reuse actors closer to this distance @@ -238,27 +188,25 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._fake_junction_ids = [] self._fake_lane_pair_keys = [] - # Vehicle variables - self._spawn_dist = 15 # Distance between spawned vehicles [m] - # Road variables self._road_dict = {} # Dictionary lane key -> actor source self._road_checker_index = 0 - self._road_ego_key = "" - self._road_front_vehicles = 3 # Amount of vehicles in front of the ego - self._road_back_vehicles = 3 # Amount of vehicles behind the ego - self._radius_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego + self._road_front_vehicles = 2 # Amount of vehicles in front of the ego + self._road_back_vehicles = 2 # Amount of vehicles behind the ego + self._radius_increase_ratio = 2.5 # Meters the radius increases per m/s of the ego + + self._base_junction_detection = 30 + self._detection_ratio = 1.5 # Meters the radius increases per m/s of the ego - self._road_num_front_vehicles = self._road_front_vehicles # Checks the real amount of actors in the front of the ego self._road_extra_front_actors = 0 # For cases where we want more space but not more vehicles - self._road_time_delay = 3 + self._road_spawn_dist = 15 # Distance between spawned vehicles [m] self._base_min_radius = 0 self._base_max_radius = 0 self._min_radius = 0 self._max_radius = 0 - self._junction_detection_dist = 0 + self._detection_dist = 0 self._get_road_radius() # Junction variables @@ -266,21 +214,25 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._active_junctions = [] # List of all the active junctions self._junction_sources_dist = 40 # Distance from the entry sources to the junction [m] - self._junction_sources_max_actors = 5 # Maximum vehicles alive at the same time per source + self._junction_sources_max_actors = 4 # Maximum vehicles alive at the same time per source + self._junction_spawn_dist = 15 # Distance between spawned vehicles [m] + + self._junction_source_perc = 70 # Probability [%] of the source being created # Opposite lane variables self._opposite_actors = [] self._opposite_sources = [] self._opposite_route_index = 0 - self._opposite_removal_dist = 30 # Distance at which actors are destroyed - self._opposite_sources_dist = 60 # Distance from the ego to the opposite sources [m] - self._opposite_sources_max_actors = 8 # Maximum vehicles alive at the same time per source + self._opposite_sources_dist = 100 # Distance from the ego to the opposite sources [m] self._opposite_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego - self._opposite_spawn_dist = self._spawn_dist # Distance between spawned vehicles [m] + self._opposite_spawn_dist = 50 # Distance between spawned vehicles [m] + + self._active_opposite_sources = True # Flag to (de)activate all opposite sources # Scenario variables: self._stopped_road_actors = [] # Actors stopped by a hard break scenario + # self._clear_actors = [] # Actors that will be cleaned up by a junction self._route_sources_active = True @@ -307,8 +259,8 @@ def _get_road_radius(self): Computes the min and max radius of the road behaviorm which will determine the speed of the vehicles. Vehicles closer than the min radius maintain full speed, while those further than max radius are stopped. Between the two, the velocity decreases linearly""" - self._base_min_radius = (self._road_num_front_vehicles + self._road_extra_front_actors) * self._spawn_dist - self._base_max_radius = (self._road_num_front_vehicles + self._road_extra_front_actors + 1) * self._spawn_dist + self._base_min_radius = (self._road_front_vehicles + self._road_extra_front_actors) * self._road_spawn_dist + self._base_max_radius = (self._road_front_vehicles + self._road_extra_front_actors + 1) * self._road_spawn_dist self._min_radius = self._base_min_radius self._max_radius = self._base_max_radius @@ -316,7 +268,6 @@ def initialise(self): """Creates the background activity actors. Pressuposes that the ego is at a road""" self._create_junction_dict() ego_wp = self._route[0] - self._road_ego_key = get_lane_key(ego_wp) same_dir_wps = get_same_dir_lanes(ego_wp) self._initialise_road_behavior(same_dir_wps) @@ -348,9 +299,11 @@ def update(self): else: self._update_road_actors() self._update_road_sources(prev_ego_index) - self._check_lane_merges(prev_ego_index) - self._move_opposite_sources(prev_ego_index) + + self._monitor_topology_changes(prev_ego_index) self._monitor_road_changes(prev_ego_index) + + self._move_opposite_sources(prev_ego_index) self._update_opposite_sources() # Update non junction sources @@ -433,47 +386,79 @@ def _get_junctions_data(self): def _filter_fake_junctions(self, data): """ - Filters fake junctions. As a general note, a fake junction is that where no road lane divide in two. - However, this might fail for some CARLA maps, so check junctions which have all lanes straight too + Filters fake junctions. A fake junction is that which has no intersecting maneuvers + (i.e, no two maneuvers start / end at the same lane). + However, this fails for highway entry / exits with dedicated lanes, so specifically check those """ fake_data = [] filtered_data = [] - threshold = math.radians(15) for junction_data in data: - used_entry_lanes = [] - used_exit_lanes = [] - for junction in junction_data.junctions: - for entry_wp, exit_wp in junction.get_waypoints(carla.LaneType.Driving): - entry_wp = self._get_junction_entry_wp(entry_wp) - if not entry_wp: - continue - if get_lane_key(entry_wp) not in used_entry_lanes: - used_entry_lanes.append(get_lane_key(entry_wp)) + if len (junction_data.junctions) > 1: + filtered_data.append(junction_data) + continue # These are always junctions - exit_wp = self._get_junction_exit_wp(exit_wp) - if not exit_wp: - continue - if get_lane_key(exit_wp) not in used_exit_lanes: - used_exit_lanes.append(get_lane_key(exit_wp)) + junction = junction_data.junctions[0] + + found_intersecting_maneuvers = False + used_entries = [] + used_exits = [] + + # Search for intersecting maneuvers + for entry_wp, exit_wp in junction.get_waypoints(carla.LaneType.Driving): + entry_key = get_lane_key(self._get_junction_entry_wp(entry_wp)) + exit_key = get_lane_key(self._get_junction_exit_wp(exit_wp)) + + # Check if a maneuver starts / ends at another one. + # Checking if it was used isn't enough as some maneuvers are repeated in CARLA maps. + # Instead, check if the index of both entry and exit are different. + entry_index = -1 if entry_key not in used_entries else used_entries.index(entry_key) + exit_index = -1 if exit_key not in used_exits else used_exits.index(exit_key) - if not used_entry_lanes and not used_exit_lanes: - fake_data.append(junction_data) + if exit_index != entry_index: + found_intersecting_maneuvers = True + break + + used_entries.append(entry_key) + used_exits.append(exit_key) + + if found_intersecting_maneuvers: + filtered_data.append(junction_data) continue - found_turn = False - for entry_wp, exit_wp in junction_data.junctions[0].get_waypoints(carla.LaneType.Driving): - entry_heading = entry_wp.transform.get_forward_vector() - exit_heading = exit_wp.transform.get_forward_vector() - dot = entry_heading.x * exit_heading.x + entry_heading.y * exit_heading.y - if dot < math.cos(threshold): - found_turn = True + # Search for highway dedicated lane entries. + found_highway = False + used_entry_roads = {} + used_exit_roads = {} + for entry_wp, exit_wp in junction.get_waypoints(carla.LaneType.Driving): + entry_road_key = get_road_key(self._get_junction_entry_wp(entry_wp)) + exit_road_key = get_road_key(self._get_junction_exit_wp(exit_wp)) + + # Entries / exits with dedicated lanes have no intersecting maneuvers + # (as the entry / exit is a lane that finishes, not a maneuvers part of a junction), + # so they are missfiltered as fake junctions. + # Detect them by an entry road having 3 or more lanes. TODO: Improve this + if entry_road_key in used_entry_roads: + used_entry_roads[entry_road_key] += 1 + else: + used_entry_roads[entry_road_key] = 0 + + if exit_road_key in used_exit_roads: + used_exit_roads[exit_road_key] += 1 + else: + used_exit_roads[exit_road_key] = 0 + + if used_entry_roads[entry_road_key] >= 3 or used_exit_roads[exit_road_key] >= 3: + found_highway = True break - if not found_turn: - fake_data.append(junction_data) - else: + if found_highway: filtered_data.append(junction_data) + continue + + fake_data.append(junction_data) + + # TODO: Recheck for old CARLA maps return fake_data, filtered_data @@ -780,28 +765,28 @@ def _add_junctions_topology(self, route_data): entry_print = '> J Entry Lanes: ' for entry_wp in entry_lane_wps: key = get_lane_key(entry_wp) - entry_print += key + ' ' * (6 - len(key)) + entry_print += key + ' ' * (8 - len(key)) print(entry_print) exit_print = '> J Exit Lanes: ' for exit_wp in exit_lane_wps: key = get_lane_key(exit_wp) - exit_print += key + ' ' * (6 - len(key)) + exit_print += key + ' ' * (8 - len(key)) print(exit_print) route_entry = '> R-J Entry Lanes: ' for entry_key in junction_data.route_entry_keys: - route_entry += entry_key + ' ' * (6 - len(entry_key)) + route_entry += entry_key + ' ' * (8 - len(entry_key)) print(route_entry) route_exit = '> R-J Route Exit Lanes: ' for exit_key in junction_data.route_exit_keys: - route_exit += exit_key + ' ' * (6 - len(exit_key)) + route_exit += exit_key + ' ' * (8 - len(exit_key)) print(route_exit) route_oppo_entry = '> R-J Oppo Entry Lanes: ' for oppo_entry_key in junction_data.opposite_entry_keys: - route_oppo_entry += oppo_entry_key + ' ' * (6 - len(oppo_entry_key)) + route_oppo_entry += oppo_entry_key + ' ' * (8 - len(oppo_entry_key)) print(route_oppo_entry) route_oppo_exit = '> R-J Oppo Exit Lanes: ' for oppo_exit_key in junction_data.opposite_exit_keys: - route_oppo_exit += oppo_exit_key + ' ' * (6 - len(oppo_exit_key)) + route_oppo_exit += oppo_exit_key + ' ' * (8 - len(oppo_exit_key)) print(route_oppo_exit) def _is_junction(self, waypoint): @@ -848,54 +833,8 @@ def _switch_to_junction_mode(self, junction): # TODO: Else should map the source to the entry and add it self._road_dict.clear() - self._road_num_front_vehicles = self._road_front_vehicles self._opposite_sources.clear() - def _initialise_junction_scenario(self, direction, remove_exits, remove_entries, remove_middle): - """ - Removes all vehicles in a particular 'direction' as well as all actors inside the junction. - Additionally, activates some flags to ensure the junction is empty at all times - """ - if self._active_junctions: - scenario_junction = self._active_junctions[0] - scenario_junction.scenario_info = { - 'direction': direction, - 'remove_entries': remove_entries, - 'remove_middle': remove_middle, - 'remove_exits': remove_exits, - } - entry_direction_keys = scenario_junction.entry_directions[direction] - actor_dict = scenario_junction.actor_dict - - if remove_entries: - for entry_source in scenario_junction.entry_sources: - if get_lane_key(entry_source.entry_lane_wp) in entry_direction_keys: - # Source is affected - actors = entry_source.actors - for actor in list(actors): - if actor_dict[actor]['state'] == JUNCTION_ENTRY: - # Actor is at the entry lane - self._destroy_actor(actor) - - if remove_exits: - for exit_dir in scenario_junction.exit_directions[direction]: - for actor in list(scenario_junction.exit_dict[exit_dir]['actors']): - self._destroy_actor(actor) - - if remove_middle: - actor_dict = scenario_junction.actor_dict - for actor in list(actor_dict): - if actor_dict[actor]['state'] == JUNCTION_MIDDLE: - self._destroy_actor(actor) - - elif self._junctions: - self._junctions[0].scenario_info = { - 'direction': direction, - 'remove_entries': remove_entries, - 'remove_middle': remove_middle, - 'remove_exits': remove_exits, - } - def _end_junction_behavior(self, junction): """ Destroys unneeded actors (those that aren't part of the route's road), @@ -956,7 +895,6 @@ def _end_junction_behavior(self, junction): self._ego_state = EGO_ROAD self._initialise_opposite_sources() self._initialise_road_checker() - self._road_ego_key = self._get_ego_route_lane_key(self._ego_wp) def _search_for_next_junction(self): """Check if closeby to a junction. The closest one will always be the first""" @@ -965,7 +903,7 @@ def _search_for_next_junction(self): ego_accum_dist = self._accum_dist[self._route_index] junction_accum_dist = self._accum_dist[self._junctions[0].route_entry_index] - if junction_accum_dist - ego_accum_dist < self._junction_detection_dist: # Junctions closeby + if junction_accum_dist - ego_accum_dist < self._detection_dist: # Junctions closeby return self._junctions.pop(0) return None @@ -1045,21 +983,17 @@ def _add_incoming_actors(self, junction, source): def _update_road_sources(self, prev_ego_index): """ Manages the sources that spawn actors behind the ego. - These are always behidn the ego and will constinuously spawn actors. + These are always behind the ego and will continuously spawn actors. These sources also track the amount of vehicles in front of the ego, removing actors if the amount is too high. """ if prev_ego_index != self._route_index: - route_wp = self._route[self._route_index] - prev_route_wp = self._route[prev_ego_index] - added_dist = route_wp.transform.location.distance(prev_route_wp.transform.location) - - min_distance = self._road_back_vehicles * self._spawn_dist + min_distance = self._road_back_vehicles * self._road_spawn_dist for lane_key in self._road_dict: source = self._road_dict[lane_key] # If no actors are found, let the last_location be ego's location - # to keep moving the source waypoint forward + # to keep moving the source waypoint forward if len(source.actors) == 0: last_location = self._ego_wp.transform.location else: @@ -1067,17 +1001,22 @@ def _update_road_sources(self, prev_ego_index): if last_location is None: continue + + # Stop the sources in front of the ego (created by new lanes) + if not self._is_location_behind_ego(source.wp.transform.location): + continue + + # Stop the source from being too close to the ego or last lane vehicle source_location = source.wp.transform.location ego_location = self._ego_wp.transform.location - # Stop the source from being too close to the ego or last lane vehicle - actor_dist = max(0, last_location.distance(source_location) - self._spawn_dist) + actor_dist = max(0, last_location.distance(source_location) - self._road_spawn_dist) ego_dist = max(0, ego_location.distance(source_location) - min_distance) move_dist = min(actor_dist, ego_dist) # Move the source forward if needed if move_dist > 0: - new_source_wps = source.wp.next(added_dist) + new_source_wps = source.wp.next(move_dist) if not new_source_wps: continue source.wp = new_source_wps[0] @@ -1092,9 +1031,7 @@ def _update_road_sources(self, prev_ego_index): front_veh = 0 for actor in source.actors: location = CarlaDataProvider.get_location(actor) - if not location: - continue - if not self._is_location_behind_ego(location): + if location and not self._is_location_behind_ego(location): front_veh += 1 if front_veh > self._road_front_vehicles: self._destroy_actor(source.actors[0]) # This is always the front most vehicle @@ -1114,12 +1051,19 @@ def _update_road_sources(self, prev_ego_index): distance = location.distance(source.wp.transform.location) # Spawn a new actor if the last one is far enough - if distance > self._spawn_dist: - actor = self._spawn_source_actor(source, ego_dist=self._spawn_dist) + if distance > self._road_spawn_dist: + actor = self._spawn_source_actor(source, self._road_spawn_dist) if actor is None: continue - self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) + # Set their initial speed, so that they don't lag behind the ego + forward_vec = source.wp.transform.get_forward_vector() + if len(source.actors): + speed = CarlaDataProvider.get_velocity(source.actors[-1]) + else: + speed = self._ego_actor.get_velocity().length() + actor.set_target_velocity(speed * forward_vec) + source.actors.append(actor) ################################ @@ -1139,7 +1083,7 @@ def _initialise_road_behavior(self, road_wps): # Front spawn points next_wp = wp for _ in range(self._road_front_vehicles): - next_wps = next_wp.next(self._spawn_dist) + next_wps = next_wp.next(self._road_spawn_dist) if len(next_wps) != 1 or self._is_junction(next_wps[0]): break # Stop when there's no next or found a junction next_wp = next_wps[0] @@ -1149,20 +1093,18 @@ def _initialise_road_behavior(self, road_wps): source_dist = 0 prev_wp = wp for _ in range(self._road_back_vehicles): - prev_wps = prev_wp.previous(self._spawn_dist) + prev_wps = prev_wp.previous(self._road_spawn_dist) if len(prev_wps) != 1 or self._is_junction(prev_wps[0]): break # Stop when there's no next or found a junction prev_wp = prev_wps[0] spawn_wps.append(prev_wp) - source_dist += self._spawn_dist + source_dist += self._road_spawn_dist # Spawn actors actors = self._spawn_actors(spawn_wps) - for actor in actors: - self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._road_dict[get_lane_key(wp)] = Source( - prev_wp, actors, dist_to_ego=source_dist, active=self._route_sources_active + prev_wp, actors, active=self._route_sources_active ) def _initialise_opposite_sources(self): @@ -1193,7 +1135,7 @@ def _initialise_opposite_sources(self): oppo_wp = self._route[self._opposite_route_index] for wp in get_opposite_dir_lanes(oppo_wp): - self._opposite_sources.append(Source(wp, [])) + self._opposite_sources.append(Source(wp, [], active=self._active_opposite_sources)) def _initialise_road_checker(self): """ @@ -1220,19 +1162,11 @@ def _initialise_junction_sources(self, junction): placed at certain distance from the junction, but are stopped if another junction is found, to ensure the spawned actors always move towards the activated one. """ - remove_entries = junction.scenario_info['remove_entries'] - direction = junction.scenario_info['direction'] - entry_lanes = [] if not direction else junction.entry_directions[direction] - for wp in junction.entry_wps: entry_lane_key = get_lane_key(wp) if entry_lane_key in junction.route_entry_keys: continue # Ignore the road from which the route enters - # TODO: Use the source.active to do this - if remove_entries and entry_lane_key in entry_lanes: - continue # Ignore entries that are part of active junction scenarios - moved_dist = 0 prev_wp = wp while moved_dist < self._junction_sources_dist: @@ -1247,8 +1181,14 @@ def _initialise_junction_sources(self, junction): if entry_lane_key in junction.inactive_entry_keys: source.active = False junction.inactive_entry_keys.remove(entry_lane_key) + junction.entry_sources.append(source) + # Real junctions aren't always full of traffic in all lanes, so deactivate some of them. + # Doing this after the source have been created in case another behavior activates them + for source in junction.entry_sources: + if 100 * self._rng.random() > self._junction_source_perc: + source.active = False def _initialise_junction_exits(self, junction): """ @@ -1258,30 +1198,27 @@ def _initialise_junction_exits(self, junction): exit_wps = junction.exit_wps route_exit_keys = junction.route_exit_keys - remove_exits = junction.scenario_info['remove_exits'] - direction = junction.scenario_info['direction'] - exit_lanes = [] if not direction else junction.exit_directions[direction] - for wp in exit_wps: max_actors = 0 max_distance = junction.exit_dict[get_lane_key(wp)]['max_distance'] exiting_wps = [] next_wp = wp - - # Move the initial distance + + # Move the initial distance (added by the `_extent_road_exit_space`) if max_distance > 0: next_wps = next_wp.next(max_distance) - if len(next_wps) > 0: - next_wp = next_wps[0] + if not next_wps: + continue + next_wp = next_wps[0] for i in range(max(self._road_front_vehicles, 1)): # Get the moving distance (first jump is higher to allow space for another vehicle) if i == 0: - move_dist = 2 * self._spawn_dist + move_dist = 2 * self._junction_spawn_dist else: - move_dist = self._spawn_dist + move_dist = self._junction_spawn_dist # And move such distance next_wps = next_wp.next(move_dist) @@ -1300,23 +1237,18 @@ def _initialise_junction_exits(self, junction): } exit_lane_key = get_lane_key(wp) - if remove_exits and exit_lane_key in exit_lanes: - continue # The direction is prohibited as a junction scenario is active + if exit_lane_key in junction.inactive_exit_keys: + continue # The exit is inactive, don't spawn anything if exit_lane_key in route_exit_keys: actors = self._spawn_actors(exiting_wps) for actor in actors: - self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(junction.actor_dict, actor, exit_lane_key=exit_lane_key) junction.exit_dict[exit_lane_key]['actors'] = actors def _update_junction_sources(self): """Checks the actor sources to see if new actors have to be created""" for junction in self._active_junctions: - remove_entries = junction.scenario_info['remove_entries'] - direction = junction.scenario_info['direction'] - entry_keys = [] if not direction else junction.entry_directions[direction] - actor_dict = junction.actor_dict for source in junction.entry_sources: if self.debug: @@ -1328,9 +1260,6 @@ def _update_junction_sources(self): if not source.active: continue - # The direction is prohibited - if remove_entries and entry_lane_key in entry_keys: - continue self._add_incoming_actors(junction, source) @@ -1349,34 +1278,51 @@ def _update_junction_sources(self): distance = actor_location.distance(source.wp.transform.location) # Spawn a new actor if the last one is far enough - if distance > self._spawn_dist: - actor = self._spawn_source_actor(source) + if distance > self._junction_spawn_dist: + actor = self._spawn_source_actor(source, self._junction_spawn_dist) if not actor: continue - if junction.stop_entries and get_lane_key(source.entry_lane_wp) not in junction.route_entry_keys: + if junction.stop_non_route_entries and get_lane_key(source.entry_lane_wp) not in junction.route_entry_keys: self._actors_speed_perc[actor] = 0 - self._tm.distance_to_leading_vehicle(actor, self._spawn_dist) self._add_actor_dict_element(actor_dict, actor, at_oppo_entry_lane=at_oppo_entry_lane) source.actors.append(actor) - def _found_a_road_change(self, old_index, new_index): - """Checks if the new route waypoint is part of a new road (excluding fake junctions)""" - if new_index == old_index: - return False + def _monitor_topology_changes(self, prev_index): + """ + Continually check the road in front to see if it has changed its topology. + If the number of lanes reduces, merge the ending lane with a side one + If the number of lanes increases, add a new road source. + """ + def get_road_wp(wp): + """Goes backwards in the lane to match the wp with the road key dictionary""" + road_wp = wp + if get_lane_key(road_wp) in self._road_dict: + return road_wp + + road_wp = wp + distance = self._max_radius + + while distance > 0: + prev_wps = road_wp.previous(1) + if not prev_wps: + return None + road_wp = prev_wps[0] + if get_lane_key(road_wp) in self._road_dict: + return road_wp + distance -= 1 - # Check if the road has changed (either in its ID or the amount of lanes) - new_wp = self._route[new_index] - old_wp = self._route[old_index] - if get_road_key(new_wp) == get_road_key(old_wp): - return False + return None - return True + def get_source_wp(wp): + """Moves the wp forward until the lane is wide enough to fit a vehicle""" + source_wp = wp + while source_wp.lane_width < self._lane_width_threshold: + source_wps = source_wp.next(1) + if not source_wps: + return None + source_wp = source_wps[0] + return source_wp - def _check_lane_merges(self, prev_index): - """ - Continually check the road in front to see if it has changed its topology. - If the number of lanes have been reduced, remove the actors part of the merging lane. - """ if self.debug: checker_wp = self._route[self._road_checker_index] draw_point(self._world, checker_wp.transform.location, DEBUG_SMALL, DEBUG_ROAD, False) @@ -1402,19 +1348,86 @@ def _check_lane_merges(self, prev_index): new_wps = get_same_dir_lanes(self._route[checker_index]) old_wps = get_same_dir_lanes(self._route[self._road_checker_index]) - if len(new_wps) >= len(old_wps): - pass - else: - new_accum_dist = self._accum_dist[checker_index] - prev_accum_dist = self._accum_dist[self._road_checker_index] - route_move_dist = new_accum_dist - prev_accum_dist + new_accum_dist = self._accum_dist[checker_index] + prev_accum_dist = self._accum_dist[self._road_checker_index] + route_move_dist = max(new_accum_dist - prev_accum_dist, 0.1) + + if len(new_wps) > len(old_wps): + for new_wp in list(new_wps): + prev_wps = new_wp.previous(2 * route_move_dist) # x2, just in case + if prev_wps: + continue + # Found the new lane, add a Source + source_wp = get_source_wp(new_wp) + if not source_wp: + continue + self._road_dict[get_lane_key(new_wp)] = Source(source_wp, [], active=self._route_sources_active) + + elif len(new_wps) < len(old_wps): for old_wp in list(old_wps): - next_wps = old_wp.next(route_move_dist) - if not next_wps: - for actor in list(self._road_dict[get_lane_key(old_wp)].actors): + next_wps = old_wp.next(2 * route_move_dist) # x2, just in case + if next_wps: + continue + + # Found the lane that ends, merge it with the one on its side + road_wp = get_road_wp(old_wp) + if not road_wp: + continue + + # Get a side lane + right_wp = old_wp.get_right_lane() + left_wp = old_wp.get_left_lane() + side_road_wp = None + if right_wp and right_wp.lane_type == carla.LaneType.Driving: + side_road_wp = get_road_wp(right_wp) + side_path = [right_wp.transform.location] + elif left_wp and left_wp.lane_type == carla.LaneType.Driving: + side_road_wp = get_road_wp(left_wp) + side_path = [left_wp.transform.location] + + if not side_road_wp: + # No side lane found part of the road dictionary, remove them + for actor in list(self._road_dict[get_lane_key(road_wp)].actors): + self._destroy_actor(actor) + self._road_dict.pop(get_lane_key(road_wp), None) + continue + + # Get the actors + lane_actors = self._road_dict[get_lane_key(road_wp)].actors + side_lane_actors = self._road_dict[get_lane_key(side_road_wp)].actors + + # Get their distance to the ego + actors_with_dist = [] + ego_location = self._ego_wp.transform.location + for actor in lane_actors + side_lane_actors: + actor_location = CarlaDataProvider.get_location(actor) + if not actor_location: self._destroy_actor(actor) - self._road_dict.pop(get_lane_key(old_wp), None) + continue + dist = ego_location.distance(actor_location) + if not self._is_location_behind_ego(actor_location): + dist *= -1 + actors_with_dist.append([actor, dist]) + + # Sort them by distance + actors_sorted_with_dist = sorted(actors_with_dist, key=lambda a: a[1]) + zero_index = len([a for a in actors_sorted_with_dist if a[1] < 0]) + min_index = max(zero_index - self._road_front_vehicles, 0) + max_index = min(zero_index + self._road_front_vehicles - 1, len(actors_sorted_with_dist) - 1) + + # Remove the unneeded ones, and make the ending lane actors perform a lane change + source_actors = [] + for i, (actor, _) in enumerate(actors_sorted_with_dist): + if i >= min_index and i <= max_index: + source_actors.append(actor) + self._tm.set_path(actor, side_path) + else: + self._destroy_actor(actor) + + # And update the road dict + self._road_dict[get_lane_key(side_road_wp)].actors = source_actors + self._road_dict.pop(get_lane_key(road_wp), None) self._road_checker_index = checker_index @@ -1448,15 +1461,18 @@ def _move_opposite_sources(self, prev_index): if not oppo_route_index: oppo_route_index = last_index + # Get the distance moved by the route reference index + new_accum_dist = self._accum_dist[oppo_route_index] + prev_accum_dist = self._accum_dist[self._opposite_route_index] + route_move_dist = new_accum_dist - prev_accum_dist + if route_move_dist <= 0: + return # Sometimes a route wp is behind the pervious one, filter these out + new_opposite_wps = get_opposite_dir_lanes(self._route[oppo_route_index]) + if len(new_opposite_wps) != len(self._opposite_sources): # The topology has changed. Remap the new lanes to the sources new_opposite_sources = [] - - # Map the old sources to the new wps, and add new ones / remove uneeded ones - new_accum_dist = self._accum_dist[oppo_route_index] - prev_accum_dist = self._accum_dist[self._opposite_route_index] - route_move_dist = new_accum_dist - prev_accum_dist for wp in new_opposite_wps: location = wp.transform.location new_source = None @@ -1474,16 +1490,11 @@ def _move_opposite_sources(self, prev_index): self._opposite_sources = new_opposite_sources else: - prev_accum_dist = self._accum_dist[prev_index] - current_accum_dist = self._accum_dist[self._route_index] - move_dist = current_accum_dist - prev_accum_dist - if move_dist <= 0: - return - + # The topology hasn't changed, move the distance backwards for source in self._opposite_sources: wp = source.wp if not self._is_junction(wp): - prev_wps = wp.previous(move_dist) + prev_wps = wp.previous(route_move_dist) if len(prev_wps) == 0: continue prev_wp = prev_wps[0] @@ -1494,11 +1505,12 @@ def _move_opposite_sources(self, prev_index): def _update_opposite_sources(self): """Checks the opposite actor sources to see if new actors have to be created""" for source in self._opposite_sources: - # Cap the amount of alive actors - if len(source.actors) >= self._opposite_sources_max_actors: + if not source.active: continue - if not source.active: + # Ending / starting lanes create issues as the lane width gradually decreases until reaching 0, + # where the lane starts / ends. Avoid spawning anything inside those parts with small lane width + if source.wp.lane_width < self._lane_width_threshold: continue # Calculate distance to the last created actor @@ -1515,8 +1527,6 @@ def _update_opposite_sources(self): actor = self._spawn_source_actor(source) if actor is None: continue - - self._tm.distance_to_leading_vehicle(actor, self._opposite_spawn_dist) self._tm.ignore_lights_percentage(actor, 100) self._tm.ignore_signs_percentage(actor, 100) self._opposite_actors.append(actor) @@ -1528,38 +1538,32 @@ def _update_parameters(self): This is done using py_trees' Blackboard variables and all behaviors should be at `background_manager.py`. The blackboard variable is reset to None to avoid changing them back again next time. """ - # General behavior - general_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeGeneralBehavior') - if general_behavior_data is not None: - self._spawn_dist = general_behavior_data - self._get_road_radius() - py_trees.blackboard.Blackboard().set('BA_ChangeGeneralBehavior', None, True) - # Road behavior road_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeRoadBehavior') if road_behavior_data is not None: - num_front_vehicles, num_back_vehicles = road_behavior_data + num_front_vehicles, num_back_vehicles, spawn_dist = road_behavior_data if num_front_vehicles is not None: self._road_front_vehicles = num_front_vehicles if num_back_vehicles is not None: self._road_back_vehicles = num_back_vehicles + if spawn_dist is not None: + self._road_spawn_dist = spawn_dist self._get_road_radius() py_trees.blackboard.Blackboard().set('BA_ChangeRoadBehavior', None, True) # Opposite behavior opposite_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeOppositeBehavior') if opposite_behavior_data is not None: - source_dist, max_actors, spawn_dist, active = opposite_behavior_data + source_dist, spawn_dist, active = opposite_behavior_data if source_dist is not None: if source_dist < self._junction_sources_dist: print('WARNING: Opposite sources distance is lower than the junction ones. Ignoring it') else: self._opposite_sources_dist = source_dist - if max_actors is not None: - self._opposite_sources_max_actors = max_actors if spawn_dist is not None: self._opposite_spawn_dist = spawn_dist if active is not None: + self._active_opposite_sources = active for source in self._opposite_sources: source.active = active py_trees.blackboard.Blackboard().set('BA_ChangeOppositeBehavior', None, True) @@ -1567,28 +1571,20 @@ def _update_parameters(self): # Junction behavior junction_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeJunctionBehavior') if junction_behavior_data is not None: - source_dist, max_actors = road_behavior_data + source_dist, spawn_dist, max_actors, source_perc = junction_behavior_data if source_dist is not None: if source_dist > self._opposite_sources_dist: print('WARNING: Junction sources distance is higher than the opposite ones. Ignoring it') else: self._junction_sources_dist = source_dist + if spawn_dist: + self._junction_spawn_dist = spawn_dist if max_actors is not None: self._junction_sources_max_actors = max_actors + if source_perc is not None: + self._junction_source_perc = source_perc py_trees.blackboard.Blackboard().set('BA_ChangeJunctionBehavior', None, True) - # Extend the space of a specific exit lane - road_exit_data = py_trees.blackboard.Blackboard().get('BA_ExtentExitRoadSpace') - if road_exit_data is not None: - self._extent_road_exit_space(road_exit_data) - py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", None, True) - - # Switch route sources - switch_sources_data = py_trees.blackboard.Blackboard().get('BA_SwitchRouteSources') - if switch_sources_data is not None: - self._switch_route_sources(switch_sources_data) - py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", None, True) - # Stop front vehicles stop_data = py_trees.blackboard.Blackboard().get('BA_StopFrontVehicles') if stop_data is not None: @@ -1601,51 +1597,48 @@ def _update_parameters(self): self._start_road_front_vehicles() py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) - # Handles junction scenarios (scenarios 7 to 10) - junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_JunctionScenario') - if junction_scenario_data is not None: - entry_direction, remove_exits = junction_scenario_data - self._initialise_junction_scenario(entry_direction, remove_exits, True, True) - py_trees.blackboard.Blackboard().set('BA_JunctionScenario', None, True) - - # Handles road accident scenario (Accident and Construction) - handle_start_accident_data = py_trees.blackboard.Blackboard().get('BA_HandleStartAccidentScenario') + # Handles road accident scenario + handle_start_accident_data = py_trees.blackboard.Blackboard().get('BA_StartObstacleScenario') if handle_start_accident_data is not None: - accident_wp, distance = handle_start_accident_data - self._handle_lanechange_scenario(accident_wp, distance) - py_trees.blackboard.Blackboard().set('BA_HandleStartAccidentScenario', None, True) + accident_wp, distance, direction, stop_back_vehicles = handle_start_accident_data + self._handle_lanechange_scenario(accident_wp, distance, direction, stop_back_vehicles) + py_trees.blackboard.Blackboard().set('BA_StartObstacleScenario', None, True) - # Handles road accident scenario (Accident and Construction) - handle_end_accident_data = py_trees.blackboard.Blackboard().get('BA_HandleEndAccidentScenario') + # Handles road accident scenario + handle_end_accident_data = py_trees.blackboard.Blackboard().get('BA_EndObstacleScenario') if handle_end_accident_data is not None: self._road_extra_front_actors = 0 - py_trees.blackboard.Blackboard().set('BA_HandleEndAccidentScenario', None, True) + py_trees.blackboard.Blackboard().set('BA_EndObstacleScenario', None, True) # Leave space in front leave_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveSpaceInFront') if leave_space_data is not None: self._leave_space_in_front(leave_space_data) py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) - - # Switch lane - switch_lane_data = py_trees.blackboard.Blackboard().get('BA_SwitchLane') - if switch_lane_data is not None: - lane_id, active = switch_lane_data - self._switch_lane(lane_id, active) - py_trees.blackboard.Blackboard().set('BA_SwitchLane', None, True) - - # Remove junction entry - remove_junction_entry_data = py_trees.blackboard.Blackboard().get('BA_RemoveJunctionEntry') - if remove_junction_entry_data is not None: - wps, all_entries = remove_junction_entry_data - self._remove_junction_entry(wps, all_entries) - py_trees.blackboard.Blackboard().set('BA_RemoveJunctionEntry', None, True) - - # Clear junction - clear_junction_data = py_trees.blackboard.Blackboard().get('BA_ClearJunction') - if clear_junction_data is not None: - self._clear_junction() - py_trees.blackboard.Blackboard().set('BA_ClearJunction', None, True) + + # Remove road lane + remove_road_lane_data = py_trees.blackboard.Blackboard().get('BA_RemoveRoadLane') + if remove_road_lane_data is not None: + self._remove_road_lane(remove_road_lane_data) + py_trees.blackboard.Blackboard().set('BA_RemoveRoadLane', None, True) + + # Re add ego road lane + add_ego_road_lane_data = py_trees.blackboard.Blackboard().get('BA_ReAddEgoRoadLane') + if add_ego_road_lane_data is not None: + self._add_ego_road_lane() + py_trees.blackboard.Blackboard().set('BA_ReAddEgoRoadLane', None, True) + + # Adapt the BA to the junction scenario + junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_HandleJunctionScenario') + if junction_scenario_data is not None: + self._handle_junction_scenario(junction_scenario_data) + py_trees.blackboard.Blackboard().set("BA_HandleJunctionScenario", None, True) + + # Switch route sources + switch_sources_data = py_trees.blackboard.Blackboard().get('BA_SwitchRouteSources') + if switch_sources_data is not None: + self._switch_route_sources(switch_sources_data) + py_trees.blackboard.Blackboard().set("BA_SwitchRouteSources", None, True) self._compute_parameters() @@ -1654,7 +1647,7 @@ def _compute_parameters(self): ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) self._min_radius = self._base_min_radius + self._radius_increase_ratio * ego_speed self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed - self._junction_detection_dist = self._max_radius + self._detection_dist = self._base_junction_detection + self._detection_ratio * ego_speed def _stop_road_front_vehicles(self): """ @@ -1667,6 +1660,7 @@ def _stop_road_front_vehicles(self): if location and not self._is_location_behind_ego(location): self._stopped_road_actors.append(actor) self._actors_speed_perc[actor] = 0 + self._tm.update_vehicle_lights(actor, False) lights = actor.get_light_state() lights |= carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) @@ -1678,31 +1672,12 @@ def _start_road_front_vehicles(self): """ for actor in self._stopped_road_actors: self._actors_speed_perc[actor] = 100 + self._tm.update_vehicle_lights(actor, True) lights = actor.get_light_state() lights &= ~carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) self._stopped_road_actors = [] - def _extent_road_exit_space(self, space): - """Increases the space left by the exit vehicles at a specific road""" - if len(self._active_junctions) > 0: - junction = self._active_junctions[0] - elif len(self._junctions) > 0: - junction = self._junctions[0] - else: - return - - route_lane_keys = junction.route_exit_keys - - for exit_lane_key in route_lane_keys: - junction.exit_dict[exit_lane_key]['max_distance'] += space - actors = junction.exit_dict[exit_lane_key]['actors'] - self._move_actors_forward(actors, space) - for actor in actors: - if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: - self._actors_speed_perc[actor] = 100 - junction.actor_dict[actor]['state'] = JUNCTION_EXIT - def _move_actors_forward(self, actors, space): """Teleports the actors forward a set distance""" for actor in list(actors): @@ -1719,24 +1694,43 @@ def _move_actors_forward(self, actors, space): else: self._destroy_actor(actor) - def _handle_lanechange_scenario(self, accident_wp, distance): + def _handle_lanechange_scenario(self, accident_wp, distance, direction, stop_back_vehicles=False): """ - Handles the scenario in which the BA has to change lane. + Handles the scenarios in which the BA has to change lane, + generally due to an obstacle in the road (accident / construction / stopped vehicle...) """ ego_wp = self._route[self._route_index] lane_change_actors = self._road_dict[get_lane_key(ego_wp)].actors - for actor in lane_change_actors: + + if direction == 'left': + first_wp = accident_wp.get_left_lane().next(distance / 4)[0] + second_wp = first_wp.next(distance / 2)[0] + third_wp = second_wp.get_right_lane().next(distance / 4)[0] + else: + first_wp = accident_wp.get_right_lane().next(distance / 4)[0] + second_wp = first_wp.next(distance / 2)[0] + third_wp = second_wp.get_left_lane().next(distance / 4)[0] + + vehicle_path = [first_wp.transform.location, + second_wp.transform.location, + third_wp.transform.location] + + for i, actor in enumerate(lane_change_actors): + location = CarlaDataProvider.get_location(actor) - if not self._is_location_behind_ego(location): - lanechange_wp = accident_wp.get_left_lane().next(distance/4)[0] - end_lanechange_wp = lanechange_wp.next(distance/2)[0] - vehicle_path = [lanechange_wp.transform.location, - end_lanechange_wp.transform.location, - end_lanechange_wp.get_right_lane().next(distance/4)[0].transform.location] + + if i == 0: + # First actor might get blocked by the accident, so teleport it + distance = location.distance(third_wp.transform.location) + if distance > 0: + self._move_actors_forward([actor], distance) + + elif not self._is_location_behind_ego(location): + # The others can just lane change, which will also teach the ego what to do self._tm.set_path(actor, vehicle_path) - ## maybe check here to activate lane changing lights self._road_extra_front_actors += 1 - else: + elif stop_back_vehicles: + # Stop the vehicles behind self._actors_speed_perc[actor] = 0 def _switch_route_sources(self, enabled): @@ -1754,131 +1748,282 @@ def _switch_route_sources(self, enabled): def _leave_space_in_front(self, space): """Teleports all the vehicles in front of the ego forward""" - # all_actors = [] if not self._active_junctions: - for lane in self._road_dict: - lane_actors = self._road_dict[lane].actors - front_actors = [] - for actor in lane_actors: - location = CarlaDataProvider.get_location(actor) - if location and not self._is_location_behind_ego(location): - front_actors.append(actor) - - last_actor_location = CarlaDataProvider.get_location(front_actors[-1]) - distance = last_actor_location.distance(self._ego_wp.transform.location) - step = space - distance - if step > 0: # Only move the needed distance - self._move_actors_forward(front_actors, step) - - # else: - # junction = self._active_junctions[0] - # if self._is_junction(self._ego_wp): # We care about the exit - # for actor, info in junction.actor_dict.items(): - # if info['exit_lane_key'] in junction.route_exit_keys: - # all_actors.append(actor) - # else: # We care about the entry - # for source in junction.entry_sources: - # if get_lane_key(source.entry_lane_wp) in junction.route_entry_keys: - # all_actors.extend(source.actors) - - # front_actors = [] - # for actor in all_actors: - # location = CarlaDataProvider.get_location(actor) - # if location and not self._is_location_behind_ego(location): - # front_actors.append(actor) - - - # self._move_actors_forward(front_actors, space) - - def _switch_lane(self, lane_id, active): - """ - active is False: remove BA actors from this lane, and BA would stop generating new actors on this lane. - active is True: recover BA on the lane. - """ - lane_id = str(lane_id) - lane_id_list = [x.split('*')[-1] for x in list(self._road_dict.keys())] - if lane_id in lane_id_list: - lane_index = lane_id_list.index(lane_id) - lane_key = list(self._road_dict.keys())[lane_index] - self._road_dict[lane_key].active = active - if not active: - for actor in list(self._road_dict[lane_key].actors): + front_actors = [] + min_distance = float('inf') + for actor in self._road_dict[self._ego_key].actors: + location = CarlaDataProvider.get_location(actor) + if not location or self._is_location_behind_ego(location): + continue + + front_actors.append(actor) + distance = location.distance(self._ego_wp.transform.location) + if distance < min_distance: + min_distance = distance + + step = space - min_distance + if step > 0: # Only move them if needed and only the minimum required distance + self._move_actors_forward(front_actors, step) + + def _remove_road_lane(self, lane_wp): + """Removes a road lane""" + lane_key = get_lane_key(lane_wp) + if lane_key not in list(self._road_dict): + print(f"WARNING: Couldn't find the lane to be removed, '{lane_key}' isn't part of the road behavior") + return + + self._road_dict[lane_key].active = False + for actor in list(self._road_dict[lane_key].actors): + self._destroy_actor(actor) + self._road_dict.pop(lane_key, None) + + def _add_ego_road_lane(self): + """Adds a ego road lane. This is expected to be used after having previously removed such lane""" + if self._ego_key in list(self._road_dict): + print(f"WARNING: Couldn't add a lane {self._ego_key} as it is already part of the road") + return + + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) + spawn_dist = self._road_spawn_dist + 2.5 * ego_speed + + spawn_wps = [] + + next_wp = self._ego_wp + for _ in range(self._road_front_vehicles): + next_wps = next_wp.next(spawn_dist) + if len(next_wps) != 1 or self._is_junction(next_wps[0]): + break # Stop when there's no next or found a junction + next_wp = next_wps[0] + spawn_wps.insert(0, next_wp) + + source_dist = 0 + prev_wp = self._ego_wp + for _ in range(self._road_back_vehicles): + prev_wps = prev_wp.previous(spawn_dist) + if len(prev_wps) != 1 or self._is_junction(prev_wps[0]): + break # Stop when there's no next or found a junction + prev_wp = prev_wps[0] + spawn_wps.append(prev_wp) + source_dist += spawn_dist + + actors = [] + for spawn_wp in spawn_wps: + actor = self._spawn_actor(spawn_wp) + if not actor: + continue + actor.set_target_velocity(spawn_wp.transform.get_forward_vector() * ego_speed) + actors.append(actor) + + self._road_dict[self._ego_key] = Source( + prev_wp, actors, active=self._route_sources_active + ) + + def _handle_junction_scenario(self, junction_data): + """ + Adapts the BA to the junction scenario by clearing the junction, + its entries, exits, and by extending the road exit to add more space + """ + clear_junction, clear_ego_entry, remove_entries, remove_exits, stop_entries, extend_road_exit = junction_data + + if clear_junction: + self._clear_junction_middle() + + if clear_ego_entry: + self._clear_ego_entry() + + if remove_entries: + self._remove_junction_entries(remove_entries) + + if remove_exits: + self._remove_junction_exits(remove_exits) + + if stop_entries: + self._stop_non_ego_route_entries() + + if extend_road_exit: + self._extent_road_exit_space(extend_road_exit) + + def _clear_junction_middle(self): + """Clears the junction, and all subsequent actors that enter it""" + if self._active_junctions: + junction = self._active_junctions[0] + actor_dict = junction.actor_dict + for actor in list(actor_dict): + if actor_dict[actor]['state'] == JUNCTION_MIDDLE: self._destroy_actor(actor) + junction.clear_middle = True + + elif self._junctions: + self._junctions[0].clear_middle = True + + def _clear_ego_entry(self): + """ + Remove all actors in front of the vehicle, and stops those behind, + letting the ego smoothly enter the junction. + """ + def handle_actors(actor_list): + for actor in list(actor_list): + location = CarlaDataProvider.get_location(actor) + if location and not self._is_location_behind_ego(location): + self._destroy_actor(actor) + else: + self._actors_speed_perc[actor] = 0 + + if self._active_junctions: + for source in self._active_junctions[0].entry_sources: + handle_actors(source.actors) + else: + for lane_key in list(self._road_dict): + handle_actors(self._road_dict[lane_key].actors) + + def _remove_junction_entries(self, wps): + """Removes a list of entries of the closest junction (or marks them so that they aren't spawned)""" + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return - def _remove_junction_entry(self, wps, all_entries): - """Removes a specific entry (or all the entries at the same road) of the closest junction""" for wp in wps: - if len(self._active_junctions) > 0: - junction = self._active_junctions[0] - elif len(self._junctions) > 0: - junction = self._junctions[0] - else: - return - - mapped_wp = None - mapped_dist = float('inf') - ref_loc = wp.transform.location - for entry_wp in junction.entry_wps: - distance = ref_loc.distance(entry_wp.transform.location) - if distance < mapped_dist: - mapped_wp = entry_wp - mapped_dist = distance - - if all_entries: - mapped_road_key = get_road_key(mapped_wp) - mapped_lane_keys = [key for key in junction.entry_lane_keys if is_lane_at_road(key, mapped_road_key)] - else: - mapped_lane_keys = [get_lane_key(mapped_wp)] + mapped_key = None + next_wp = wp + while not self._is_junction(next_wp): + lane_key = get_lane_key(next_wp) + if lane_key in junction.entry_lane_keys: + mapped_key = lane_key + break + next_wps = next_wp.next(1) + if not next_wps: + break + next_wp = next_wps[0] + + if not mapped_key: + print("WARNING: Couldn't find the asked entry to be removed") + continue if len(self._active_junctions) > 0: for source in junction.entry_sources: - if get_lane_key(source.wp) in mapped_lane_keys: + if get_lane_key(source.wp) == mapped_key: for actor in list(source.actors): self._destroy_actor(actor) source.active = False else: - junction.inactive_entry_keys = mapped_lane_keys + junction.inactive_entry_keys.append(mapped_key) + + def _remove_junction_exits(self, wps): + """Removes a list of exit of the closest junction (or marks them so that they aren't spawned)""" + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return + + for wp in wps: - def _clear_junction(self): + mapped_key = None + prev_wp = wp + while not self._is_junction(prev_wp): + lane_key = get_lane_key(prev_wp) + if lane_key in junction.exit_dict: + mapped_key = lane_key + break + prev_wps = prev_wp.next(1) + if not prev_wps: + break + prev_wp = prev_wps[0] + + if not mapped_key: + print("WARNING: Couldn't find the asked exit to be removed") + continue + + if len(self._active_junctions) > 0: + for actor in list(junction.exit_dict[mapped_key]['actors']): + self._destroy_actor(actor) + else: + junction.inactive_exit_keys.append(mapped_key) + + def _stop_non_ego_route_entries(self): """Clears the junction, and all subsequent actors that enter it""" if self._active_junctions: # Remove all actors in the middle junction = self._active_junctions[0] actor_dict = junction.actor_dict - for actor in list(actor_dict): - if actor_dict[actor]['state'] == JUNCTION_MIDDLE: - self._destroy_actor(actor) - junction.scenario_info['remove_middle'] = True - # Stop all entry actors entry_sources = junction.entry_sources route_entry_keys = junction.route_entry_keys for source in entry_sources: - if get_lane_key(source.entry_lane_wp) not in route_entry_keys: for actor in source.actors: if actor_dict[actor]['state'] == JUNCTION_ENTRY: self._actors_speed_perc[actor] = 0 elif self._junctions: - self._junctions[0].scenario_info['remove_middle'] = True - self._junctions[0].stop_entries = True + self._junctions[0].stop_non_route_entries = True + + def _extent_road_exit_space(self, space): + """Increases the space left by the exit vehicles at a specific road""" + if len(self._active_junctions) > 0: + junction = self._active_junctions[0] + elif len(self._junctions) > 0: + junction = self._junctions[0] + else: + return + + route_lane_keys = junction.route_exit_keys + + for exit_lane_key in route_lane_keys: + junction.exit_dict[exit_lane_key]['max_distance'] += space + actors = junction.exit_dict[exit_lane_key]['actors'] + self._move_actors_forward(actors, space) + for actor in actors: + if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: + self._actors_speed_perc[actor] = 100 + junction.actor_dict[actor]['state'] = JUNCTION_EXIT ############################# ## Actor functions ## ############################# - def _initialize_actor(self, actor): - """Save the actor into the needed structures and disable its lane changes""" + def _initialise_actor(self, actor): + """ + Save the actor into the needed structures, disable its lane changes and set the leading distance. + """ self._tm.auto_lane_change(actor, False) self._tm.update_vehicle_lights(actor, True) + self._tm.distance_to_leading_vehicle(actor, 5) self._actors_speed_perc[actor] = 100 self._all_actors.append(actor) - def _spawn_actors(self, spawn_wps): + def _spawn_actor(self, spawn_wp, ego_dist=0): + """Spawns an actor""" + ego_location = CarlaDataProvider.get_location(self._ego_actor) + if ego_location.distance(spawn_wp.transform.location) < ego_dist: + return None + + spawn_transform = carla.Transform( + spawn_wp.transform.location + carla.Location(z=self._spawn_vertical_shift), + spawn_wp.transform.rotation + ) + + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', spawn_transform, 'background', True, + attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + ) + + if not actor: + return actor + self._initialise_actor(actor) + return actor + + def _spawn_actors(self, spawn_wps, ego_dist=0): """Spawns several actors in batch""" spawn_transforms = [] + ego_location = CarlaDataProvider.get_location(self._ego_actor) for wp in spawn_wps: + if ego_location.distance(wp.transform.location) < ego_dist: + continue spawn_transforms.append( carla.Transform(wp.transform.location + carla.Location(z=self._spawn_vertical_shift), wp.transform.rotation) @@ -1892,7 +2037,7 @@ def _spawn_actors(self, spawn_wps): return actors for actor in actors: - self._initialize_actor(actor) + self._initialise_actor(actor) return actors @@ -1914,8 +2059,7 @@ def _spawn_source_actor(self, source, ego_dist=0): if not actor: return actor - self._initialize_actor(actor) - + self._initialise_actor(actor) return actor def _is_location_behind_ego(self, location): @@ -1927,66 +2071,30 @@ def _is_location_behind_ego(self, location): return True return False - def _get_ego_route_lane_key(self, route_wp): - """ - Gets the route lane key of the ego. This corresponds to the same lane if the ego is driving normally, - but if is is going in opposite direction, the route's leftmost one is chosen instead - """ - location = CarlaDataProvider.get_location(self._ego_actor) - ego_true_wp = self._map.get_waypoint(location) - if get_road_key(ego_true_wp) != get_road_key(route_wp): - # Just return the default value as two different roads are being compared. - # This might happen for when moving to a new road and should be fixed next frame - return get_lane_key(route_wp) - - yaw_diff = (ego_true_wp.transform.rotation.yaw - route_wp.transform.rotation.yaw) % 360 - if yaw_diff < 90 or yaw_diff > 270: - return get_lane_key(ego_true_wp) - else: - # Get the first lane of the opposite direction - leftmost_wp = route_wp - while True: - possible_left_wp = leftmost_wp.get_left_lane() - if possible_left_wp is None or possible_left_wp.lane_id * leftmost_wp.lane_id < 0: - break - leftmost_wp = possible_left_wp - return get_lane_key(leftmost_wp) - def _update_road_actors(self): """ Dynamically controls the actor speed in front of the ego. Not applied to those behind it so that they can catch up it """ # Updates their speed + # scenario_actors = self._stopped_road_actors + self._clear_actors scenario_actors = self._stopped_road_actors - for lane in self._road_dict: - for i, actor in enumerate(self._road_dict[lane].actors): + for lane_key in self._road_dict: + for i, actor in enumerate(self._road_dict[lane_key].actors): location = CarlaDataProvider.get_location(actor) if not location: continue if self.debug: string = 'R_' string += 'B' if self._is_location_behind_ego(location) else 'F' - string += '(' + str(i) + ')' + string += '_(' + str(i) + ')' + string += '_[' + lane_key + ']' draw_string(self._world, location, string, DEBUG_ROAD, False) if actor in scenario_actors or self._is_location_behind_ego(location): continue self._set_road_actor_speed(location, actor) - if not self._road_ego_key in self._road_dict: - self._road_num_front_vehicles = self._road_front_vehicles - return - - front_actors = 0 - for actor in self._road_dict[self._road_ego_key].actors: - if not self._is_location_behind_ego(actor.get_location()): - front_actors += 1 - self._road_num_front_vehicles = front_actors - - self._get_road_radius() - self._compute_parameters() - def _set_road_actor_speed(self, location, actor, multiplier=1): """Changes the speed of the vehicle depending on its distance to the ego""" distance = location.distance(self._ego_wp.transform.location) @@ -2009,45 +2117,83 @@ def get_lane_waypoints(reference_wp, index=0): wps.append(junction_wps[index]) return wps - route_wp = self._route[ self._route_index] + def get_wp_pairs(old_wps_, new_wps_, dist): + wp_pairs = [] + + unmapped_wps = new_wps_ + for old_wp_ in old_wps_: + mapped = False + location = old_wp_.transform.location + for new_wp_ in unmapped_wps: + if location.distance(new_wp_.transform.location) < dist: + unmapped_wps.remove(new_wp_) + wp_pairs.append([old_wp_, new_wp_]) + mapped = True + break + + if not mapped: + wp_pairs.append([old_wp_, None]) + + for unmapped_wp in unmapped_wps: + wp_pairs.append([None, unmapped_wp]) + + return wp_pairs + + def is_remapping_needed(current_wp, prev_wp): + """The road dict mapping is needed if """ + # If the ego just exitted a junction, remap isn't needed + if self._is_junction(prev_wp): + return False + + # If the road changes, remap + if get_road_key(prev_wp) != get_road_key(current_wp): + return True + + # Some roads have ending lanes in the middle. Remap if that is detected + prev_wps = get_same_dir_lanes(prev_wp) + current_wps = get_same_dir_lanes(current_wp) + if len(prev_wps) != len(current_wps): + return True + + return False + + if prev_route_index == self._route_index: + return + route_wp = self._route[self._route_index] prev_route_wp = self._route[prev_route_index] + + if not is_remapping_needed(route_wp, prev_route_wp): + return + + new_road_dict = {} + old_wps = get_lane_waypoints(prev_route_wp, 1) + new_wps = get_lane_waypoints(route_wp, 0) check_dist = 1.1 * route_wp.transform.location.distance(prev_route_wp.transform.location) - if prev_route_index != self._route_index: - road_change = self._found_a_road_change(prev_route_index, self._route_index) - - if not self._is_junction(prev_route_wp) and road_change: - old_wps = get_lane_waypoints(prev_route_wp, 1) - new_wps = get_lane_waypoints(route_wp, 0) - - # Map the new lanes to the old ones - mapped_keys = {} - - unmapped_wps = new_wps - for old_wp in old_wps: - location = old_wp.transform.location - for new_wp in unmapped_wps: - if location.distance(new_wp.transform.location) < check_dist: - unmapped_wps.remove(new_wp) - mapped_keys[get_lane_key(old_wp)] = get_lane_key(new_wp) - break + wp_pairs = get_wp_pairs(old_wps, new_wps, check_dist) + # TODO: these pairs are sometimes wrong as some fake intersections have overlapping lanes (highway entries) - # Remake the road back actors dictionary - new_road_dict = {} - for lane_key in self._road_dict: - if lane_key not in mapped_keys: - continue # A lane ended at that road - new_lane_key = mapped_keys[lane_key] - new_road_dict[new_lane_key] = self._road_dict[lane_key] + for old_wp, new_wp in wp_pairs: + old_key = get_lane_key(old_wp) + new_key = get_lane_key(new_wp) - self._road_dict = new_road_dict + # Lane has ended / started, no need to remap it + if not new_wp or not old_wp: + continue - if not self._road_ego_key in mapped_keys: - # Return the default. This might happen when the route lane ends and should be fixed next frame - self._road_ego_key = get_lane_key(route_wp) - else: - self._road_ego_key = mapped_keys[self._road_ego_key] - else: - self._road_ego_key = self._get_ego_route_lane_key(route_wp) + if self.debug: + draw_arrow(self._world, old_wp.transform.location, new_wp.transform.location, DEBUG_MEDIUM, DEBUG_ROAD, True) + + # Check that the lane is part of the road dictionary + if old_key in list(self._road_dict): + new_road_dict[new_key] = self._road_dict[old_key] + self._road_dict.pop(old_key) + continue + + # Add the rest of the unmapped road sources, mainly those created forward at lanes that are starting + for lane_key in list(self._road_dict): + new_road_dict[lane_key] = self._road_dict.pop(lane_key) + + self._road_dict = new_road_dict def _update_junction_actors(self): """ @@ -2073,7 +2219,6 @@ def _update_junction_actors(self): actor_dict = junction.actor_dict exit_dict = junction.exit_dict - remove_middle = junction.scenario_info['remove_middle'] for j, actor in enumerate(list(actor_dict)): if actor not in actor_dict: continue # Actor was removed during the loop @@ -2090,7 +2235,7 @@ def _update_junction_actors(self): if state == JUNCTION_ENTRY: actor_wp = self._map.get_waypoint(location) if self._is_junction(actor_wp) and junction.contains(actor_wp.get_junction()): - if remove_middle: + if junction.clear_middle: self._destroy_actor(actor) # Don't clutter the junction if a junction scenario is active continue actor_dict[actor]['state'] = JUNCTION_MIDDLE @@ -2142,27 +2287,34 @@ def _update_opposite_actors(self): removing them if too far behind the ego. """ ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - max_dist = max(self._opposite_removal_dist + ego_speed * self._opposite_increase_ratio, self._opposite_spawn_dist) + max_dist = max(self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio, self._opposite_spawn_dist) for actor in list(self._opposite_actors): location = CarlaDataProvider.get_location(actor) if not location: continue if self.debug: draw_string(self._world, location, 'O', DEBUG_OPPOSITE, False) + distance = location.distance(self._ego_wp.transform.location) if distance > max_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) + continue + + # Ending / starting lanes create issues as the lane width gradually decreases until reaching 0, + # where the lane starts / ends. Set their speed to 0, and they'll eventually dissapear. + actor_wp = self._map.get_waypoint(location) + if actor_wp.lane_width < self._lane_width_threshold: + self._actors_speed_perc[actor] = 0 def _set_actors_speed(self): """ - Sets the speed of all the BA actors. Done by changing a given percentage it the TM value. - Additionaly, avoid issues with speed limits trigger boxes by using the ego's target speed as reference. + Sets the speed of all the BA actors, using the ego's target speed as reference. + This avoids issues with the speed limits, as newly created actors don't have that information """ for actor, percentage in self._actors_speed_perc.items(): percentage = min(100, max(0, percentage)) - real_percentage = self._ego_target_speed / actor.get_speed_limit() * percentage - speed_red = (100 - real_percentage) - self._tm.vehicle_percentage_speed_difference(actor, speed_red) + speed = self._ego_target_speed * percentage / 100 + self._tm.set_desired_speed(actor, speed) def _remove_actor_info(self, actor): """Removes all the references of the actor""" @@ -2228,6 +2380,7 @@ def _update_ego_data(self): self._route_index = index self._ego_wp = self._route[self._route_index] + self._ego_key = get_lane_key(self._ego_wp) if self.debug: string = 'EGO_' + self._ego_state[0].upper() diff --git a/srunner/scenarios/background_activity_parametrizer.py b/srunner/scenarios/background_activity_parametrizer.py new file mode 100644 index 000000000..9da61fcce --- /dev/null +++ b/srunner/scenarios/background_activity_parametrizer.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python + +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Module used to parse all the route and scenario configuration parameters. +""" + +from __future__ import print_function + +import py_trees + +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import (ChangeRoadBehavior, + ChangeOppositeBehavior, + ChangeJunctionBehavior) + +class BackgroundActivityParametrizer(BasicScenario): + """ + This class holds everything required to change the parameters of the background activity. + Mainly used to change its behavior when, for example, moving from a highway into the city, + where we might want a different BA behavior. + """ + + 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 + """ + # Road + self._num_front_vehicles = None + if 'num_front_vehicles' in config.other_parameters: + self._num_front_vehicles = float(config.other_parameters['num_front_vehicles']['value']) + self._num_back_vehicles = None + if 'num_back_vehicles' in config.other_parameters: + self._num_back_vehicles = float(config.other_parameters['num_back_vehicles']['value']) + self._road_spawn_dist = None + if 'road_spawn_dist' in config.other_parameters: + self._road_spawn_dist = float(config.other_parameters['road_spawn_dist']['value']) + + # Opposite + self._opposite_source_dist = None + if 'opposite_source_dist' in config.other_parameters: + self._opposite_source_dist = float(config.other_parameters['opposite_source_dist']['value']) + self._opposite_max_actors = None + if 'opposite_max_actors' in config.other_parameters: + self._opposite_max_actors = float(config.other_parameters['opposite_max_actors']['value']) + self._opposite_spawn_dist = None + if 'opposite_spawn_dist' in config.other_parameters: + self._opposite_spawn_dist = float(config.other_parameters['opposite_spawn_dist']['value']) + self._opposite_active = None + if 'opposite_active' in config.other_parameters: + self._opposite_active = float(config.other_parameters['opposite_active']['value']) + + # Junction + self._junction_source_dist = None + if 'junction_source_dist' in config.other_parameters: + self._junction_source_dist = float(config.other_parameters['junction_source_dist']['value']) + self._junction_max_actors = None + if 'junction_max_actors' in config.other_parameters: + self._junction_max_actors = float(config.other_parameters['junction_max_actors']['value']) + self._junction_spawn_dist = None + if 'junction_spawn_dist' in config.other_parameters: + self._junction_spawn_dist = float(config.other_parameters['junction_spawn_dist']['value']) + self._junction_source_perc = None + if 'junction_source_perc' in config.other_parameters: + self._junction_source_perc = float(config.other_parameters['junction_source_perc']['value']) + + super().__init__("BackgroundActivityParametrizer", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + + sequence = py_trees.composites.Sequence() + sequence.add_child(ChangeRoadBehavior(self._num_front_vehicles, self._num_back_vehicles, self._road_spawn_dist)) + sequence.add_child(ChangeJunctionBehavior( + self._junction_source_dist, self._junction_max_actors, self._junction_spawn_dist, self._junction_source_perc)) + sequence.add_child(ChangeOppositeBehavior( + self._opposite_source_dist, self._opposite_spawn_dist, self._opposite_active)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() + diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 16af87599..679190cf2 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -21,7 +21,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import \ InTriggerDistanceToVehicle from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import RemoveJunctionEntry +from srunner.tools.background_manager import HandleJunctionScenario def convert_dict_to_location(actor_dict): @@ -148,9 +148,14 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(RemoveJunctionEntry( - [self._reference_waypoint, self._blocker_waypoint], all_road_entries=True)) - + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[], + remove_exits=[], + stop_entries=True, + extend_road_exit=0 + )) # Ego go behind the blocker sequence.add_child(InTriggerDistanceToVehicle( self.other_actors[-1], self.ego_vehicles[0], self._block_distance)) diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index 8342f39ca..81fd8c8b0 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -14,20 +14,23 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + ActorTransformSetter, + SwitchWrongDirectionTest) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle -from srunner.scenarios.object_crash_vehicle import StationaryObjectCrossing -from srunner.tools.background_manager import HandleStartAccidentScenario, HandleEndAccidentScenario +from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import (ChangeOppositeBehavior, + RemoveRoadLane, + ReAddEgoRoadLane) -class ConstructionSetupCrossing(StationaryObjectCrossing): - +class ConstructionObstacle(BasicScenario): """ This class holds everything required for a construction scenario The ego vehicle is passing through a road and encounters - a stationary rectangular construction cones setup and traffic warning. + a stationary rectangular construction cones setup and traffic warning, + forcing it to lane change. This is a single ego vehicle scenario """ @@ -41,37 +44,31 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._drive_distance = 150 - self._distance_to_construction = 100 - self.construction_wp = None + if 'distance' in config.other_parameters: + self._distance = int(config.other_parameters['distance']['value']) + else: + self._distance = 100 + + self._takeover_max_dist = 60 + self._drive_distance = self._distance + self._takeover_max_dist + + self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) + self._construction_wp = None - super(ConstructionSetupCrossing, self).__init__(world, - ego_vehicles, - config, - randomize, - debug_mode, - criteria_enable) + self._construction_transforms = [] + + super().__init__("ConstructionObstacle", ego_vehicles, config, world, debug_mode, False, criteria_enable) def _initialize_actors(self, config): - """ - Custom initialization - """ - lane_width = self._reference_waypoint.lane_width - starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - construction_wps = starting_wp.next(self._distance_to_construction) - if not construction_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self.construction_wp = construction_wps[0] - - self._create_construction_setup(self.construction_wp.transform, lane_width) - pre_accident_wps = starting_wp.next(self._distance_to_construction/2) - if not construction_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self.construction_wp = pre_accident_wps[0] - - def create_cones_side(self, start_transform, forward_vector, z_inc=0, - cone_length=0, cone_offset=0): - + """Creates all props part of the construction""" + wps = self._reference_waypoint.next(self._distance) + if not wps: + raise ValueError("Couldn't find a viable position to set up the construction actors") + construction_wp = wps[0] + self._create_construction_setup(construction_wp.transform, self._reference_waypoint.lane_width) + + def create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_length=0, cone_offset=0): + """Creates the cones at tthe side""" _dist = 0 while _dist < (cone_length * cone_offset): # Move forward @@ -80,17 +77,18 @@ def create_cones_side(self, start_transform, forward_vector, z_inc=0, location = start_transform.location + forward_dist location.z += z_inc - transform = carla.Transform(location, start_transform.rotation) + spawn_transform = carla.Transform(location, start_transform.rotation) + spawn_transform.location.z -= 200 + cone_transform = carla.Transform(location, start_transform.rotation) - cone = CarlaDataProvider.request_new_actor( - 'static.prop.constructioncone', transform) - cone.set_simulate_physics(True) + cone = CarlaDataProvider.request_new_actor('static.prop.constructioncone', spawn_transform) + cone.set_simulate_physics(False) self.other_actors.append(cone) + self._construction_transforms.append([cone, cone_transform]) + def _create_construction_setup(self, start_transform, lane_width): - """ - Create Construction Setup - """ + """Create construction setup""" _initial_offset = {'cones': {'yaw': 180, 'k': lane_width / 2.0}, 'warning_sign': {'yaw': 180, 'k': 5, 'z': 0}, @@ -114,11 +112,16 @@ def _create_construction_setup(self, start_transform, lane_width): transform.rotation.get_forward_vector() transform.location.z += value['z'] transform.rotation.yaw += _perp_angle + + spawn_transform = carla.Transform(transform.location, transform.rotation) + spawn_transform.location.z -= 200 static = CarlaDataProvider.request_new_actor( - _prop_names[key], transform) - static.set_simulate_physics(True) + _prop_names[key], spawn_transform) + static.set_simulate_physics(False) self.other_actors.append(static) + self._construction_transforms.append([static, transform]) + # Cones side_transform = carla.Transform( start_transform.location, @@ -144,15 +147,22 @@ def _create_behavior(self): Only behavior here is to wait """ root = py_trees.composites.Sequence() - if self.route_mode: - root.add_child(HandleStartAccidentScenario(self.construction_wp, self._distance_to_construction)) + + for actor, transform in self._construction_transforms: + root.add_child(ActorTransformSetter(actor, transform, True)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if self.route_mode: - root.add_child(HandleEndAccidentScenario()) - root.add_child(Idle(15)) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) - return root + + if not self.route_mode: + return root + + sequence = py_trees.composites.Sequence() + sequence.add_child(RemoveRoadLane(self._reference_waypoint)) + sequence.add_child(root) + sequence.add_child(ReAddEgoRoadLane()) + return sequence def _create_test_criteria(self): """ @@ -168,3 +178,41 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class ConstructionObstacleTwoWays(ConstructionObstacle): + """ + Variation of ConstructionObstacle where the ego has to invade the opposite lane + """ + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): + + if 'frequency' in config.other_parameters: + self._opposite_frequency = float(config.other_parameters['frequency']['value']) + else: + self._opposite_frequency = 200 + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _create_behavior(self): + """ + Only behavior here is to wait + """ + root = py_trees.composites.Sequence() + for actor, transform in self._construction_transforms: + root.add_child(ActorTransformSetter(actor, transform, True)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + for i, _ in enumerate(self.other_actors): + root.add_child(ActorDestroy(self.other_actors[i])) + + if not self.route_mode: + return root + + sequence = py_trees.composites.Sequence() + sequence.add_child(SwitchWrongDirectionTest(False)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + sequence.add_child(RemoveRoadLane(self._reference_waypoint)) + sequence.add_child(root) + sequence.add_child(SwitchWrongDirectionTest(True)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=50)) + sequence.add_child(ReAddEgoRoadLane()) + return sequence diff --git a/srunner/scenarios/control_loss.py b/srunner/scenarios/control_loss.py index 2dff7e95b..d78209ecc 100644 --- a/srunner/scenarios/control_loss.py +++ b/srunner/scenarios/control_loss.py @@ -53,31 +53,35 @@ def _initialize_actors(self, config): """ Custom initialization """ + self._rng = CarlaDataProvider.get_random_seed() if self._randomize: - self._distance = random.randint(low=10, high=80, size=3) + self._distance = list(self._rng.randint(low=10, high=80, size=3)) self._distance = sorted(self._distance) + self._offset = list(2 * random.rand(3) - 1) else: self._distance = [14, 48, 74] + self._offset = [-0.6, 0.8, 0.2] self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) # Get the debris locations first_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[0]) - first_ground_loc = self.world.ground_projection(first_wp.transform.location, 2) - self.first_loc_prev = first_ground_loc.location if first_ground_loc else first_wp.transform.location + first_ground_loc = self.world.ground_projection(first_wp.transform.location + carla.Location(z=1), 2) + first_loc = first_ground_loc.location if first_ground_loc else first_wp.transform.location + first_loc = self._get_offset_location(first_loc, first_wp.transform.get_right_vector(), self._offset[0]) + self.first_transform = carla.Transform(first_loc, first_wp.transform.rotation) second_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[1]) - second_ground_loc = self.world.ground_projection(second_wp.transform.location, 2) - self.second_loc_prev = second_ground_loc.location if second_ground_loc else second_wp.transform.location + second_ground_loc = self.world.ground_projection(second_wp.transform.location + carla.Location(z=1), 2) + second_loc = second_ground_loc.location if second_ground_loc else second_wp.transform.location + second_loc = self._get_offset_location(second_loc, second_wp.transform.get_right_vector(), self._offset[1]) + self.second_transform = carla.Transform(second_loc, second_wp.transform.rotation) third_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[2]) - third_ground_loc = self.world.ground_projection(third_wp.transform.location, 2) - self.third_loc_prev = third_ground_loc.location if third_ground_loc else third_wp.transform.location - - # Get the debris transforms - self.first_transform = carla.Transform(self.first_loc_prev, first_wp.transform.rotation) - self.second_transform = carla.Transform(self.second_loc_prev, second_wp.transform.rotation) - self.third_transform = carla.Transform(self.third_loc_prev, third_wp.transform.rotation) + third_ground_loc = self.world.ground_projection(third_wp.transform.location + carla.Location(z=1), 2) + third_loc = third_ground_loc.location if third_ground_loc else third_wp.transform.location + third_loc = self._get_offset_location(third_loc, third_wp.transform.get_right_vector(), self._offset[2]) + self.third_transform = carla.Transform(third_loc, third_wp.transform.rotation) # Spawn the debris first_debris = CarlaDataProvider.request_new_actor( @@ -87,10 +91,6 @@ def _initialize_actors(self, config): third_debris = CarlaDataProvider.request_new_actor( 'static.prop.dirtdebris01', self.third_transform, rolename='prop') - first_debris.set_transform(self.first_transform) - second_debris.set_transform(self.second_transform) - third_debris.set_transform(self.third_transform) - # Remove their physics first_debris.set_simulate_physics(False) second_debris.set_simulate_physics(False) @@ -100,6 +100,10 @@ def _initialize_actors(self, config): self.other_actors.append(second_debris) self.other_actors.append(third_debris) + def _get_offset_location(self, location, direction, distance): + """Offset the debris a bit to the side. Debris in the center rarely affect the ego wheels""" + return location + carla.Location(x=distance*direction.x, y=distance*direction.y, z=distance*direction.z) + def _create_behavior(self): """ The scenario defined after is a "control loss vehicle" scenario. diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py index 2f736f397..571fd27bc 100644 --- a/srunner/scenarios/cross_bicycle_flow.py +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -14,16 +14,13 @@ import py_trees import carla -from agents.navigation.global_route_planner import GlobalRoutePlanner - from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import BicycleFlow, TrafficLightFreezer from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.scenario_helper import get_closest_traffic_light -from srunner.tools.background_manager import ClearJunction +from srunner.tools.background_manager import HandleJunctionScenario from agents.navigation.local_planner import RoadOption @@ -39,7 +36,7 @@ def convert_dict_to_location(actor_dict): return location -class CrossingBycicleFlow(BasicScenario): +class CrossingBicycleFlow(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, @@ -82,39 +79,46 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) - super().__init__("CrossingBycicleFlow", + super().__init__("CrossingBicycleFlow", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) - def _create_behavior(self): - """ - Hero vehicle is entering a junction in an urban area, at a signalized intersection, - while another actor runs a red lift, forcing the ego to break. - """ - source_wp = self._map.get_waypoint(self._start_flow, lane_type=carla.LaneType.Biking) - if not source_wp: - raise ValueError("Couldn't find a biking lane") - elif source_wp.transform.location.distance(self._start_flow) > 10: - raise ValueError("Couldn't find a biking lane at the specified location") + def _initialize_actors(self, config): + + ego_location = config.trigger_points[0].location + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + + # Get the junction + starting_wp = self._ego_wp + ego_junction_dist = 0 + while not starting_wp.is_junction: + starting_wps = starting_wp.next(1.0) + if len(starting_wps) == 0: + raise ValueError("Failed to find junction as a waypoint with no next was detected") + starting_wp = starting_wps[0] + ego_junction_dist += 1 + junction = starting_wp.get_junction() # Get the plan - plan = [] - junction_id = -1 + self._source_wp = self._map.get_waypoint(self._start_flow, lane_type=carla.LaneType.Biking) + if not self._source_wp or self._source_wp.transform.location.distance(self._start_flow) > 10: + raise ValueError("Couldn't find a biking lane at the specified location") + + self._plan = [] plan_step = 0 - wp = source_wp + wp = self._source_wp while True: next_wps = wp.next(1) if not next_wps: raise ValueError("Couldn't find a proper plan for the bicycle flow") next_wp = next_wps wp = next_wp[0] - plan.append([next_wp[0], RoadOption.LANEFOLLOW]) + self._plan.append([next_wp[0], RoadOption.LANEFOLLOW]) if plan_step == 0 and wp.is_junction: - junction_id = wp.get_junction().id plan_step += 1 elif plan_step == 1 and not wp.is_junction: plan_step += 1 @@ -122,22 +126,36 @@ def _create_behavior(self): elif plan_step == 2 and exit_loc.distance(wp.transform.location) > self._end_dist_flow: break - # Get the relevant traffic lights - tls = self._world.get_traffic_lights_in_junction(junction_id) - ego_tl = get_closest_traffic_light(self._reference_waypoint, tls) + self._get_traffic_lights(junction, ego_junction_dist) + + def _get_traffic_lights(self, junction, ego_dist): + """Get the traffic light of the junction, mapping their states""" + tls = self._world.get_traffic_lights_in_junction(junction.id) + if not tls: + raise ValueError("No traffic lights found, use the NonSignalized version instead") + + ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] + ego_tl = self._world.get_traffic_light(ego_landmark) self._flow_tl_dict = {} self._init_tl_dict = {} for tl in tls: - if tl == ego_tl: + if tl.id == ego_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Red else: self._flow_tl_dict[tl] = carla.TrafficLightState.Red self._init_tl_dict[tl] = carla.TrafficLightState.Red + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(BicycleFlow(plan, self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(BicycleFlow(self._plan, self._source_dist_interval, self._sink_distance, self._flow_speed, True)) # End condition, when the ego exits the junction end_condition = py_trees.composites.Sequence() @@ -155,7 +173,14 @@ def _create_behavior(self): return root sequence = py_trees.composites.Sequence() - sequence.add_child(ClearJunction()) + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[], + remove_exits=[], + stop_entries=False, + extend_road_exit=0 + )) sequence.add_child(root) return sequence diff --git a/srunner/scenarios/follow_leading_vehicle.py b/srunner/scenarios/follow_leading_vehicle.py index 38d9b0044..5f600611d 100644 --- a/srunner/scenarios/follow_leading_vehicle.py +++ b/srunner/scenarios/follow_leading_vehicle.py @@ -27,8 +27,7 @@ ActorDestroy, KeepVelocity, StopVehicle, - WaypointFollower, - Idle) + WaypointFollower) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToVehicle, InTriggerDistanceToNextIntersection, @@ -38,8 +37,6 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_waypoint_in_distance -from srunner.tools.background_manager import StopFrontVehicles, StartFrontVehicles - class FollowLeadingVehicle(BasicScenario): @@ -310,62 +307,3 @@ def __del__(self): Remove all actors upon deletion """ self.remove_all_actors() - - -class FollowLeadingVehicleRoute(BasicScenario): - - """ - This class is the route version of FollowLeadingVehicle where the backgrounda activity is used, - instead of spawning a specific vehicle and making it brake. - - This is a single ego vehicle scenario - """ - - timeout = 120 # Timeout of scenario in seconds - - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, - timeout=60): - """ - Setup all relevant parameters and create scenario - """ - self.timeout = timeout - self._stop_duration = 15 - self.end_distance = 15 - - super(FollowLeadingVehicleRoute, self).__init__("FollowLeadingVehicleRoute", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - pass - - def _create_behavior(self): - """ - Uses the Background Activity API to force a hard break on the vehicles in front of the actor, - then waits for a bit to check if the actor has collided. - """ - sequence = py_trees.composites.Sequence("FollowLeadingVehicleRoute") - sequence.add_child(StopFrontVehicles()) - sequence.add_child(Idle(self._stop_duration)) - sequence.add_child(StartFrontVehicles()) - sequence.add_child(DriveDistance(self.ego_vehicles[0], self.end_distance)) - - return sequence - - def _create_test_criteria(self): - """ - Empty, the route already has a collision criteria - """ - return [] - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/green_traffic_light.py b/srunner/scenarios/green_traffic_light.py new file mode 100644 index 000000000..c05fdd111 --- /dev/null +++ b/srunner/scenarios/green_traffic_light.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python + +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Sets the ego incoming traffic light to green. Support scenario at routes +to let the ego gather speed +""" + +import py_trees + +import carla + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import TrafficLightFreezer +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenarios.basic_scenario import BasicScenario + + +class PriorityAtJunction(BasicScenario): + """ + Sets the ego incoming traffic light to green. Support scenario at routes + to let the ego gather speed + """ + + timeout = 80 # Timeout of scenario in seconds + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=80): + """ + Setup all relevant parameters and create scenario + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self._tl_dict = {} + + self.timeout = timeout + super().__init__("PriorityAtJunction", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Get the junction and traffic lights + """ + ego_location = config.trigger_points[0].location + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + + # Get the junction + starting_wp = self._ego_wp + ego_junction_dist = 0 + while not starting_wp.is_junction: + starting_wps = starting_wp.next(1.0) + if len(starting_wps) == 0: + raise ValueError("Failed to find junction") + starting_wp = starting_wps[0] + ego_junction_dist += 1 + self._junction = starting_wp.get_junction() + + self._get_traffic_lights(self._junction, ego_junction_dist) + + def _get_traffic_lights(self, junction, junction_dist): + """Get the traffic light of the junction, mapping their states""" + tls = self._world.get_traffic_lights_in_junction(junction.id) + if not tls: + raise ValueError("No traffic lights found, nothing to do here") + + ego_landmark = self._ego_wp.get_landmarks_of_type(junction_dist + 1, "1000001")[0] + ego_tl = self._world.get_traffic_light(ego_landmark) + for tl in tls: + self._tl_dict[tl] = carla.TrafficLightState.Green if tl.id == ego_tl.id else carla.TrafficLightState.Red + + def _create_behavior(self): + """ + Freeze the traffic lights until the ego has exited the junction + """ + root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(WaitEndIntersection(self.ego_vehicles[0], self._junction.id)) + root.add_child(TrafficLightFreezer(self._tl_dict)) + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/hard_break.py b/srunner/scenarios/hard_break.py new file mode 100644 index 000000000..13f8e77f4 --- /dev/null +++ b/srunner/scenarios/hard_break.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python + +# Copyright (c) 2018-2022 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Hard break scenario: + +The scenario spawn a vehicle in front of the ego that drives for a while before +suddenly hard breaking, forcing the ego to avoid the collision +""" + +import py_trees + +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenarios.basic_scenario import BasicScenario + +from srunner.tools.background_manager import StopFrontVehicles, StartFrontVehicles + + +class HardBreakRoute(BasicScenario): + + """ + This class uses the is the Background Activity at routes to create a hard break scenario. + + This is a single ego vehicle scenario + """ + + timeout = 120 # Timeout of scenario in seconds + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=60): + """ + Setup all relevant parameters and create scenario + """ + self.timeout = timeout + self._stop_duration = 10 + self.end_distance = 15 + + super().__init__("HardBreak", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + pass + + def _create_behavior(self): + """ + Uses the Background Activity to force a hard break on the vehicles in front of the actor, + then waits for a bit to check if the actor has collided. After a set duration, + the front vehicles will resume their movement + """ + sequence = py_trees.composites.Sequence("HardBreak") + sequence.add_child(StopFrontVehicles()) + sequence.add_child(Idle(self._stop_duration)) + sequence.add_child(StartFrontVehicles()) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self.end_distance)) + + return sequence + + def _create_test_criteria(self): + """ + Empty, the route already has a collision criteria + """ + return [] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index 0167edb22..e0c08bcf0 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -23,10 +23,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import (SwitchRouteSources, - ExtentExitRoadSpace, - RemoveJunctionEntry, - ClearJunction) +from srunner.tools.background_manager import HandleJunctionScenario from srunner.tools.scenario_helper import generate_target_waypoint @@ -41,7 +38,7 @@ def convert_dict_to_location(actor_dict): ) return location -class HighwayCutInRoute(BasicScenario): +class HighwayCutIn(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, @@ -67,7 +64,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._start_location = convert_dict_to_location(config.other_parameters['other_actor_location']) - super().__init__("HighwayEntryCutIn", + super().__init__("HighwayCutIn", ego_vehicles, config, world, @@ -97,15 +94,17 @@ def _create_behavior(self): Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ - # behavior.add_child(SwitchRouteSources(False)) - - behavior = py_trees.composites.Sequence("HighwayCutInRoute") + behavior = py_trees.composites.Sequence("HighwayCutIn") if self.route_mode: - behavior.add_child(RemoveJunctionEntry([self._other_waypoint], True)) - behavior.add_child(ClearJunction()) - behavior.add_child(ExtentExitRoadSpace(self._extra_space)) - + behavior.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=False, + remove_entries=[self._other_waypoint], + remove_exits=[], + stop_entries=False, + extend_road_exit=self._extra_space + )) behavior.add_child(ActorTransformSetter(self._cut_in_vehicle, self._other_transform)) # Sync behavior diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 9808c543d..a3e63e91a 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -22,7 +22,6 @@ from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, - WaitEndIntersection, DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route @@ -76,7 +75,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) self._ego_route = config.route - self._start_distance = 10 + self._start_distance = 11 self._spawn_dist = self._start_distance self._number_of_attempts = 6 self._retry_dist = 0.4 @@ -84,8 +83,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_transform = None self._collision_wp = None - self._adversary_speed = 4.0 # Speed of the adversary [m/s] - self._reaction_time = 0.5 # Time the agent has to react to avoid the collision [s] + self._adversary_speed = 2.0 # Speed of the adversary [m/s] + self._reaction_time = 1.6 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 @@ -159,34 +158,24 @@ def _create_behavior(self): collision_location = self._collision_wp.transform.location collision_distance = collision_location.distance(self._adversary_transform.location) collision_duration = collision_distance / self._adversary_speed - collision_time_trigger = collision_duration + self._reaction_time - - # First waiting behavior - sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) # Adversary trigger behavior trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") trigger_adversary.add_child(InTimeToArrivalToLocation( - self.ego_vehicles[0], collision_time_trigger, collision_location)) + self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) sequence.add_child(HandBrakeVehicle(self.other_actors[0], False)) - if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._spawn_dist)) - # Move the adversary. speed_duration = 2.0 * collision_duration speed_distance = 2.0 * collision_distance sequence.add_child(KeepVelocity( - self.other_actors[0], - self._adversary_speed, - True, - speed_duration, - speed_distance, - name="AdversaryCrossing") + self.other_actors[0], self._adversary_speed, True, + speed_duration, speed_distance, name="AdversaryCrossing") ) # Remove everything diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 4b1176e13..79b5290cd 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -25,7 +25,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_location_in_distance_from_wp -from srunner.tools.background_manager import LeaveSpaceInFront, RemoveJunctionEntry +from srunner.tools.background_manager import LeaveSpaceInFront class StationaryObjectCrossing(BasicScenario): @@ -65,9 +65,9 @@ def _initialize_actors(self, config): """ Custom initialization """ - _start_distance = 40 + _distance = 40 lane_width = self._reference_waypoint.lane_width - location, _ = get_location_in_distance_from_wp(self._reference_waypoint, _start_distance) + location, _ = get_location_in_distance_from_wp(self._reference_waypoint, _distance) waypoint = self._wmap.get_waypoint(location) offset = {"orientation": 270, "position": 90, "z": 0.4, "k": 0.2} position_yaw = waypoint.transform.rotation.yaw + offset['position'] @@ -139,9 +139,7 @@ class DynamicObjectCrossing(BasicScenario): This is a single ego vehicle scenario """ - def __init__(self, world, ego_vehicles, config, - adversary_type='walker.*', blocker_type='static.prop.vendingmachine', - 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 """ @@ -150,19 +148,25 @@ def __init__(self, world, ego_vehicles, config, self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) self._num_lane_changes = 0 - self._start_distance = 12 + if 'distance' in config.other_parameters: + self._distance = int(config.other_parameters['distance']['value']) + else: + self._distance = 12 + + if 'blocker_model' in config.other_parameters: + self._blocker_model = config.other_parameters['blocker_model']['value'] + else: + self._blocker_model = 'static.prop.vendingmachine' # blueprint filter of the blocker + self._blocker_shift = 0.9 self._retry_dist = 0.4 - self._adversary_type = adversary_type # blueprint filter of the adversary - self._blocker_type = blocker_type # blueprint filter of the blocker self._adversary_transform = None self._blocker_transform = None self._collision_wp = None - self._adversary_speed = 4.0 # Speed of the adversary [m/s] - self._reaction_time = 0.8 # Time the agent has to react to avoid the collision [s] - self._reaction_ratio = 0.12 # The higehr the number of lane changes, the smaller the reaction time + self._adversary_speed = 2.0 # Speed of the adversary [m/s] + self._reaction_time = 1.8 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout @@ -190,7 +194,7 @@ def _get_sidewalk_transform(self, waypoint, offset): new_location = waypoint.transform.location else: right_vector = waypoint.transform.get_right_vector() - offset_dist = waypoint.lane_width * offset["k"] + offset_dist = offset["k"] offset_location = carla.Location(offset_dist * right_vector.x, offset_dist * right_vector.y) new_location = waypoint.transform.location + offset_location new_location.z += offset['z'] @@ -202,10 +206,10 @@ def _initialize_actors(self, config): Custom initialization """ # Get the waypoint in front of the ego. - move_dist = self._start_distance + move_dist = self._distance waypoint = self._reference_waypoint while self._number_of_attempts > 0: - self._num_lane_changes = 0 + self._collision_dist = 0 # Move to the front location, _ = get_location_in_distance_from_wp(waypoint, move_dist, False) @@ -219,36 +223,42 @@ def _initialize_actors(self, config): if right_wp is None: break # No more right lanes sidewalk_waypoint = right_wp - self._num_lane_changes += 1 - # Get the adversary transform and spawn it - offset = {"yaw": 270, "z": 0.5, "k": 1.0} - self._adversary_transform = self._get_sidewalk_transform(sidewalk_waypoint, offset) - adversary = CarlaDataProvider.request_new_actor(self._adversary_type, self._adversary_transform) - if adversary is None: + # Get the blocker transform and spawn it + offset = {"yaw": 90, "z": 0.0, "k": 1.5} + self._blocker_transform = self._get_sidewalk_transform(sidewalk_waypoint, offset) + blocker = CarlaDataProvider.request_new_actor(self._blocker_model, self._blocker_transform) + if not blocker: self._number_of_attempts -= 1 move_dist = self._retry_dist + print("Failed blocker") continue - # Get the blocker transform and spawn it - blocker_wp = sidewalk_waypoint.previous(self._blocker_shift)[0] - offset = {"yaw": 90, "z": 0.0, "k": 1.0} - self._blocker_transform = self._get_sidewalk_transform(blocker_wp, offset) - blocker = CarlaDataProvider.request_new_actor(self._blocker_type, self._blocker_transform) - if not blocker: - adversary.destroy() + # Get the adversary transform and spawn it + walker_dist = blocker.bounding_box.extent.x + 0.5 + wps = sidewalk_waypoint.next(walker_dist) + if not wps: + raise ValueError("Couldn't find a location to spawn the adversary") + walker_wp = wps[0] + + offset = {"yaw": 270, "z": 0.5, "k": 1.2} + self._adversary_transform = self._get_sidewalk_transform(walker_wp, offset) + adversary = CarlaDataProvider.request_new_actor('walker.*', self._adversary_transform) + if adversary is None: + blocker.destroy() self._number_of_attempts -= 1 move_dist = self._retry_dist + print("Failed adversary") continue - # Both actors where summoned, end + self._collision_dist += waypoint.transform.location.distance(self._adversary_transform.location) + + # Both actors were succesfully spawned, end break if self._number_of_attempts == 0: raise Exception("Couldn't find viable position for the adversary and blocker actors") - if isinstance(adversary, carla.Vehicle): - adversary.apply_control(carla.VehicleControl(hand_brake=True)) blocker.set_simulate_physics(enabled=False) self.other_actors.append(adversary) self.other_actors.append(blocker) @@ -262,29 +272,192 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence(name="CrossingActor") if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._start_distance)) + sequence.add_child(LeaveSpaceInFront(self._distance)) collision_location = self._collision_wp.transform.location - collision_distance = collision_location.distance(self._adversary_transform.location) - collision_duration = collision_distance / self._adversary_speed - reaction_time = self._reaction_time - self._reaction_ratio * self._num_lane_changes - collision_time_trigger = collision_duration + reaction_time # Wait until ego is close to the adversary trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") trigger_adversary.add_child(InTimeToArrivalToLocation( - self.ego_vehicles[0], collision_time_trigger, collision_location)) + self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) # Move the adversary - speed_duration = 2.0 * collision_duration - speed_distance = 2.0 * collision_distance + move_distance = 2 * self._collision_dist # Cross the whole road (supposing symetry in both directions) + move_duration = move_distance / self._adversary_speed sequence.add_child(KeepVelocity( self.other_actors[0], self._adversary_speed, - duration=speed_duration, distance=speed_distance, name="AdversaryCrossing")) + duration=move_duration, distance=move_distance, name="AdversaryCrossing")) + + # Remove everything + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) + sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + + +class ParkingCrossingPedestrian(BasicScenario): + + """ + Variation of DynamicObjectCrossing but now the blocker is now a vehicle + This class holds everything required for a simple object crash + without prior vehicle action involving a vehicle and a cyclist/pedestrian, + The ego vehicle is passing through a road, + And encounters a cyclist/pedestrian crossing the road. + + This is a single ego vehicle scenario + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): + """ + Setup all relevant parameters and create scenario + """ + self._wmap = CarlaDataProvider.get_map() + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + self._num_lane_changes = 0 + + if 'distance' in config.other_parameters: + self._distance = int(config.other_parameters['distance']['value']) + else: + self._distance = 12 + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = 'right' + + if self._direction not in ('right', 'left'): + raise ValueError("'direction' value must be either 'left' or 'right'") + + self._adversary_speed = 3.0 # Speed of the adversary [m/s] + self._reaction_time = 1.9 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] + self._ego_end_distance = 40 + self.timeout = timeout + + super().__init__("ParkingCrossingPedestrian", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _get_blocker_transform(self, waypoint): + """Processes the driving wp to get a waypoint at the side that looks at the road""" + if waypoint.lane_type == carla.LaneType.Sidewalk: + new_location = waypoint.transform.location + else: + vector = waypoint.transform.get_right_vector() + if self._direction == 'left': + vector *= -1 + + offset_location = carla.Location(waypoint.lane_width * vector.x, waypoint.lane_width * vector.y) + new_location = waypoint.transform.location + offset_location + new_location.z += 0.5 + + return carla.Transform(new_location, waypoint.transform.rotation) + + def _get_walker_transform(self, waypoint): + """Processes the driving wp to get a waypoint at the side that looks at the road""" + + new_rotation = waypoint.transform.rotation + new_rotation.yaw += 270 if self._direction == 'right' else 90 + + if waypoint.lane_type == carla.LaneType.Sidewalk: + new_location = waypoint.transform.location + else: + vector = waypoint.transform.get_right_vector() + if self._direction == 'left': + vector *= -1 + + offset_location = carla.Location(waypoint.lane_width * vector.x, waypoint.lane_width * vector.y) + new_location = waypoint.transform.location + offset_location + new_location.z += 0.5 + + return carla.Transform(new_location, new_rotation) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Get the adversary transform and spawn it + wps = self._reference_waypoint.next(self._distance) + if not wps: + raise ValueError("Couldn't find a location to spawn the adversary") + blocker_wp = wps[0] + + # Get the adversary transform and spawn it + self._blocker_transform = self._get_blocker_transform(blocker_wp) + blocker = CarlaDataProvider.request_new_actor('vehicle.*', self._blocker_transform, attribute_filter={'base_type': 'car'}) + if blocker is None: + raise ValueError("Couldn't spawn the adversary") + self.other_actors.append(blocker) + blocker.apply_control(carla.VehicleControl(hand_brake=True)) + + walker_dist = blocker.bounding_box.extent.x + 0.5 + wps = blocker_wp.next(walker_dist) + if not wps: + raise ValueError("Couldn't find a location to spawn the adversary") + walker_wp = wps[0] + + # Get the adversary transform and spawn it + self._walker_transform = self._get_walker_transform(walker_wp) + walker = CarlaDataProvider.request_new_actor('walker.*', self._walker_transform) + if walker is None: + raise ValueError("Couldn't spawn the adversary") + self.other_actors.append(walker) + + self._collision_wp = walker_wp + + def _create_behavior(self): + """ + After invoking this scenario, cyclist will wait for the user + controlled vehicle to enter trigger distance region, + the cyclist starts crossing the road once the condition meets, + then after 60 seconds, a timeout stops the scenario + """ + sequence = py_trees.composites.Sequence(name="ParkingCrossingPedestrian") + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._distance)) + + collision_location = self._collision_wp.transform.location + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) + + # Move the adversary + distance = 8.0 # Scenario is meant to be used at a one lane - one direction road + duration = distance / self._adversary_speed + + sequence.add_child(KeepVelocity( + self.other_actors[1], self._adversary_speed, + duration=duration, distance=distance, name="AdversaryCrossing")) # Remove everything sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 173ef7f39..b14f8c563 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -28,10 +28,9 @@ from srunner.tools.scenario_helper import (get_geometric_linear_intersection, generate_target_waypoint, get_junction_topology, - filter_junction_wp_direction, - get_closest_traffic_light) + filter_junction_wp_direction) -from srunner.tools.background_manager import JunctionScenarioManager +from srunner.tools.background_manager import HandleJunctionScenario class OppositeVehicleRunningRedLight(BasicScenario): @@ -51,11 +50,19 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self._source_dist = 30 self._sink_dist = 20 - self._direction = None - self._opposite_bp_wildcards = ['*firetruck*', '*ambulance*', '*police*'] # Wildcard patterns of the blueprints + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = "right" + + if 'adversary_speed' in config.other_parameters: + self._adversary_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._adversary_speed = 70 / 3.6 # m/s + self.timeout = timeout - self._adversary_speed = 70 / 3.6 # Speed of the adversary [m/s] self._sync_time = 2.2 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 9.0 # Min distance to the collision location that triggers the adversary [m] self._speed_duration_ratio = 2.0 @@ -63,55 +70,48 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 - # Get the CDP seed or at routes, all copies of the scenario will have the same configuration - self._rng = CarlaDataProvider.get_random_seed() - - super(OppositeVehicleRunningRedLight, self).__init__("OppositeVehicleRunningRedLight", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("OppositeVehicleRunningRedLight", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ ego_location = config.trigger_points[0].location - ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction - starting_wp = ego_wp + starting_wp = self._ego_wp + ego_junction_dist = 0 while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError("Failed to find junction as a waypoint with no next was detected") starting_wp = starting_wps[0] + ego_junction_dist += 1 junction = starting_wp.get_junction() # Get the opposite entry lane wp - possible_directions = ['right', 'left'] - self._rng.shuffle(possible_directions) - for direction in possible_directions: - entry_wps, _ = get_junction_topology(junction) - source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, direction) - if source_entry_wps: - self._direction = direction - break - if not self._direction: - raise ValueError("Trying to find a lane to spawn the opposite actor but none was found") + entry_wps, _ = get_junction_topology(junction) + source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) + if not source_entry_wps: + raise ValueError("Couldn't find a lane for the given direction") # Get the source transform spawn_wp = source_entry_wps[0] - added_dist = 0 - while added_dist < self._source_dist: + source_junction_dist = 0 + while source_junction_dist < self._source_dist: spawn_wps = spawn_wp.previous(1.0) if len(spawn_wps) == 0: raise ValueError("Failed to find a source location as a waypoint with no previous was detected") if spawn_wps[0].is_junction: break spawn_wp = spawn_wps[0] - added_dist += 1 + source_junction_dist += 1 self._spawn_wp = spawn_wp source_transform = spawn_wp.transform @@ -121,8 +121,8 @@ def _initialize_actors(self, config): ) # Spawn the actor and move it below ground - opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards) - opposite_actor = CarlaDataProvider.request_new_actor(opposite_bp_wildcard, self._spawn_location) + opposite_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._spawn_location, attribute_filter={'special_type': 'emergency'}) if not opposite_actor: raise Exception("Couldn't spawn the actor") lights = opposite_actor.get_light_state() @@ -146,17 +146,29 @@ def _initialize_actors(self, config): # get the collision location self._collision_location = get_geometric_linear_intersection( - starting_wp.transform.location, source_entry_wps[0].transform.location) + starting_wp.transform.location, source_entry_wps[0].transform.location, True) if not self._collision_location: raise ValueError("Couldn't find an intersection point") - # Get the relevant traffic lights + # Get the z component + collision_wp = self._map.get_waypoint(self._collision_location) + self._collision_location.z = collision_wp.transform.location.z + + self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + + def _get_traffic_lights(self, junction, ego_dist, source_dist): + """Get the traffic light of the junction, mapping their states""" tls = self._world.get_traffic_lights_in_junction(junction.id) - ego_tl = get_closest_traffic_light(ego_wp, tls) - source_tl = get_closest_traffic_light(self._spawn_wp, tls,) + if not tls: + raise ValueError("No traffic lights found, use the NonSignalized version instead") + + ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] + ego_tl = self._world.get_traffic_light(ego_landmark) + source_landmark = self._spawn_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] + source_tl = self._world.get_traffic_light(source_landmark) self._tl_dict = {} for tl in tls: - if tl in (ego_tl, source_tl): + if tl.id in (ego_tl.id, source_tl.id): self._tl_dict[tl] = carla.TrafficLightState.Green else: self._tl_dict[tl] = carla.TrafficLightState.Red @@ -179,7 +191,7 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) sequence.add_child(ConstantVelocityAgentBehavior( self.other_actors[0], target_location=self._sink_wp.transform.location, - target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True}, name="AdversaryCrossing")) + target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, name="AdversaryCrossing")) main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) main_behavior.add_child(TrafficLightFreezer(self._tl_dict)) @@ -187,7 +199,195 @@ def _create_behavior(self): root = py_trees.composites.Sequence() if self.route_mode: - root.add_child(JunctionScenarioManager(self._direction, True)) + root.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[self._spawn_wp], + remove_exits=[self._sink_wp], + stop_entries=False, + extend_road_exit=0 + )) + root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) + root.add_child(main_behavior) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() + + +class OppositeVehicleTakingPriority(BasicScenario): + """ + This class holds everything required for a scenario in which another vehicle takes + priority at an intersection, forcing the ego to break. + This vehicles are 'special' ones such as police cars, ambulances or firetrucks. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self._source_dist = 30 + self._sink_dist = 20 + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = "right" + + if 'adversary_speed' in config.other_parameters: + self._adversary_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._adversary_speed = 70 / 3.6 # m/s + + self._opposite_bp_wildcards = ['*firetruck*', '*ambulance*', '*police*'] # Wildcard patterns of the blueprints + self.timeout = timeout + + self._sync_time = 2.2 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 9.0 # Min distance to the collision location that triggers the adversary [m] + self._speed_duration_ratio = 2.0 + self._speed_distance_ratio = 1.5 + + self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 + + # Get the CDP seed or at routes, all copies of the scenario will have the same configuration + self._rng = CarlaDataProvider.get_random_seed() + + super().__init__("OppositeVehicleTakingPriority", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + ego_location = config.trigger_points[0].location + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + + # Get the junction + starting_wp = self._ego_wp + ego_junction_dist = 0 + while not starting_wp.is_junction: + starting_wps = starting_wp.next(1.0) + if len(starting_wps) == 0: + raise ValueError("Failed to find junction as a waypoint with no next was detected") + starting_wp = starting_wps[0] + ego_junction_dist += 1 + junction = starting_wp.get_junction() + + # Get the opposite entry lane wp + entry_wps, _ = get_junction_topology(junction) + source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) + if not source_entry_wps: + raise ValueError("Couldn't find a lane for the given direction") + + # Get the source transform + spawn_wp = source_entry_wps[0] + source_junction_dist = 0 + while source_junction_dist < self._source_dist: + spawn_wps = spawn_wp.previous(1.0) + if len(spawn_wps) == 0: + raise ValueError("Failed to find a source location as a waypoint with no previous was detected") + if spawn_wps[0].is_junction: + break + spawn_wp = spawn_wps[0] + source_junction_dist += 1 + self._spawn_wp = spawn_wp + + source_transform = spawn_wp.transform + self._spawn_location = carla.Transform( + source_transform.location + carla.Location(z=0.1), + source_transform.rotation + ) + + # Spawn the actor and move it below ground + opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards) + opposite_actor = CarlaDataProvider.request_new_actor(opposite_bp_wildcard, self._spawn_location) + if not opposite_actor: + raise Exception("Couldn't spawn the actor") + opposite_actor.set_light_state(carla.VehicleLightState( + carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) + self.other_actors.append(opposite_actor) + + opposite_transform = carla.Transform( + source_transform.location - carla.Location(z=500), + source_transform.rotation + ) + opposite_actor.set_transform(opposite_transform) + opposite_actor.set_simulate_physics(enabled=False) + + # Get the sink location + sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) + sink_wps = sink_exit_wp.next(self._sink_dist) + if len(sink_wps) == 0: + raise ValueError("Failed to find a sink location as a waypoint with no next was detected") + self._sink_wp = sink_wps[0] + + # get the collision location + self._collision_location = get_geometric_linear_intersection( + starting_wp.transform.location, source_entry_wps[0].transform.location, True) + if not self._collision_location: + raise ValueError("Couldn't find an intersection point") + + # Get the z component + collision_wp = self._map.get_waypoint(self._collision_location) + self._collision_location.z = collision_wp.transform.location.z + + def _create_behavior(self): + """ + Hero vehicle is entering a junction in an urban area, at a signalized intersection, + while another actor runs a red lift, forcing the ego to break. + """ + sequence = py_trees.composites.Sequence(name="OppositeVehicleTakingPriority") + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._sync_time, self._collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._collision_location, self._min_trigger_dist)) + + sequence.add_child(trigger_adversary) + sequence.add_child(ConstantVelocityAgentBehavior( + self.other_actors[0], target_location=self._sink_wp.transform.location, + target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, name="AdversaryCrossing")) + + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(sequence) + + root = py_trees.composites.Sequence() + if self.route_mode: + root.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[self._spawn_wp], + remove_exits=[self._sink_wp], + stop_entries=True, + extend_road_exit=0 + )) + root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(main_behavior) root.add_child(ActorDestroy(self.other_actors[0])) diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index 269b35f1f..fcc8bcfee 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -14,7 +14,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, - WaypointFollower) + BasicAgentBehavior) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -38,10 +38,10 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._cut_in_distance = 25 - self._blocker_distance = 18 + self._cut_in_distance = 35 + self._blocker_distance = 28 - self._adversary_speed = 10.0 # Speed of the adversary [m/s] + self._adversary_speed = 13.0 # Speed of the adversary [m/s] self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 10.0 # Min distance to the collision location that triggers the adversary [m] self._end_distance = 40 @@ -81,8 +81,8 @@ def _initialize_actors(self, config): self._blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(self._blocker_actor) - side_transform = self._get_displaced_transform(self._blocker_actor, parking_wp) - self._blocker_actor.set_location(side_transform) + side_location = self._get_displaced_location(self._blocker_actor, parking_wp) + self._blocker_actor.set_location(side_location) collision_wps = self._reference_waypoint.next(self._cut_in_distance) if not collision_wps: @@ -101,12 +101,12 @@ def _initialize_actors(self, config): raise ValueError("Couldn't spawn the parked actor") self.other_actors.append(self._parked_actor) - side_transform = self._get_displaced_transform(self._parked_actor, parking_wp) - self._parked_actor.set_location(side_transform) + side_location = self._get_displaced_location(self._parked_actor, parking_wp) + self._parked_actor.set_location(side_location) - def _get_displaced_transform(self, actor, wp): + def _get_displaced_location(self, actor, wp): """ - Calculates the transforming such that the actor is at the sidemost part of the lane + Calculates the location such that the actor is at the sidemost part of the lane """ # Move the actor to the edge of the lane near the sidewalk displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 @@ -127,7 +127,7 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence(name="CrossingActor") if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._cut_in_distance)) + sequence.add_child(LeaveSpaceInFront(self._cut_in_distance + 10)) collision_location = self._collision_wp.transform.location @@ -143,7 +143,7 @@ def _create_behavior(self): # Move the adversary cut_in = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") - cut_in.add_child(WaypointFollower(self.other_actors[1], self._adversary_speed)) + cut_in.add_child(BasicAgentBehavior(self.other_actors[1])) cut_in.add_child(DriveDistance(self.other_actors[1], self._end_distance)) sequence.add_child(cut_in) diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index f399a007e..832139f4b 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -16,11 +16,13 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import SwitchOutsideRouteLanesTest, ActorTransformSetter, ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario +from srunner.tools.background_manager import ChangeRoadBehavior + def convert_dict_to_location(actor_dict): """ @@ -76,6 +78,11 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl else: self._direction = "right" + if 'flow_distance' in config.other_parameters: + self._flow_distance = float(config.other_parameters['flow_distance']['value']) + else: + self._flow_distance = 30 + # Get parking_waypoint based on trigger_point self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint(self._trigger_location) @@ -88,6 +95,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl raise Exception( "Couldn't find parking point on the {} side".format(self._direction)) + self._bp_attributes = {'base_type': 'car', 'has_lights': False} + super(ParkingExit, self).__init__("ParkingExit", ego_vehicles, config, @@ -107,15 +116,15 @@ def _initialize_actors(self, config): "Couldn't find viable position for the vehicle in front of the parking point") actor_front = CarlaDataProvider.request_new_actor( - 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter=self._bp_attributes) if actor_front is None: raise ValueError( "Couldn't spawn the vehicle in front of the parking point") self.other_actors.append(actor_front) # And move it to the side - side_transform = self._get_displaced_transform(actor_front, front_points[0]) - actor_front.set_location(side_transform) + side_location = self._get_displaced_location(actor_front, front_points[0]) + actor_front.set_location(side_location) # Spawn the actor behind the ego behind_points = self._parking_waypoint.previous( @@ -125,21 +134,21 @@ def _initialize_actors(self, config): "Couldn't find viable position for the vehicle behind the parking point") actor_behind = CarlaDataProvider.request_new_actor( - 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter={'base_type': 'car'}) + 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter=self._bp_attributes) if actor_behind is None: raise ValueError( "Couldn't spawn the vehicle behind the parking point") self.other_actors.append(actor_behind) # And move it to the side - side_transform = self._get_displaced_transform(actor_behind, behind_points[0]) - actor_behind.set_location(side_transform) + side_location = self._get_displaced_location(actor_behind, behind_points[0]) + actor_behind.set_location(side_location) # Move the ego to its side position - self._ego_transform = self._get_displaced_transform(self.ego_vehicles[0], self._parking_waypoint) + self._ego_transform = self._get_displaced_location(self.ego_vehicles[0], self._parking_waypoint) self.ego_vehicles[0].set_location(self._ego_transform) - def _get_displaced_transform(self, actor, wp): + def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane """ @@ -163,10 +172,7 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence() - - # Deactivate OutsideRouteLanesTest - sequence.add_child(SwitchOutsideRouteLanesTest(False)) - + sequence.add_child(ChangeRoadBehavior(spawn_dist=self._flow_distance)) root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) @@ -176,10 +182,9 @@ def _create_behavior(self): root.add_child(end_condition) sequence.add_child(root) - sequence.add_child(SwitchOutsideRouteLanesTest(True)) - for actor in self.other_actors: sequence.add_child(ActorDestroy(actor)) + sequence.add_child(ChangeRoadBehavior(spawn_dist=15)) return sequence diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index d8fabe378..4d786df15 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -17,7 +17,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import RemoveJunctionEntry +from srunner.tools.background_manager import HandleJunctionScenario def convert_dict_to_location(actor_dict): @@ -78,7 +78,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl for item in end_walker_flow_list: self._sink_locations.append(convert_dict_to_location(item)) - self._sink_locations_prob.append(float(item['p'])) + prob = float(item['p']) if 'p' in item else 0.5 + self._sink_locations_prob.append(prob) if 'source_dist_interval' in config.other_parameters: self._source_dist_interval = [ @@ -107,7 +108,14 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence(name="CrossingPedestrian") if self.route_mode: - sequence.add_child(RemoveJunctionEntry([self._reference_waypoint])) + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=[], + remove_exits=[], + stop_entries=True, + extend_road_exit=0 + )) # Move the adversary root = py_trees.composites.Parallel( diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 926e9e65b..986fb75f1 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -16,22 +16,19 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, SwitchOutsideRouteLanesTest, \ - BasicAgentBehavior, BicycleFlow, ConstantVelocityAgentBehavior +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + SwitchWrongDirectionTest, + ConstantVelocityAgentBehavior) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tests.carla_mocks.agents.navigation.local_planner import RoadOption -from srunner.tools.background_manager import (HandleStartAccidentScenario, - HandleEndAccidentScenario, - LeaveSpaceInFront, - ChangeOppositeBehavior) +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior class Accident(BasicScenario): """ This class holds everything required for a scenario in which there is an accident - in front of the ego, forcing it to react. A police vehicle is located before + in front of the ego, forcing it to lane change. A police vehicle is located before two other cars that have been in an accident. """ @@ -44,30 +41,39 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._drive_distance = 120 - self._distance_to_accident = 100 + if 'distance' in config.other_parameters: + self._distance = int(config.other_parameters['distance']['value']) + else: + self._distance = 120 + + if 'direction' in config.other_parameters: + self._direction = config.other_parameters['direction']['value'] + else: + self._direction = 'right' + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + self._offset = 0.75 self._first_distance = 10 self._second_distance = 6 + + self._takeover_max_dist = self._first_distance + self._second_distance + 40 + self._drive_distance = self._distance + self._takeover_max_dist + self._accident_wp = None self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 - super(Accident, self).__init__("Accident", - ego_vehicles, - config, - world, - randomize, - debug_mode, - criteria_enable=criteria_enable) + super().__init__( + "Accident", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - accident_wps = starting_wp.next(self._distance_to_accident) - pre_accident_wps = starting_wp.next(self._distance_to_accident/2) + accident_wps = starting_wp.next(self._distance) + pre_accident_wps = starting_wp.next(self._distance / 2) if not accident_wps: raise ValueError("Couldn't find a viable position to set up the accident actors") if not pre_accident_wps: @@ -77,10 +83,14 @@ def _initialize_actors(self, config): # Create the police vehicle displacement = self._offset * self._accident_wp.lane_width / 2 r_vec = self._accident_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 w_loc = self._accident_wp.transform.location w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) + if not police_car: + raise ValueError("Couldn't spawn the police car") lights = police_car.get_light_state() lights |= self._lights police_car.set_light_state(carla.VehicleLightState(lights)) @@ -94,40 +104,49 @@ def _initialize_actors(self, config): self._accident_wp = pre_accident_wps[0] displacement = self._offset * vehicle_wp.lane_width / 2 r_vec = vehicle_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 w_loc = vehicle_wp.transform.location w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) vehicle_1_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_1_car = CarlaDataProvider.request_new_actor('vehicle.tesla.model3', vehicle_1_transform) + vehicle_1_car = CarlaDataProvider.request_new_actor( + 'vehicle.*', vehicle_1_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) + if not vehicle_1_car: + raise ValueError("Couldn't spawn the accident car") self.other_actors.append(vehicle_1_car) # Create the second vehicle that has been in the accident vehicle_wps = vehicle_wp.next(self._second_distance) - if not vehicle_wps: + if not vehicle_wps: raise ValueError("Couldn't find a viable position to set up the accident actors") vehicle_wp = vehicle_wps[0] displacement = self._offset * vehicle_wp.lane_width / 2 r_vec = vehicle_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 w_loc = vehicle_wp.transform.location w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) vehicle_2_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_2_car = CarlaDataProvider.request_new_actor('vehicle.mercedes.coupe_2020', vehicle_2_transform) + vehicle_2_car = CarlaDataProvider.request_new_actor( + 'vehicle.*', vehicle_2_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) + if not vehicle_2_car: + raise ValueError("Couldn't spawn the accident car") self.other_actors.append(vehicle_2_car) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. """ + total_dist = self._distance + self._first_distance + self._second_distance + 20 + root = py_trees.composites.Sequence() if self.route_mode: - root.add_child(HandleStartAccidentScenario(self._accident_wp, self._distance_to_accident)) + root.add_child(LeaveSpaceInFront(total_dist)) root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if self.route_mode: - root.add_child(HandleEndAccidentScenario()) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(ActorDestroy(self.other_actors[1])) root.add_child(ActorDestroy(self.other_actors[2])) - return root def _create_test_criteria(self): @@ -258,11 +277,45 @@ def __del__(self): """ self.remove_all_actors() -class AccidentTwoWays(BasicScenario): +class AccidentTwoWays(Accident): """ - This class holds everything required for a scenario in which there is an accident - in front of the ego, forcing it to react. A police vehicle is located before - two other cars that have been in an accident. + Variation of the Accident scenario but the ego now has to invade the opposite lane + """ + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): + if 'frequency' in config.other_parameters: + self._opposite_frequency = float(config.other_parameters['frequency']['value']) + else: + self._opposite_frequency = 200 + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to + let the ego invade the opposite lane. + """ + total_dist = self._distance + self._first_distance + self._second_distance + 20 + + root = py_trees.composites.Sequence() + if self.route_mode: + root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(SwitchWrongDirectionTest(False)) + root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(spawn_dist=50)) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(ActorDestroy(self.other_actors[2])) + + return root + + +class ParkedObstacle(BasicScenario): + """ + Scenarios in which a parked vehicle is incorrectly parked, + forcing the ego to lane change out of the route's lane """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -274,92 +327,61 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._drive_distance = 120 - self._distance_to_accident = 100 - self._offset = 0.75 - self._first_distance = 10 - self._second_distance = 6 - self._accident_wp = None + if 'distance' in config.other_parameters: + self._distance = int(config.other_parameters['distance']['value']) + else: + self._distance = 120 + self._drive_distance = self._distance + 20 + self._offset = 1.0 - super().__init__("AccidentTwoWays", - ego_vehicles, - config, - world, - randomize, - debug_mode, - criteria_enable=criteria_enable) + self._lights = carla.VehicleLightState.RightBlinker | carla.VehicleLightState.LeftBlinker + + super().__init__( + "ParkedObstacle", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - accident_wps = starting_wp.next(self._distance_to_accident) - pre_accident_wps = starting_wp.next(self._distance_to_accident/2) - if not accident_wps: + parked_wps = starting_wp.next(self._distance) + if not parked_wps: raise ValueError("Couldn't find a viable position to set up the accident actors") - if not pre_accident_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self._accident_wp = accident_wps[0] + self._parked_wp = parked_wps[0] - # Create the police vehicle - displacement = self._offset * self._accident_wp.lane_width / 2 - r_vec = self._accident_wp.transform.get_right_vector() - w_loc = self._accident_wp.transform.location + # Create the parked vehicle + displacement = self._offset * self._parked_wp.lane_width / 2 + r_vec = self._parked_wp.transform.get_right_vector() + w_loc = self._parked_wp.transform.location w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) - police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) - police_lights = carla.VehicleLightState.Special1 - police_lights |= carla.VehicleLightState.Special2 - police_lights |= carla.VehicleLightState.Position - police_car.set_light_state(carla.VehicleLightState(police_lights)) - self.other_actors.append(police_car) - - # Create the first vehicle that has been in the accident - vehicle_wps = self._accident_wp.next(self._first_distance) - if not vehicle_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - vehicle_wp = vehicle_wps[0] - self._accident_wp = pre_accident_wps[0] - displacement = self._offset * vehicle_wp.lane_width / 2 - r_vec = vehicle_wp.transform.get_right_vector() - w_loc = vehicle_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - vehicle_1_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_1_car = CarlaDataProvider.request_new_actor('vehicle.tesla.model3', vehicle_1_transform) - self.other_actors.append(vehicle_1_car) + parked_transform = carla.Transform(w_loc, self._parked_wp.transform.rotation) + parked_car = CarlaDataProvider.request_new_actor( + 'vehicle.*', parked_transform, attribute_filter={'base_type': 'car', 'has_lights': True}) + if not parked_car: + raise ValueError("Couldn't spawn the parked car") + self.other_actors.append(parked_car) + + lights = parked_car.get_light_state() + lights |= self._lights + parked_car.set_light_state(carla.VehicleLightState(lights)) - # Create the second vehicle that has been in the accident - vehicle_wps = vehicle_wp.next(self._second_distance) - if not vehicle_wps: + pre_parked_wps = starting_wp.next(self._distance / 2) + if not pre_parked_wps: raise ValueError("Couldn't find a viable position to set up the accident actors") - vehicle_wp = vehicle_wps[0] + self._pre_parked_wp = pre_parked_wps[0] - displacement = self._offset * vehicle_wp.lane_width / 2 - r_vec = vehicle_wp.transform.get_right_vector() - w_loc = vehicle_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - vehicle_2_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_2_car = CarlaDataProvider.request_new_actor('vehicle.mercedes.coupe_2020', vehicle_2_transform) - self.other_actors.append(vehicle_2_car) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. """ + total_dist = self._distance + 20 + root = py_trees.composites.Sequence() if self.route_mode: - total_dist = self._distance_to_accident + self._first_distance + self._second_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(SwitchOutsideRouteLanesTest(False)) - root.add_child(ChangeOppositeBehavior(active=False)) root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if self.route_mode: - root.add_child(SwitchOutsideRouteLanesTest(True)) - root.add_child(ChangeOppositeBehavior(active=True)) root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) - root.add_child(ActorDestroy(self.other_actors[2])) return root @@ -377,3 +399,35 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class ParkedObstacleTwoWays(ParkedObstacle): + """ + Variation of the ParkedObstacle scenario but the ego now has to invade the opposite lane + """ + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): + if 'frequency' in config.other_parameters: + self._opposite_frequency = float(config.other_parameters['frequency']['value']) + else: + self._opposite_frequency = 200 + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to + let the ego invade the opposite lane. + """ + total_dist = self._distance + 20 + + root = py_trees.composites.Sequence() + if self.route_mode: + root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(SwitchWrongDirectionTest(False)) + root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(spawn_dist=50)) + root.add_child(ActorDestroy(self.other_actors[0])) + + return root diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index ded78eaa7..240cb646a 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -36,10 +36,10 @@ RunningRedLightTest, RunningStopTest, ActorBlockedTest, - CheckMinSpeed) + MinSpeedRouteTest) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.scenarios.background_activity import BackgroundActivity +from srunner.scenarios.background_activity import BackgroundBehavior from srunner.scenariomanager.weather_sim import RouteWeatherBehavior from srunner.scenariomanager.lights_sim import RouteLightsBehavior from srunner.scenariomanager.timer import RouteTimeoutBehavior @@ -278,15 +278,11 @@ def _create_behavior(self): # Add the behavior that manages the scenario trigger conditions scenario_triggerer = ScenarioTriggerer( - self.ego_vehicles[0], self.route, blackboard_list, scenario_trigger_distance, repeat_scenarios=False - ) + self.ego_vehicles[0], self.route, blackboard_list, scenario_trigger_distance) behavior.add_child(scenario_triggerer) # Tick the ScenarioTriggerer before the scenarios # Add the Background Activity - background_activity = BackgroundActivity( - self.world, self.ego_vehicles[0], self.config, self.route, timeout=self.timeout - ) - behavior.add_child(background_activity.behavior_tree) + behavior.add_child(BackgroundBehavior(self.ego_vehicles[0], self.route, name="BackgroundActivity")) behavior.add_children(scenario_behaviors) return behavior @@ -309,7 +305,7 @@ def _create_test_criteria(self): criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='walker', name="CollisionPedestrianTest")) criteria.add_child(RunningRedLightTest(self.ego_vehicles[0])) criteria.add_child(RunningStopTest(self.ego_vehicles[0])) - criteria.add_child(CheckMinSpeed(self.ego_vehicles[0], name="MinSpeedTest")) + criteria.add_child(MinSpeedRouteTest(self.ego_vehicles[0], name="MinSpeedTest")) # These stop the route early to save computational time criteria.add_child(InRouteTest( diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index c62597cc7..b18d93edd 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -22,9 +22,9 @@ from srunner.tools.scenario_helper import (generate_target_waypoint, get_junction_topology, filter_junction_wp_direction, - get_closest_traffic_light) + get_same_dir_lanes) -from srunner.tools.background_manager import JunctionScenarioManager +from srunner.tools.background_manager import HandleJunctionScenario, ChangeOppositeBehavior class SignalizedJunctionLeftTurn(BasicScenario): @@ -46,20 +46,38 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals """ self._world = world self._map = CarlaDataProvider.get_map() - self._source_dist = 40 - self._sink_dist = 20 - self._source_dist_interval = [25, 50] - self._opposite_speed = 35 / 3.6 self._rng = random.RandomState(2000) - self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 20 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [25, 50] # m + self._direction = 'opposite' + + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed + + self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green + self._flow_tl_dict = {} + self._init_tl_dict = {} + self.timeout = timeout - super(SignalizedJunctionLeftTurn, self).__init__("SignalizedJunctionLeftTurn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("SignalizedJunctionLeftTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ @@ -67,15 +85,17 @@ def _initialize_actors(self, config): Override this method in child class to provide custom initialization. """ ego_location = config.trigger_points[0].location - ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction - starting_wp = ego_wp + starting_wp = self._ego_wp + ego_junction_dist = 0 while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError("Failed to find junction as a waypoint with no next was detected") starting_wp = starting_wps[0] + ego_junction_dist += 1 junction = starting_wp.get_junction() # Get the opposite entry lane wp @@ -89,15 +109,15 @@ def _initialize_actors(self, config): # Get the source transform source_wp = source_entry_wp - accum_dist = 0 - while accum_dist < self._source_dist: + source_junction_dist = 0 + while source_junction_dist < self._source_dist: source_wps = source_wp.previous(5) if len(source_wps) == 0: raise ValueError("Failed to find a source location as a waypoint with no previous was detected") if source_wps[0].is_junction: break source_wp = source_wps[0] - accum_dist += 5 + source_junction_dist += 5 self._source_wp = source_wp source_transform = self._source_wp.transform @@ -109,17 +129,23 @@ def _initialize_actors(self, config): raise ValueError("Failed to find a sink location as a waypoint with no next was detected") self._sink_wp = sink_wps[0] - # get traffic lights + self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + + def _get_traffic_lights(self, junction, ego_dist, source_dist): + """Get the traffic light of the junction, mapping their states""" tls = self._world.get_traffic_lights_in_junction(junction.id) - ego_tl = get_closest_traffic_light(ego_wp, tls) - source_tl = get_closest_traffic_light(self._source_wp, tls) - self._flow_tl_dict = {} - self._init_tl_dict = {} + if not tls: + raise ValueError("No traffic lights found, use the NonSignalized version instead") + + ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] + ego_tl = self._world.get_traffic_light(ego_landmark) + source_landmark = self._source_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] + source_tl = self._world.get_traffic_light(source_landmark) for tl in tls: - if tl == ego_tl: + if tl.id == ego_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Red - elif tl == source_tl: + elif tl.id == source_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Green else: @@ -131,22 +157,176 @@ def _create_behavior(self): Hero vehicle is turning left in an urban area at a signalized intersection, where, a flow of actors coming straight is present. """ + sequence = py_trees.composites.Sequence(name="SignalizedJunctionLeftTurn") + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=get_same_dir_lanes(self._source_wp), + remove_exits=get_same_dir_lanes(self._sink_wp), + stop_entries=False, + extend_road_exit=self._sink_dist + )) + sequence.add_child(ChangeOppositeBehavior(active=False)) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._opposite_speed)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) root.add_child(tl_freezer_sequence) - sequence = py_trees.composites.Sequence(name="SignalizedJunctionLeftTurn") + sequence.add_child(root) + if self.route_mode: - sequence.add_child(JunctionScenarioManager(self._direction, True)) + sequence.add_child(ChangeOppositeBehavior(active=True)) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + + +class NonSignalizedJunctionLeftTurn(BasicScenario): + + """ + Implementation class for Hero + Vehicle turning left at signalized junction scenario, + Traffic Scenario 08. + + This is a single ego vehicle scenario + """ + + timeout = 80 # Timeout of scenario in seconds + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=80): + """ + Setup all relevant parameters and create scenario + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self._rng = random.RandomState(2000) + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 20 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [25, 50] # m + + self._direction = 'opposite' + + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed + + self.timeout = timeout + super().__init__("NonSignalizedJunctionLeftTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Default initialization of other actors. + Override this method in child class to provide custom initialization. + """ + ego_location = config.trigger_points[0].location + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + + # Get the junction + starting_wp = self._ego_wp + ego_junction_dist = 0 + while not starting_wp.is_junction: + starting_wps = starting_wp.next(1.0) + if len(starting_wps) == 0: + raise ValueError("Failed to find junction as a waypoint with no next was detected") + starting_wp = starting_wps[0] + ego_junction_dist += 1 + junction = starting_wp.get_junction() + + # Get the opposite entry lane wp + entry_wps, _ = get_junction_topology(junction) + source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) + if not source_entry_wps: + raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) + + # Get the source transform + source_entry_wp = self._rng.choice(source_entry_wps) + + # Get the source transform + source_wp = source_entry_wp + source_junction_dist = 0 + while source_junction_dist < self._source_dist: + source_wps = source_wp.previous(5) + if len(source_wps) == 0: + raise ValueError("Failed to find a source location as a waypoint with no previous was detected") + if source_wps[0].is_junction: + break + source_wp = source_wps[0] + source_junction_dist += 5 + + self._source_wp = source_wp + source_transform = self._source_wp.transform + + # Get the sink location + sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) + sink_wps = sink_exit_wp.next(self._sink_dist) + if len(sink_wps) == 0: + raise ValueError("Failed to find a sink location as a waypoint with no next was detected") + self._sink_wp = sink_wps[0] + + def _create_behavior(self): + """ + Hero vehicle is turning left in an urban area at a signalized intersection, + where, a flow of actors coming straight is present. + """ + sequence = py_trees.composites.Sequence(name="NonSignalizedJunctionLeftTurn") + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=get_same_dir_lanes(self._source_wp), + remove_exits=get_same_dir_lanes(self._sink_wp), + stop_entries=True, + extend_road_exit=self._sink_dist + )) + sequence.add_child(ChangeOppositeBehavior(active=False)) + + root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + root.add_child(ActorFlow( + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + sequence.add_child(root) + if self.route_mode: + sequence.add_child(ChangeOppositeBehavior(active=True)) + return sequence def _create_test_criteria(self): diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index b115ff2ee..51f569735 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -14,7 +14,6 @@ import py_trees import carla -from agents.navigation.global_route_planner import GlobalRoutePlanner from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer @@ -24,9 +23,9 @@ from srunner.tools.scenario_helper import (generate_target_waypoint, get_junction_topology, filter_junction_wp_direction, - get_closest_traffic_light) + get_same_dir_lanes) -from srunner.tools.background_manager import JunctionScenarioManager +from srunner.tools.background_manager import HandleJunctionScenario class SignalizedJunctionRightTurn(BasicScenario): @@ -43,35 +42,54 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals """ self._world = world self._map = CarlaDataProvider.get_map() - self._source_dist = 40 - self._sink_dist = 10 - self._source_dist_interval = [25, 50] - self._opposite_speed = 35 / 3.6 - self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green - self._direction = 'left' - self._route_planner = GlobalRoutePlanner(self._map, 2.0) self.timeout = timeout - super(SignalizedJunctionRightTurn, self).__init__("SignalizedJunctionRightTurn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 20 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [25, 50] # m + + self._direction = 'left' + + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed + + self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green + self._flow_tl_dict = {} + self._init_tl_dict = {} + + super().__init__("SignalizedJunctionRightTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ ego_location = config.trigger_points[0].location - ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction - starting_wp = ego_wp + starting_wp = self._ego_wp + ego_junction_dist = 0 while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: - raise ValueError("Failed to find junction as a waypoint with no next was detected") + raise ValueError("Failed to find a junction") starting_wp = starting_wps[0] + ego_junction_dist += 1 junction = starting_wp.get_junction() # Get the source entry lane wp @@ -90,15 +108,15 @@ def _initialize_actors(self, config): # Get the source transform source_wp = source_entry_wp - accum_dist = 0 - while accum_dist < self._source_dist: + source_junction_dist = 0 + while source_junction_dist < self._source_dist: source_wps = source_wp.previous(5) if len(source_wps) == 0: - raise ValueError("Failed to find a source location as a waypoint with no previous was detected") + raise ValueError("Failed to find a source location") if source_wps[0].is_junction: break source_wp = source_wps[0] - accum_dist += 5 + source_junction_dist += 5 self._source_wp = source_wp source_transform = self._source_wp.transform @@ -107,20 +125,26 @@ def _initialize_actors(self, config): sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) sink_wps = sink_exit_wp.next(self._sink_dist) if len(sink_wps) == 0: - raise ValueError("Failed to find a sink location as a waypoint with no next was detected") + raise ValueError("Failed to find a sink location") self._sink_wp = sink_wps[0] - # Get the relevant traffic lights + self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + + def _get_traffic_lights(self, junction, ego_dist, source_dist): + """Get the traffic light of the junction, mapping their states""" tls = self._world.get_traffic_lights_in_junction(junction.id) - ego_tl = get_closest_traffic_light(ego_wp, tls) - source_tl = get_closest_traffic_light(source_wps[0], tls) - self._flow_tl_dict = {} - self._init_tl_dict = {} + if not tls: + raise ValueError("No traffic lights found, use the NonSignalized version instead") + + ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] + ego_tl = self._world.get_traffic_light(ego_landmark) + source_landmark = self._source_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] + source_tl = self._world.get_traffic_light(source_landmark) for tl in tls: - if tl == ego_tl: + if tl.id == ego_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Red - elif tl == source_tl: + elif tl.id == source_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Green else: @@ -132,21 +156,166 @@ def _create_behavior(self): Hero vehicle is turning right in an urban area, at a signalized intersection, while other actor coming straight from the left. The ego has to avoid colliding with it """ + + sequence = py_trees.composites.Sequence(name="JunctionRightTurn") + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=get_same_dir_lanes(self._source_wp), + remove_exits=[], + stop_entries=False, + extend_road_exit=self._sink_dist + )) + root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._opposite_speed)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) root.add_child(tl_freezer_sequence) - sequence = py_trees.composites.Sequence(name="SignalizedJunctionRightTurn") - if self.route_mode: - sequence.add_child(JunctionScenarioManager(self._direction, False)) sequence.add_child(root) + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + + +class NonSignalizedJunctionRightTurn(BasicScenario): + + """ + Scenario where the vehicle is turning right at an intersection an has to avoid + colliding with a vehicle coming from its left + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=80): + """ + Setup all relevant parameters and create scenario + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + + if 'flow_speed' in config.other_parameters: + self._flow_speed = float(config.other_parameters['flow_speed']['value']) + else: + self._flow_speed = 20 # m/s + + if 'source_dist_interval' in config.other_parameters: + self._source_dist_interval = [ + float(config.other_parameters['source_dist_interval']['from']), + float(config.other_parameters['source_dist_interval']['to']) + ] + else: + self._source_dist_interval = [25, 50] # m + + self._direction = 'left' + + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed + + super().__init__("NonSignalizedJunctionRightTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + def _initialize_actors(self, config): + """ + Custom initialization + """ + ego_location = config.trigger_points[0].location + self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) + + # Get the junction + starting_wp = self._ego_wp + ego_junction_dist = 0 + while not starting_wp.is_junction: + starting_wps = starting_wp.next(1.0) + if len(starting_wps) == 0: + raise ValueError("Failed to find a junction") + starting_wp = starting_wps[0] + ego_junction_dist += 1 + junction = starting_wp.get_junction() + + # Get the source entry lane wp + entry_wps, _ = get_junction_topology(junction) + source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) + if not source_entry_wps: + raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) + + # Get the rightmost lane + source_entry_wp = source_entry_wps[0] + while True: + right_wp = source_entry_wp.get_right_lane() + if not right_wp or right_wp.lane_type != carla.LaneType.Driving: + break + source_entry_wp = right_wp + + # Get the source transform + source_wp = source_entry_wp + source_junction_dist = 0 + while source_junction_dist < self._source_dist: + source_wps = source_wp.previous(5) + if len(source_wps) == 0: + raise ValueError("Failed to find a source location") + if source_wps[0].is_junction: + break + source_wp = source_wps[0] + source_junction_dist += 5 + + self._source_wp = source_wp + source_transform = self._source_wp.transform + + # Get the sink location + sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) + sink_wps = sink_exit_wp.next(self._sink_dist) + if len(sink_wps) == 0: + raise ValueError("Failed to find a sink location") + self._sink_wp = sink_wps[0] + + def _create_behavior(self): + """ + Hero vehicle is turning right in an urban area, at a signalized intersection, + while other actor coming straight from the left. The ego has to avoid colliding with it + """ + + sequence = py_trees.composites.Sequence(name="JunctionRightTurn") + if self.route_mode: + sequence.add_child(HandleJunctionScenario( + clear_junction=True, + clear_ego_entry=True, + remove_entries=get_same_dir_lanes(self._source_wp), + remove_exits=[], + stop_entries=True, + extend_road_exit=self._sink_dist + )) + + root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + root.add_child(ActorFlow( + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + + sequence.add_child(root) return sequence def _create_test_criteria(self): diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index 07e15a1c9..fc2ff24a3 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -20,7 +20,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, OpenVehicleDoor, - SwitchOutsideRouteLanesTest) + SwitchWrongDirectionTest) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, DriveDistance) @@ -32,9 +32,8 @@ class VehicleOpensDoor(BasicScenario): """ - This class holds everything required for a scenario in which another vehicle runs a red light - in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, - ambulances or firetrucks. + This class holds everything required for a scenario in which another vehicle parked at the side lane + opens the door, forcing the ego to lane change. """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -48,12 +47,12 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._wait_duration = 15 - self._end_distance = 40 + self._takeover_distance = 60 self._min_trigger_dist = 10 self._reaction_time = 3.0 if 'distance' in config.other_parameters: - self._parked_distance = config.other_parameters['distance']['value'] + self._parked_distance = float(config.other_parameters['distance']['value']) else: self._parked_distance = 50 @@ -64,12 +63,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") - if 'cross_onto_opposite_lane' in config.other_parameters: - self._cross_opposite_lane = True - self._opposite_frequency = 70 - else: - self._cross_opposite_lane = False - super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): @@ -100,16 +93,15 @@ def _initialize_actors(self, config): self.other_actors.append(self._parked_actor) # And move it to the side - side_transform = self._get_displaced_transform(self._parked_actor, parked_wp) - self._parked_actor.set_location(side_transform) + side_location = self._get_displaced_location(self._parked_actor, parked_wp) + self._parked_actor.set_location(side_location) self._parked_actor.apply_control(carla.VehicleControl(hand_brake=True)) def _create_behavior(self): """ - Hero vehicle is entering a junction in an urban area, at a signalized intersection, - while another actor runs a red lift, forcing the ego to break. + Leave space in front, as the TM doesn't detect open doors """ - sequence = py_trees.composites.Sequence(name="CrossingActor") + sequence = py_trees.composites.Sequence(name="VehicleOpensDoor") if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._parked_distance)) @@ -118,7 +110,7 @@ def _create_behavior(self): # Wait until ego is close to the adversary trigger_adversary = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerOpenDoor") trigger_adversary.add_child(InTimeToArrivalToLocation( self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( @@ -126,20 +118,13 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL - sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) - if self._cross_opposite_lane: - sequence.add_child(SwitchOutsideRouteLanesTest(False)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - - sequence.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) - if self._cross_opposite_lane: - sequence.add_child(SwitchOutsideRouteLanesTest(True)) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) sequence.add_child(ActorDestroy(self._parked_actor)) return sequence - def _get_displaced_transform(self, actor, wp): + def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane """ @@ -154,6 +139,7 @@ def _get_displaced_transform(self, actor, wp): z=displacement*displacement_vector.z) new_location.z += 0.05 # Just in case, avoid collisions with the ground return new_location + def _create_test_criteria(self): """ A list of all test criteria will be created that is later used @@ -168,3 +154,52 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class VehicleOpensDoorTwoWays(VehicleOpensDoor): + """ + Variation of VehicleOpensDoor wher ethe vehicle has to invade an opposite lane + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=180): + if 'frequency' in config.other_parameters: + self._opposite_frequency = float(config.other_parameters['frequency']['value']) + else: + self._opposite_frequency = 100 + + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _create_behavior(self): + """ + Leave space in front, as the TM doesn't detect open doors, and change the opposite frequency + so that the ego can pass + """ + sequence = py_trees.composites.Sequence(name="VehicleOpensDoorTwoWays") + + if self.route_mode: + sequence.add_child(LeaveSpaceInFront(self._parked_distance)) + + collision_location = self._front_wp.transform.location + + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerOpenDoor") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) + + door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL + + sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) + sequence.add_child(SwitchWrongDirectionTest(False)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + sequence.add_child(SwitchWrongDirectionTest(True)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=50)) + sequence.add_child(ActorDestroy(self._parked_actor)) + + return sequence diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py index fb1ca3762..efc28278e 100644 --- a/srunner/scenarios/yield_to_emergency_vehicle.py +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -17,9 +17,8 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, Idle, AdaptiveConstantVelocityAgentBehavior from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, YieldToEmergencyVehicleTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import SwitchLane +from srunner.tools.background_manager import RemoveRoadLane, ReAddEgoRoadLane class YieldToEmergencyVehicle(BasicScenario): @@ -57,12 +56,12 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._trigger_location) self._ev_start_transform = None - super(YieldToEmergencyVehicle, self).__init__("YieldToEmergencyVehicle", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("YieldToEmergencyVehicle", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ @@ -109,8 +108,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() if self.route_mode: - sequence.add_child(SwitchLane( - self._reference_waypoint.lane_id, False)) + sequence.add_child(RemoveRoadLane(self._reference_waypoint)) # Teleport EV behind the ego sequence.add_child(ActorTransformSetter( @@ -130,8 +128,7 @@ def _create_behavior(self): sequence.add_child(ActorDestroy(self.other_actors[0])) if self.route_mode: - sequence.add_child(SwitchLane( - self._reference_waypoint.lane_id, True)) + sequence.add_child(ReAddEgoRoadLane()) return sequence diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 0f485a9a6..5f1f5694b 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -13,28 +13,6 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior -class ChangeGeneralBehavior(AtomicBehavior): - """ - Updates the blackboard to change the general parameters. - None values imply that these values won't be changed. - - Args: - spawn_dist (float): Minimum distance between spawned vehicles. Must be positive - target_speed (float): Target speed of all BA vehicles - """ - - def __init__(self, spawn_dist=None, target_speed=None, name="ChangeGeneralBehavior"): - self._spawn_dist = spawn_dist - self._target_speed = target_speed - super().__init__(name) - - def update(self): - py_trees.blackboard.Blackboard().set( - "BA_ChangeGeneralBehavior", [self._spawn_dist, self._target_speed], overwrite=True - ) - return py_trees.common.Status.SUCCESS - - class ChangeRoadBehavior(AtomicBehavior): """ Updates the blackboard to change the parameters of the road behavior. @@ -46,14 +24,15 @@ class ChangeRoadBehavior(AtomicBehavior): switch_source (bool): (De)activatea the road sources. """ - def __init__(self, num_front_vehicles=None, num_back_vehicles=None, name="ChangeRoadBehavior"): - self._num_front_vehicles = num_front_vehicles - self._num_back_vehicles = num_back_vehicles + def __init__(self, num_front_vehicles=None, num_back_vehicles=None, spawn_dist=None, name="ChangeRoadBehavior"): + self._num_front = num_front_vehicles + self._num_back = num_back_vehicles + self._spawn_dist = spawn_dist super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeRoadBehavior", [self._num_front_vehicles, self._num_back_vehicles], overwrite=True + "BA_ChangeRoadBehavior", [self._num_front, self._num_back, self._spawn_dist], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -68,16 +47,15 @@ class ChangeOppositeBehavior(AtomicBehavior): max_actors (int): Max amount of concurrent alive actors spawned by the same source. Can't be negative """ - def __init__(self, source_dist=None, max_actors=None, spawn_dist=None, active=None, name="ChangeOppositeBehavior"): + def __init__(self, source_dist=None, spawn_dist=None, active=None, name="ChangeOppositeBehavior"): self._source_dist = source_dist - self._max_actors = max_actors self._spawn_dist = spawn_dist self._active = active super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", [self._source_dist, self._max_actors, self._spawn_dist, self._active], overwrite=True + "BA_ChangeOppositeBehavior", [self._source_dist, self._spawn_dist, self._active], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -92,15 +70,16 @@ class ChangeJunctionBehavior(AtomicBehavior): max_actors (int): Max amount of concurrent alive actors spawned by the same source. Can't be negative """ - def __init__(self, source_dist=None, vehicle_dist=None, spawn_dist=None, - max_actors=None, name="ChangeJunctionBehavior"): + def __init__(self, source_dist=None, spawn_dist=None, max_actors=None, source_perc=None, name="ChangeJunctionBehavior"): self._source_dist = source_dist + self._spawn_dist = spawn_dist self._max_actors = max_actors + self._perc = source_perc super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeJunctionBehavior", [self._source_dist, self._max_actors], overwrite=True + "BA_ChangeJunctionBehavior", [self._source_dist, self._spawn_dist, self._max_actors, self._perc], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -133,42 +112,6 @@ def update(self): return py_trees.common.Status.SUCCESS -class JunctionScenarioManager(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that a JunctionScenarioManager has been triggered - 'entry_direction' is the direction from which the incoming traffic enters the junction. It should be - something like 'left', 'right' or 'opposite' - """ - - def __init__(self, entry_direction, remove_exit=True, name="JunctionScenarioManager"): - self._entry_direction = entry_direction - self._remove_exit = remove_exit - super().__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set( - "BA_JunctionScenario", - [self._entry_direction, self._remove_exit], - overwrite=True - ) - return py_trees.common.Status.SUCCESS - - -class ExtentExitRoadSpace(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that an exit road needs more space - """ - def __init__(self, distance, name="ExtentExitRoadSpace"): - self._distance = distance - super().__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_ExtentExitRoadSpace", self._distance, overwrite=True) - return py_trees.common.Status.SUCCESS - - class LeaveSpaceInFront(AtomicBehavior): """ Updates the blackboard to tell the background activity that the ego needs more space in front. @@ -180,7 +123,7 @@ def __init__(self, space, name="LeaveSpaceInFront"): def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_LeaveSpaceInFront", self._space, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_LeaveSpaceInFront", [self._space], overwrite=True) return py_trees.common.Status.SUCCESS @@ -198,34 +141,38 @@ def update(self): return py_trees.common.Status.SUCCESS -class HandleStartAccidentScenario(AtomicBehavior): +class StartObstacleScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity that the road behavior has to be initialized """ - def __init__(self, accident_wp, distance, name="HandleStartAccidentScenario"): + def __init__(self, accident_wp, distance, direction='left', stop_back_vehicles=False, name="StartObstacleScenario"): self._accident_wp = accident_wp self._distance = distance + self._direction = direction + self._stop_back_vehicles = stop_back_vehicles super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_HandleStartAccidentScenario", [self._accident_wp, self._distance], overwrite=True) + py_trees.blackboard.Blackboard().set("BA_StartObstacleScenario", + [self._accident_wp, self._distance, self._direction, self._stop_back_vehicles], overwrite=True) return py_trees.common.Status.SUCCESS -class HandleEndAccidentScenario(AtomicBehavior): +class EndObstacleScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity that the road behavior has to be initialized """ - def __init__(self, name="HandleEndAccidentScenario"): + def __init__(self, name="EndObstacleScenario"): super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_HandleEndAccidentScenario", True, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_EndObstacleScenario", True, overwrite=True) return py_trees.common.Status.SUCCESS -class SwitchLane(AtomicBehavior): + +class RemoveRoadLane(AtomicBehavior): """ Updates the blackboard to tell the background activity to remove its actors from the given lane and stop generating new ones on this lane, or recover from stopping. @@ -234,46 +181,78 @@ class SwitchLane(AtomicBehavior): lane_id (str): A carla.Waypoint.lane_id active (bool) """ - def __init__(self, lane_id=None, active=True, name="SwitchLane"): - self._lane_id = lane_id - self._active = active + def __init__(self, lane_wp, name="RemoveRoadLane"): + self._lane_wp = lane_wp super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_SwitchLane", [self._lane_id, self._active], overwrite=True) + py_trees.blackboard.Blackboard().set("BA_RemoveRoadLane", self._lane_wp, overwrite=True) return py_trees.common.Status.SUCCESS -class RemoveJunctionEntry(AtomicBehavior): + +class ReAddEgoRoadLane(AtomicBehavior): """ - Updates the blackboard to tell the background activity to remove its actors from the given lane, - and stop generating new ones on this lane. + Updates the blackboard to tell the background activity to readd the ego road lane. Args: - wp (carla.Waypoint): A list of waypoint used as reference to the entry lane - all_road_entries (bool): Boolean to remove all entries part of the same road, or just one + lane_id (str): A carla.Waypoint.lane_id + active (bool) """ - def __init__(self, wps, all_road_entries=False, name="RemoveJunctionEntry"): - self._wps = wps - self._all_road_entries = all_road_entries + def __init__(self, name="BA_ReAddEgoRoadLane"): super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_RemoveJunctionEntry", [self._wps, self._all_road_entries], overwrite=True) + py_trees.blackboard.Blackboard().set("BA_ReAddEgoRoadLane", True, overwrite=True) return py_trees.common.Status.SUCCESS -class ClearJunction(AtomicBehavior): +class LeaveSpaceInFront(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that the ego needs more space in front. + This only works at roads, not junctions. """ - Updates the blackboard to tell the background activity to remove all actors inside the junction, - and stop the ones that are about to enter it, leaving an empty space inside the junction. + def __init__(self, space, name="LeaveSpaceInFront"): + self._space = space + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_LeaveSpaceInFront", self._space, overwrite=True) + return py_trees.common.Status.SUCCESS + + +class HandleJunctionScenario(AtomicBehavior): """ + Updates the blackboard to tell the background activity to adapt to a junction scenario - def __init__(self, name="ClearJunction"): + Args: + clear_junction (bool): Remove all actors inside the junction, and all that enter it afterwards + clear_ego_entry (bool): Remove all actors part of the ego road to ensure a smooth entry of the ego to the junction. + remove_entries (list): list of waypoint representing a junction entry that needs to be removed + remove_exits (list): list of waypoint representing a junction exit that needs to be removed + stop_entries (bool): Stops all the junction entries + extend_road_exit (float): Moves the road junction actors forward to leave more space for the scenario. + It also deactivates the road sources. + active (bool) + """ + def __init__(self, clear_junction=True, clear_ego_entry=True, remove_entries=[], + remove_exits=[], stop_entries=True, extend_road_exit=0, + name="HandleJunctionScenario"): + self._clear_junction = clear_junction + self._clear_ego_entry = clear_ego_entry + self._remove_entries = remove_entries + self._remove_exits = remove_exits + self._stop_entries = stop_entries + self._extend_road_exit = extend_road_exit super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_ClearJunction", True, overwrite=True) + py_trees.blackboard.Blackboard().set( + "BA_HandleJunctionScenario", + [self._clear_junction, self._clear_ego_entry, self._remove_entries, + self._remove_exits, self._stop_entries, self._extend_road_exit], + overwrite=True) return py_trees.common.Status.SUCCESS diff --git a/srunner/tools/scenario_helper.py b/srunner/tools/scenario_helper.py index d278deaac..ab8e98a38 100644 --- a/srunner/tools/scenario_helper.py +++ b/srunner/tools/scenario_helper.py @@ -135,7 +135,7 @@ def get_crossing_point(actor): return crossing -def get_geometric_linear_intersection(ego_location, other_location): +def get_geometric_linear_intersection(ego_location, other_location, move_to_junction=False): """ Obtain a intersection point between two actor's location by using their waypoints (wp) @@ -144,11 +144,31 @@ def get_geometric_linear_intersection(ego_location, other_location): wp_ego_1 = CarlaDataProvider.get_map().get_waypoint(ego_location) wp_ego_2 = wp_ego_1.next(1)[0] + + if move_to_junction: + while True: + next_wp = wp_ego_2.next(1)[0] + if next_wp.is_junction: + break + else: + wp_ego_1 = wp_ego_2 + wp_ego_2 = next_wp + ego_1_loc = wp_ego_1.transform.location ego_2_loc = wp_ego_2.transform.location wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint(other_location) wp_other_2 = wp_other_1.next(1)[0] + + if move_to_junction: + while True: + next_wp = wp_other_2.next(1)[0] + if next_wp.is_junction: + break + else: + wp_other_1 = wp_other_2 + wp_other_2 = next_wp + other_1_loc = wp_other_1.transform.location other_2_loc = wp_other_2.transform.location From 21be2d59e777b967ab367a5d7f4541a9eab6c9e2 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Tue, 5 Jul 2022 09:17:27 +0200 Subject: [PATCH 36/86] Fixed several merge errors --- srunner/routes/leaderboard_2.0_testing.xml | 42 ++-- .../scenarioatomics/atomic_behaviors.py | 15 +- srunner/scenariomanager/timer.py | 1 - .../scenarios/cut_in_with_static_vehicle.py | 23 +- srunner/scenarios/route_obstacles.py | 227 +++++++++--------- 5 files changed, 159 insertions(+), 149 deletions(-) diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index ee313d5d1..ad7061f22 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -39,7 +39,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -65,7 +65,7 @@ - + @@ -83,7 +83,7 @@ - + @@ -103,7 +103,7 @@ - + @@ -119,7 +119,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -200,7 +200,7 @@ - + @@ -216,25 +216,24 @@ - + - - + + - + + + + + - - - - - - + @@ -244,13 +243,12 @@ - + - - + - + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 6d71f36c4..6618c7151 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1941,23 +1941,30 @@ class BasicAgentBehavior(AtomicBehavior): The behavior terminates after reaching the target_location (within 2 meters) """ - def __init__(self, actor, target_location=None, 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 """ super(BasicAgentBehavior, self).__init__(name, actor) self._map = CarlaDataProvider.get_map() self._target_location = target_location + self._target_speed = target_speed + self._plan = plan + self._opt_dict = opt_dict if opt_dict else {} self._control = carla.VehicleControl() self._agent = None - self._plan = None + + if self._target_location and self._plan: + raise ValueError("Choose either a destiantion or a plan, but not both") def initialise(self): """Initialises the agent""" - self._agent = BasicAgent(self._actor, opt_dict=self._opt_dict, + 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._target_location: + if self._plan: + self._agent.set_global_plan(self._plan) + elif self._target_location: self._plan = self._agent.set_destination( self._target_location, CarlaDataProvider.get_location(self._actor)) self._agent.set_global_plan(self._plan) diff --git a/srunner/scenariomanager/timer.py b/srunner/scenariomanager/timer.py index 696c208e9..0e66f4ee3 100644 --- a/srunner/scenariomanager/timer.py +++ b/srunner/scenariomanager/timer.py @@ -234,7 +234,6 @@ def update(self): self._current_index = new_index elapsed_time = GameTime.get_time() - self._start_time - print(f"Timeout node: {round(elapsed_time, 2)}/{round(self._timeout_value, 2)}s") if elapsed_time > self._timeout_value: new_status = py_trees.common.Status.SUCCESS self.timeout = True diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 16080f15e..8d7922d9c 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -11,17 +11,17 @@ import py_trees import carla +from agents.navigation.local_planner import RoadOption + from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, - WaypointFollower, - BasicAgentBehavior - ) + WaypointFollower, + BasicAgentBehavior) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, - InTimeToArrivalToLocation, - DriveDistance) + InTimeToArrivalToLocation) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import SwitchLane +from srunner.tools.background_manager import RemoveRoadLane class CutInWithStaticVehicle(BasicScenario): @@ -94,7 +94,10 @@ def _initialize_actors(self, config): self.other_actors.append(self._parked_actor) self._front_wps = self._collision_wp.next(self._front_distance) - self._front_wp = self._front_wps[0].transform.location + self._front_wp = self._front_wps[0] + + self._plan = [[self._collision_wp, RoadOption.STRAIGHT], + [self._front_wp, RoadOption.STRAIGHT] ] def _create_behavior(self): """ @@ -104,7 +107,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence(name="CrossingActor") if self.route_mode: other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) - sequence.add_child(SwitchLane(other_car.lane_id, False)) + sequence.add_child(RemoveRoadLane(other_car)) collision_location = self._collision_wp.transform.location @@ -119,13 +122,13 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) # The adversary change the lane - sequence.add_child(BasicAgentBehavior(self.other_actors[1],target_location=self._front_wp)) + sequence.add_child(BasicAgentBehavior(self.other_actors[1], plan=self._plan)) # Move the adversary cut_in = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) - sequence.add_child(SwitchLane(other_car.lane_id, True)) + sequence.add_child(RemoveRoadLane(other_car)) cut_in.add_child(WaypointFollower(self.other_actors[1], self._adversary_speed)) sequence.add_child(cut_in) diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 986fb75f1..b48e77cc6 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -163,119 +163,7 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() - -class BicycleFlowAtSideLane(BasicScenario): - """ - Added the dangerous scene of ego vehicles driving on roads without sidewalks, - with three bicycles encroaching on some roads in front. - """ - - 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 - """ - self._world = world - self._map = CarlaDataProvider.get_map() - self.timeout = timeout - self._drive_distance = 100 - self._offset = [0.6,0.75,0.9] - self._bicycle_wp = [] - self._target_location=None - self._plan=[] - - if 'distance' in config.other_parameters: - self._distance_to_Trigger = [ - float(config.other_parameters['distance']['first']), - float(config.other_parameters['distance']['second']), - float(config.other_parameters['distance']['third']) - ] - else: - self._distance_to_Trigger = [74,76,88] # m - - super().__init__("Hazard", - ego_vehicles, - config, - world, - randomize, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - - starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - if 'end_bycicle_distance' in config.other_parameters: - self._end_bycicle_distance = float( - config.other_parameters['end_bycicle_distance']['value']) - else: - self._end_bycicle_distance = 150 - self._target_location = starting_wp.next(self._end_bycicle_distance)[0].transform.location - for offset,distance in zip(self._offset,self._distance_to_Trigger): - - bicycle_wps = starting_wp.next(distance) - - if not bicycle_wps: - raise ValueError("Couldn't find a viable position to set up the bicycle actors") - self._bicycle_wp.append(bicycle_wps[0]) - displacement = offset* bicycle_wps[0].lane_width / 2 - r_vec = bicycle_wps[0].transform.get_right_vector() - w_loc = bicycle_wps[0].transform.location - w_loc = w_loc + carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - bycicle_transform = carla.Transform(w_loc, bicycle_wps[0].transform.rotation) - bycicle = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', bycicle_transform) - self.other_actors.append(bycicle) - - def _create_behavior(self): - """ - The vehicle has to drive the whole predetermined distance. - """ - - root = py_trees.composites.Sequence() - if self.route_mode: - total_dist = self._distance_to_Trigger[2] + 30 - root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(SwitchOutsideRouteLanesTest(False)) - root.add_child(ChangeOppositeBehavior(active=False)) - bycicle = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[2], self._target_location,target_speed=3.1,opt_dict={'offset':self._offset[2]* self._bicycle_wp[2].lane_width / 2})) - - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[1], self._target_location, target_speed=3, - opt_dict={'offset': self._offset[1] * self._bicycle_wp[1].lane_width / 2})) - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[0], self._target_location, target_speed=3, - opt_dict={'offset': self._offset[0] * self._bicycle_wp[0].lane_width / 2})) - root.add_child(bycicle) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - if self.route_mode: - root.add_child(SwitchOutsideRouteLanesTest(True)) - root.add_child(ChangeOppositeBehavior(active=True)) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) - root.add_child(ActorDestroy(self.other_actors[2])) - - return root - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() class AccidentTwoWays(Accident): """ @@ -431,3 +319,118 @@ def _create_behavior(self): root.add_child(ActorDestroy(self.other_actors[0])) return root + + +class BicycleFlowAtSideLane(BasicScenario): + """ + Added the dangerous scene of ego vehicles driving on roads without sidewalks, + with three bicycles encroaching on some roads in front. + """ + + 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 + """ + self._world = world + self._map = CarlaDataProvider.get_map() + self.timeout = timeout + self._drive_distance = 100 + self._offset = [0.6, 0.75, 0.9] + self._bicycle_wp = [] + self._target_location = None + self._plan = [] + self._bicycle_speed = 3 # m/s + + if 'distance' in config.other_parameters: + self._distance_to_Trigger = [ + float(config.other_parameters['distance']['first']), + float(config.other_parameters['distance']['second']), + float(config.other_parameters['distance']['third']) + ] + else: + self._distance_to_Trigger = [74,76,88] # m + + super().__init__("BicycleFlowAtSideLane", + ego_vehicles, + config, + world, + randomize, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + + starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + if 'end_bycicle_distance' in config.other_parameters: + self._end_bycicle_distance = float( + config.other_parameters['end_bycicle_distance']['value']) + else: + self._end_bycicle_distance = 150 + self._target_location = starting_wp.next(self._end_bycicle_distance)[0].transform.location + for offset,distance in zip(self._offset,self._distance_to_Trigger): + + bicycle_wps = starting_wp.next(distance) + + if not bicycle_wps: + raise ValueError("Couldn't find a viable position to set up the bicycle actors") + self._bicycle_wp.append(bicycle_wps[0]) + displacement = offset* bicycle_wps[0].lane_width / 2 + r_vec = bicycle_wps[0].transform.get_right_vector() + w_loc = bicycle_wps[0].transform.location + w_loc = w_loc + carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) + bycicle_transform = carla.Transform(w_loc, bicycle_wps[0].transform.rotation) + bycicle = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', bycicle_transform) + self.other_actors.append(bycicle) + + def _create_behavior(self): + """ + The vehicle has to drive the whole predetermined distance. + """ + + root = py_trees.composites.Sequence() + if self.route_mode: + total_dist = self._distance_to_Trigger[2] + 30 + root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(SwitchWrongDirectionTest(False)) + root.add_child(ChangeOppositeBehavior(active=False)) + bycicle = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[2], self._target_location, target_speed = self._bicycle_speed, + opt_dict={'offset': self._offset[2] * self._bicycle_wp[2].lane_width / 2})) + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[1], self._target_location, target_speed = self._bicycle_speed, + opt_dict={'offset': self._offset[1] * self._bicycle_wp[1].lane_width / 2})) + bycicle.add_child(ConstantVelocityAgentBehavior( + self.other_actors[0], self._target_location, target_speed = self._bicycle_speed, + opt_dict={'offset': self._offset[0] * self._bicycle_wp[0].lane_width / 2})) + root.add_child(bycicle) + root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(active=True)) + root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(ActorDestroy(self.other_actors[2])) + + return root + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() From 098a00ce4b2e7d0edac2dcb860f2e864d32e0ba4 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Thu, 7 Jul 2022 10:59:33 +0200 Subject: [PATCH 37/86] New scenario improvements --- srunner/routes/leaderboard_2.0_testing.xml | 194 ++++++++++-------- .../scenarioatomics/atomic_behaviors.py | 18 +- .../scenarioatomics/atomic_criteria.py | 2 +- srunner/scenarios/actor_flow.py | 2 +- srunner/scenarios/background_activity.py | 36 +++- srunner/scenarios/object_crash_vehicle.py | 10 +- srunner/scenarios/parking_exit.py | 32 ++- 7 files changed, 187 insertions(+), 107 deletions(-) diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml index ad7061f22..21a0b4842 100644 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ b/srunner/routes/leaderboard_2.0_testing.xml @@ -1,19 +1,19 @@ - - + + - - - - - - - - - - - - + + + + + + + + + + + + @@ -25,38 +25,41 @@ - + + - - + + - - - + + + - + + - - + + - - - + + + - + + - - + + @@ -65,33 +68,39 @@ - + + - - + + - - + + - + - + - + + - - + + - - - + + + @@ -103,14 +112,17 @@ - + + - - + + - - + + @@ -119,13 +131,15 @@ - + + - + - - + + @@ -138,40 +152,44 @@ - + + - + - - + + - - - + + + - + - + + - - + + - + - + + - - + + @@ -183,7 +201,8 @@ - + + @@ -195,28 +214,33 @@ - - + + - + + - - + + - + - - + + @@ -224,30 +248,32 @@ - + - + - + - - + + - - - + + + - + diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 6618c7151..8af40b7c8 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1965,8 +1965,9 @@ def initialise(self): if self._plan: self._agent.set_global_plan(self._plan) elif self._target_location: - self._plan = self._agent.set_destination( - self._target_location, 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) def update(self): @@ -2613,8 +2614,7 @@ def update(self): new_status = py_trees.common.Status.FAILURE if calculate_distance(self._actor.get_location(), self._transform.location) < 1.0: - if self._physics: - self._actor.set_simulate_physics(enabled=True) + self._actor.set_simulate_physics(self._physics) new_status = py_trees.common.Status.SUCCESS return new_status @@ -2814,13 +2814,14 @@ def _spawn_actor(self, transform): actor.set_autopilot(True) self._tm.set_path(actor, [self._source_transform.location, self._sink_location]) + self._tm.auto_lane_change(actor, False) + self._tm.set_desired_speed(actor, 3.6 * self._speed) + self._tm.update_vehicle_lights(actor, True) if self._is_constant_velocity_active: self._tm.ignore_vehicles_percentage(actor, 100) - self._tm.auto_lane_change(actor, False) - self._tm.set_desired_speed(actor, 3.6 * self._speed) - self._tm.update_vehicle_lights(actor, True) actor.enable_constant_velocity(carla.Vector3D(self._speed, 0, 0)) # For when physics are active + self._actor_list.append(actor) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) @@ -2894,7 +2895,6 @@ def __init__(self, plan, spawn_dist_interval, sink_dist=2, """ super().__init__(name) self._rng = CarlaDataProvider.get_random_seed() - self._world = CarlaDataProvider.get_world() self._plan = plan self._sink_dist = sink_dist @@ -2953,7 +2953,7 @@ def update(self): actor.destroy() self._actor_data.remove(actor_data) else: - controller.run_step() + actor.apply_control(controller.run_step()) # Spawn new actors if needed if len(self._actor_data) == 0: diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index be22f8e71..dbb5812db 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -2112,7 +2112,7 @@ def terminate(self, new_status): if self.test_status == "FAILURE": traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE) - traffic_event.set_dict({'speed_percentage': self.actual_value}) + traffic_event.set_dict({'percentage': self.actual_value}) traffic_event.set_message( f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") self.events.append(traffic_event) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 420b556d4..0693dc85d 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -364,7 +364,7 @@ def _create_behavior(self): sequence.add_child(HandleJunctionScenario( clear_junction=False, clear_ego_entry=True, - remove_entries=source_wp, + remove_entries=[source_wp], remove_exits=[], stop_entries=False, extend_road_exit=extra_space diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index cc728ae56..15d9dc03b 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -194,7 +194,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._road_front_vehicles = 2 # Amount of vehicles in front of the ego self._road_back_vehicles = 2 # Amount of vehicles behind the ego - self._radius_increase_ratio = 2.5 # Meters the radius increases per m/s of the ego + self._radius_increase_ratio = 2.3 # Meters the radius increases per m/s of the ego self._base_junction_detection = 30 self._detection_ratio = 1.5 # Meters the radius increases per m/s of the ego @@ -232,7 +232,6 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): # Scenario variables: self._stopped_road_actors = [] # Actors stopped by a hard break scenario - # self._clear_actors = [] # Actors that will be cleaned up by a junction self._route_sources_active = True @@ -1868,6 +1867,7 @@ def handle_actors(actor_list): self._destroy_actor(actor) else: self._actors_speed_perc[actor] = 0 + self._stopped_road_actors.append(actor) if self._active_junctions: for source in self._active_junctions[0].entry_sources: @@ -2077,8 +2077,6 @@ def _update_road_actors(self): Not applied to those behind it so that they can catch up it """ # Updates their speed - # scenario_actors = self._stopped_road_actors + self._clear_actors - scenario_actors = self._stopped_road_actors for lane_key in self._road_dict: for i, actor in enumerate(self._road_dict[lane_key].actors): location = CarlaDataProvider.get_location(actor) @@ -2091,18 +2089,40 @@ def _update_road_actors(self): string += '_[' + lane_key + ']' draw_string(self._world, location, string, DEBUG_ROAD, False) - if actor in scenario_actors or self._is_location_behind_ego(location): + # if actor in scenario_actors or self._is_location_behind_ego(location): + if actor in self._stopped_road_actors: continue - self._set_road_actor_speed(location, actor) + if not self._is_location_behind_ego(location): + self._set_road_actor_speed(location, actor) + else: + self._set_road_back_actor_speed(location, actor) def _set_road_actor_speed(self, location, actor, multiplier=1): - """Changes the speed of the vehicle depending on its distance to the ego""" + """ + Changes the speed of the vehicle depending on its distance to the ego. + Capped at 100% for all actors closer than `self._min_radius`, + it gradually reduces for larger distances up to 0. + """ distance = location.distance(self._ego_wp.transform.location) percentage = (self._max_radius - distance) / (self._max_radius - self._min_radius) * 100 percentage *= multiplier - percentage = min(percentage, 100) + percentage = max(min(percentage, 100), 0) self._actors_speed_perc[actor] = percentage + draw_string(self._world, location, str(percentage), DEBUG_ROAD, False) + + def _set_road_back_actor_speed(self, location, actor): + """ + Changes the speed of the vehicle depending on its distance to the ego. + The further they are, the higher their speed, helping them catch up to the ego. + """ + distance = location.distance(self._ego_wp.transform.location) + percentage = distance / (self._max_radius - self._min_radius) * 100 + 100 + percentage = max(min(percentage, 200), 0) + self._actors_speed_perc[actor] = percentage + + draw_string(self._world, location, str(percentage), DEBUG_ROAD, False) + def _monitor_road_changes(self, prev_route_index): """ Checks if the ego changes road, remapping the route keys. diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 79b5290cd..b2a614048 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -158,6 +158,14 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._blocker_model = 'static.prop.vendingmachine' # blueprint filter of the blocker + if 'crossing_angle' in config.other_parameters: + self._crossing_angle = float(config.other_parameters['crossing_angle']['value']) + else: + self._crossing_angle = 0 # Crossing angle of the pedestrian + + if abs(self._crossing_angle) > 90: + raise ValueError("'crossing_angle' must be between -90 and 90º for the pedestrian to cross the road") + self._blocker_shift = 0.9 self._retry_dist = 0.4 @@ -241,7 +249,7 @@ def _initialize_actors(self, config): raise ValueError("Couldn't find a location to spawn the adversary") walker_wp = wps[0] - offset = {"yaw": 270, "z": 0.5, "k": 1.2} + offset = {"yaw": 270 - self._crossing_angle, "z": 0.5, "k": 1.2} self._adversary_transform = self._get_sidewalk_transform(walker_wp, offset) adversary = CarlaDataProvider.request_new_actor('walker.*', self._adversary_transform) if adversary is None: diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 832139f4b..d5a75972f 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -14,9 +14,11 @@ import py_trees import carla - from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + ActorTransformSetter, + Idle, + ChangeAutoPilot) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario @@ -57,6 +59,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl """ self._world = world self._map = CarlaDataProvider.get_map() + self._tm = CarlaDataProvider.get_client().get_trafficmanager( + CarlaDataProvider.get_traffic_manager_port()) self.timeout = timeout if 'front_vehicle_distance' in config.other_parameters: @@ -81,7 +85,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl if 'flow_distance' in config.other_parameters: self._flow_distance = float(config.other_parameters['flow_distance']['value']) else: - self._flow_distance = 30 + self._flow_distance = 25 # Get parking_waypoint based on trigger_point self._trigger_location = config.trigger_points[0].location @@ -97,6 +101,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._bp_attributes = {'base_type': 'car', 'has_lights': False} + self._side_end_distance = 50 + super(ParkingExit, self).__init__("ParkingExit", ego_vehicles, config, @@ -148,6 +154,18 @@ def _initialize_actors(self, config): self._ego_transform = self._get_displaced_location(self.ego_vehicles[0], self._parking_waypoint) self.ego_vehicles[0].set_location(self._ego_transform) + # Spawn the actor at the side of the ego + actor_side = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._reference_waypoint.transform, rolename='scenario', attribute_filter=self._bp_attributes) + if actor_side is None: + raise ValueError( + "Couldn't spawn the vehicle at the side of the parking point") + self.other_actors.append(actor_side) + self._tm.update_vehicle_lights(actor_side, True) + + self._end_side_location = self.ego_vehicles[0].get_transform() + self._end_side_location.location.z -= 500 + def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane @@ -176,10 +194,18 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + side_actor_behavior = py_trees.composites.Sequence() + side_actor_behavior.add_child(ChangeAutoPilot(self.other_actors[2], True)) + side_actor_behavior.add_child(DriveDistance(self.other_actors[2], self._side_end_distance)) + side_actor_behavior.add_child(ActorTransformSetter(self.other_actors[2], self._end_side_location, False)) + side_actor_behavior.add_child(Idle()) + root.add_child(side_actor_behavior) + end_condition = py_trees.composites.Sequence() end_condition.add_child(DriveDistance( self.ego_vehicles[0], self._end_distance)) root.add_child(end_condition) + sequence.add_child(root) for actor in self.other_actors: From f2cfed27be40ec82ab73375ff68eba939cf1ef39 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Thu, 7 Jul 2022 14:16:43 +0200 Subject: [PATCH 38/86] Removed debug messages --- srunner/scenarios/background_activity.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 15d9dc03b..17eaa5e69 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -2109,8 +2109,6 @@ def _set_road_actor_speed(self, location, actor, multiplier=1): percentage = max(min(percentage, 100), 0) self._actors_speed_perc[actor] = percentage - draw_string(self._world, location, str(percentage), DEBUG_ROAD, False) - def _set_road_back_actor_speed(self, location, actor): """ Changes the speed of the vehicle depending on its distance to the ego. @@ -2121,8 +2119,6 @@ def _set_road_back_actor_speed(self, location, actor): percentage = max(min(percentage, 200), 0) self._actors_speed_perc[actor] = percentage - draw_string(self._world, location, str(percentage), DEBUG_ROAD, False) - def _monitor_road_changes(self, prev_route_index): """ Checks if the ego changes road, remapping the route keys. From 850eed87a36150298c0d72f7377b6cad553aa871 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Tue, 12 Jul 2022 08:49:44 +0200 Subject: [PATCH 39/86] Added scenario timeouts --- .../scenarioatomics/atomic_behaviors.py | 57 +++++++++++++ .../scenarioatomics/atomic_criteria.py | 53 +++++++++++- srunner/scenariomanager/timer.py | 2 +- srunner/scenariomanager/traffic_events.py | 1 + srunner/scenarios/actor_flow.py | 82 +++++++++++++------ .../scenarios/construction_crash_vehicle.py | 27 ++++-- srunner/scenarios/cross_bicycle_flow.py | 23 +++--- srunner/scenarios/invading_turn.py | 25 ++++-- srunner/scenarios/parking_exit.py | 27 +++--- srunner/scenarios/route_obstacles.py | 71 ++++++++++++---- .../signalized_junction_left_turn.py | 32 ++++++-- .../signalized_junction_right_turn.py | 30 +++++-- srunner/scenarios/vehicle_opens_door.py | 27 ++++-- 13 files changed, 354 insertions(+), 103 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 8af40b7c8..5c55ed9bd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3604,6 +3604,7 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status + class SwitchWrongDirectionTest(AtomicBehavior): """ @@ -3625,6 +3626,7 @@ def update(self): 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"): @@ -3643,6 +3645,7 @@ def update(self): py_trees.blackboard.Blackboard().set("SwitchMinSpeedCriteria", self._active, overwrite=True) return new_status + class WalkerFlow(AtomicBehavior): """ Behavior that indefinitely creates walkers at a location, @@ -3822,3 +3825,57 @@ def terminate(self, new_status): self._destroy_walker(self._walker, self._controller) except RuntimeError: pass # Actor was already destroyed + + +class ScenarioTimeout(AtomicBehavior): + + """ + This class is an idle behavior that waits for a set amount of time + before stoping. + + It is meant to be used with the `ScenarioTimeoutTest` to be used at scenarios + that block the ego's route (such as adding obstacles) so that if the ego is + incapable of surpassing them, it isn't blocked forever. Instead, + the scenario will timeout, but it will be penalized by the `ScenarioTimeoutTest` + + Parameters: + - duration: Duration in seconds of this behavior + """ + + def __init__(self, duration, scenario_name, name="ScenarioTimeout"): + """ + Setup actor + """ + super().__init__(name) + self._duration = duration + self._scenario_name = scenario_name + self._start_time = 0 + self._scenario_timeout = False + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def initialise(self): + """ + Set start time + """ + self._start_time = GameTime.get_time() + super().initialise() + + def update(self): + """ + Keep running until termination condition is satisfied + """ + new_status = py_trees.common.Status.RUNNING + + if GameTime.get_time() - self._start_time > self._duration: + self._scenario_timeout = True + new_status = py_trees.common.Status.SUCCESS + + return new_status + + def terminate(self, new_status): + """ + Modifies the blackboard to tell the `ScenarioTimeoutTest` if the timeout was triggered + """ + py_trees.blackboard.Blackboard().set(f"ScenarioTimeout_{self._scenario_name}", self._scenario_timeout, overwrite=True) + self._scenario_timeout = False # py_trees calls the terminate several times for some reason. + super().terminate(new_status) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index dbb5812db..795ebf29d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -2039,14 +2039,15 @@ def terminate(self, new_status): super().terminate(new_status) + class YieldToEmergencyVehicleTest(Criterion): """ Atomic Criterion to detect if the actor yields its lane to the emergency vehicle behind it Args: - actor (carla.ACtor): CARLA actor to be used for this test - ev (carla.ACtor): The emergency vehicle + actor (carla.Actor): CARLA actor to be used for this test + ev (carla.Actor): The emergency vehicle optional (bool): If True, the result is not considered for an overall pass/fail result """ @@ -2056,7 +2057,7 @@ def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest" """ Constructor """ - super(YieldToEmergencyVehicleTest, self).__init__(name, actor, ev, optional) + super().__init__(name, actor, optional) self.units = "%" self.success_value = 70 self.actual_value = 0 @@ -2068,7 +2069,7 @@ def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest" def initialise(self): # self._last_time = GameTime.get_time() - super(YieldToEmergencyVehicleTest, self).initialise() + super().initialise() def update(self): """ @@ -2118,3 +2119,47 @@ def terminate(self, new_status): self.events.append(traffic_event) super().terminate(new_status) + + +class ScenarioTimeoutTest(Criterion): + + """ + Atomic Criterion to detect if the actor has been incapable of finishing an scenario + + Args: + actor (carla.Actor): CARLA actor to be used for this test + 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"): + """ + Constructor + """ + super().__init__(name, actor, optional) + self.units = "" + self.success_value = 0 + self.actual_value = 0 + self._scenario_name = scenario_name + + 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)) + return new_status + + def terminate(self, new_status): + """check the blackboard for the data and update the criteria if one found""" + + blackboard_name = f"ScenarioTimeout_{self._scenario_name}" + + timeout = py_trees.blackboard.Blackboard().get(blackboard_name) + if timeout: + self.actual_value = 1 + self.test_status = "FAILURE" + + traffic_event = TrafficEvent(event_type=TrafficEventType.SCENARIO_TIMEOUT) + traffic_event.set_message("Agent timed out a scenario") + self.events.append(traffic_event) + py_trees.blackboard.Blackboard().set(blackboard_name, None, True) + + super().terminate(new_status) diff --git a/srunner/scenariomanager/timer.py b/srunner/scenariomanager/timer.py index 0e66f4ee3..a17ba218a 100644 --- a/srunner/scenariomanager/timer.py +++ b/srunner/scenariomanager/timer.py @@ -165,7 +165,7 @@ class RouteTimeoutBehavior(py_trees.behaviour.Behaviour): it increases every time the agent advanced through the route, and is dependent on the road's speed. """ MIN_TIMEOUT = 180 - TIMEOUT_ROUTE_PERC = 25 + TIMEOUT_ROUTE_PERC = 15 def __init__(self, ego_vehicle, route, debug=False, name="RouteTimeoutBehavior"): """ diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index ee746e3bb..365bde38a 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -32,6 +32,7 @@ class TrafficEventType(Enum): VEHICLE_BLOCKED = 13 MIN_SPEED_INFRACTION = 14 YIELD_TO_EMERGENCY_VEHICLE = 15 + SCENARIO_TIMEOUT = 16 class TrafficEvent(object): diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 0693dc85d..a007ada08 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -16,8 +16,8 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToLocation, WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario @@ -75,6 +75,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals ego_location = config.trigger_points[0].location self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("EnterActorFlow", ego_vehicles, config, @@ -101,6 +106,7 @@ def _create_behavior(self): root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence = py_trees.composites.Sequence() if self.route_mode: @@ -133,9 +139,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -165,6 +172,7 @@ def _create_behavior(self): source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) for sink_wp in sink_wps: root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match @@ -227,6 +235,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._source_dist_interval = [5, 7] # m + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("HighwayExit", ego_vehicles, config, @@ -253,10 +266,8 @@ def _create_behavior(self): policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) - end_condition = py_trees.composites.Sequence() - end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) - - root.add_child(end_condition) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) sequence = py_trees.composites.Sequence() @@ -271,9 +282,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -324,6 +336,12 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals ego_location = config.trigger_points[0].location self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + + super().__init__("MergerIntoSlowTraffic", ego_vehicles, config, @@ -348,6 +366,7 @@ def _create_behavior(self): root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence = py_trees.composites.Sequence() if self.route_mode: @@ -381,9 +400,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -412,6 +432,7 @@ def _create_behavior(self): source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) for sink_wp in sink_wps: root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match @@ -476,6 +497,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals if not self._other_entry_wp or self._other_entry_wp.lane_type != carla.LaneType.Driving: raise ValueError("Couldn't find an end position") + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("InterurbanActorFlow", ego_vehicles, config, @@ -495,10 +521,8 @@ def _create_behavior(self): policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) - end_condition = py_trees.composites.Sequence() - end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) - - root.add_child(end_condition) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(WaitEndIntersection(self.ego_vehicles[0])) sequence = py_trees.composites.Sequence() @@ -523,9 +547,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -572,6 +597,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) self._exit_wp = generate_target_waypoint_in_route(self._reference_wp, config.route) + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("InterurbanAdvancedActorFlow", ego_vehicles, config, @@ -600,6 +630,7 @@ def _create_behavior(self): source_wp_1, sink_wp_1, self._source_dist_interval, self._sink_distance, self._flow_speed)) root.add_child(ActorFlow( source_wp_2, sink_wp_2, self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence = py_trees.composites.Sequence() if self.route_mode: @@ -637,9 +668,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index 81fd8c8b0..f70021612 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -16,9 +16,10 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, ActorTransformSetter, - SwitchWrongDirectionTest) + SwitchWrongDirectionTest, + ScenarioTimeout) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import (ChangeOppositeBehavior, RemoveRoadLane, @@ -57,6 +58,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._construction_transforms = [] + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("ConstructionObstacle", ego_vehicles, config, world, debug_mode, False, criteria_enable) def _initialize_actors(self, config): @@ -151,7 +157,10 @@ def _create_behavior(self): for actor, transform in self._construction_transforms: root.add_child(ActorTransformSetter(actor, transform, True)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) @@ -169,9 +178,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -199,7 +209,10 @@ def _create_behavior(self): root = py_trees.composites.Sequence() for actor, transform in self._construction_transforms: root.add_child(ActorTransformSetter(actor, transform, True)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py index 571fd27bc..4795990b8 100644 --- a/srunner/scenarios/cross_bicycle_flow.py +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -15,8 +15,8 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import BicycleFlow, TrafficLightFreezer -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import BicycleFlow, TrafficLightFreezer, ScenarioTimeout +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario @@ -79,6 +79,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("CrossingBicycleFlow", ego_vehicles, config, @@ -156,11 +161,8 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(BicycleFlow(self._plan, self._source_dist_interval, self._sink_distance, self._flow_speed, True)) - - # End condition, when the ego exits the junction - end_condition = py_trees.composites.Sequence() - end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) - root.add_child(end_condition) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(WaitEndIntersection(self.ego_vehicles[0])) # Freeze the traffic lights to allow the flow to populate the junction tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") @@ -189,9 +191,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py index 7d48e84d4..30fdf6208 100644 --- a/srunner/scenarios/invading_turn.py +++ b/srunner/scenarios/invading_turn.py @@ -16,8 +16,11 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, BasicAgentBehavior -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, + ActorDestroy, + BasicAgentBehavior, + ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import LeaveSpaceInFront @@ -72,6 +75,11 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl else: self._offset = 0.5 + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super(InvadingTurn, self).__init__("InvadingTurn", ego_vehicles, config, @@ -130,9 +138,13 @@ def _create_behavior(self): sequence.add_child(ActorTransformSetter( self.other_actors[0], self._adversary_start_transform)) - sequence.add_child(BasicAgentBehavior( + behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + behavior.add_child(BasicAgentBehavior( self.other_actors[0], self._adversary_end, opt_dict={'offset': self._offset})) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._distance)) + behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + sequence.add_child(behavior) sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence @@ -142,9 +154,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index d5a75972f..8d9e77759 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -18,8 +18,9 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, ActorTransformSetter, Idle, - ChangeAutoPilot) -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest + ChangeAutoPilot, + ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario @@ -103,6 +104,11 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._side_end_distance = 50 + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 120 + super(ParkingExit, self).__init__("ParkingExit", ego_vehicles, config, @@ -191,8 +197,7 @@ def _create_behavior(self): sequence = py_trees.composites.Sequence() sequence.add_child(ChangeRoadBehavior(spawn_dist=self._flow_distance)) - root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) side_actor_behavior = py_trees.composites.Sequence() side_actor_behavior.add_child(ChangeAutoPilot(self.other_actors[2], True)) @@ -201,9 +206,9 @@ def _create_behavior(self): side_actor_behavior.add_child(Idle()) root.add_child(side_actor_behavior) - end_condition = py_trees.composites.Sequence() - end_condition.add_child(DriveDistance( - self.ego_vehicles[0], self._end_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) root.add_child(end_condition) sequence.add_child(root) @@ -219,10 +224,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return[] - - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index b48e77cc6..a93106e81 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -18,8 +18,9 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, SwitchWrongDirectionTest, - ConstantVelocityAgentBehavior) -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest + ConstantVelocityAgentBehavior, + ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior @@ -64,6 +65,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__( "Accident", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) @@ -94,6 +100,7 @@ def _initialize_actors(self, config): lights = police_car.get_light_state() lights |= self._lights police_car.set_light_state(carla.VehicleLightState(lights)) + police_car.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(police_car) # Create the first vehicle that has been in the accident @@ -113,6 +120,7 @@ def _initialize_actors(self, config): 'vehicle.*', vehicle_1_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) if not vehicle_1_car: raise ValueError("Couldn't spawn the accident car") + vehicle_1_car.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(vehicle_1_car) # Create the second vehicle that has been in the accident @@ -132,6 +140,7 @@ def _initialize_actors(self, config): 'vehicle.*', vehicle_2_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) if not vehicle_2_car: raise ValueError("Couldn't spawn the accident car") + vehicle_2_car.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(vehicle_2_car) def _create_behavior(self): @@ -143,7 +152,11 @@ def _create_behavior(self): root = py_trees.composites.Sequence() if self.route_mode: root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(ActorDestroy(self.other_actors[1])) root.add_child(ActorDestroy(self.other_actors[2])) @@ -154,9 +167,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -189,7 +203,10 @@ def _create_behavior(self): root.add_child(LeaveSpaceInFront(total_dist)) root.add_child(SwitchWrongDirectionTest(False)) root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) root.add_child(ChangeOppositeBehavior(spawn_dist=50)) @@ -224,6 +241,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._lights = carla.VehicleLightState.RightBlinker | carla.VehicleLightState.LeftBlinker + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__( "ParkedObstacle", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) @@ -252,6 +274,7 @@ def _initialize_actors(self, config): lights = parked_car.get_light_state() lights |= self._lights parked_car.set_light_state(carla.VehicleLightState(lights)) + parked_car.apply_control(carla.VehicleControl(hand_brake=True)) pre_parked_wps = starting_wp.next(self._distance / 2) if not pre_parked_wps: @@ -268,7 +291,10 @@ def _create_behavior(self): root = py_trees.composites.Sequence() if self.route_mode: root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) root.add_child(ActorDestroy(self.other_actors[0])) return root @@ -278,9 +304,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -312,7 +339,10 @@ def _create_behavior(self): root.add_child(LeaveSpaceInFront(total_dist)) root.add_child(SwitchWrongDirectionTest(False)) root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) root.add_child(ChangeOppositeBehavior(spawn_dist=50)) @@ -352,6 +382,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals else: self._distance_to_Trigger = [74,76,88] # m + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("BicycleFlowAtSideLane", ego_vehicles, config, @@ -410,7 +445,10 @@ def _create_behavior(self): self.other_actors[0], self._target_location, target_speed = self._bicycle_speed, opt_dict={'offset': self._offset[0] * self._bicycle_wp[0].lane_width / 2})) root.add_child(bycicle) - root.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + root.add_child(end_condition) if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) root.add_child(ChangeOppositeBehavior(active=True)) @@ -425,9 +463,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index b18d93edd..9cdf67727 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -15,9 +15,9 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer, ScenarioTimeout from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import (generate_target_waypoint, get_junction_topology, @@ -72,6 +72,12 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._init_tl_dict = {} self.timeout = timeout + + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("SignalizedJunctionLeftTurn", ego_vehicles, config, @@ -173,6 +179,7 @@ def _create_behavior(self): root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) @@ -191,9 +198,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -243,6 +251,12 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._sink_dist = 3 * self._flow_speed self.timeout = timeout + + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("NonSignalizedJunctionLeftTurn", ego_vehicles, config, @@ -321,6 +335,7 @@ def _create_behavior(self): root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence.add_child(root) @@ -334,9 +349,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index 51f569735..2128b1e85 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -16,8 +16,8 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer, ScenarioTimeout +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import (generate_target_waypoint, @@ -67,6 +67,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._flow_tl_dict = {} self._init_tl_dict = {} + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("SignalizedJunctionRightTurn", ego_vehicles, config, @@ -172,6 +177,7 @@ def _create_behavior(self): root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) @@ -186,9 +192,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -232,6 +239,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._source_dist = 5 * self._flow_speed self._sink_dist = 3 * self._flow_speed + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("NonSignalizedJunctionRightTurn", ego_vehicles, config, @@ -314,6 +326,7 @@ def _create_behavior(self): root.add_child(WaitEndIntersection(self.ego_vehicles[0])) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence.add_child(root) return sequence @@ -323,9 +336,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index fc2ff24a3..72a99e75a 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -16,11 +16,12 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, OpenVehicleDoor, - SwitchWrongDirectionTest) + SwitchWrongDirectionTest, + ScenarioTimeout) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, DriveDistance) @@ -63,6 +64,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + if 'timeout' in config.other_parameters: + self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) + else: + self._scenario_timeout = 180 + super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): @@ -119,7 +125,10 @@ def _create_behavior(self): door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) - sequence.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + sequence.add_child(end_condition) sequence.add_child(ActorDestroy(self._parked_actor)) return sequence @@ -145,9 +154,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ @@ -197,7 +207,10 @@ def _create_behavior(self): sequence.add_child(SwitchWrongDirectionTest(False)) sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - sequence.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + sequence.add_child(end_condition) sequence.add_child(SwitchWrongDirectionTest(True)) sequence.add_child(ChangeOppositeBehavior(spawn_dist=50)) sequence.add_child(ActorDestroy(self._parked_actor)) From 93ec2dc82eabdfb3e79eb0cfb85bfb4e4ef4c2c6 Mon Sep 17 00:00:00 2001 From: joel-mb Date: Tue, 12 Jul 2022 09:57:50 +0200 Subject: [PATCH 40/86] Added frame to traffic events --- .../scenarioatomics/atomic_criteria.py | 40 +++++++++++-------- srunner/scenariomanager/traffic_events.py | 8 +++- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 795ebf29d..196436ef5 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -396,7 +396,7 @@ def _count_collisions(self, event): # pylint: disable=too-many-return-statem else: return - collision_event = TrafficEvent(event_type=actor_type) + 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( @@ -444,7 +444,7 @@ def update(self): self.test_status = "FAILURE" vehicle_location = CarlaDataProvider.get_location(self.actor) - event = TrafficEvent(event_type=TrafficEventType.VEHICLE_BLOCKED) + 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), @@ -877,7 +877,7 @@ def update(self): self.actual_value += 1 - onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION) + 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) self._set_event_dict( @@ -892,7 +892,7 @@ def update(self): self.actual_value += 1 - outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION) + 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) self._set_event_dict( @@ -915,7 +915,7 @@ def terminate(self, new_status): self.actual_value += 1 - onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION) + 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) self._set_event_dict( @@ -930,7 +930,7 @@ def terminate(self, new_status): self.actual_value += 1 - outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION) + 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) self._set_event_dict( @@ -1140,7 +1140,7 @@ def terminate(self, new_status): percentage = self._wrong_distance / self._total_distance * 100 - outside_lane = TrafficEvent(event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION) + outside_lane = TrafficEvent(event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION, frame=GameTime.get_frame()) outside_lane.set_message( "Agent went outside its route lanes for about {} meters " "({}% of the completed route)".format( @@ -1264,7 +1264,7 @@ 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) + 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, @@ -1292,7 +1292,7 @@ def terminate(self, new_status): current_lane_id = lane_waypoint.lane_id current_road_id = lane_waypoint.road_id - wrong_way_event = TrafficEvent(event_type=TrafficEventType.WRONG_WAY_INFRACTION) + 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, @@ -1362,7 +1362,7 @@ def update(self): if self.test_status != "SUCCESS": 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) + 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" @@ -1484,7 +1484,7 @@ def update(self): blackv = py_trees.blackboard.Blackboard() _ = blackv.set("InRoute", False) - route_deviation_event = TrafficEvent(event_type=TrafficEventType.ROUTE_DEVIATION) + 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), @@ -1535,7 +1535,9 @@ 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) + 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): @@ -1578,6 +1580,10 @@ def update(self): 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.test_status = "SUCCESS" @@ -1715,7 +1721,7 @@ def update(self): self.test_status = "FAILURE" self.actual_value += 1 location = traffic_light.get_transform().location - red_light_event = TrafficEvent(event_type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION) + 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, @@ -1924,7 +1930,7 @@ def update(self): 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) + 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, @@ -2032,7 +2038,7 @@ def terminate(self, new_status): self.test_status = "FAILURE" if self.test_status == "FAILURE": - self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION) + self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION, frame=GameTime.get_frame()) self._traffic_event.set_dict({'percentage': self.actual_value}) self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) self.events.append(self._traffic_event) @@ -2112,7 +2118,7 @@ def terminate(self, new_status): self.test_status = "FAILURE" if self.test_status == "FAILURE": - traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE) + traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, frame=GameTime.get_frame()) traffic_event.set_dict({'percentage': self.actual_value}) traffic_event.set_message( f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") @@ -2157,7 +2163,7 @@ def terminate(self, new_status): self.actual_value = 1 self.test_status = "FAILURE" - traffic_event = TrafficEvent(event_type=TrafficEventType.SCENARIO_TIMEOUT) + 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/traffic_events.py b/srunner/scenariomanager/traffic_events.py index 365bde38a..d175207e4 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -41,15 +41,17 @@ class TrafficEvent(object): TrafficEvent definition """ - def __init__(self, event_type, message="", dictionary=None): + 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 message: optional message to inform users of the event :param dictionary: optional dictionary with arbitrary keys and values """ self._type = event_type + self._frame = frame self._message = message self._dict = dictionary @@ -57,6 +59,10 @@ def get_type(self): """return the type""" return self._type + def get_frame(self): + """return the frame""" + return self._frame + def set_message(self, message): """Set message""" self._message = message From 4fd3009ace1d1172ecd48543ac3ec0efb7d3f173 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Thu, 14 Jul 2022 11:39:25 +0200 Subject: [PATCH 41/86] Added realtime to OutsideRouteLanesTest --- .../scenarioatomics/atomic_criteria.py | 56 ++++++++++--------- srunner/scenariomanager/traffic_events.py | 4 ++ 2 files changed, 33 insertions(+), 27 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 196436ef5..a51ea5759 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1006,6 +1006,8 @@ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): self._wrong_distance = 0 self._wrong_direction_active = True + self._traffic_event = None + def update(self): """ Transforms the actor location and its four corners to waypoints. Depending on its types, @@ -1054,12 +1056,39 @@ def update(self): if self._outside_lane_active or (self._wrong_direction_active and self._wrong_lane_active): self._wrong_distance += new_dist + self._set_traffic_event() + self._current_index = index self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status + 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.events.append(self._traffic_event) + + percentage = self._wrong_distance / self._total_distance * 100 + self.actual_value = round(percentage, 2) + + 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))) + + self._traffic_event.set_dict({ + 'distance': self._wrong_distance, + 'percentage': percentage + }) + + self._traffic_event.set_frame(GameTime.get_frame()) + + def _is_outside_driving_lanes(self, location): """ Detects if the ego_vehicle is outside driving lanes @@ -1131,33 +1160,6 @@ def _is_at_wrong_lane(self, location): self._last_road_id = current_road_id self._pre_ego_waypoint = current_waypoint - def terminate(self, new_status): - """ - If there is currently an event running, it is registered - """ - - if self._wrong_distance > 0: - - percentage = self._wrong_distance / self._total_distance * 100 - - outside_lane = TrafficEvent(event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION, frame=GameTime.get_frame()) - outside_lane.set_message( - "Agent went outside its route lanes for about {} meters " - "({}% of the completed route)".format( - round(self._wrong_distance, 3), - round(percentage, 2))) - - outside_lane.set_dict({ - 'distance': self._wrong_distance, - 'percentage': percentage - }) - - self._wrong_distance = 0 - self.events.append(outside_lane) - self.actual_value = round(percentage, 2) - - super(OutsideRouteLanesTest, self).terminate(new_status) - class WrongLaneTest(Criterion): diff --git a/srunner/scenariomanager/traffic_events.py b/srunner/scenariomanager/traffic_events.py index d175207e4..d7fe1bda9 100644 --- a/srunner/scenariomanager/traffic_events.py +++ b/srunner/scenariomanager/traffic_events.py @@ -63,6 +63,10 @@ def get_frame(self): """return the frame""" return self._frame + def set_frame(self, frame): + """return the frame""" + self._frame = frame + def set_message(self, message): """Set message""" self._message = message From aab83e1206852acbc10095b91fdc467d8768af33 Mon Sep 17 00:00:00 2001 From: glopezdiest <58212725+glopezdiest@users.noreply.github.com> Date: Tue, 26 Jul 2022 09:06:45 +0200 Subject: [PATCH 42/86] More Leaderboard 2.0 scenario improvements --- srunner/examples/RouteObstacles.xml | 6 - .../scenarioatomics/atomic_behaviors.py | 33 +- .../scenarioatomics/atomic_criteria.py | 180 ++--- srunner/scenarios/actor_flow.py | 209 +++--- srunner/scenarios/background_activity.py | 227 +++--- .../background_activity_parametrizer.py | 50 +- srunner/scenarios/bycicle_group.py | 97 --- .../scenarios/construction_crash_vehicle.py | 198 +++-- srunner/scenarios/cross_bicycle_flow.py | 63 +- .../scenarios/cut_in_with_static_vehicle.py | 234 ++++-- srunner/scenarios/highway_cut_in.py | 4 +- srunner/scenarios/invading_turn.py | 10 +- .../scenarios/object_crash_intersection.py | 6 +- srunner/scenarios/object_crash_vehicle.py | 95 +-- .../opposite_vehicle_taking_priority.py | 272 +++---- srunner/scenarios/parking_cut_in.py | 6 +- srunner/scenarios/parking_exit.py | 68 +- srunner/scenarios/pedestrian_crossing.py | 178 +++-- srunner/scenarios/route_obstacles.py | 680 ++++++++++++------ srunner/scenarios/route_scenario.py | 4 +- .../signalized_junction_left_turn.py | 258 +++---- .../signalized_junction_right_turn.py | 252 +++---- srunner/scenarios/vehicle_opens_door.py | 87 ++- .../scenarios/yield_to_emergency_vehicle.py | 8 +- srunner/tools/background_manager.py | 40 +- 25 files changed, 1711 insertions(+), 1554 deletions(-) delete mode 100644 srunner/scenarios/bycicle_group.py diff --git a/srunner/examples/RouteObstacles.xml b/srunner/examples/RouteObstacles.xml index 7981d2915..17c824fcf 100644 --- a/srunner/examples/RouteObstacles.xml +++ b/srunner/examples/RouteObstacles.xml @@ -1,11 +1,5 @@ - - - - - - diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 5c55ed9bd..d20e9b8e7 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1431,17 +1431,6 @@ def __init__(self, actor, target_velocity, force_speed=False, self._start_time = 0 self._location = None - self._collision_sensor = None - - def _set_collision_sensor(self): - blueprint = self._world.get_blueprint_library().find('sensor.other.collision') - self._collision_sensor = self._world.spawn_actor(blueprint, carla.Transform(), attach_to=self._actor) - self._collision_sensor.listen(lambda event: self._stop_constant_velocity(event)) - - def _stop_constant_velocity(self, event): - """Stops the constant velocity behavior""" - self._forced_speed = False - def initialise(self): self._location = CarlaDataProvider.get_location(self._actor) self._start_time = GameTime.get_time() @@ -1454,8 +1443,6 @@ def initialise(self): self._control.hand_brake = False self._actor.apply_control(self._control) - self._set_collision_sensor() - super(KeepVelocity, self).initialise() def update(self): @@ -1479,7 +1466,7 @@ def update(self): 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)) + self._actor.apply_control(carla.VehicleControl(throttle=1.0)) new_location = CarlaDataProvider.get_location(self._actor) self._distance += calculate_distance(self._location, new_location) @@ -1507,9 +1494,6 @@ def terminate(self, new_status): self._control.speed = 0.0 if self._actor is not None and self._actor.is_alive: self._actor.apply_control(self._control) - if self._collision_sensor: - self._collision_sensor.stop() - self._collision_sensor.destroy() except RuntimeError: pass super(KeepVelocity, self).terminate(new_status) @@ -1956,7 +1940,7 @@ def __init__(self, actor, target_location=None, plan=None, target_speed=20, opt_ self._agent = None if self._target_location and self._plan: - raise ValueError("Choose either a destiantion or a plan, but not both") + raise ValueError("Choose either a destination or a plan, but not both") def initialise(self): """Initialises the agent""" @@ -2090,7 +2074,7 @@ def __init__(self, actor, reference_actor, target_location=None, speed_incremen def initialise(self): """Initialises the agent""" # Get target speed - self._target_speed = get_speed(self._reference_actor) + self._speed_increment*3.6 + self._target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 py_trees.blackboard.Blackboard().set( "ACVAB_speed_{}".format(self._reference_actor.id), self._target_speed, overwrite=True) @@ -2614,7 +2598,8 @@ def update(self): new_status = py_trees.common.Status.FAILURE if calculate_distance(self._actor.get_location(), self._transform.location) < 1.0: - self._actor.set_simulate_physics(self._physics) + if self._physics is not None: + self._actor.set_simulate_physics(self._physics) new_status = py_trees.common.Status.SUCCESS return new_status @@ -2798,6 +2783,8 @@ def initialise(self): ref_loc = plan[0][0].transform.location for wp, _ in plan: + if wp.is_junction: + continue # Spawning at junctions might break the path, so don't if wp.transform.location.distance(ref_loc) < self._spawn_dist: continue self._spawn_actor(wp.transform) @@ -2813,7 +2800,7 @@ def _spawn_actor(self, transform): return py_trees.common.Status.RUNNING actor.set_autopilot(True) - self._tm.set_path(actor, [self._source_transform.location, self._sink_location]) + self._tm.set_path(actor, [self._sink_location]) self._tm.auto_lane_change(actor, False) self._tm.set_desired_speed(actor, 3.6 * self._speed) self._tm.update_vehicle_lights(actor, True) @@ -2918,6 +2905,8 @@ def initialise(self): if self._add_initial_actors: ref_loc = self._plan[0][0].transform.location for wp, _ in self._plan: + if wp.is_junction: + continue # Spawning at junctions might break the path, so don't if wp.transform.location.distance(ref_loc) < self._spawn_dist: continue self._spawn_actor(wp.transform) @@ -2926,7 +2915,7 @@ def initialise(self): def _spawn_actor(self, transform): actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', transform, rolename='scenario', + 'vehicle.*', transform, rolename='scenario no lights', attribute_filter={'base_type': 'bicycle'}, tick=False ) if actor is None: diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index a51ea5759..68fcdf10d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1056,11 +1056,11 @@ def update(self): if self._outside_lane_active or (self._wrong_direction_active and self._wrong_lane_active): self._wrong_distance += new_dist + if self._wrong_distance: self._set_traffic_event() self._current_index = index - self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status @@ -1800,9 +1800,9 @@ 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 = 50.0 # meters + PROXIMITY_THRESHOLD = 5.0 # meters SPEED_THRESHOLD = 0.1 - WAYPOINT_STEP = 1.0 # meters + EXTENT_MULTIPLIER = 1.5 def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): """ @@ -1813,22 +1813,15 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): self._list_stop_signs = [] self._target_stop_sign = None self._stop_completed = False - self._affected_by_stop = False all_actors = self._world.get_actors() for _actor in all_actors: if 'traffic.stop' in _actor.type_id: self._list_stop_signs.append(_actor) - @staticmethod - def point_inside_boundingbox(point, bb_center, bb_extent): - """ - X - :param point: - :param bb_center: - :param bb_extent: - :return: - """ + def point_inside_boundingbox(self, point, bb_center, bb_extent): + """Checks whether or not a point is inside a bounding box""" + bb_extent = self.EXTENT_MULTIPLIER * bb_extent # pylint: disable=invalid-name A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y) @@ -1846,56 +1839,40 @@ def point_inside_boundingbox(point, bb_center, bb_extent): return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad # pylint: disable=chained-comparison - def is_actor_affected_by_stop(self, actor, stop, multi_step=20): + def is_actor_affected_by_stop(self, location, stop): """ Check if the given actor is affected by the stop """ - affected = False - # first we run a fast coarse test - current_location = actor.get_location() - stop_location = stop.get_transform().location - if stop_location.distance(current_location) > self.PROXIMITY_THRESHOLD: - return affected + # Quick distance test + stop_transform = stop.get_transform() + if stop_transform.location.distance(location) > self.PROXIMITY_THRESHOLD: + return False + # Check if the actor is inside the stop's bounding box stop_t = stop.get_transform() transformed_tv = stop_t.transform(stop.trigger_volume.location) + if self.point_inside_boundingbox(location, transformed_tv, stop.trigger_volume.extent): + return True - # slower and accurate test based on waypoint's horizon and geometric test - list_locations = [current_location] - waypoint = self._map.get_waypoint(current_location) - for _ in range(multi_step): - if waypoint: - next_wps = waypoint.next(self.WAYPOINT_STEP) - if not next_wps: - break - waypoint = next_wps[0] - if not waypoint: - break - list_locations.append(waypoint.transform.location) - - for actor_location in list_locations: - if self.point_inside_boundingbox(actor_location, transformed_tv, stop.trigger_volume.extent): - affected = True - - return affected - - def _scan_for_stop_sign(self): - target_stop_sign = None + return False + def _scan_for_stop_sign(self, location): ve_tra = CarlaDataProvider.get_transform(self.actor) ve_dir = ve_tra.get_forward_vector() wp = self._map.get_waypoint(ve_tra.location) wp_dir = wp.transform.get_forward_vector() - if ve_dir.dot(wp_dir) > 0: # Ignore all when going in a wrong lane - for stop_sign in self._list_stop_signs: - if self.is_actor_affected_by_stop(self.actor, stop_sign): - # this stop sign is affecting the vehicle - target_stop_sign = stop_sign - break + if ve_dir.dot(wp_dir) < 0: # Ignore all when going in a wrong lane + return None + + ve_vec = self.actor.get_velocity() + if ve_vec.dot(wp_dir) < 0: # Ignore all when going backwards + return None - return target_stop_sign + for stop_sign in self._list_stop_signs: + if self.is_actor_affected_by_stop(location, stop_sign): + return stop_sign # This stop sign is affecting the vehicle def update(self): """ @@ -1908,45 +1885,34 @@ def update(self): return new_status if not self._target_stop_sign: - # scan for stop signs - self._target_stop_sign = self._scan_for_stop_sign() - else: - # we were in the middle of dealing with a stop sign + self._target_stop_sign = self._scan_for_stop_sign(location) + return new_status + + if not self._stop_completed: + current_speed = CarlaDataProvider.get_velocity(self.actor) + if current_speed < self.SPEED_THRESHOLD: + self._stop_completed = True + + if not self.is_actor_affected_by_stop(location, self._target_stop_sign): if not self._stop_completed: - # did the ego-vehicle stop? - current_speed = CarlaDataProvider.get_velocity(self.actor) - if current_speed < self.SPEED_THRESHOLD: - self._stop_completed = True - - if not self._affected_by_stop: - stop_location = self._target_stop_sign.get_location() - stop_extent = self._target_stop_sign.trigger_volume.extent - - if self.point_inside_boundingbox(location, stop_location, stop_extent): - self._affected_by_stop = True - - if not self.is_actor_affected_by_stop(self.actor, self._target_stop_sign): - # is the vehicle out of the influence of this stop sign now? - if not self._stop_completed and self._affected_by_stop: - # 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.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}) - - self.events.append(running_stop_event) - - # reset state - self._target_stop_sign = None - self._stop_completed = False - self._affected_by_stop = False + # 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.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}) + + self.events.append(running_stop_event) + + # Reset state + self._target_stop_sign = None + self._stop_completed = False if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE @@ -2069,15 +2035,12 @@ def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest" self.units = "%" self.success_value = 70 self.actual_value = 0 - # self._last_time = 0 self._ev = ev self._target_speed = None self._ev_speed_log = [] self._map = CarlaDataProvider.get_map() - def initialise(self): - # self._last_time = GameTime.get_time() - super().initialise() + self._terminated = False def update(self): """ @@ -2107,24 +2070,30 @@ def update(self): return new_status def terminate(self, new_status): - if not len(self._ev_speed_log): - self.actual_value = 100 - else: - mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) - self.actual_value = mean_speed / self._target_speed *100 - self.actual_value = round(self.actual_value, 2) + """Set the traffic event to the according value if needed""" - if self.actual_value >= self.success_value: - self.test_status = "SUCCESS" + # Terminates are called multiple times. Do this only once + if not self._terminated: + if not len(self._ev_speed_log): + self.actual_value = 100 else: - self.test_status = "FAILURE" + mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) + self.actual_value = mean_speed / self._target_speed *100 + self.actual_value = round(self.actual_value, 2) - if self.test_status == "FAILURE": - traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, frame=GameTime.get_frame()) - traffic_event.set_dict({'percentage': self.actual_value}) - traffic_event.set_message( - f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") - self.events.append(traffic_event) + if self.actual_value >= self.success_value: + self.test_status = "SUCCESS" + else: + self.test_status = "FAILURE" + + if self.test_status == "FAILURE": + traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, frame=GameTime.get_frame()) + traffic_event.set_dict({'percentage': self.actual_value}) + traffic_event.set_message( + f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") + self.events.append(traffic_event) + + self._terminated = True super().terminate(new_status) @@ -2144,7 +2113,6 @@ def __init__(self, actor, scenario_name, optional=False, name="ScenarioTimeoutTe Constructor """ super().__init__(name, actor, optional) - self.units = "" self.success_value = 0 self.actual_value = 0 self._scenario_name = scenario_name diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index a007ada08..780bb15d9 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -15,6 +15,8 @@ import py_trees import carla +from agents.navigation.local_planner import RoadOption + from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest @@ -38,6 +40,21 @@ def convert_dict_to_location(actor_dict): ) return location +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default + class EnterActorFlow(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light @@ -55,30 +72,17 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) - self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._sink_distance = 2 - - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - ego_location = config.trigger_points[0].location self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._sink_distance = 2 + + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) + + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) + self._scenario_timeout = 240 super().__init__("EnterActorFlow", ego_vehicles, @@ -218,27 +222,13 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._sink_distance = 2 + self._sink_distance = 2 self._end_distance = 40 - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) + self._scenario_timeout = 240 super().__init__("HighwayExit", ego_vehicles, @@ -314,33 +304,18 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout + ego_location = config.trigger_points[0].location + self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) + self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) self._trigger_point=config.trigger_points[0].location self._sink_distance = 2 - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - - ego_location = config.trigger_points[0].location - self._reference_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location) - - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 - + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) + self._scenario_timeout = 240 super().__init__("MergerIntoSlowTraffic", ego_vehicles, @@ -473,34 +448,24 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._start_actor_flow = convert_dict_to_location(config.other_parameters['start_actor_flow']) self._end_actor_flow = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._sink_distance = 2 + self._sink_distance = 2 self._end_distance = 40 - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) + self._scenario_timeout = 240 self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) - exit_wp = generate_target_waypoint_in_route(self._reference_wp, config.route) + + self._middle_entry_wp, exit_wp = self._get_entry_exit_route_lanes(self._reference_wp, config.route) exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match self._other_entry_wp = exit_wp.get_left_lane() if not self._other_entry_wp or self._other_entry_wp.lane_type != carla.LaneType.Driving: raise ValueError("Couldn't find an end position") - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._source_wp = self._map.get_waypoint(self._start_actor_flow) + self._sink_wp = self._map.get_waypoint(self._end_actor_flow) super().__init__("InterurbanActorFlow", ego_vehicles, @@ -509,18 +474,48 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals debug_mode, criteria_enable=criteria_enable) + def _get_entry_exit_route_lanes(self, wp, route): + + entry_wp = None + exit_wp = None + + # Get the middle entry + dist = float('inf') + index = 0 + for route_index, route_pos in enumerate(route): + route_location = route_pos[0].location + trigger_location = wp.transform.location + + route_dist = trigger_location.distance(route_location) + if route_dist <= dist: + index = route_index + dist = route_dist + + for i in range(index, len(route)): + route_transform, road_option = route[i] + + # Enter the junction + if not reached_junction and (road_option in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): + reached_junction = True + entry_wp = self._map.get_waypoint(route[i - 1][0].location) + + # End condition for the behavior, at the end of the junction + if reached_junction and (road_option not in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): + exit_wp = self._map.get_waypoint(route_transform.location) + break + + return (entry_wp, exit_wp) + + def _create_behavior(self): """ Create an actor flow at the opposite lane which the ego has to cross """ - source_wp = self._map.get_waypoint(self._start_actor_flow) - sink_wp = self._map.get_waypoint(self._end_actor_flow) - root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) + self._source_wp, self._sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) @@ -530,7 +525,7 @@ def _create_behavior(self): sequence.add_child(HandleJunctionScenario( clear_junction=False, clear_ego_entry=False, - remove_entries=[source_wp, self._other_entry_wp], + remove_entries=[self._source_wp, self._middle_entry_wp, self._other_entry_wp], remove_exits=[], stop_entries=False, extend_road_exit=0 @@ -576,31 +571,17 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._start_actor_flow_1 = convert_dict_to_location(config.other_parameters['start_actor_flow']) - self._end_actor_flow_1 = convert_dict_to_location(config.other_parameters['end_actor_flow']) - self._sink_distance = 2 - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) self._exit_wp = generate_target_waypoint_in_route(self._reference_wp, config.route) - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._start_actor_flow_1 = convert_dict_to_location(config.other_parameters['start_actor_flow']) + self._end_actor_flow_1 = convert_dict_to_location(config.other_parameters['end_actor_flow']) + + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) + self._scenario_timeout = 240 super().__init__("InterurbanAdvancedActorFlow", ego_vehicles, @@ -644,10 +625,30 @@ def _create_behavior(self): if current_wp.is_junction: break + # Get the junction entry lane (1) + entry_wp_1 = source_wp_1 + while True: + next_wps = entry_wp_1.next(1) + if not next_wps: + break + if next_wps[0].is_junction: + break + entry_wp_1 = next_wps[0] + + # Get the junction entry lane (1) + entry_wp_2 = source_wp_2 + while True: + next_wps = entry_wp_2.next(1) + if not next_wps: + break + if next_wps[0].is_junction: + break + entry_wp_2 = next_wps[0] + sequence.add_child(HandleJunctionScenario( clear_junction=True, clear_ego_entry=True, - remove_entries=[source_wp_1, source_wp_2], + remove_entries=[entry_wp_1, entry_wp_2], remove_exits=[self._exit_wp], stop_entries=False, extend_road_exit=extra_space diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 17eaa5e69..4fa4192fc 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -144,9 +144,11 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind self.inactive_entry_keys = [] self.inactive_exit_keys = [] - def contains(self, other_junction): + def contains_wp(self, wp): """Checks whether or not a carla.Junction is part of the class""" - other_id = other_junction.id + if not wp.is_junction: + return False + other_id = wp.get_junction().id for junction in self.junctions: if other_id == junction.id: return True @@ -194,7 +196,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._road_front_vehicles = 2 # Amount of vehicles in front of the ego self._road_back_vehicles = 2 # Amount of vehicles behind the ego - self._radius_increase_ratio = 2.3 # Meters the radius increases per m/s of the ego + self._radius_increase_ratio = 1.4 # Meters the radius increases per m/s of the ego self._base_junction_detection = 30 self._detection_ratio = 1.5 # Meters the radius increases per m/s of the ego @@ -225,13 +227,15 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._opposite_route_index = 0 self._opposite_sources_dist = 100 # Distance from the ego to the opposite sources [m] - self._opposite_increase_ratio = 3.0 # Meters the radius increases per m/s of the ego - self._opposite_spawn_dist = 50 # Distance between spawned vehicles [m] + self._opposite_spawn_dist = 40 # Distance between spawned vehicles [m] self._active_opposite_sources = True # Flag to (de)activate all opposite sources # Scenario variables: - self._stopped_road_actors = [] # Actors stopped by a hard break scenario + self._scenario_stopped_actors = [] # Actors stopped by a hard break scenario + self._scenario_max_speed = 0 # Max speed of the Background Activity. Deactivated with a value of 0 + self._scenario_junction_entry = False # Flag indicating the ego is entering a junction + self._scenario_junction_entry_distance = self._road_spawn_dist # Min distance between vehicles and ego self._route_sources_active = True @@ -806,7 +810,7 @@ def _add_actor_dict_element(self, actor_dict, actor, exit_lane_key='', at_oppo_e """ actor_dict[actor] = { 'state': JUNCTION_ENTRY if not exit_lane_key else JUNCTION_EXIT, - 'exit_lane_key': exit_lane_key, + 'exit_lane_key': exit_lane_key, 'at_oppo_entry_lane': at_oppo_entry_lane } @@ -820,7 +824,7 @@ def _switch_to_junction_mode(self, junction): for actor in self._road_dict[lane].actors: # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' self._add_actor_dict_element(junction.actor_dict, actor) - if actor not in self._stopped_road_actors: + if actor not in self._scenario_stopped_actors: self._actors_speed_perc[actor] = 100 for lane_key in self._road_dict: @@ -890,6 +894,9 @@ def _end_junction_behavior(self, junction): # Destroy the rest self._destroy_actor(actor) + # If the junction was part of a scenario, forget about it + self._scenario_junction_entry = False + if not self._active_junctions: self._ego_state = EGO_ROAD self._initialise_opposite_sources() @@ -1057,10 +1064,7 @@ def _update_road_sources(self, prev_ego_index): # Set their initial speed, so that they don't lag behind the ego forward_vec = source.wp.transform.get_forward_vector() - if len(source.actors): - speed = CarlaDataProvider.get_velocity(source.actors[-1]) - else: - speed = self._ego_actor.get_velocity().length() + speed = self._ego_actor.get_velocity().length() actor.set_target_velocity(speed * forward_vec) source.actors.append(actor) @@ -1119,12 +1123,9 @@ def _initialise_opposite_sources(self): else: next_junction_index = self._junctions[0].route_entry_index - ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - source_dist = self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio - ego_accum_dist = self._accum_dist[self._route_index] for i in range(self._route_index, next_junction_index): - if self._accum_dist[i] - ego_accum_dist > source_dist: + if self._accum_dist[i] - ego_accum_dist > self._opposite_sources_dist: self._opposite_route_index = i break if not self._opposite_route_index: @@ -1315,7 +1316,7 @@ def get_road_wp(wp): def get_source_wp(wp): """Moves the wp forward until the lane is wide enough to fit a vehicle""" source_wp = wp - while source_wp.lane_width < self._lane_width_threshold: + while source_wp.lane_width < self._lane_width_threshold + 0.2: source_wps = source_wp.next(1) if not source_wps: return None @@ -1445,16 +1446,13 @@ def _move_opposite_sources(self, prev_index): if prev_index == self._route_index: return - ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - source_dist = self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio - # Get the new route tracking wp oppo_route_index = None last_index = self._junctions[0].route_entry_index if self._junctions else self._route_length - 1 current_accum_dist = self._accum_dist[self._route_index] for i in range(self._opposite_route_index, last_index): accum_dist = self._accum_dist[i] - if accum_dist - current_accum_dist >= source_dist: + if accum_dist - current_accum_dist >= self._opposite_sources_dist: oppo_route_index = i break if not oppo_route_index: @@ -1503,6 +1501,7 @@ def _move_opposite_sources(self, prev_index): def _update_opposite_sources(self): """Checks the opposite actor sources to see if new actors have to be created""" + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) for source in self._opposite_sources: if not source.active: continue @@ -1514,7 +1513,7 @@ def _update_opposite_sources(self): # Calculate distance to the last created actor if len(source.actors) == 0: - distance = self._opposite_spawn_dist + 1 + distance = self._opposite_sources_dist + 1 else: actor_location = CarlaDataProvider.get_location(source.actors[-1]) if not actor_location: @@ -1584,6 +1583,12 @@ def _update_parameters(self): self._junction_source_perc = source_perc py_trees.blackboard.Blackboard().set('BA_ChangeJunctionBehavior', None, True) + # Max speed + max_speed = py_trees.blackboard.Blackboard().get('BA_SetMaxSpeed') + if max_speed is not None: + self._scenario_max_speed = max_speed + py_trees.blackboard.Blackboard().set('BA_SetMaxSpeed', None, True) + # Stop front vehicles stop_data = py_trees.blackboard.Blackboard().get('BA_StopFrontVehicles') if stop_data is not None: @@ -1615,17 +1620,23 @@ def _update_parameters(self): self._leave_space_in_front(leave_space_data) py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) + # Leave space in front + leave_crossing_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveCrossingSpace') + if leave_crossing_space_data is not None: + self._leave_crossing_space(leave_crossing_space_data) + py_trees.blackboard.Blackboard().set('BA_LeaveCrossingSpace', None, True) + # Remove road lane remove_road_lane_data = py_trees.blackboard.Blackboard().get('BA_RemoveRoadLane') if remove_road_lane_data is not None: self._remove_road_lane(remove_road_lane_data) py_trees.blackboard.Blackboard().set('BA_RemoveRoadLane', None, True) - # Re add ego road lane - add_ego_road_lane_data = py_trees.blackboard.Blackboard().get('BA_ReAddEgoRoadLane') - if add_ego_road_lane_data is not None: - self._add_ego_road_lane() - py_trees.blackboard.Blackboard().set('BA_ReAddEgoRoadLane', None, True) + # Readd road lane + readd_road_lane_data = py_trees.blackboard.Blackboard().get('BA_ReAddRoadLane') + if readd_road_lane_data is not None: + self._readd_road_lane(readd_road_lane_data) + py_trees.blackboard.Blackboard().set('BA_ReAddRoadLane', None, True) # Adapt the BA to the junction scenario junction_scenario_data = py_trees.blackboard.Blackboard().get('BA_HandleJunctionScenario') @@ -1657,7 +1668,7 @@ def _stop_road_front_vehicles(self): for actor in self._road_dict[lane].actors: location = CarlaDataProvider.get_location(actor) if location and not self._is_location_behind_ego(location): - self._stopped_road_actors.append(actor) + self._scenario_stopped_actors.append(actor) self._actors_speed_perc[actor] = 0 self._tm.update_vehicle_lights(actor, False) lights = actor.get_light_state() @@ -1669,13 +1680,13 @@ def _start_road_front_vehicles(self): Manages the break scenario, where all road vehicles in front of the ego suddenly stop, wait for a bit, and start moving again. This will never trigger unless done so from outside. """ - for actor in self._stopped_road_actors: + for actor in self._scenario_stopped_actors: self._actors_speed_perc[actor] = 100 self._tm.update_vehicle_lights(actor, True) lights = actor.get_light_state() lights &= ~carla.VehicleLightState.Brake actor.set_light_state(carla.VehicleLightState(lights)) - self._stopped_road_actors = [] + self._scenario_stopped_actors = [] def _move_actors_forward(self, actors, space): """Teleports the actors forward a set distance""" @@ -1747,22 +1758,48 @@ def _switch_route_sources(self, enabled): def _leave_space_in_front(self, space): """Teleports all the vehicles in front of the ego forward""" - if not self._active_junctions: - front_actors = [] - min_distance = float('inf') - for actor in self._road_dict[self._ego_key].actors: - location = CarlaDataProvider.get_location(actor) - if not location or self._is_location_behind_ego(location): - continue + if self._ego_key not in self._road_dict: + return + + if self._active_junctions: + return + + front_actors = [] + min_distance = float('inf') + for actor in self._road_dict[self._ego_key].actors: + location = CarlaDataProvider.get_location(actor) + if not location or self._is_location_behind_ego(location): + continue + + front_actors.append(actor) + distance = location.distance(self._ego_wp.transform.location) + if distance < min_distance: + min_distance = distance + + step = space - min_distance + if step > 0: # Only move them if needed and only the minimum required distance + self._move_actors_forward(front_actors, step) + + def _leave_crossing_space(self, collision_wp): + """Removes all vehicle in the middle of crossing trajectory and stops the nearby ones""" + destruction_dist = 10 + stop_dist = 15 - front_actors.append(actor) - distance = location.distance(self._ego_wp.transform.location) - if distance < min_distance: - min_distance = distance + opposite_wp = collision_wp.get_left_lane() + opposite_loc = opposite_wp.transform.location + if not opposite_wp: + return # Nothing else to do - step = space - min_distance - if step > 0: # Only move them if needed and only the minimum required distance - self._move_actors_forward(front_actors, step) + for actor in list(self._opposite_actors): + location = actor.get_location() + if not location: + continue + + collision_dist = location.distance(opposite_loc) + if collision_dist < destruction_dist: + self._destroy_actor(actor) + elif collision_dist < stop_dist: + actor.set_target_velocity(carla.Vector3D()) def _remove_road_lane(self, lane_wp): """Removes a road lane""" @@ -1776,10 +1813,25 @@ def _remove_road_lane(self, lane_wp): self._destroy_actor(actor) self._road_dict.pop(lane_key, None) - def _add_ego_road_lane(self): + def _readd_road_lane(self, lane_offset): """Adds a ego road lane. This is expected to be used after having previously removed such lane""" - if self._ego_key in list(self._road_dict): - print(f"WARNING: Couldn't add a lane {self._ego_key} as it is already part of the road") + + if lane_offset == 0: + add_lane_wp = self._ego_wp + add_lane_key = self._ego_key + else: + side_wp = self._ego_wp + for _ in range(abs(lane_offset)): + side_wp = side_wp.get_right_lane() if lane_offset > 0 else side_wp.get_left_lane() + if not side_wp: + print(f"WARNING: Couldn't find a lane with the desired offset") + return + + add_lane_wp = side_wp + add_lane_key = get_lane_key(side_wp) + + if add_lane_key in list(self._road_dict): + print(f"WARNING: Couldn't add a lane {add_lane_key} as it is already part of the road") return ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) @@ -1787,7 +1839,7 @@ def _add_ego_road_lane(self): spawn_wps = [] - next_wp = self._ego_wp + next_wp = add_lane_wp for _ in range(self._road_front_vehicles): next_wps = next_wp.next(spawn_dist) if len(next_wps) != 1 or self._is_junction(next_wps[0]): @@ -1796,7 +1848,7 @@ def _add_ego_road_lane(self): spawn_wps.insert(0, next_wp) source_dist = 0 - prev_wp = self._ego_wp + prev_wp = add_lane_wp for _ in range(self._road_back_vehicles): prev_wps = prev_wp.previous(spawn_dist) if len(prev_wps) != 1 or self._is_junction(prev_wps[0]): @@ -1865,9 +1917,8 @@ def handle_actors(actor_list): location = CarlaDataProvider.get_location(actor) if location and not self._is_location_behind_ego(location): self._destroy_actor(actor) - else: - self._actors_speed_perc[actor] = 0 - self._stopped_road_actors.append(actor) + + self._scenario_junction_entry = True if self._active_junctions: for source in self._active_junctions[0].entry_sources: @@ -1992,7 +2043,8 @@ def _initialise_actor(self, actor): """ self._tm.auto_lane_change(actor, False) self._tm.update_vehicle_lights(actor, True) - self._tm.distance_to_leading_vehicle(actor, 5) + self._tm.distance_to_leading_vehicle(actor, 10) + self._tm.vehicle_lane_offset(actor, 0.1) self._actors_speed_perc[actor] = 100 self._all_actors.append(actor) @@ -2067,7 +2119,7 @@ def _is_location_behind_ego(self, location): ego_transform = self._route[self._route_index].transform ego_heading = ego_transform.get_forward_vector() ego_actor_vec = location - ego_transform.location - if ego_heading.x * ego_actor_vec.x + ego_heading.y * ego_actor_vec.y < - 0.17: # 100º + if ego_heading.dot(ego_actor_vec) < - 0.17: # 100º return True return False @@ -2090,33 +2142,33 @@ def _update_road_actors(self): draw_string(self._world, location, string, DEBUG_ROAD, False) # if actor in scenario_actors or self._is_location_behind_ego(location): - if actor in self._stopped_road_actors: + if actor in self._scenario_stopped_actors: continue - if not self._is_location_behind_ego(location): - self._set_road_actor_speed(location, actor) - else: - self._set_road_back_actor_speed(location, actor) + + self._set_road_actor_speed(location, actor) def _set_road_actor_speed(self, location, actor, multiplier=1): """ Changes the speed of the vehicle depending on its distance to the ego. - Capped at 100% for all actors closer than `self._min_radius`, - it gradually reduces for larger distances up to 0. + - Front vehicles: Gradually reduces the speed the further they are. + - Back vehicles: Gradually reduces the speed the further they are to help them catch up to the ego. + - Junction scenario behavior: Don't let vehicles behind the ego surpass it. """ distance = location.distance(self._ego_wp.transform.location) - percentage = (self._max_radius - distance) / (self._max_radius - self._min_radius) * 100 - percentage *= multiplier - percentage = max(min(percentage, 100), 0) - self._actors_speed_perc[actor] = percentage + if not self._is_location_behind_ego(location): + percentage = (self._max_radius - distance) / (self._max_radius - self._min_radius) * 100 + percentage *= multiplier + percentage = max(min(percentage, 100), 0) + elif not self._scenario_junction_entry: + percentage = distance / (self._max_radius - self._min_radius) * 100 + 100 + percentage = max(min(percentage, 200), 0) + else: + ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) + base_percentage = ego_speed / self._ego_target_speed * 100 + true_distance = distance - self._scenario_junction_entry_distance + percentage = true_distance / (self._max_radius - self._min_radius) * 100 + base_percentage + percentage = max(min(percentage, 100), 0) - def _set_road_back_actor_speed(self, location, actor): - """ - Changes the speed of the vehicle depending on its distance to the ego. - The further they are, the higher their speed, helping them catch up to the ego. - """ - distance = location.distance(self._ego_wp.transform.location) - percentage = distance / (self._max_radius - self._min_radius) * 100 + 100 - percentage = max(min(percentage, 200), 0) self._actors_speed_perc[actor] = percentage def _monitor_road_changes(self, prev_route_index): @@ -2235,7 +2287,14 @@ def _update_junction_actors(self): actor_dict = junction.actor_dict exit_dict = junction.exit_dict - for j, actor in enumerate(list(actor_dict)): + + scenario_entry_actor_ids = [] + if self._scenario_junction_entry: + for source in junction.entry_sources: + if get_lane_key(source.wp) in junction.route_entry_keys: + scenario_entry_actor_ids.extend([x.id for x in source.actors]) + + for actor in list(actor_dict): if actor not in actor_dict: continue # Actor was removed during the loop location = CarlaDataProvider.get_location(actor) @@ -2247,10 +2306,14 @@ def _update_junction_actors(self): string = 'J' + str(i+1) + '_' + state[:2] draw_string(self._world, location, string, DEBUG_JUNCTION, False) + # Special scenario actors. Treat them as road actors + if actor.id in scenario_entry_actor_ids: + self._set_road_actor_speed(location, actor) + # Monitor its entry - if state == JUNCTION_ENTRY: + elif state == JUNCTION_ENTRY: actor_wp = self._map.get_waypoint(location) - if self._is_junction(actor_wp) and junction.contains(actor_wp.get_junction()): + if self._is_junction(actor_wp) and junction.contains_wp(actor_wp): if junction.clear_middle: self._destroy_actor(actor) # Don't clutter the junction if a junction scenario is active continue @@ -2302,8 +2365,7 @@ def _update_opposite_actors(self): Updates the opposite actors. This involves tracking their position, removing them if too far behind the ego. """ - ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - max_dist = max(self._opposite_sources_dist + ego_speed * self._opposite_increase_ratio, self._opposite_spawn_dist) + opposite_dist = max(self._opposite_sources_dist, self._opposite_spawn_dist) for actor in list(self._opposite_actors): location = CarlaDataProvider.get_location(actor) if not location: @@ -2312,7 +2374,7 @@ def _update_opposite_actors(self): draw_string(self._world, location, 'O', DEBUG_OPPOSITE, False) distance = location.distance(self._ego_wp.transform.location) - if distance > max_dist and self._is_location_behind_ego(location): + if distance > opposite_dist and self._is_location_behind_ego(location): self._destroy_actor(actor) continue @@ -2328,8 +2390,9 @@ def _set_actors_speed(self): This avoids issues with the speed limits, as newly created actors don't have that information """ for actor, percentage in self._actors_speed_perc.items(): - percentage = min(100, max(0, percentage)) speed = self._ego_target_speed * percentage / 100 + if self._scenario_max_speed: + speed = min(speed, self._scenario_max_speed) self._tm.set_desired_speed(actor, speed) def _remove_actor_info(self, actor): @@ -2342,8 +2405,8 @@ def _remove_actor_info(self, actor): if actor in self._opposite_actors: self._opposite_actors.remove(actor) - if actor in self._stopped_road_actors: - self._stopped_road_actors.remove(actor) + if actor in self._scenario_stopped_actors: + self._scenario_stopped_actors.remove(actor) for opposite_source in self._opposite_sources: if actor in opposite_source.actors: diff --git a/srunner/scenarios/background_activity_parametrizer.py b/srunner/scenarios/background_activity_parametrizer.py index 9da61fcce..f11b8e7b7 100644 --- a/srunner/scenarios/background_activity_parametrizer.py +++ b/srunner/scenarios/background_activity_parametrizer.py @@ -16,6 +16,12 @@ ChangeOppositeBehavior, ChangeJunctionBehavior) +def get_parameter(config, name): + if name in config.other_parameters: + return float(config.other_parameters[name]['value']) + else: + return None + class BackgroundActivityParametrizer(BasicScenario): """ This class holds everything required to change the parameters of the background activity. @@ -30,43 +36,21 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals and instantiate scenario manager """ # Road - self._num_front_vehicles = None - if 'num_front_vehicles' in config.other_parameters: - self._num_front_vehicles = float(config.other_parameters['num_front_vehicles']['value']) - self._num_back_vehicles = None - if 'num_back_vehicles' in config.other_parameters: - self._num_back_vehicles = float(config.other_parameters['num_back_vehicles']['value']) - self._road_spawn_dist = None - if 'road_spawn_dist' in config.other_parameters: - self._road_spawn_dist = float(config.other_parameters['road_spawn_dist']['value']) + self._num_front_vehicles = get_parameter(config, "num_front_vehicles") + self._num_back_vehicles = get_parameter(config, "num_back_vehicles") + self._road_spawn_dist = get_parameter(config, "road_spawn_dist") # Opposite - self._opposite_source_dist = None - if 'opposite_source_dist' in config.other_parameters: - self._opposite_source_dist = float(config.other_parameters['opposite_source_dist']['value']) - self._opposite_max_actors = None - if 'opposite_max_actors' in config.other_parameters: - self._opposite_max_actors = float(config.other_parameters['opposite_max_actors']['value']) - self._opposite_spawn_dist = None - if 'opposite_spawn_dist' in config.other_parameters: - self._opposite_spawn_dist = float(config.other_parameters['opposite_spawn_dist']['value']) - self._opposite_active = None - if 'opposite_active' in config.other_parameters: - self._opposite_active = float(config.other_parameters['opposite_active']['value']) + self._opposite_source_dist = get_parameter(config, "opposite_source_dist") + self._opposite_max_actors = get_parameter(config, "opposite_max_actors") + self._opposite_spawn_dist = get_parameter(config, "opposite_spawn_dist") + self._opposite_active = get_parameter(config, "opposite_active") # Junction - self._junction_source_dist = None - if 'junction_source_dist' in config.other_parameters: - self._junction_source_dist = float(config.other_parameters['junction_source_dist']['value']) - self._junction_max_actors = None - if 'junction_max_actors' in config.other_parameters: - self._junction_max_actors = float(config.other_parameters['junction_max_actors']['value']) - self._junction_spawn_dist = None - if 'junction_spawn_dist' in config.other_parameters: - self._junction_spawn_dist = float(config.other_parameters['junction_spawn_dist']['value']) - self._junction_source_perc = None - if 'junction_source_perc' in config.other_parameters: - self._junction_source_perc = float(config.other_parameters['junction_source_perc']['value']) + self._junction_source_dist = get_parameter(config, "junction_source_dist") + self._junction_max_actors = get_parameter(config, "junction_max_actors") + self._junction_spawn_dist = get_parameter(config, "junction_spawn_dist") + self._junction_source_perc = get_parameter(config, "junction_source_perc") super().__init__("BackgroundActivityParametrizer", ego_vehicles, diff --git a/srunner/scenarios/bycicle_group.py b/srunner/scenarios/bycicle_group.py deleted file mode 100644 index f1c8e3854..000000000 --- a/srunner/scenarios/bycicle_group.py +++ /dev/null @@ -1,97 +0,0 @@ -#!/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 . - -""" -Scenarios in which another (opposite) vehicle 'illegally' takes -priority, e.g. by running a red traffic light. -""" - -from __future__ import print_function - -import py_trees -import carla - -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, BasicAgentBehavior -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTimeToArrivalToVehicle -from srunner.scenarios.basic_scenario import BasicScenario - - -class BycicleGroup(BasicScenario): - """ - This class holds everything required for a scenario in which another vehicle runs a red light - in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, - ambulances or firetrucks. - """ - - 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 - """ - self._world = world - self._map = CarlaDataProvider.get_map() - self._bycicle_speed = float(config.other_parameters['bycicle_speed']['value']) - self._offset = float(config.other_parameters['bycicle_offset']['value']) - self.timeout = timeout - self._drive_distance = 200 - self._arrival_time = 7 - - super(BycicleGroup, self).__init__("BycicleGroup", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - bycicle_transform = config.other_actors[0].transform - bycicle_wp = self._map.get_waypoint(bycicle_transform.location) - - # Displace the wp to the side - self._displacement = self._offset * bycicle_wp.lane_width / 2 - r_vec = bycicle_wp.transform.get_right_vector() - w_loc = bycicle_wp.transform.location - w_loc += carla.Location(x=self._displacement * r_vec.x, y=self._displacement * r_vec.y) - bycicle_transform = carla.Transform(w_loc, bycicle_wp.transform.rotation) - - bycicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, bycicle_transform) - self.other_actors.append(bycicle) - - def _create_behavior(self): - """ - Hero vehicle is entering a junction in an urban area, at a signalized intersection, - while another actor runs a red lift, forcing the ego to break. - """ - sequence = py_trees.composites.Sequence() - behavior = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - sequence.add_child(InTimeToArrivalToVehicle(self.ego_vehicles[0], self.other_actors[0], self._arrival_time)) - behavior.add_child(BasicAgentBehavior( - self.other_actors[0], target_speed=self._bycicle_speed, opt_dict={'offset': self._displacement})) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - sequence.add_child(behavior) - sequence.add_child(ActorDestroy(self.other_actors[0])) - return sequence - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - return [CollisionTest(self.ego_vehicles[0])] - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index f70021612..06f515210 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -17,13 +17,22 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, ActorTransformSetter, SwitchWrongDirectionTest, - ScenarioTimeout) -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance + ScenarioTimeout, + Idle) +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTriggerDistanceToLocation from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import (ChangeOppositeBehavior, RemoveRoadLane, - ReAddEgoRoadLane) + ReAddRoadLane, + SetMaxSpeed) + + +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default class ConstructionObstacle(BasicScenario): @@ -45,36 +54,65 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - if 'distance' in config.other_parameters: - self._distance = int(config.other_parameters['distance']['value']) - else: - self._distance = 100 + self._trigger_distance = 30 self._takeover_max_dist = 60 - self._drive_distance = self._distance + self._takeover_max_dist + self._drive_distance = self._trigger_distance + self._takeover_max_dist + + self._opposite_wait_duration = 5 self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) self._construction_wp = None self._construction_transforms = [] - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._distance = get_value_parameter(config, 'distance', float, 100) + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._scenario_timeout = 240 + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") super().__init__("ConstructionObstacle", ego_vehicles, config, world, debug_mode, False, criteria_enable) def _initialize_actors(self, config): """Creates all props part of the construction""" + self._spawn_side_prop(self._reference_waypoint) + wps = self._reference_waypoint.next(self._distance) - if not wps: + if not wps: raise ValueError("Couldn't find a viable position to set up the construction actors") - construction_wp = wps[0] - self._create_construction_setup(construction_wp.transform, self._reference_waypoint.lane_width) - - def create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_length=0, cone_offset=0): - """Creates the cones at tthe side""" + self._construction_wp = wps[0] + self._create_construction_setup(self._construction_wp.transform, self._reference_waypoint.lane_width) + + def _spawn_side_prop(self, wp): + """Spawn the accident indication signal""" + prop_wp = wp + while True: + if self._direction == "right": + wp = prop_wp.get_right_lane() + else: + wp = prop_wp.get_right_lane() + if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): + break + prop_wp = wp + + displacement = prop_wp.lane_width / 2 + r_vec = prop_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=0.2) + spawn_transform.rotation.yaw += 90 + signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningconstruction', spawn_transform) + if not signal_prop: + raise ValueError("Couldn't spawn the indication prop asset") + # signal_prop.set_simulate_physics(True) + self.other_actors.append(signal_prop) + + def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_length=0, cone_offset=0): + """Creates the cones at the side""" _dist = 0 while _dist < (cone_length * cone_offset): # Move forward @@ -138,7 +176,7 @@ def _create_construction_setup(self, start_transform, lane_width): side_transform.rotation.yaw += _initial_offset['cones']['yaw'] for i in range(len(_setup['lengths'])): - self.create_cones_side( + self._create_cones_side( side_transform, forward_vector=side_transform.rotation.get_forward_vector(), z_inc=_z_increment, @@ -150,28 +188,54 @@ def _create_construction_setup(self, start_transform, lane_width): def _create_behavior(self): """ - Only behavior here is to wait + Remove the lane that would collide with the construction and add the construction props. + Wait until the ego is close to the construction (and a bit more) before changing the side traffic + Readd the traffic at the end + + Sequence: + - RemoveRoadLane + - TimeoutParallel (Parallel): + - ScenarioTimeout + - Behavior (Sequence): + - ActorTransformSetter (1 for each prop) + - InTriggerDistanceToLocation + - Idle + - BA stuff + - ActorDestory + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - ReAddRoadLane """ - root = py_trees.composites.Sequence() + root = py_trees.composites.Sequence(name="ConstructionObstacle") + if self.route_mode: + root.add_child(RemoveRoadLane(self._reference_waypoint)) + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() for actor, transform in self._construction_transforms: - root.add_child(ActorTransformSetter(actor, transform, True)) + behavior.add_child(ActorTransformSetter(actor, transform, True)) + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._construction_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._opposite_wait_duration)) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(self._max_speed)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) - if not self.route_mode: - return root + if self.route_mode: + root.add_child(SetMaxSpeed(self._max_speed)) + root.add_child(ReAddRoadLane(0)) + + return root - sequence = py_trees.composites.Sequence() - sequence.add_child(RemoveRoadLane(self._reference_waypoint)) - sequence.add_child(root) - sequence.add_child(ReAddEgoRoadLane()) - return sequence def _create_test_criteria(self): """ @@ -196,36 +260,58 @@ class ConstructionObstacleTwoWays(ConstructionObstacle): """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): - if 'frequency' in config.other_parameters: - self._opposite_frequency = float(config.other_parameters['frequency']['value']) - else: - self._opposite_frequency = 200 + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): """ - Only behavior here is to wait + Remove the lane that would collide with the construction and add the construction props. + Wait until the ego is close to the construction (and a bit more) before changing the opposite traffic + Readd the traffic at the end, and allow the ego to invade the opposite lane by deactivating the criteria + + Sequence: + - RemoveRoadLane + - TimeoutParallel (Parallel): + - ScenarioTimeout + - Behavior (Sequence): + - ActorTransformSetter (1 for each prop) + - InTriggerDistanceToLocation + - Idle + - BA stuff + - ActorDestory + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - ReAddRoadLane """ - root = py_trees.composites.Sequence() + root = py_trees.composites.Sequence(name="ConstructionObstacle") + if self.route_mode: + root.add_child(RemoveRoadLane(self._reference_waypoint)) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() for actor, transform in self._construction_transforms: - root.add_child(ActorTransformSetter(actor, transform, True)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + behavior.add_child(ActorTransformSetter(actor, transform, True)) + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._construction_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._opposite_wait_duration)) + if self.route_mode: + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) for i, _ in enumerate(self.other_actors): root.add_child(ActorDestroy(self.other_actors[i])) - if not self.route_mode: - return root - - sequence = py_trees.composites.Sequence() - sequence.add_child(SwitchWrongDirectionTest(False)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - sequence.add_child(RemoveRoadLane(self._reference_waypoint)) - sequence.add_child(root) - sequence.add_child(SwitchWrongDirectionTest(True)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=50)) - sequence.add_child(ReAddEgoRoadLane()) - return sequence + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(spawn_dist=40)) + root.add_child(ReAddRoadLane(0)) + + return root diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py index 4795990b8..e8119a9c5 100644 --- a/srunner/scenarios/cross_bicycle_flow.py +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -36,6 +36,22 @@ def convert_dict_to_location(actor_dict): return location +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + + +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default + class CrossingBicycleFlow(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light @@ -59,30 +75,14 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._end_distance = 40 - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 10 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [5, 7] # m - - if 'green_light_delay' in config.other_parameters: - self._green_light_delay = float(config.other_parameters['green_light_delay']['value']) - else: - self._green_light_delay = 3 # s + self._signalized_junction = False self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._green_light_delay = 5 + self._scenario_timeout = 240 + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 10) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [20, 50]) super().__init__("CrossingBicycleFlow", ego_vehicles, @@ -131,13 +131,15 @@ def _initialize_actors(self, config): elif plan_step == 2 and exit_loc.distance(wp.transform.location) > self._end_dist_flow: break - self._get_traffic_lights(junction, ego_junction_dist) - - def _get_traffic_lights(self, junction, ego_dist): - """Get the traffic light of the junction, mapping their states""" tls = self._world.get_traffic_lights_in_junction(junction.id) if not tls: - raise ValueError("No traffic lights found, use the NonSignalized version instead") + self._signalized_junction = False + else: + self._signalized_junction = True + self._get_traffic_lights(tls, ego_junction_dist) + + def _get_traffic_lights(self, tls, ego_dist): + """Get the traffic light of the junction, mapping their states""" ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] ego_tl = self._world.get_traffic_light(ego_landmark) @@ -165,10 +167,11 @@ def _create_behavior(self): root.add_child(WaitEndIntersection(self.ego_vehicles[0])) # Freeze the traffic lights to allow the flow to populate the junction - tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") - tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) - tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) - root.add_child(tl_freezer_sequence) + if self._signalized_junction: + tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") + tl_freezer_sequence.add_child(TrafficLightFreezer(self._init_tl_dict, duration=self._green_light_delay)) + tl_freezer_sequence.add_child(TrafficLightFreezer(self._flow_tl_dict)) + root.add_child(tl_freezer_sequence) # Add the BackgroundActivity behaviors if not self.route_mode: diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 8d7922d9c..392c7d480 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -15,15 +15,25 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, - WaypointFollower, - BasicAgentBehavior) + ActorTransformSetter, + CutIn, + BasicAgentBehavior, + Idle) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import RemoveRoadLane +from srunner.tools.background_manager import RemoveRoadLane, LeaveSpaceInFront, ReAddRoadLane -class CutInWithStaticVehicle(BasicScenario): + +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + + +class StaticCutIn(BasicScenario): """ Cut in(with static vehicle) scenario synchronizes a vehicle that is parked at a side lane @@ -35,20 +45,32 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals Setup all relevant parameters and create scenario """ self._wmap = CarlaDataProvider.get_map() + self.timeout = timeout + self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._cut_in_distance = 25 - self._blocker_distance = 16 - self._front_distance = 30 - self._adversary_speed = 10.0 # Speed of the adversary [m/s] - self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] - self._min_trigger_dist = 18.0 # Min distance to the collision location that triggers the adversary [m] - self.timeout = timeout - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = "right" - super().__init__("CutInWithStaticVehicle", + + self._reaction_time = 3.0 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 15.0 # Min distance to the collision location that triggers the adversary [m] + + self._back_vehicles = 2 + self._front_vehicles = 3 + self._vehicle_gap = 11 + + self._adversary_end_distance = 70 + + self._side_transforms = [] + self._side_wp = None + + self._attributes = {'base_type': 'car', 'has_lights': True} + + self._blocker_distance = get_value_parameter(config, 'distance', float, 100) + self._speed = get_value_parameter(config, 'speed', float, 60 / 3.6) + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + + super().__init__("StaticCutIn", ego_vehicles, config, world, @@ -60,54 +82,129 @@ def _initialize_actors(self, config): Custom initialization """ # Spawn the blocker vehicle - blocker_wps = self._reference_waypoint.next(self._blocker_distance) - if not blocker_wps: + next_wps = self._reference_waypoint.next(self._blocker_distance) + if not next_wps: raise ValueError("Couldn't find a proper position for the cut in vehicle") - self._blocker_wp = blocker_wps[0] - if self._direction == 'left': - sideblocker_wp = self._blocker_wp.get_left_lane() - else: - sideblocker_wp = self._blocker_wp.get_right_lane() - - self._blocker_actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', sideblocker_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) - if not self._blocker_actor: - raise ValueError("Couldn't spawn the parked actor") - self._blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) - self.other_actors.append(self._blocker_actor) - - collision_wps = self._reference_waypoint.next(self._cut_in_distance) - if not collision_wps: - raise ValueError("Couldn't find a proper position for the cut in vehicle") - self._collision_wp = collision_wps[0] + blocker_wp = next_wps[0] + + # Spawn the vehicles behind the cut in one + for i in range(self._back_vehicles): + # Move to the side + side_wp = blocker_wp.get_left_lane() if self._direction == 'left' else blocker_wp.get_right_lane() + if not side_wp: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + + if i == 1: + self._side_wp = side_wp + + # Spawn the actor + blocker_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', side_wp.transform, 'scenario', attribute_filter=self._attributes) + if not blocker_actor: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't spawn an actor") + blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) - # GGet the parking direction of the car that will be cut in - if self._direction == 'left': - cutin_wp = self._collision_wp.get_left_lane() - else: - cutin_wp = self._collision_wp.get_right_lane() + blocker_actor.set_simulate_physics(False) + blocker_actor.set_location(side_wp.transform.location + carla.Location(z=-500)) + self._side_transforms.append([blocker_actor, side_wp.transform]) + self.other_actors.append(blocker_actor) - self._parked_actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', cutin_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) - if not self._parked_actor: - raise ValueError("Couldn't spawn the parked actor") - self.other_actors.append(self._parked_actor) + # Move to the front + next_wps = blocker_wp.next(self._vehicle_gap) + if not next_wps: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + blocker_wp = next_wps[0] - self._front_wps = self._collision_wp.next(self._front_distance) - self._front_wp = self._front_wps[0] + # Get the cut in behavior + self._collision_wp = blocker_wp + next_wps = self._collision_wp.next(self._adversary_end_distance) + if not next_wps: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + end_wp = next_wps[0] self._plan = [[self._collision_wp, RoadOption.STRAIGHT], - [self._front_wp, RoadOption.STRAIGHT] ] + [end_wp, RoadOption.STRAIGHT]] + + # Spawn the cut in vehicle + side_wp = blocker_wp.get_left_lane() if self._direction == 'left' else blocker_wp.get_right_lane() + if not side_wp: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + + self._adversary_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', side_wp.transform, 'scenario', attribute_filter=self._attributes) + if not self._adversary_actor: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't spawn an actor") + + self._adversary_actor.set_simulate_physics(False) + self._adversary_actor.set_location(side_wp.transform.location + carla.Location(z=-500)) + self._side_transforms.append([self._adversary_actor, side_wp.transform]) + self.other_actors.append(self._adversary_actor) + + # Move to the front + next_wps = blocker_wp.next(self._vehicle_gap) + if not next_wps: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + blocker_wp = next_wps[0] + + # Spawn the vehicles in front of the cut in one + for i in range(self._front_vehicles): + # Move to the side + side_wp = blocker_wp.get_left_lane() if self._direction == 'left' else blocker_wp.get_right_lane() + if not side_wp: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + + # Spawn the actor + blocker_actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', side_wp.transform, 'scenario', attribute_filter=self._attributes) + if not blocker_actor: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't spawn an actor") + blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) + + blocker_actor.set_simulate_physics(False) + blocker_actor.set_location(side_wp.transform.location + carla.Location(z=-500)) + self._side_transforms.append([blocker_actor, side_wp.transform]) + self.other_actors.append(blocker_actor) + + # Move to the front + next_wps = blocker_wp.next(self._vehicle_gap) + if not next_wps: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + blocker_wp = next_wps[0] def _create_behavior(self): """ After invoking this scenario, a parked vehicle will wait for the ego to be close-by, merging into its lane, forcing it to break. """ - sequence = py_trees.composites.Sequence(name="CrossingActor") + sequence = py_trees.composites.Sequence(name="StaticCutIn") if self.route_mode: - other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) - sequence.add_child(RemoveRoadLane(other_car)) + total_dist = self._blocker_distance + total_dist += self._vehicle_gap * (self._back_vehicles + self._front_vehicles + 1) + sequence.add_child(LeaveSpaceInFront(total_dist)) + sequence.add_child(RemoveRoadLane(self._side_wp)) + + for actor, transform in self._side_transforms: + sequence.add_child(ActorTransformSetter(actor, transform)) collision_location = self._collision_wp.transform.location @@ -115,27 +212,32 @@ def _create_behavior(self): trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") trigger_adversary.add_child(InTimeToArrivalToLocation( - self.ego_vehicles[0], self._reaction_time, collision_location)) + self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( - self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) - # The adversary change the lane - sequence.add_child(BasicAgentBehavior(self.other_actors[1], plan=self._plan)) + cut_in_behavior = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="CutIn") + cut_in_direction = 'right' if self._direction == 'left' else 'left' + + cut_in_movement = py_trees.composites.Sequence() + cut_in_movement.add_child(CutIn( + self._adversary_actor, self.ego_vehicles[0], cut_in_direction)) + cut_in_movement.add_child(BasicAgentBehavior( + self._adversary_actor, plan=self._plan, target_speed=3.6 * self._speed)) - # Move the adversary - cut_in = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") - other_car = self._wmap.get_waypoint(self.other_actors[1].get_location()) - sequence.add_child(RemoveRoadLane(other_car)) - cut_in.add_child(WaypointFollower(self.other_actors[1], self._adversary_speed)) + cut_in_behavior.add_child(cut_in_movement) + cut_in_behavior.add_child(Idle(self._adversary_end_distance / self._speed)) - sequence.add_child(cut_in) + sequence.add_child(cut_in_behavior) - # Remove everything - sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) - sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + for actor in self.other_actors: + sequence.add_child(ActorDestroy(actor)) + + if self.route_mode: + sequence.add_child(ReAddRoadLane(1 if self._direction == 'right' else -1)) return sequence @@ -153,5 +255,3 @@ def __del__(self): Remove all actors upon deletion """ self.remove_all_actors() - - diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index e0c08bcf0..676a7f230 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -55,11 +55,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._same_lane_time = 1 + self._same_lane_time = 0.5 self._other_lane_time = 3 self._change_time = 2 self._speed_perc = 80 - self._cut_in_distance = 8 + self._cut_in_distance = 15 self._extra_space = 170 self._start_location = convert_dict_to_location(config.other_parameters['other_actor_location']) diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py index 30fdf6208..a36a83178 100644 --- a/srunner/scenarios/invading_turn.py +++ b/srunner/scenarios/invading_turn.py @@ -20,6 +20,7 @@ ActorDestroy, BasicAgentBehavior, ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import LeaveSpaceInFront @@ -59,6 +60,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._reference_waypoint = self._map.get_waypoint( self._trigger_location) + self._speed = 30 # Km/h + # Distance between the trigger point and the start location of the adversary if 'distance' in config.other_parameters: self._distance = float( @@ -75,10 +78,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl else: self._offset = 0.5 - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._scenario_timeout = 240 super(InvadingTurn, self).__init__("InvadingTurn", ego_vehicles, @@ -140,7 +140,7 @@ def _create_behavior(self): behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) behavior.add_child(BasicAgentBehavior( - self.other_actors[0], self._adversary_end, opt_dict={'offset': self._offset})) + self.other_actors[0], self._adversary_end, target_speed=self._speed, opt_dict={'offset': self._offset})) behavior.add_child(DriveDistance(self.ego_vehicles[0], self._distance)) behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index a3e63e91a..9509ae6f5 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -26,7 +26,7 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import LeaveCrossingSpace def get_sidewalk_transform(waypoint): @@ -139,7 +139,7 @@ def _initialize_actors(self, config): break if self._number_of_attempts == 0: - raise Exception("Couldn't find viable position for the adversary actor") + raise ValueError("Couldn't find viable position for the adversary") if isinstance(adversary, carla.Vehicle): adversary.apply_control(carla.VehicleControl(hand_brake=True)) @@ -173,6 +173,8 @@ def _create_behavior(self): # Move the adversary. speed_duration = 2.0 * collision_duration speed_distance = 2.0 * collision_distance + if self.route_mode: + sequence.add_child(LeaveCrossingSpace(self._collision_wp)) sequence.add_child(KeepVelocity( self.other_actors[0], self._adversary_speed, True, speed_duration, speed_distance, name="AdversaryCrossing") diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index b2a614048..ad5eb06f9 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -25,7 +25,14 @@ from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_location_in_distance_from_wp -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import LeaveSpaceInFront, LeaveCrossingSpace + + +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default class StationaryObjectCrossing(BasicScenario): @@ -148,24 +155,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) self._num_lane_changes = 0 - if 'distance' in config.other_parameters: - self._distance = int(config.other_parameters['distance']['value']) - else: - self._distance = 12 - - if 'blocker_model' in config.other_parameters: - self._blocker_model = config.other_parameters['blocker_model']['value'] - else: - self._blocker_model = 'static.prop.vendingmachine' # blueprint filter of the blocker - - if 'crossing_angle' in config.other_parameters: - self._crossing_angle = float(config.other_parameters['crossing_angle']['value']) - else: - self._crossing_angle = 0 # Crossing angle of the pedestrian - - if abs(self._crossing_angle) > 90: - raise ValueError("'crossing_angle' must be between -90 and 90º for the pedestrian to cross the road") - self._blocker_shift = 0.9 self._retry_dist = 0.4 @@ -174,13 +163,22 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._collision_wp = None self._adversary_speed = 2.0 # Speed of the adversary [m/s] - self._reaction_time = 1.8 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.1 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout self._number_of_attempts = 6 + self._distance = get_value_parameter(config, 'distance', float, 12) + self._blocker_model = get_value_parameter(config, 'blocker_model', str, 'static.prop.vendingmachine') + self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) + if abs(self._crossing_angle) > 90: + raise ValueError("'crossing_angle' must be between -90 and 90º for the pedestrian to cross the road") + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + super(DynamicObjectCrossing, self).__init__("DynamicObjectCrossing", ego_vehicles, config, @@ -194,6 +192,9 @@ def _get_sidewalk_transform(self, waypoint, offset): It first rotates the transform so that it is pointing towards the road and then moves a bit to the side waypoint that aren't part of sidewalks, as they might be invading the road """ + if self._direction == "left": + offset['yaw'] *= -1 + offset['k'] *= -1 new_rotation = waypoint.transform.rotation new_rotation.yaw += offset['yaw'] @@ -227,19 +228,23 @@ def _initialize_actors(self, config): # Move to the right sidewalk_waypoint = waypoint while sidewalk_waypoint.lane_type != carla.LaneType.Sidewalk: - right_wp = sidewalk_waypoint.get_right_lane() - if right_wp is None: - break # No more right lanes - sidewalk_waypoint = right_wp + if self._direction == "right": + side_wp = sidewalk_waypoint.get_right_lane() + else: + side_wp = sidewalk_waypoint.get_left_lane() + if side_wp is None: + break # No more side lanes + sidewalk_waypoint = side_wp # Get the blocker transform and spawn it - offset = {"yaw": 90, "z": 0.0, "k": 1.5} + offset = {"yaw": 0 if 'vehicle' in self._blocker_model else 90, "z": 0.0, "k": 1.5} self._blocker_transform = self._get_sidewalk_transform(sidewalk_waypoint, offset) - blocker = CarlaDataProvider.request_new_actor(self._blocker_model, self._blocker_transform) + blocker = CarlaDataProvider.request_new_actor( + self._blocker_model, self._blocker_transform, rolename="scenario no lights") if not blocker: self._number_of_attempts -= 1 move_dist = self._retry_dist - print("Failed blocker") + print("Failed to spawn the blocker") continue # Get the adversary transform and spawn it @@ -256,7 +261,7 @@ def _initialize_actors(self, config): blocker.destroy() self._number_of_attempts -= 1 move_dist = self._retry_dist - print("Failed adversary") + print("Failed to spawn an adversary") continue self._collision_dist += waypoint.transform.location.distance(self._adversary_transform.location) @@ -296,6 +301,8 @@ def _create_behavior(self): # Move the adversary move_distance = 2 * self._collision_dist # Cross the whole road (supposing symetry in both directions) move_duration = move_distance / self._adversary_speed + if self.route_mode: + sequence.add_child(LeaveCrossingSpace(self._collision_wp)) sequence.add_child(KeepVelocity( self.other_actors[0], self._adversary_speed, duration=move_duration, distance=move_distance, name="AdversaryCrossing")) @@ -344,25 +351,22 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) self._num_lane_changes = 0 - if 'distance' in config.other_parameters: - self._distance = int(config.other_parameters['distance']['value']) - else: - self._distance = 12 - - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = 'right' - - if self._direction not in ('right', 'left'): - raise ValueError("'direction' value must be either 'left' or 'right'") - - self._adversary_speed = 3.0 # Speed of the adversary [m/s] - self._reaction_time = 1.9 # Time the agent has to react to avoid the collision [s] + self._adversary_speed = 2.0 # Speed of the adversary [m/s] + self._reaction_time = 2.1 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout + self._bp_attributes = {'base_type': 'car', 'has_lights': False} + + self._distance = get_value_parameter(config, 'distance', float, 12) + self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) + if abs(self._crossing_angle) > 90: + raise ValueError("'crossing_angle' must be between -90 and 90º for the pedestrian to cross the road") + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + super().__init__("ParkingCrossingPedestrian", ego_vehicles, config, @@ -389,7 +393,7 @@ def _get_walker_transform(self, waypoint): """Processes the driving wp to get a waypoint at the side that looks at the road""" new_rotation = waypoint.transform.rotation - new_rotation.yaw += 270 if self._direction == 'right' else 90 + new_rotation.yaw += 270 - self._crossing_angle if self._direction == 'right' else 90 + self._crossing_angle if waypoint.lane_type == carla.LaneType.Sidewalk: new_location = waypoint.transform.location @@ -416,7 +420,8 @@ def _initialize_actors(self, config): # Get the adversary transform and spawn it self._blocker_transform = self._get_blocker_transform(blocker_wp) - blocker = CarlaDataProvider.request_new_actor('vehicle.*', self._blocker_transform, attribute_filter={'base_type': 'car'}) + blocker = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._blocker_transform, attribute_filter=self._bp_attributes) if blocker is None: raise ValueError("Couldn't spawn the adversary") self.other_actors.append(blocker) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index b14f8c563..602d686d9 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -19,7 +19,8 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, ActorDestroy, TrafficLightFreezer, - ConstantVelocityAgentBehavior) + ConstantVelocityAgentBehavior, + Idle) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -28,16 +29,17 @@ from srunner.tools.scenario_helper import (get_geometric_linear_intersection, generate_target_waypoint, get_junction_topology, - filter_junction_wp_direction) + filter_junction_wp_direction, + get_closest_traffic_light) from srunner.tools.background_manager import HandleJunctionScenario -class OppositeVehicleRunningRedLight(BasicScenario): + +class OppositeVehicleJunction(BasicScenario): """ - This class holds everything required for a scenario in which another vehicle runs a red light - in front of the ego, forcing it to react. This vehicles are 'special' ones such as police cars, - ambulances or firetrucks. + Scenario in which another vehicle enters the junction a tthe same time as the ego, + forcing it to break to avoid a collision """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -49,28 +51,23 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self._source_dist = 30 - self._sink_dist = 20 + self._sink_dist = 10 + self._adversary_speed = 60 / 3.6 # m/s if 'direction' in config.other_parameters: self._direction = config.other_parameters['direction']['value'] else: self._direction = "right" - if 'adversary_speed' in config.other_parameters: - self._adversary_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._adversary_speed = 70 / 3.6 # m/s self.timeout = timeout self._sync_time = 2.2 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 9.0 # Min distance to the collision location that triggers the adversary [m] - self._speed_duration_ratio = 2.0 - self._speed_distance_ratio = 1.5 self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 - super().__init__("OppositeVehicleRunningRedLight", + super().__init__("OppositeVehicleJunction", ego_vehicles, config, world, @@ -93,10 +90,10 @@ def _initialize_actors(self, config): raise ValueError("Failed to find junction as a waypoint with no next was detected") starting_wp = starting_wps[0] ego_junction_dist += 1 - junction = starting_wp.get_junction() + self._junction = starting_wp.get_junction() # Get the opposite entry lane wp - entry_wps, _ = get_junction_topology(junction) + entry_wps, _ = get_junction_topology(self._junction) source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) if not source_entry_wps: raise ValueError("Couldn't find a lane for the given direction") @@ -154,21 +151,49 @@ def _initialize_actors(self, config): collision_wp = self._map.get_waypoint(self._collision_location) self._collision_location.z = collision_wp.transform.location.z - self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + def _create_behavior(self): + raise NotImplementedError("Found missing behavior") + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + if self.route_mode: + return [] + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors and traffic lights upon deletion + """ + self.remove_all_actors() + + +class OppositeVehicleRunningRedLight(OppositeVehicleJunction): + """ + Signalized junction version, where the other vehicle runs a red light + """ + + 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 + """ + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) - def _get_traffic_lights(self, junction, ego_dist, source_dist): - """Get the traffic light of the junction, mapping their states""" - tls = self._world.get_traffic_lights_in_junction(junction.id) - if not tls: - raise ValueError("No traffic lights found, use the NonSignalized version instead") + def _initialize_actors(self, config): + """ + Custom initialization + """ + super()._initialize_actors(config) - ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] - ego_tl = self._world.get_traffic_light(ego_landmark) - source_landmark = self._spawn_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] - source_tl = self._world.get_traffic_light(source_landmark) + tls = self._world.get_traffic_lights_in_junction(self._junction.id) + ego_tl = get_closest_traffic_light(self._ego_wp, tls) self._tl_dict = {} for tl in tls: - if tl.id in (ego_tl.id, source_tl.id): + if tl == ego_tl: self._tl_dict[tl] = carla.TrafficLightState.Green else: self._tl_dict[tl] = carla.TrafficLightState.Red @@ -178,7 +203,7 @@ def _create_behavior(self): Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ - sequence = py_trees.composites.Sequence(name="OppositeVehicleTakingPriority") + sequence = py_trees.composites.Sequence(name="OppositeVehicleRunningRedLight") # Wait until ego is close to the adversary trigger_adversary = py_trees.composites.Parallel( @@ -189,13 +214,26 @@ def _create_behavior(self): self.ego_vehicles[0], self._collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) - sequence.add_child(ConstantVelocityAgentBehavior( - self.other_actors[0], target_location=self._sink_wp.transform.location, - target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, name="AdversaryCrossing")) + + + end_location = self._sink_wp.transform.location + start_location = self._spawn_wp.transform.location + time = start_location.distance(end_location) / self._adversary_speed main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - main_behavior.add_child(TrafficLightFreezer(self._tl_dict)) - main_behavior.add_child(sequence) + main_behavior.add_child(ConstantVelocityAgentBehavior( + self.other_actors[0], target_location=end_location, + target_speed=self._adversary_speed, + opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, + name="AdversaryCrossing") + ) + main_behavior.add_child(Idle(time)) + + sequence.add_child(main_behavior) + + tls_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + tls_behavior.add_child(TrafficLightFreezer(self._tl_dict)) + tls_behavior.add_child(sequence) root = py_trees.composites.Sequence() if self.route_mode: @@ -208,33 +246,16 @@ def _create_behavior(self): extend_road_exit=0 )) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) - root.add_child(main_behavior) + root.add_child(tls_behavior) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) return root - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() - -class OppositeVehicleTakingPriority(BasicScenario): +class OppositeVehicleTakingPriority(OppositeVehicleJunction): """ - This class holds everything required for a scenario in which another vehicle takes - priority at an intersection, forcing the ego to break. - This vehicles are 'special' ones such as police cars, ambulances or firetrucks. + Non signalized version """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -243,116 +264,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals Setup all relevant parameters and create scenario and instantiate scenario manager """ - self._world = world - self._map = CarlaDataProvider.get_map() - self._source_dist = 30 - self._sink_dist = 20 - - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = "right" - - if 'adversary_speed' in config.other_parameters: - self._adversary_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._adversary_speed = 70 / 3.6 # m/s - - self._opposite_bp_wildcards = ['*firetruck*', '*ambulance*', '*police*'] # Wildcard patterns of the blueprints - self.timeout = timeout - - self._sync_time = 2.2 # Time the agent has to react to avoid the collision [s] - self._min_trigger_dist = 9.0 # Min distance to the collision location that triggers the adversary [m] - self._speed_duration_ratio = 2.0 - self._speed_distance_ratio = 1.5 - - self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 - - # Get the CDP seed or at routes, all copies of the scenario will have the same configuration - self._rng = CarlaDataProvider.get_random_seed() - - super().__init__("OppositeVehicleTakingPriority", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - ego_location = config.trigger_points[0].location - self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) - - # Get the junction - starting_wp = self._ego_wp - ego_junction_dist = 0 - while not starting_wp.is_junction: - starting_wps = starting_wp.next(1.0) - if len(starting_wps) == 0: - raise ValueError("Failed to find junction as a waypoint with no next was detected") - starting_wp = starting_wps[0] - ego_junction_dist += 1 - junction = starting_wp.get_junction() - - # Get the opposite entry lane wp - entry_wps, _ = get_junction_topology(junction) - source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) - if not source_entry_wps: - raise ValueError("Couldn't find a lane for the given direction") - - # Get the source transform - spawn_wp = source_entry_wps[0] - source_junction_dist = 0 - while source_junction_dist < self._source_dist: - spawn_wps = spawn_wp.previous(1.0) - if len(spawn_wps) == 0: - raise ValueError("Failed to find a source location as a waypoint with no previous was detected") - if spawn_wps[0].is_junction: - break - spawn_wp = spawn_wps[0] - source_junction_dist += 1 - self._spawn_wp = spawn_wp - - source_transform = spawn_wp.transform - self._spawn_location = carla.Transform( - source_transform.location + carla.Location(z=0.1), - source_transform.rotation - ) - - # Spawn the actor and move it below ground - opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards) - opposite_actor = CarlaDataProvider.request_new_actor(opposite_bp_wildcard, self._spawn_location) - if not opposite_actor: - raise Exception("Couldn't spawn the actor") - opposite_actor.set_light_state(carla.VehicleLightState( - carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) - self.other_actors.append(opposite_actor) - - opposite_transform = carla.Transform( - source_transform.location - carla.Location(z=500), - source_transform.rotation - ) - opposite_actor.set_transform(opposite_transform) - opposite_actor.set_simulate_physics(enabled=False) - - # Get the sink location - sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) - sink_wps = sink_exit_wp.next(self._sink_dist) - if len(sink_wps) == 0: - raise ValueError("Failed to find a sink location as a waypoint with no next was detected") - self._sink_wp = sink_wps[0] - - # get the collision location - self._collision_location = get_geometric_linear_intersection( - starting_wp.transform.location, source_entry_wps[0].transform.location, True) - if not self._collision_location: - raise ValueError("Couldn't find an intersection point") - - # Get the z component - collision_wp = self._map.get_waypoint(self._collision_location) - self._collision_location.z = collision_wp.transform.location.z + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): """ @@ -370,12 +282,21 @@ def _create_behavior(self): self.ego_vehicles[0], self._collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) - sequence.add_child(ConstantVelocityAgentBehavior( - self.other_actors[0], target_location=self._sink_wp.transform.location, - target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, name="AdversaryCrossing")) + + end_location = self._sink_wp.transform.location + start_location = self._spawn_wp.transform.location + time = start_location.distance(end_location) / self._adversary_speed main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - main_behavior.add_child(sequence) + main_behavior.add_child(ConstantVelocityAgentBehavior( + self.other_actors[0], target_location=end_location, + target_speed=self._adversary_speed, + opt_dict={'ignore_vehicles': True, 'ignore_traffic_lights': True}, + name="AdversaryCrossing") + ) + main_behavior.add_child(Idle(time)) + + sequence.add_child(main_behavior) root = py_trees.composites.Sequence() if self.route_mode: @@ -389,23 +310,8 @@ def _create_behavior(self): )) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) - root.add_child(main_behavior) + root.add_child(sequence) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) return root - - def _create_test_criteria(self): - """ - A list of all test criteria will be created that is later used - in parallel behavior tree. - """ - if self.route_mode: - return [] - return [CollisionTest(self.ego_vehicles[0])] - - def __del__(self): - """ - Remove all actors and traffic lights upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index fcc8bcfee..6e77ea1dd 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -44,7 +44,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_speed = 13.0 # Speed of the adversary [m/s] self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 10.0 # Min distance to the collision location that triggers the adversary [m] - self._end_distance = 40 + self._end_distance = 30 self.timeout = timeout if 'direction' in config.other_parameters: @@ -125,7 +125,7 @@ def _create_behavior(self): After invoking this scenario, a parked vehicle will wait for the ego to be close-by, merging into its lane, forcing it to break. """ - sequence = py_trees.composites.Sequence(name="CrossingActor") + sequence = py_trees.composites.Sequence(name="ParkingCutIn") if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._cut_in_distance + 10)) @@ -143,7 +143,7 @@ def _create_behavior(self): # Move the adversary cut_in = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") - cut_in.add_child(BasicAgentBehavior(self.other_actors[1])) + cut_in.add_child(BasicAgentBehavior(self.other_actors[1], opt_dict={'ignore_traffic_lights': True})) cut_in.add_child(DriveDistance(self.other_actors[1], self._end_distance)) sequence.add_child(cut_in) diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 8d9e77759..1f886a472 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -39,6 +39,13 @@ def convert_dict_to_location(actor_dict): return location +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + + class ParkingExit(BasicScenario): """ This class holds everything required for a scenario in which the ego would be teleported to the parking lane. @@ -64,29 +71,20 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl CarlaDataProvider.get_traffic_manager_port()) self.timeout = timeout - if 'front_vehicle_distance' in config.other_parameters: - self._front_vehicle_distance = float( - config.other_parameters['front_vehicle_distance']['value']) - else: - self._front_vehicle_distance = 20 # m - - if 'behind_vehicle_distance' in config.other_parameters: - self._behind_vehicle_distance = float( - config.other_parameters['behind_vehicle_distance']['value']) - else: - self._behind_vehicle_distance = 5 # m + self._bp_attributes = {'base_type': 'car'} + self._side_end_distance = 50 - self._end_distance = self._front_vehicle_distance + 15 + self._front_vehicle_distance = get_value_parameter(config, 'front_vehicle_distance', float, 20) + self._behind_vehicle_distance = get_value_parameter(config, 'behind_vehicle_distance', float, 10) + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = "right" + self._flow_distance = get_value_parameter(config, 'flow_distance', float, 25) + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._scenario_timeout = 120 - if 'flow_distance' in config.other_parameters: - self._flow_distance = float(config.other_parameters['flow_distance']['value']) - else: - self._flow_distance = 25 + self._end_distance = self._front_vehicle_distance + 15 # Get parking_waypoint based on trigger_point self._trigger_location = config.trigger_points[0].location @@ -100,21 +98,12 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl raise Exception( "Couldn't find parking point on the {} side".format(self._direction)) - self._bp_attributes = {'base_type': 'car', 'has_lights': False} - - self._side_end_distance = 50 - - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 120 - - super(ParkingExit, self).__init__("ParkingExit", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + super().__init__("ParkingExit", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ @@ -128,10 +117,11 @@ def _initialize_actors(self, config): "Couldn't find viable position for the vehicle in front of the parking point") actor_front = CarlaDataProvider.request_new_actor( - 'vehicle.*', front_points[0].transform, rolename='scenario', attribute_filter=self._bp_attributes) + 'vehicle.*', front_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_front is None: raise ValueError( "Couldn't spawn the vehicle in front of the parking point") + actor_front.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(actor_front) # And move it to the side @@ -146,10 +136,12 @@ def _initialize_actors(self, config): "Couldn't find viable position for the vehicle behind the parking point") actor_behind = CarlaDataProvider.request_new_actor( - 'vehicle.*', behind_points[0].transform, rolename='scenario', attribute_filter=self._bp_attributes) + 'vehicle.*', behind_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_behind is None: + actor_front.destroy() raise ValueError( "Couldn't spawn the vehicle behind the parking point") + actor_behind.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(actor_behind) # And move it to the side @@ -195,7 +187,7 @@ def _create_behavior(self): After ego drives away, activate OutsideRouteLanesTest, end scenario. """ - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="ParkingExit") sequence.add_child(ChangeRoadBehavior(spawn_dist=self._flow_distance)) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index 4d786df15..f48b2619f 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -12,13 +12,17 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import WalkerFlow, AIWalkerBehavior +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + InTimeToArrivalToLocation, + DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import HandleJunctionScenario +from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes + def convert_dict_to_location(actor_dict): """ @@ -50,89 +54,127 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl """ self._wmap = CarlaDataProvider.get_map() self._trigger_location = config.trigger_points[0].location - self._reference_waypoint = self._wmap.get_waypoint( - self._trigger_location) - - # Get the start point of the initial pedestrian - sidewalk_waypoint = self._reference_waypoint - while sidewalk_waypoint.lane_type != carla.LaneType.Sidewalk: - right_wp = sidewalk_waypoint.get_right_lane() - if right_wp is None: - raise RuntimeError("Can't find sidewalk to spawn pedestrian") - sidewalk_waypoint = right_wp - self._init_walker_start = sidewalk_waypoint.next_until_lane_end( - 1)[-1].transform.location - self._init_walker_start.z += 1 - - # The initial pedestrian will walk to end_walker_flow_1 - self._init_walker_end = convert_dict_to_location( - config.other_parameters['end_walker_flow_1']) - - self._start_walker_flow = convert_dict_to_location( - config.other_parameters['start_walker_flow']) - self._sink_locations = [] - self._sink_locations_prob = [] - - end_walker_flow_list = [ - v for k, v in config.other_parameters.items() if 'end_walker_flow' in k] - - for item in end_walker_flow_list: - self._sink_locations.append(convert_dict_to_location(item)) - prob = float(item['p']) if 'p' in item else 0.5 - self._sink_locations_prob.append(prob) - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [2, 8] # m + self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + self._adversary_speed = 1.1 # Speed of the adversary [m/s] + self._reaction_time = 4.0 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 12.0 # Min distance to the collision location that triggers the adversary [m] + self._ego_end_distance = 40 self.timeout = timeout - super(PedestrianCrossing, self).__init__("PedestrianCrossing", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + self._crosswalk_dist = 1 + self._crosswalk_right_dist = 1.5 + + super().__init__("PedestrianCrossing", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + + # Get the start point of the initial pedestrian + collision_wp = self._reference_waypoint + while True: + next_wps = collision_wp.next(1) + if not next_wps: + raise ValueError("Couldn't find a waypoint to spawn the pedestrians") + if next_wps[0].is_junction: + break + collision_wp = next_wps[0] + + self._collision_wp = collision_wp + + # Get the crosswalk start point + start_wp = collision_wp + while start_wp.lane_type != carla.LaneType.Sidewalk: + wp = start_wp.get_right_lane() + if wp is None: + raise ValueError("Couldn't find a waypoint to start the flow") + start_wp = wp + + # Displace it to the crosswalk. Move forwards towards the crosswalk + start_vec = start_wp.transform.get_forward_vector() + start_right_vec = start_wp.transform.get_right_vector() + + spawn_loc = start_wp.transform.location + carla.Location( + self._crosswalk_dist * start_vec.x + self._crosswalk_right_dist * start_right_vec.x, + self._crosswalk_dist * start_vec.y + self._crosswalk_right_dist * start_right_vec.y, + self._crosswalk_dist * start_vec.z + self._crosswalk_right_dist * start_right_vec.z + 0.5 + ) + + # left_wp = get_opposite_dir_lanes(collision_wp)[-1] + + # # Get the crosswalk end point + # end_wp = left_wp + # while end_wp.lane_type != carla.LaneType.Sidewalk: + # wp = end_wp.get_right_lane() + # if wp is None: + # raise ValueError("Couldn't find a waypoint to start the flow") + # end_wp = wp + + # # Displace it to the crosswalk. Move backwards towards the crosswalk + # end_vec = end_wp.transform.get_forward_vector() + # end_right_vec = end_wp.transform.get_right_vector() + + # end_loc = end_wp.transform.location + carla.Location( + # - self._crosswalk_dist * end_vec.x + self._crosswalk_right_dist * end_right_vec.x, + # - self._crosswalk_dist * end_vec.y + self._crosswalk_right_dist * end_right_vec.y, + # - self._crosswalk_dist * end_vec.z + self._crosswalk_right_dist * end_right_vec.z + # ) + + spawn_rotation = start_wp.transform.rotation + spawn_rotation.yaw += 270 + spawn_transform = carla.Transform(spawn_loc, spawn_rotation) + + adversary = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) + if adversary is None: + raise ValueError("Failed to spawn an adversary") + + self._collision_dist = spawn_transform.location.distance(self._collision_wp.transform.location) + + self.other_actors.append(adversary) def _create_behavior(self): """ - After invoking this scenario, pedestrians will start to walk to two different destinations randomly, and some of them will cross the road. - Pedestrians will be removed when they arrive at their destinations. - - Ego is expected to cross the junction when there is enough space to go through. - Ego is not expected to wait for pedestrians crossing the road for a long time. + After invoking this scenario, cyclist will wait for the user + controlled vehicle to enter trigger distance region, + the cyclist starts crossing the road once the condition meets, + then after 60 seconds, a timeout stops the scenario """ - sequence = py_trees.composites.Sequence(name="CrossingPedestrian") + sequence = py_trees.composites.Sequence(name="CrossingActor") if self.route_mode: sequence.add_child(HandleJunctionScenario( - clear_junction=True, + clear_junction=False, clear_ego_entry=True, remove_entries=[], remove_exits=[], - stop_entries=True, + stop_entries=False, extend_road_exit=0 )) - # Move the adversary - root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) + collision_location = self._collision_wp.transform.location - root.add_child(AIWalkerBehavior( - self._init_walker_start, self._init_walker_end)) + # Wait until ego is close to the adversary + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + sequence.add_child(trigger_adversary) - walker_root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="PedestrianMove") - walker_root.add_child(WalkerFlow( - self._start_walker_flow, self._sink_locations, self._sink_locations_prob, self._source_dist_interval)) - walker_root.add_child(WaitEndIntersection(self.ego_vehicles[0])) - - root.add_child(walker_root) + # Move the adversary + move_distance = 2 * self._collision_dist # Cross the whole road (supposing symetry in both directions) + move_duration = move_distance / self._adversary_speed + sequence.add_child(KeepVelocity( + self.other_actors[0], self._adversary_speed, + duration=move_duration, distance=move_distance, name="AdversaryCrossing")) - sequence.add_child(root) + # Remove everything + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) return sequence @@ -143,4 +185,4 @@ def _create_test_criteria(self): """ if self.route_mode: return [] - return [CollisionTest(self.ego_vehicles[0])] + return [CollisionTest(self.ego_vehicles[0])] \ No newline at end of file diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index a93106e81..eed6b1ff5 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -18,12 +18,24 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, SwitchWrongDirectionTest, - ConstantVelocityAgentBehavior, - ScenarioTimeout) + BasicAgentBehavior, + ScenarioTimeout, + Idle, + HandBrakeVehicle) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (DriveDistance, + InTriggerDistanceToLocation, + InTriggerDistanceToVehicle, + WaitUntilInFront) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior, SetMaxSpeed + + +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default class Accident(BasicScenario): @@ -42,61 +54,97 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - if 'distance' in config.other_parameters: - self._distance = int(config.other_parameters['distance']['value']) - else: - self._distance = 120 - - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = 'right' - if self._direction not in ('left', 'right'): - raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") - - self._offset = 0.75 + self._offset = 0.6 self._first_distance = 10 self._second_distance = 6 + self._trigger_distance = 30 self._takeover_max_dist = self._first_distance + self._second_distance + 40 - self._drive_distance = self._distance + self._takeover_max_dist + self._drive_distance = self._trigger_distance + self._takeover_max_dist - self._accident_wp = None + self._wait_duration = 5 self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._distance = get_value_parameter(config, 'distance', float, 120) + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._scenario_timeout = 240 super().__init__( "Accident", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) + def _move_waypoint_forward(self, wp, distance): + next_wps = wp.next(distance) + if not next_wps: + raise ValueError("Couldn't find a viable position to set up an accident actor") + return next_wps[0] + + def _spawn_side_prop(self, wp): + # Spawn the accident indication signal + prop_wp = wp + while True: + if self._direction == "right": + wp = prop_wp.get_right_lane() + else: + wp = prop_wp.get_right_lane() + if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): + break + prop_wp = wp + + displacement = prop_wp.lane_width / 2 + r_vec = prop_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=0.2) + spawn_transform.rotation.yaw += 90 + signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) + if not signal_prop: + raise ValueError("Couldn't spawn the indication prop asset") + # signal_prop.set_simulate_physics(True) + self.other_actors.append(signal_prop) + + def _spawn_obstacle(self, wp, blueprint, accident_actor=False): + """ + Spawns the obstacle actor by displacing its position to the right + """ + displacement = self._offset * wp.lane_width / 2 + r_vec = wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=1) + if accident_actor: + actor = CarlaDataProvider.request_new_actor( + blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car'}) + else: + actor = CarlaDataProvider.request_new_actor( + blueprint, spawn_transform, rolename='scenario') + if not actor: + raise ValueError("Couldn't spawn an obstacle actor") + + return actor + def _initialize_actors(self, config): """ Custom initialization """ starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - accident_wps = starting_wp.next(self._distance) - pre_accident_wps = starting_wp.next(self._distance / 2) - if not accident_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - if not pre_accident_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self._accident_wp = accident_wps[0] - - # Create the police vehicle - displacement = self._offset * self._accident_wp.lane_width / 2 - r_vec = self._accident_wp.transform.get_right_vector() - if self._direction == 'left': - r_vec *= -1 - w_loc = self._accident_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - police_transform = carla.Transform(w_loc, self._accident_wp.transform.rotation) - police_car = CarlaDataProvider.request_new_actor('vehicle.dodge.charger_police_2020', police_transform) - if not police_car: - raise ValueError("Couldn't spawn the police car") + + # Spawn the accident indication signal + self._spawn_side_prop(starting_wp) + + # Spawn the police vehicle + self._accident_wp = self._move_waypoint_forward(starting_wp, self._distance) + police_car = self._spawn_obstacle(self._accident_wp, 'vehicle.dodge.charger_police_2020') + + # Set its initial conditions lights = police_car.get_light_state() lights |= self._lights police_car.set_light_state(carla.VehicleLightState(lights)) @@ -104,62 +152,53 @@ def _initialize_actors(self, config): self.other_actors.append(police_car) # Create the first vehicle that has been in the accident - vehicle_wps = self._accident_wp.next(self._first_distance) - if not vehicle_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - vehicle_wp = vehicle_wps[0] - self._accident_wp = pre_accident_wps[0] - displacement = self._offset * vehicle_wp.lane_width / 2 - r_vec = vehicle_wp.transform.get_right_vector() - if self._direction == 'left': - r_vec *= -1 - w_loc = vehicle_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - vehicle_1_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_1_car = CarlaDataProvider.request_new_actor( - 'vehicle.*', vehicle_1_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) - if not vehicle_1_car: - raise ValueError("Couldn't spawn the accident car") - vehicle_1_car.apply_control(carla.VehicleControl(hand_brake=True)) - self.other_actors.append(vehicle_1_car) + first_vehicle_wp = self._move_waypoint_forward(self._accident_wp, self._first_distance) + first_actor = self._spawn_obstacle(first_vehicle_wp, 'vehicle.*', True) + + # Set its initial conditions + first_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(first_actor) # Create the second vehicle that has been in the accident - vehicle_wps = vehicle_wp.next(self._second_distance) - if not vehicle_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - vehicle_wp = vehicle_wps[0] + second_vehicle_wp = self._move_waypoint_forward(first_vehicle_wp, self._second_distance) + second_actor = self._spawn_obstacle(second_vehicle_wp, 'vehicle.*', True) - displacement = self._offset * vehicle_wp.lane_width / 2 - r_vec = vehicle_wp.transform.get_right_vector() - if self._direction == 'left': - r_vec *= -1 - w_loc = vehicle_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - vehicle_2_transform = carla.Transform(w_loc, vehicle_wp.transform.rotation) - vehicle_2_car = CarlaDataProvider.request_new_actor( - 'vehicle.*', vehicle_2_transform, attribute_filter={'base_type': 'car', 'has_lights': False}) - if not vehicle_2_car: - raise ValueError("Couldn't spawn the accident car") - vehicle_2_car.apply_control(carla.VehicleControl(hand_brake=True)) - self.other_actors.append(vehicle_2_car) + # Set its initial conditions + second_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(second_actor) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. """ - total_dist = self._distance + self._first_distance + self._second_distance + 20 - root = py_trees.composites.Sequence() if self.route_mode: + total_dist = self._distance + self._first_distance + self._second_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._accident_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(self._max_speed)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(0)) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(ActorDestroy(self.other_actors[1])) root.add_child(ActorDestroy(self.other_actors[2])) + root.add_child(ActorDestroy(self.other_actors[3])) + return root def _create_test_criteria(self): @@ -184,32 +223,41 @@ class AccidentTwoWays(Accident): Variation of the Accident scenario but the ego now has to invade the opposite lane """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - if 'frequency' in config.other_parameters: - self._opposite_frequency = float(config.other_parameters['frequency']['value']) - else: - self._opposite_frequency = 200 - super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to let the ego invade the opposite lane. """ - total_dist = self._distance + self._first_distance + self._second_distance + 20 - root = py_trees.composites.Sequence() if self.route_mode: + total_dist = self._distance + self._first_distance + self._second_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(SwitchWrongDirectionTest(False)) - root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._accident_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + + if self.route_mode: + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=50)) + root.add_child(ChangeOppositeBehavior(spawn_dist=40)) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(ActorDestroy(self.other_actors[1])) root.add_child(ActorDestroy(self.other_actors[2])) @@ -232,70 +280,125 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - if 'distance' in config.other_parameters: - self._distance = int(config.other_parameters['distance']['value']) - else: - self._distance = 120 - self._drive_distance = self._distance + 20 - self._offset = 1.0 + + self._trigger_distance = 30 + self._drive_distance = self._trigger_distance + 40 + self._offset = 0.7 + + self._wait_duration = 5 self._lights = carla.VehicleLightState.RightBlinker | carla.VehicleLightState.LeftBlinker - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._distance = get_value_parameter(config, 'distance', float, 120) + self._direction = get_value_parameter(config, 'direction', str, 'right') + if self._direction not in ('left', 'right'): + raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._scenario_timeout = 240 super().__init__( "ParkedObstacle", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) + def _move_waypoint_forward(self, wp, distance): + next_wps = wp.next(distance) + if not next_wps: + raise ValueError("Couldn't find a viable position to set up an accident actor") + return next_wps[0] + + def _spawn_side_prop(self, wp): + # Spawn the accident indication signal + prop_wp = wp + while True: + if self._direction == "right": + wp = prop_wp.get_right_lane() + else: + wp = prop_wp.get_right_lane() + if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): + break + prop_wp = wp + + displacement = prop_wp.lane_width / 2 + r_vec = prop_wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=0.2) + spawn_transform.rotation.yaw += 90 + signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) + if not signal_prop: + raise ValueError("Couldn't spawn the indication prop asset") + # signal_prop.set_simulate_physics(True) + self.other_actors.append(signal_prop) + + def _spawn_obstacle(self, wp, blueprint): + """ + Spawns the obstacle actor by displacing its position to the right + """ + displacement = self._offset * wp.lane_width / 2 + r_vec = wp.transform.get_right_vector() + if self._direction == 'left': + r_vec *= -1 + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=1) + actor = CarlaDataProvider.request_new_actor( + blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car'}) + if not actor: + raise ValueError("Couldn't spawn an obstacle actor") + + return actor + def _initialize_actors(self, config): """ Custom initialization """ starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - parked_wps = starting_wp.next(self._distance) - if not parked_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self._parked_wp = parked_wps[0] - - # Create the parked vehicle - displacement = self._offset * self._parked_wp.lane_width / 2 - r_vec = self._parked_wp.transform.get_right_vector() - w_loc = self._parked_wp.transform.location - w_loc += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - parked_transform = carla.Transform(w_loc, self._parked_wp.transform.rotation) - parked_car = CarlaDataProvider.request_new_actor( - 'vehicle.*', parked_transform, attribute_filter={'base_type': 'car', 'has_lights': True}) - if not parked_car: - raise ValueError("Couldn't spawn the parked car") - self.other_actors.append(parked_car) - - lights = parked_car.get_light_state() - lights |= self._lights - parked_car.set_light_state(carla.VehicleLightState(lights)) - parked_car.apply_control(carla.VehicleControl(hand_brake=True)) - pre_parked_wps = starting_wp.next(self._distance / 2) - if not pre_parked_wps: - raise ValueError("Couldn't find a viable position to set up the accident actors") - self._pre_parked_wp = pre_parked_wps[0] + # Create the side prop + self._spawn_side_prop(starting_wp) + + # Create the first vehicle that has been in the accident + self._vehicle_wp = self._move_waypoint_forward(starting_wp, self._distance) + parked_actor = self._spawn_obstacle(self._vehicle_wp, 'vehicle.*') + + lights = parked_actor.get_light_state() + lights |= self._lights + parked_actor.set_light_state(carla.VehicleLightState(lights)) + parked_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(parked_actor) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. """ - total_dist = self._distance + 20 - root = py_trees.composites.Sequence() if self.route_mode: + total_dist = self._distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._vehicle_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(self._max_speed)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(0)) root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ActorDestroy(self.other_actors[1])) return root @@ -321,10 +424,8 @@ class ParkedObstacleTwoWays(ParkedObstacle): Variation of the ParkedObstacle scenario but the ego now has to invade the opposite lane """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - if 'frequency' in config.other_parameters: - self._opposite_frequency = float(config.other_parameters['frequency']['value']) - else: - self._opposite_frequency = 200 + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): @@ -332,26 +433,37 @@ def _create_behavior(self): The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to let the ego invade the opposite lane. """ - total_dist = self._distance + 20 - root = py_trees.composites.Sequence() if self.route_mode: + total_dist = self._distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(SwitchWrongDirectionTest(False)) - root.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self._vehicle_wp.transform.location, self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + + if self.route_mode: + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=50)) + root.add_child(ChangeOppositeBehavior(spawn_dist=40)) root.add_child(ActorDestroy(self.other_actors[0])) return root -class BicycleFlowAtSideLane(BasicScenario): +class HazardAtSideLane(BasicScenario): """ Added the dangerous scene of ego vehicles driving on roads without sidewalks, with three bicycles encroaching on some roads in front. @@ -366,28 +478,25 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._drive_distance = 100 - self._offset = [0.6, 0.75, 0.9] - self._bicycle_wp = [] - self._target_location = None - self._plan = [] - self._bicycle_speed = 3 # m/s - - if 'distance' in config.other_parameters: - self._distance_to_Trigger = [ - float(config.other_parameters['distance']['first']), - float(config.other_parameters['distance']['second']), - float(config.other_parameters['distance']['third']) - ] - else: - self._distance_to_Trigger = [74,76,88] # m - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._obstacle_distance = 9 + self._trigger_distance = 30 + self._end_distance = 40 + + self._offset = 0.55 + self._wait_duration = 5 + + self._target_locs = [] + + self._bicycle_bps = ["vehicle.bh.crossbike", "vehicle.diamondback.century", "vehicle.gazelle.omafiets"] + + self._distance = get_value_parameter(config, 'distance', float, 100) + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._bicycle_speed = get_value_parameter(config, 'bicycle_speed', float, 10) + self._bicycle_drive_distance = get_value_parameter(config, 'bicycle_drive_distance', float, 50) + self._scenario_timeout = 240 - super().__init__("BicycleFlowAtSideLane", + super().__init__("HazardAtSideLane", ego_vehicles, config, world, @@ -395,66 +504,119 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals debug_mode, criteria_enable=criteria_enable) + def _move_waypoint_forward(self, wp, distance): + next_wps = wp.next(distance) + if not next_wps: + raise ValueError("Couldn't find a viable position to set up an accident actor") + return next_wps[0] + + def _spawn_obstacle(self, wp, blueprint): + """ + Spawns the obstacle actor by displacing its position to the right + """ + displacement = self._offset * wp.lane_width / 2 + r_vec = wp.transform.get_right_vector() + + spawn_transform = wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=1) + actor = CarlaDataProvider.request_new_actor(blueprint, spawn_transform) + if not actor: + raise ValueError("Couldn't spawn an obstacle actor") + + return actor + def _initialize_actors(self, config): """ Custom initialization """ + rng = CarlaDataProvider.get_random_seed() + self._starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - starting_wp = self._map.get_waypoint(config.trigger_points[0].location) - if 'end_bycicle_distance' in config.other_parameters: - self._end_bycicle_distance = float( - config.other_parameters['end_bycicle_distance']['value']) - else: - self._end_bycicle_distance = 150 - self._target_location = starting_wp.next(self._end_bycicle_distance)[0].transform.location - for offset,distance in zip(self._offset,self._distance_to_Trigger): - - bicycle_wps = starting_wp.next(distance) - - if not bicycle_wps: - raise ValueError("Couldn't find a viable position to set up the bicycle actors") - self._bicycle_wp.append(bicycle_wps[0]) - displacement = offset* bicycle_wps[0].lane_width / 2 - r_vec = bicycle_wps[0].transform.get_right_vector() - w_loc = bicycle_wps[0].transform.location - w_loc = w_loc + carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y) - bycicle_transform = carla.Transform(w_loc, bicycle_wps[0].transform.rotation) - bycicle = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', bycicle_transform) - self.other_actors.append(bycicle) + # Spawn the first bicycle + first_wp = self._move_waypoint_forward(self._starting_wp, self._distance) + bicycle_1 = self._spawn_obstacle(first_wp, rng.choice(self._bicycle_bps)) + + wps = first_wp.next(self._bicycle_drive_distance) + if not wps: + raise ValueError("Couldn't find an end location for the bicycles") + self._target_locs.append(wps[0].transform.location) + + # Set its initial conditions + bicycle_1.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(bicycle_1) + + # Spawn the second bicycle + second_wp = self._move_waypoint_forward(first_wp, self._obstacle_distance) + bicycle_2 = self._spawn_obstacle(second_wp, rng.choice(self._bicycle_bps)) + + wps = second_wp.next(self._bicycle_drive_distance) + if not wps: + raise ValueError("Couldn't find an end location for the bicycles") + self._target_locs.append(wps[0].transform.location) + + # Set its initial conditions + bicycle_2.apply_control(carla.VehicleControl(hand_brake=True)) + self.other_actors.append(bicycle_2) def _create_behavior(self): """ - The vehicle has to drive the whole predetermined distance. + Activate the bicycles and wait for the ego to be close-by before changing the side traffic. + End condition is based on the ego behind in front of the bicycles, or timeout based. + + Sequence: + - LeaveSpaceInFront + - Parallel + - ScenarioTimeout + - Bicycles (Sequences) + - BasicAgentBehavior + - HandBrakeVehicle + - Idle + - Behavior (Sequence) + - InTriggerDistanceToVehicle + - Idle + - SetMaxSpeed + - WaitUntilInFront + - DriveDistance + - SetMaxSpeed + - ActorDestroys """ - root = py_trees.composites.Sequence() if self.route_mode: - total_dist = self._distance_to_Trigger[2] + 30 + total_dist = self._distance + self._obstacle_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - root.add_child(SwitchWrongDirectionTest(False)) - root.add_child(ChangeOppositeBehavior(active=False)) - bycicle = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[2], self._target_location, target_speed = self._bicycle_speed, - opt_dict={'offset': self._offset[2] * self._bicycle_wp[2].lane_width / 2})) - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[1], self._target_location, target_speed = self._bicycle_speed, - opt_dict={'offset': self._offset[1] * self._bicycle_wp[1].lane_width / 2})) - bycicle.add_child(ConstantVelocityAgentBehavior( - self.other_actors[0], self._target_location, target_speed = self._bicycle_speed, - opt_dict={'offset': self._offset[0] * self._bicycle_wp[0].lane_width / 2})) - root.add_child(bycicle) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - root.add_child(end_condition) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + # Bicycle movement. Move them for a set distance, then + offset = self._offset * self._starting_wp.lane_width / 2 + opt_dict = {'offset': offset} + for actor, target_loc in zip(self.other_actors, self._target_locs): + bicycle = py_trees.composites.Sequence() + bicycle.add_child(BasicAgentBehavior(actor, target_loc, target_speed=self._bicycle_speed, opt_dict=opt_dict)) + bicycle.add_child(HandBrakeVehicle(actor, 1)) # In case of collisions + bicycle.add_child(Idle()) # Don't make the bicycle stop the parallel behavior + timeout_parallel.add_child(bicycle) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToVehicle( + self.ego_vehicles[0], self.other_actors[0], self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + if self.route_mode: - root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(active=True)) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) - root.add_child(ActorDestroy(self.other_actors[2])) + behavior.add_child(SetMaxSpeed(self._max_speed)) + behavior.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + + if self.route_mode: + behavior.add_child(SetMaxSpeed(0)) + + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root @@ -473,3 +635,83 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() + + +class HazardAtSideLaneTwoWays(HazardAtSideLane): + """ + Variation of the HazardAtSideLane scenario but the ego now has to invade the opposite lane + """ + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): + + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _create_behavior(self): + """ + Activate the bicycles and wait for the ego to be close-by before changing the opposite traffic. + End condition is based on the ego behind in front of the bicycles, or timeout based. + + Sequence: + - LeaveSpaceInFront + - Parallel + - ScenarioTimeout + - Bicycles (Sequences) + - BasicAgentBehavior + - HandBrakeVehicle + - Idle + - Behavior (Sequence) + - InTriggerDistanceToVehicle + - Idle + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - WaitUntilInFront + - DriveDistance + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - ActorDestroys + """ + root = py_trees.composites.Sequence() + if self.route_mode: + total_dist = self._distance + self._obstacle_distance + 20 + root.add_child(LeaveSpaceInFront(total_dist)) + + timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + # Bicycle movement. Move them for a set distance, then + offset = self._offset * self._starting_wp.lane_width / 2 + opt_dict = {'offset': offset} + for actor, target_loc in zip(self.other_actors, self._target_locs): + bicycle = py_trees.composites.Sequence() + movement = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + movement.add_child(BasicAgentBehavior(actor, target_speed=self._bicycle_speed, opt_dict=opt_dict)) + movement.add_child(InTriggerDistanceToLocation(actor, target_loc, 5)) + bicycle.add_child(movement) + bicycle.add_child(HandBrakeVehicle(actor, 1)) # In case of collisions + bicycle.add_child(Idle()) # Don't make the bicycle stop the parallel behavior + timeout_parallel.add_child(bicycle) + + behavior = py_trees.composites.Sequence() + behavior.add_child(InTriggerDistanceToVehicle( + self.ego_vehicles[0], self.other_actors[0], self._trigger_distance)) + behavior.add_child(Idle(self._wait_duration)) + + if self.route_mode: + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + behavior.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) + behavior.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + + timeout_parallel.add_child(behavior) + + root.add_child(timeout_parallel) + + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(spawn_dist=40)) + + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) + + return root diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 240cb646a..189bbff9b 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -300,9 +300,7 @@ def _create_test_criteria(self): # 'Normal' criteria criteria.add_child(OutsideRouteLanesTest(self.ego_vehicles[0], route=self.route)) - criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='vehicle', name="CollisionVehicleTest")) - criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='miscellaneous', name="CollisionLayoutTest")) - criteria.add_child(CollisionTest(self.ego_vehicles[0], other_actor_type='walker', name="CollisionPedestrianTest")) + criteria.add_child(CollisionTest(self.ego_vehicles[0], name="CollisionTest")) criteria.add_child(RunningRedLightTest(self.ego_vehicles[0])) criteria.add_child(RunningStopTest(self.ego_vehicles[0])) criteria.add_child(MinSpeedRouteTest(self.ego_vehicles[0], name="MinSpeedTest")) diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index 9cdf67727..d84b7d370 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -16,28 +16,38 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer, ScenarioTimeout -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection, DriveDistance from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import (generate_target_waypoint, get_junction_topology, filter_junction_wp_direction, - get_same_dir_lanes) + get_same_dir_lanes, + get_closest_traffic_light) from srunner.tools.background_manager import HandleJunctionScenario, ChangeOppositeBehavior +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default -class SignalizedJunctionLeftTurn(BasicScenario): +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default - """ - Implementation class for Hero - Vehicle turning left at signalized junction scenario, - Traffic Scenario 08. - This is a single ego vehicle scenario +class JunctionLeftTurn(BasicScenario): + """ + Vehicle turning left at junction scenario, with actors coming in the opposite direction. + The ego has to react to them, safely crossing the opposite lane """ - - timeout = 80 # Timeout of scenario in seconds def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=80): @@ -46,39 +56,26 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals """ self._world = world self._map = CarlaDataProvider.get_map() - self._rng = random.RandomState(2000) - - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 20 # m/s + self._rng = CarlaDataProvider.get_random_seed() - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [25, 50] # m + self.timeout = timeout self._direction = 'opposite' - # The faster the flow, the further they are spawned, leaving time to react to them - self._source_dist = 5 * self._flow_speed - self._sink_dist = 3 * self._flow_speed - self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green self._flow_tl_dict = {} self._init_tl_dict = {} + self._end_distance = 10 - self.timeout = timeout + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 20) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [25, 50]) + self._scenario_timeout = 240 - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed - super().__init__("SignalizedJunctionLeftTurn", + super().__init__("JunctionLeftTurn", ego_vehicles, config, world, @@ -102,10 +99,10 @@ def _initialize_actors(self, config): raise ValueError("Failed to find junction as a waypoint with no next was detected") starting_wp = starting_wps[0] ego_junction_dist += 1 - junction = starting_wp.get_junction() + self._junction = starting_wp.get_junction() # Get the opposite entry lane wp - entry_wps, _ = get_junction_topology(junction) + entry_wps, _ = get_junction_topology(self._junction) source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) if not source_entry_wps: raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) @@ -135,18 +132,50 @@ def _initialize_actors(self, config): raise ValueError("Failed to find a sink location as a waypoint with no next was detected") self._sink_wp = sink_wps[0] - self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + def _create_behavior(self): + raise NotImplementedError("Found missing behavior") + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + - def _get_traffic_lights(self, junction, ego_dist, source_dist): - """Get the traffic light of the junction, mapping their states""" - tls = self._world.get_traffic_lights_in_junction(junction.id) +class SignalizedJunctionLeftTurn(JunctionLeftTurn): + """ + Signalized version of 'JunctionLeftTurn` + """ + + timeout = 80 # Timeout of scenario in seconds + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=80): + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _initialize_actors(self, config): + """ + Default initialization of other actors. + Override this method in child class to provide custom initialization. + """ + super()._initialize_actors(config) + + tls = self._world.get_traffic_lights_in_junction(self._junction.id) if not tls: - raise ValueError("No traffic lights found, use the NonSignalized version instead") + raise ValueError("Found no traffic lights, use the non signalized version instead") + ego_tl = get_closest_traffic_light(self._ego_wp, tls) + source_tl = get_closest_traffic_light(self._source_wp, tls) - ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] - ego_tl = self._world.get_traffic_light(ego_landmark) - source_landmark = self._source_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] - source_tl = self._world.get_traffic_light(source_landmark) for tl in tls: if tl.id == ego_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green @@ -176,7 +205,10 @@ def _create_behavior(self): sequence.add_child(ChangeOppositeBehavior(active=False)) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + root.add_child(end_condition) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -193,126 +225,17 @@ def _create_behavior(self): 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 = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] - if not self.route_mode: - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() - - -class NonSignalizedJunctionLeftTurn(BasicScenario): +class NonSignalizedJunctionLeftTurn(JunctionLeftTurn): """ - Implementation class for Hero - Vehicle turning left at signalized junction scenario, - Traffic Scenario 08. - - This is a single ego vehicle scenario + Non signalized version of 'JunctionLeftTurn` """ timeout = 80 # Timeout of scenario in seconds def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=80): - """ - Setup all relevant parameters and create scenario - """ - self._world = world - self._map = CarlaDataProvider.get_map() - self._rng = random.RandomState(2000) - - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 20 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [25, 50] # m - - self._direction = 'opposite' - - # The faster the flow, the further they are spawned, leaving time to react to them - self._source_dist = 5 * self._flow_speed - self._sink_dist = 3 * self._flow_speed - - self.timeout = timeout - - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 - - super().__init__("NonSignalizedJunctionLeftTurn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Default initialization of other actors. - Override this method in child class to provide custom initialization. - """ - ego_location = config.trigger_points[0].location - self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) - - # Get the junction - starting_wp = self._ego_wp - ego_junction_dist = 0 - while not starting_wp.is_junction: - starting_wps = starting_wp.next(1.0) - if len(starting_wps) == 0: - raise ValueError("Failed to find junction as a waypoint with no next was detected") - starting_wp = starting_wps[0] - ego_junction_dist += 1 - junction = starting_wp.get_junction() - - # Get the opposite entry lane wp - entry_wps, _ = get_junction_topology(junction) - source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) - if not source_entry_wps: - raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) - - # Get the source transform - source_entry_wp = self._rng.choice(source_entry_wps) - - # Get the source transform - source_wp = source_entry_wp - source_junction_dist = 0 - while source_junction_dist < self._source_dist: - source_wps = source_wp.previous(5) - if len(source_wps) == 0: - raise ValueError("Failed to find a source location as a waypoint with no previous was detected") - if source_wps[0].is_junction: - break - source_wp = source_wps[0] - source_junction_dist += 5 - - self._source_wp = source_wp - source_transform = self._source_wp.transform - - # Get the sink location - sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) - sink_wps = sink_exit_wp.next(self._sink_dist) - if len(sink_wps) == 0: - raise ValueError("Failed to find a sink location as a waypoint with no next was detected") - self._sink_wp = sink_wps[0] + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): """ @@ -332,7 +255,10 @@ def _create_behavior(self): sequence.add_child(ChangeOppositeBehavior(active=False)) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + root.add_child(end_condition) root.add_child(ActorFlow( self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -343,19 +269,3 @@ def _create_behavior(self): sequence.add_child(ChangeOppositeBehavior(active=True)) 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 = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] - if not self.route_mode: - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index 2128b1e85..ced01b498 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -18,21 +18,37 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, TrafficLightFreezer, ScenarioTimeout from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitEndIntersection, DriveDistance from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import (generate_target_waypoint, get_junction_topology, filter_junction_wp_direction, - get_same_dir_lanes) + get_same_dir_lanes, + get_closest_traffic_light) from srunner.tools.background_manager import HandleJunctionScenario -class SignalizedJunctionRightTurn(BasicScenario): +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default + + +class JunctionRightTurn(BasicScenario): """ Scenario where the vehicle is turning right at an intersection an has to avoid - colliding with a vehicle coming from its left + colliding with vehicles coming from its left """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, @@ -44,35 +60,23 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 20 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [25, 50] # m - self._direction = 'left' - # The faster the flow, the further they are spawned, leaving time to react to them - self._source_dist = 5 * self._flow_speed - self._sink_dist = 3 * self._flow_speed - self._green_light_delay = 5 # Wait before the ego's lane traffic light turns green self._flow_tl_dict = {} self._init_tl_dict = {} - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._end_distance = 10 # Distance after the junction before the scenario ends - super().__init__("SignalizedJunctionRightTurn", + self._flow_speed = get_value_parameter(config, 'flow_speed', float, 20) + self._source_dist_interval = get_interval_parameter(config, 'source_dist_interval', float, [25, 50]) + self._scenario_timeout = 240 + + # The faster the flow, the further they are spawned, leaving time to react to them + self._source_dist = 5 * self._flow_speed + self._sink_dist = 3 * self._flow_speed + + super().__init__("JunctionRightTurn", ego_vehicles, config, world, @@ -95,10 +99,10 @@ def _initialize_actors(self, config): raise ValueError("Failed to find a junction") starting_wp = starting_wps[0] ego_junction_dist += 1 - junction = starting_wp.get_junction() + self._junction = starting_wp.get_junction() # Get the source entry lane wp - entry_wps, _ = get_junction_topology(junction) + entry_wps, _ = get_junction_topology(self._junction) source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) if not source_entry_wps: raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) @@ -133,18 +137,45 @@ def _initialize_actors(self, config): raise ValueError("Failed to find a sink location") self._sink_wp = sink_wps[0] - self._get_traffic_lights(junction, ego_junction_dist, source_junction_dist) + def _create_behavior(self): + raise NotImplementedError("Found missing behavior") + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() - def _get_traffic_lights(self, junction, ego_dist, source_dist): - """Get the traffic light of the junction, mapping their states""" - tls = self._world.get_traffic_lights_in_junction(junction.id) - if not tls: - raise ValueError("No traffic lights found, use the NonSignalized version instead") - ego_landmark = self._ego_wp.get_landmarks_of_type(ego_dist + 2, "1000001")[0] - ego_tl = self._world.get_traffic_light(ego_landmark) - source_landmark = self._source_wp.get_landmarks_of_type(source_dist + 2, "1000001")[0] - source_tl = self._world.get_traffic_light(source_landmark) +class SignalizedJunctionRightTurn(JunctionRightTurn): + """ + Signalized version of JunctionRightTurn + """ + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=80): + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + super()._initialize_actors(config) + + tls = self._world.get_traffic_lights_in_junction(self._junction.id) + ego_tl = get_closest_traffic_light(self._ego_wp, tls) + source_tl = get_closest_traffic_light(self._source_wp, tls) + for tl in tls: if tl.id == ego_tl.id: self._flow_tl_dict[tl] = carla.TrafficLightState.Green @@ -174,9 +205,12 @@ def _create_behavior(self): )) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + root.add_child(end_condition) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, add_initial_actors=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") @@ -187,130 +221,21 @@ def _create_behavior(self): sequence.add_child(root) 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 = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] - if not self.route_mode: - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() - - -class NonSignalizedJunctionRightTurn(BasicScenario): +class NonSignalizedJunctionRightTurn(JunctionRightTurn): """ - Scenario where the vehicle is turning right at an intersection an has to avoid - colliding with a vehicle coming from its left + Non signalized version of JunctionRightTurn """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=80): - """ - Setup all relevant parameters and create scenario - """ - self._world = world - self._map = CarlaDataProvider.get_map() - self.timeout = timeout - - if 'flow_speed' in config.other_parameters: - self._flow_speed = float(config.other_parameters['flow_speed']['value']) - else: - self._flow_speed = 20 # m/s - - if 'source_dist_interval' in config.other_parameters: - self._source_dist_interval = [ - float(config.other_parameters['source_dist_interval']['from']), - float(config.other_parameters['source_dist_interval']['to']) - ] - else: - self._source_dist_interval = [25, 50] # m - - self._direction = 'left' - - # The faster the flow, the further they are spawned, leaving time to react to them - self._source_dist = 5 * self._flow_speed - self._sink_dist = 3 * self._flow_speed - - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 - - super().__init__("NonSignalizedJunctionRightTurn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) - - def _initialize_actors(self, config): - """ - Custom initialization - """ - ego_location = config.trigger_points[0].location - self._ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) - - # Get the junction - starting_wp = self._ego_wp - ego_junction_dist = 0 - while not starting_wp.is_junction: - starting_wps = starting_wp.next(1.0) - if len(starting_wps) == 0: - raise ValueError("Failed to find a junction") - starting_wp = starting_wps[0] - ego_junction_dist += 1 - junction = starting_wp.get_junction() - - # Get the source entry lane wp - entry_wps, _ = get_junction_topology(junction) - source_entry_wps = filter_junction_wp_direction(starting_wp, entry_wps, self._direction) - if not source_entry_wps: - raise ValueError("Trying to find a lane in the {} direction but none was found".format(self._direction)) - - # Get the rightmost lane - source_entry_wp = source_entry_wps[0] - while True: - right_wp = source_entry_wp.get_right_lane() - if not right_wp or right_wp.lane_type != carla.LaneType.Driving: - break - source_entry_wp = right_wp - - # Get the source transform - source_wp = source_entry_wp - source_junction_dist = 0 - while source_junction_dist < self._source_dist: - source_wps = source_wp.previous(5) - if len(source_wps) == 0: - raise ValueError("Failed to find a source location") - if source_wps[0].is_junction: - break - source_wp = source_wps[0] - source_junction_dist += 5 - - self._source_wp = source_wp - source_transform = self._source_wp.transform - - # Get the sink location - sink_exit_wp = generate_target_waypoint(self._map.get_waypoint(source_transform.location), 0) - sink_wps = sink_exit_wp.next(self._sink_dist) - if len(sink_wps) == 0: - raise ValueError("Failed to find a sink location") - self._sink_wp = sink_wps[0] + super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): """ Hero vehicle is turning right in an urban area, at a signalized intersection, while other actor coming straight from the left. The ego has to avoid colliding with it """ - sequence = py_trees.composites.Sequence(name="JunctionRightTurn") if self.route_mode: sequence.add_child(HandleJunctionScenario( @@ -323,26 +248,13 @@ def _create_behavior(self): )) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition = py_trees.composites.Sequence() + end_condition.add_child(WaitEndIntersection(self.ego_vehicles[0])) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + root.add_child(end_condition) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, add_initial_actors=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence.add_child(root) 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 = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] - if not self.route_mode: - criteria.append(CollisionTest(self.ego_vehicles[0])) - return criteria - - def __del__(self): - """ - Remove all actors upon deletion - """ - self.remove_all_actors() diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index 72a99e75a..a91ca60d4 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -21,15 +21,22 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, OpenVehicleDoor, SwitchWrongDirectionTest, - ScenarioTimeout) + ScenarioTimeout, + Idle) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior, SetMaxSpeed +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + class VehicleOpensDoor(BasicScenario): """ @@ -47,27 +54,19 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._wait_duration = 15 self._takeover_distance = 60 self._min_trigger_dist = 10 self._reaction_time = 3.0 - if 'distance' in config.other_parameters: - self._parked_distance = float(config.other_parameters['distance']['value']) - else: - self._parked_distance = 50 + self._opposite_wait_duration = 5 - if 'direction' in config.other_parameters: - self._direction = config.other_parameters['direction']['value'] - else: - self._direction = 'right' + self._parked_distance = get_value_parameter(config, 'distance', float, 50) + self._direction = get_value_parameter(config, 'direction', str, 'right') if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") - if 'timeout' in config.other_parameters: - self._scenario_timeout = float(config.other_parameters['flow_distance']['value']) - else: - self._scenario_timeout = 180 + self._max_speed = get_value_parameter(config, 'speed', float, 60) + self._scenario_timeout = 240 super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) @@ -111,6 +110,7 @@ def _create_behavior(self): if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._parked_distance)) + sequence.add_child(SetMaxSpeed(self._max_speed)) collision_location = self._front_wp.transform.location @@ -131,6 +131,9 @@ def _create_behavior(self): sequence.add_child(end_condition) sequence.add_child(ActorDestroy(self._parked_actor)) + if self.route_mode: + sequence.add_child(SetMaxSpeed(0)) + return sequence def _get_displaced_location(self, actor, wp): @@ -173,10 +176,8 @@ class VehicleOpensDoorTwoWays(VehicleOpensDoor): def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - if 'frequency' in config.other_parameters: - self._opposite_frequency = float(config.other_parameters['frequency']['value']) - else: - self._opposite_frequency = 100 + + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 100) super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) @@ -184,35 +185,61 @@ def _create_behavior(self): """ Leave space in front, as the TM doesn't detect open doors, and change the opposite frequency so that the ego can pass + + Sequence: + - LeaveSpaceInFront + - Parallel: + - ScenarioTimeout: + - Sequence + - Trigger adversary + - OpenVehicleDoor + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - DriveDistance + - SwitchWrongDirectionTest + - ChangeOppositeBehavior + - ActorDestroy """ + sequence = py_trees.composites.Sequence(name="VehicleOpensDoorTwoWays") if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._parked_distance)) - collision_location = self._front_wp.transform.location + main_behavior = py_trees.composites.Sequence() # Wait until ego is close to the adversary + collision_location = self._front_wp.transform.location trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerOpenDoor") trigger_adversary.add_child(InTimeToArrivalToLocation( self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], collision_location, self._min_trigger_dist)) - sequence.add_child(trigger_adversary) + main_behavior.add_child(trigger_adversary) door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL + main_behavior.add_child(OpenVehicleDoor(self._parked_actor, door)) + main_behavior.add_child(Idle(self._opposite_wait_duration)) - sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) - sequence.add_child(SwitchWrongDirectionTest(False)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) + if self.route_mode: + main_behavior.add_child(SwitchWrongDirectionTest(False)) + main_behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - sequence.add_child(end_condition) - sequence.add_child(SwitchWrongDirectionTest(True)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=50)) + main_behavior.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) + + # Add the main behavior to the scenario timeout + timeout_parallel = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + + timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + timeout_parallel.add_child(main_behavior) + + sequence.add_child(timeout_parallel) + + if self.route_mode: + sequence.add_child(SwitchWrongDirectionTest(True)) + sequence.add_child(ChangeOppositeBehavior(spawn_dist=40)) sequence.add_child(ActorDestroy(self._parked_actor)) return sequence diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py index efc28278e..75dec88e9 100644 --- a/srunner/scenarios/yield_to_emergency_vehicle.py +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -18,7 +18,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, Idle, AdaptiveConstantVelocityAgentBehavior from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, YieldToEmergencyVehicleTest from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import RemoveRoadLane, ReAddEgoRoadLane +from srunner.tools.background_manager import RemoveRoadLane, ReAddRoadLane class YieldToEmergencyVehicle(BasicScenario): @@ -42,14 +42,14 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self.timeout = timeout self._ev_drive_time = 12 # seconds - # m/s. How much the EV is expected to be faster than the EGO + # km/h. How much the EV is expected to be faster than the EGO self._speed_increment = 15 if 'distance' in config.other_parameters: self._distance = float( config.other_parameters['distance']['value']) else: - self._distance = 15 # m + self._distance = 100 # m self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint( @@ -128,7 +128,7 @@ def _create_behavior(self): sequence.add_child(ActorDestroy(self.other_actors[0])) if self.route_mode: - sequence.add_child(ReAddEgoRoadLane()) + sequence.add_child(ReAddRoadLane(0)) return sequence diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 5f1f5694b..0c2b39afe 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -84,6 +84,20 @@ def update(self): return py_trees.common.Status.SUCCESS +class SetMaxSpeed(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that its behavior is restriced to a maximum speed + """ + + def __init__(self, max_speed, name="SetMaxSpeed"): + self._max_speed = max_speed + super().__init__(name) + + def update(self): + py_trees.blackboard.Blackboard().set("BA_SetMaxSpeed", self._max_speed, overwrite=True) + return py_trees.common.Status.SUCCESS + + class StopFrontVehicles(AtomicBehavior): """ Updates the blackboard to tell the background activity that a HardBreak scenario has to be triggered. @@ -178,7 +192,7 @@ class RemoveRoadLane(AtomicBehavior): and stop generating new ones on this lane, or recover from stopping. Args: - lane_id (str): A carla.Waypoint.lane_id + lane_wp (carla.Waypoint): A carla.Waypoint active (bool) """ def __init__(self, lane_wp, name="RemoveRoadLane"): @@ -191,20 +205,21 @@ def update(self): return py_trees.common.Status.SUCCESS -class ReAddEgoRoadLane(AtomicBehavior): +class ReAddRoadLane(AtomicBehavior): """ Updates the blackboard to tell the background activity to readd the ego road lane. Args: - lane_id (str): A carla.Waypoint.lane_id + offset: 0 to readd the ego lane, 1 for the right side lane, -1 for the left... active (bool) """ - def __init__(self, name="BA_ReAddEgoRoadLane"): + def __init__(self, offset, name="BA_ReAddRoadLane"): + self._offset = offset super().__init__(name) def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_ReAddEgoRoadLane", True, overwrite=True) + py_trees.blackboard.Blackboard().set("BA_ReAddRoadLane", self._offset, overwrite=True) return py_trees.common.Status.SUCCESS @@ -223,6 +238,21 @@ def update(self): return py_trees.common.Status.SUCCESS +class LeaveCrossingSpace(AtomicBehavior): + """ + Updates the blackboard to tell the background activity that the ego needs more space in front. + This only works at roads, not junctions. + """ + def __init__(self, collision_wp, name="LeaveCrossingSpace"): + self._collision_wp = collision_wp + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_LeaveCrossingSpace", self._collision_wp, overwrite=True) + return py_trees.common.Status.SUCCESS + + class HandleJunctionScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity to adapt to a junction scenario From 05ef0d7d33a6be2a90ea2edf1b9e8013f2aee957 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Sat, 30 Jul 2022 10:54:54 +0200 Subject: [PATCH 43/86] Route obstacles + parked vehicle destruction --- .../scenariomanager/carla_data_provider.py | 17 ++++ srunner/scenariomanager/lights_sim.py | 6 +- .../scenarioatomics/atomic_behaviors.py | 40 ++++++++-- .../scenarioatomics/atomic_criteria.py | 63 +++++++++------ srunner/scenarios/actor_flow.py | 5 +- srunner/scenarios/background_activity.py | 79 +++++++++++++++---- .../scenarios/construction_crash_vehicle.py | 23 ++++-- srunner/scenarios/cross_bicycle_flow.py | 2 +- .../scenarios/cut_in_with_static_vehicle.py | 37 +++++---- srunner/scenarios/highway_cut_in.py | 2 +- srunner/scenarios/object_crash_vehicle.py | 17 +++- srunner/scenarios/parking_cut_in.py | 13 +++ srunner/scenarios/parking_exit.py | 19 ++++- srunner/scenarios/pedestrian_crossing.py | 26 +----- srunner/scenarios/route_obstacles.py | 12 +-- srunner/scenarios/vehicle_opens_door.py | 11 +++ .../scenarios/yield_to_emergency_vehicle.py | 17 ++-- srunner/tools/openscenario_parser.py | 6 +- srunner/tools/scenario_helper.py | 5 +- 19 files changed, 274 insertions(+), 126 deletions(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index dcc25af9e..95d782c0b 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -60,6 +60,7 @@ class CarlaDataProvider(object): # pylint: disable=too-many-public-methods _spawn_points = None _spawn_index = 0 _blueprint_library = None + _all_actors = None _ego_vehicle_route = None _traffic_manager_port = 8000 _random_seed = 2000 @@ -133,6 +134,8 @@ def on_carla_tick(): if world is None: print("WARNING: CarlaDataProvider couldn't find the world") + CarlaDataProvider._all_actors = None + @staticmethod def get_velocity(actor): """ @@ -239,6 +242,19 @@ def get_global_route_planner(): """ return CarlaDataProvider._grp + @staticmethod + def get_all_actors(): + """ + @return all the world actors. This is an expensive call, hence why it is part of the CDP, + but as this might not be used by everyone, only get the actors the first time someone + calls asks for them. 'CarlaDataProvider._all_actors' is reset each tick to None. + """ + if CarlaDataProvider._all_actors: + return CarlaDataProvider._all_actors + + CarlaDataProvider._all_actors = CarlaDataProvider._world.get_actors() + return CarlaDataProvider._all_actors + @staticmethod def is_sync_mode(): """ @@ -829,6 +845,7 @@ def cleanup(): CarlaDataProvider._world = None CarlaDataProvider._sync_flag = False CarlaDataProvider._ego_vehicle_route = None + CarlaDataProvider._all_actors = None CarlaDataProvider._carla_actor_pool = {} CarlaDataProvider._client = None CarlaDataProvider._spawn_points = None diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py index 14182cca0..0ec451c06 100644 --- a/srunner/scenariomanager/lights_sim.py +++ b/srunner/scenariomanager/lights_sim.py @@ -77,7 +77,7 @@ def _get_night_mode(self, weather): def _turn_close_lights_on(self, location): """Turns on the lights of all the objects close to the ego vehicle""" ego_speed = CarlaDataProvider.get_velocity(self._ego_vehicle) - radius = max(self._radius, 5 * ego_speed) + radius = max(self._radius, 10 * ego_speed) # Street lights on_lights = [] @@ -96,7 +96,7 @@ def _turn_close_lights_on(self, location): self._light_manager.turn_off(off_lights) # Vehicles - all_vehicles = self._world.get_actors().filter('*vehicle.*') + 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: @@ -121,7 +121,7 @@ def _turn_all_lights_off(self): self._light_manager.turn_off(off_lights) # Vehicles - all_vehicles = self._world.get_actors().filter('*vehicle.*') + 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: diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index d20e9b8e7..0c51d0c8d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -290,7 +290,7 @@ def update(self): py_trees.common.Status.SUCCESS """ - for actor in CarlaDataProvider.get_world().get_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') @@ -2679,7 +2679,7 @@ def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name, def update(self): new_status = py_trees.common.Status.RUNNING if self._actor_limit > 0: - world_actors = self._world.get_actors() + 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): @@ -2889,7 +2889,6 @@ def __init__(self, plan, spawn_dist_interval, sink_dist=2, self._source_transform = self._plan[0][0].transform self._source_location = self._source_transform.location - self._source_vector = self._source_transform.get_forward_vector() self._sink_location = self._plan[-1][0].transform.location self._min_spawn_dist = spawn_dist_interval[0] @@ -2898,6 +2897,8 @@ def __init__(self, plan, spawn_dist_interval, sink_dist=2, self._add_initial_actors = add_initial_actors + self._opt_dict = {"ignore_traffic_lights": True, "ignore_vehicles": True} + self._actor_data = [] self._grp = CarlaDataProvider.get_global_route_planner() @@ -2914,6 +2915,23 @@ def initialise(self): self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) def _spawn_actor(self, transform): + """Spawn the actor""" + # Initial actors don't want all the plan. Remove the points behind them + plan = self._plan + actor_loc = transform.location + while len(plan) > 0: + wp, _ = plan[0] + loc = wp.transform.location + actor_heading = transform.get_forward_vector() + actor_wp_vec = loc - actor_loc + if actor_heading.dot(actor_wp_vec) < 0 or loc.distance(actor_loc) < 10: + plan.pop(0) + else: + break + + if not plan: + return + actor = CarlaDataProvider.request_new_actor( 'vehicle.*', transform, rolename='scenario no lights', attribute_filter={'base_type': 'bicycle'}, tick=False @@ -2921,10 +2939,12 @@ def _spawn_actor(self, transform): if actor is None: return - actor.set_target_velocity(self._speed * self._source_vector) - controller = BasicAgent(actor, 3.6 * self._speed, + 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(self._plan) + controller.set_global_plan(plan) + + initial_vec = plan[0][0].transform.get_forward_vector() + actor.set_target_velocity(self._speed * initial_vec) self._actor_data.append([actor, controller]) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) @@ -3840,6 +3860,7 @@ def __init__(self, duration, scenario_name, name="ScenarioTimeout"): self._scenario_name = scenario_name self._start_time = 0 self._scenario_timeout = False + self._terminated = False self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def initialise(self): @@ -3847,6 +3868,7 @@ def initialise(self): Set start time """ self._start_time = GameTime.get_time() + py_trees.blackboard.Blackboard().set("AC_SwitchActorBlockedTest", False, overwrite=True) super().initialise() def update(self): @@ -3865,6 +3887,8 @@ def terminate(self, new_status): """ Modifies the blackboard to tell the `ScenarioTimeoutTest` if the timeout was triggered """ - py_trees.blackboard.Blackboard().set(f"ScenarioTimeout_{self._scenario_name}", self._scenario_timeout, overwrite=True) - self._scenario_timeout = False # py_trees calls the terminate several times for some reason. + 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) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 68fcdf10d..a90e38e89 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -424,10 +424,11 @@ def __init__(self, actor, min_speed, max_time, name="ActorBlockedTest", optional """ Class constructor """ - super(ActorBlockedTest, self).__init__(name, actor, optional, terminate_on_failure) + super().__init__(name, actor, optional, terminate_on_failure) self._min_speed = min_speed self._max_time = max_time self._time_last_valid_state = None + self._active = True self.units = None # We care about whether or not it fails, no units attached def update(self): @@ -436,24 +437,32 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - 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: - # The actor has been "blocked" for too long, save the data - self.test_status = "FAILURE" + # Deactivate/Activate checking by blackboard message + 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) - 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.set_dict({'location': vehicle_location}) - self.events.append(event) - else: - self._time_last_valid_state = GameTime.get_time() + 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: + # 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.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 @@ -1024,7 +1033,7 @@ def update(self): if location is None: return new_status - # Deactivate/Activate checking by blackboard message + # Deactivate / activate checking by blackboard message active = py_trees.blackboard.Blackboard().get('AC_SwitchWrongDirectionTest') if active is not None: self._wrong_direction_active = active @@ -1634,7 +1643,7 @@ def __init__(self, actor, name="RunningRedLightTest", terminate_on_failure=False self._last_red_light_id = None self.debug = False - all_actors = self._world.get_actors() + all_actors = CarlaDataProvider.get_all_actors() for _actor in all_actors: if 'traffic_light' in _actor.type_id: center, waypoints = self.get_traffic_light_waypoints(_actor) @@ -1814,7 +1823,7 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): self._target_stop_sign = None self._stop_completed = False - all_actors = self._world.get_actors() + all_actors = CarlaDataProvider.get_all_actors() for _actor in all_actors: if 'traffic.stop' in _actor.type_id: self._list_stop_signs.append(_actor) @@ -1968,7 +1977,7 @@ def update(self): if self._active: # Get the speed of the surrounding Background Activity - all_vehicles = self._world.get_actors().filter('vehicle*') + 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: @@ -2033,15 +2042,20 @@ def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest" """ super().__init__(name, actor, optional) self.units = "%" - self.success_value = 70 + self.success_value = 95 self.actual_value = 0 self._ev = ev self._target_speed = None self._ev_speed_log = [] self._map = CarlaDataProvider.get_map() + self.initialized = False self._terminated = False + def initialise(self): + self.initialized = True + return super().initialise() + def update(self): """ Collect ev's actual speed on each time-step @@ -2073,7 +2087,7 @@ def terminate(self, new_status): """Set the traffic event to the according value if needed""" # Terminates are called multiple times. Do this only once - if not self._terminated: + if not self._terminated and self.initialized: if not len(self._ev_speed_log): self.actual_value = 100 else: @@ -2094,6 +2108,7 @@ def terminate(self, new_status): self.events.append(traffic_event) self._terminated = True + print(f"ACTUAL VALUE: {self.actual_value}") super().terminate(new_status) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 780bb15d9..fc235d6a2 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -332,13 +332,12 @@ def _create_behavior(self): sink_wp = self._map.get_waypoint(self._end_actor_flow) # Get all lanes - source_wps = get_same_dir_lanes(source_wp) sink_wps = get_same_dir_lanes(sink_wp) root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - for source_wp, sink_wp in zip(source_wps, sink_wps): - root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) + for wp in sink_wps: + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 4fa4192fc..5c8c28170 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -13,6 +13,8 @@ import carla +from agents.navigation.local_planner import RoadOption + from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes @@ -196,7 +198,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._road_front_vehicles = 2 # Amount of vehicles in front of the ego self._road_back_vehicles = 2 # Amount of vehicles behind the ego - self._radius_increase_ratio = 1.4 # Meters the radius increases per m/s of the ego + self._radius_increase_ratio = 1.7 # Meters the radius increases per m/s of the ego self._base_junction_detection = 30 self._detection_ratio = 1.5 # Meters the radius increases per m/s of the ego @@ -236,16 +238,20 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._scenario_max_speed = 0 # Max speed of the Background Activity. Deactivated with a value of 0 self._scenario_junction_entry = False # Flag indicating the ego is entering a junction self._scenario_junction_entry_distance = self._road_spawn_dist # Min distance between vehicles and ego + self._scenario_removed_lane = False # Flag indicating a scenario has removed a lane + self._scenario_remove_lane_offset = 0 self._route_sources_active = True def _get_route_data(self, route): """Extract the information from the route""" self._route = [] # Transform the route into a list of waypoints + self._route_options = [] # Extract the RoadOptions from the route self._accum_dist = [] # Save the total traveled distance for each waypoint prev_trans = None - for trans, _ in route: + for trans, option in route: self._route.append(self._map.get_waypoint(trans.location)) + self._route_options.append(option) if prev_trans: dist = trans.location.distance(prev_trans.location) self._accum_dist.append(dist + self._accum_dist[-1]) @@ -301,7 +307,9 @@ def update(self): self._update_junction_sources() else: self._update_road_actors() - self._update_road_sources(prev_ego_index) + + self._move_road_sources(prev_ego_index) + self._update_road_sources() self._monitor_topology_changes(prev_ego_index) self._monitor_road_changes(prev_ego_index) @@ -326,7 +334,7 @@ def terminate(self, new_status): def _check_background_actors(self): """Checks if the Traffic Manager has removed a backgroudn actor""" - alive_ids = [actor.id for actor in self._world.get_actors().filter('vehicle*')] + alive_ids = [actor.id for actor in CarlaDataProvider.get_all_actors().filter('vehicle*')] for actor in list(self._all_actors): if actor.id not in alive_ids: self._remove_actor_info(actor) @@ -947,6 +955,10 @@ def _monitor_incoming_junctions(self): self._initialise_connecting_lanes(junction) self._active_junctions.append(junction) + # Forget the fact that a lane was removed, so that it isn't readded in the middle of the junction + self._scenario_removed_lane = False + self._scenario_remove_lane_offset = 0 + def _monitor_ego_junction_exit(self): """ Monitors when the ego exits the junctions, preparing the road mode when that happens @@ -986,12 +998,9 @@ def _add_incoming_actors(self, junction, source): return actor - def _update_road_sources(self, prev_ego_index): + def _move_road_sources(self, prev_ego_index): """ - Manages the sources that spawn actors behind the ego. - These are always behind the ego and will continuously spawn actors. - These sources also track the amount of vehicles in front of the ego, - removing actors if the amount is too high. + Moves the road sources so that they are always following the ego from behind """ if prev_ego_index != self._route_index: min_distance = self._road_back_vehicles * self._road_spawn_dist @@ -1027,6 +1036,13 @@ def _update_road_sources(self, prev_ego_index): continue source.wp = new_source_wps[0] + def _update_road_sources(self): + """ + Manages the sources that spawn actors behind the ego. + These are always behind the ego and will continuously spawn actors. + These sources also track the amount of vehicles in front of the ego, + removing actors if the amount is too high. + """ for lane_key in self._road_dict: source = self._road_dict[lane_key] if self.debug: @@ -1062,9 +1078,12 @@ def _update_road_sources(self, prev_ego_index): if actor is None: continue - # Set their initial speed, so that they don't lag behind the ego + # Set their initial speed, so that they don't lag behind the ego. + # Set the speed to the ego's one, but never surpassing by the lane's last vehicle's one forward_vec = source.wp.transform.get_forward_vector() speed = self._ego_actor.get_velocity().length() + if len(source.actors): + speed = min(speed, source.actors[-1].get_velocity().length()) actor.set_target_velocity(speed * forward_vec) source.actors.append(actor) @@ -1803,6 +1822,8 @@ def _leave_crossing_space(self, collision_wp): def _remove_road_lane(self, lane_wp): """Removes a road lane""" + self._scenario_removed_lane = True + self._scenario_remove_lane_offset = 0 lane_key = get_lane_key(lane_wp) if lane_key not in list(self._road_dict): print(f"WARNING: Couldn't find the lane to be removed, '{lane_key}' isn't part of the road behavior") @@ -1815,6 +1836,11 @@ def _remove_road_lane(self, lane_wp): def _readd_road_lane(self, lane_offset): """Adds a ego road lane. This is expected to be used after having previously removed such lane""" + # Check that the ego hasn't moved close to a junction, where we don't want to reinitialize the lane + if not self._scenario_removed_lane: + return + + lane_offset += self._scenario_remove_lane_offset if lane_offset == 0: add_lane_wp = self._ego_wp @@ -1865,9 +1891,10 @@ def _readd_road_lane(self, lane_offset): actor.set_target_velocity(spawn_wp.transform.get_forward_vector() * ego_speed) actors.append(actor) - self._road_dict[self._ego_key] = Source( - prev_wp, actors, active=self._route_sources_active - ) + self._road_dict[add_lane_key] = Source(prev_wp, actors, active=self._route_sources_active) + + self._scenario_removed_lane = False + self._scenario_remove_lane_offset = 0 def _handle_junction_scenario(self, junction_data): """ @@ -1894,6 +1921,9 @@ def _handle_junction_scenario(self, junction_data): if extend_road_exit: self._extent_road_exit_space(extend_road_exit) + self._scenario_removed_lane = False + self._scenario_remove_lane_offset = 0 + def _clear_junction_middle(self): """Clears the junction, and all subsequent actors that enter it""" if self._active_junctions: @@ -2393,6 +2423,9 @@ def _set_actors_speed(self): speed = self._ego_target_speed * percentage / 100 if self._scenario_max_speed: speed = min(speed, self._scenario_max_speed) + + # TODO: Fix very high speed traffic + speed = min(speed, 90) self._tm.set_desired_speed(actor, speed) def _remove_actor_info(self, actor): @@ -2448,6 +2481,7 @@ def _update_ego_data(self): """ location = CarlaDataProvider.get_location(self._ego_actor) + prev_index = self._route_index for index in range(self._route_index, min(self._route_index + self._route_buffer, self._route_length)): route_wp = self._route[index] @@ -2458,12 +2492,27 @@ def _update_ego_data(self): if dot_ve_wp > 0: self._route_index = index + # Monitor route changes for those scenario that remove and readd a specific lane + if self._scenario_removed_lane: + for i in range(prev_index, self._route_index): + option_1 = self._route_options[i] + option_2 = self._route_options[i+1] + if option_1 == RoadOption.CHANGELANELEFT or option_2 == RoadOption.CHANGELANELEFT: + loc_1 = self._route[i].transform.location + loc_2 = self._route[i+1].transform.location + if abs(loc_1.distance(loc_2)) > 2.5: # Lane offset plus a bit forward + self._scenario_remove_lane_offset += 1 + elif option_1 == RoadOption.CHANGELANERIGHT or option_2 == RoadOption.CHANGELANERIGHT: + loc_1 = self._route[i].transform.location + loc_2 = self._route[i+1].transform.location + if abs(loc_1.distance(loc_2)) > 2.5: # Lane offset plus a bit forward + self._scenario_remove_lane_offset -= 1 + self._ego_wp = self._route[self._route_index] self._ego_key = get_lane_key(self._ego_wp) + self._ego_target_speed = self._ego_actor.get_speed_limit() if self.debug: string = 'EGO_' + self._ego_state[0].upper() debug_name = DEBUG_ROAD if self._ego_state == EGO_ROAD else DEBUG_JUNCTION draw_string(self._world, location, string, debug_name, False) - - self._ego_target_speed = self._ego_actor.get_speed_limit() diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index 06f515210..de2094290 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -92,12 +92,12 @@ def _spawn_side_prop(self, wp): if self._direction == "right": wp = prop_wp.get_right_lane() else: - wp = prop_wp.get_right_lane() + wp = prop_wp.get_left_lane() if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): break prop_wp = wp - displacement = prop_wp.lane_width / 2 + displacement = 0.3 * prop_wp.lane_width r_vec = prop_wp.transform.get_right_vector() if self._direction == 'left': r_vec *= -1 @@ -108,7 +108,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningconstruction', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - # signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(True) self.other_actors.append(signal_prop) def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_length=0, cone_offset=0): @@ -134,14 +134,14 @@ def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_leng def _create_construction_setup(self, start_transform, lane_width): """Create construction setup""" - _initial_offset = {'cones': {'yaw': 180, 'k': lane_width / 2.0}, + _initial_offset = {'cones': {'yaw': 270, 'k': lane_width / 2.0}, 'warning_sign': {'yaw': 180, 'k': 5, 'z': 0}, 'debris': {'yaw': 0, 'k': 2, 'z': 1}} _prop_names = {'warning_sign': 'static.prop.trafficwarning', 'debris': 'static.prop.dirtdebris02'} _perp_angle = 90 - _setup = {'lengths': [0, 6, 3], 'offsets': [0, 2, 1]} + _setup = {'lengths': [6, 3], 'offsets': [2, 1]} _z_increment = 0.1 # Traffic warning and debris @@ -171,8 +171,12 @@ def _create_construction_setup(self, start_transform, lane_width): start_transform.location, start_transform.rotation) side_transform.rotation.yaw += _perp_angle - side_transform.location -= _initial_offset['cones']['k'] * \ - side_transform.rotation.get_forward_vector() + offset_vec = _initial_offset['cones']['k'] * side_transform.rotation.get_forward_vector() + if self._direction == 'right': + side_transform.location -= offset_vec + else: + side_transform.location += offset_vec + side_transform.rotation.yaw += _initial_offset['cones']['yaw'] for i in range(len(_setup['lengths'])): @@ -184,7 +188,10 @@ def _create_construction_setup(self, start_transform, lane_width): cone_offset=_setup['offsets'][i]) side_transform.location += side_transform.get_forward_vector() * \ _setup['lengths'][i] * _setup['offsets'][i] - side_transform.rotation.yaw += _perp_angle + if i == 0 and self._direction == 'left': + side_transform.rotation.yaw -= _perp_angle + else: + side_transform.rotation.yaw += _perp_angle def _create_behavior(self): """ diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py index e8119a9c5..e7930420f 100644 --- a/srunner/scenarios/cross_bicycle_flow.py +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -116,7 +116,7 @@ def _initialize_actors(self, config): plan_step = 0 wp = self._source_wp while True: - next_wps = wp.next(1) + next_wps = wp.next(2) if not next_wps: raise ValueError("Couldn't find a proper plan for the bicycle flow") next_wp = next_wps diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 392c7d480..2c35b2d96 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -50,13 +50,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._reaction_time = 3.0 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 3.1 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 15.0 # Min distance to the collision location that triggers the adversary [m] self._back_vehicles = 2 self._front_vehicles = 3 self._vehicle_gap = 11 + self._speed = 60 # Km/h + self._adversary_end_distance = 70 self._side_transforms = [] @@ -65,7 +67,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._attributes = {'base_type': 'car', 'has_lights': True} self._blocker_distance = get_value_parameter(config, 'distance', float, 100) - self._speed = get_value_parameter(config, 'speed', float, 60 / 3.6) self._direction = get_value_parameter(config, 'direction', str, 'right') if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") @@ -121,17 +122,21 @@ def _initialize_actors(self, config): raise ValueError("Couldn't find a proper position for the cut in vehicle") blocker_wp = next_wps[0] - # Get the cut in behavior self._collision_wp = blocker_wp - next_wps = self._collision_wp.next(self._adversary_end_distance) - if not next_wps: - for actor in self.other_actors: - actor.destroy() - raise ValueError("Couldn't find a proper position for the cut in vehicle") - end_wp = next_wps[0] - self._plan = [[self._collision_wp, RoadOption.STRAIGHT], - [end_wp, RoadOption.STRAIGHT]] + # Get the cut in behavior + self._plan, dist, step = ([], 0, 5) + next_wp = self._collision_wp + while dist < self._adversary_end_distance: + next_wps = next_wp.next(step) + if not next_wps: + for actor in self.other_actors: + actor.destroy() + raise ValueError("Couldn't find a proper position for the cut in vehicle") + next_wp = next_wps[0] + self._plan.append([next_wp, RoadOption.STRAIGHT]) + + dist += step # Spawn the cut in vehicle side_wp = blocker_wp.get_left_lane() if self._direction == 'left' else blocker_wp.get_right_lane() @@ -201,7 +206,6 @@ def _create_behavior(self): total_dist = self._blocker_distance total_dist += self._vehicle_gap * (self._back_vehicles + self._front_vehicles + 1) sequence.add_child(LeaveSpaceInFront(total_dist)) - sequence.add_child(RemoveRoadLane(self._side_wp)) for actor, transform in self._side_transforms: sequence.add_child(ActorTransformSetter(actor, transform)) @@ -218,18 +222,21 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) + if self.route_mode: + sequence.add_child(RemoveRoadLane(self._side_wp)) + cut_in_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="CutIn") cut_in_direction = 'right' if self._direction == 'left' else 'left' cut_in_movement = py_trees.composites.Sequence() cut_in_movement.add_child(CutIn( - self._adversary_actor, self.ego_vehicles[0], cut_in_direction)) + self._adversary_actor, self.ego_vehicles[0], cut_in_direction, change_time=3, other_lane_time=2)) cut_in_movement.add_child(BasicAgentBehavior( - self._adversary_actor, plan=self._plan, target_speed=3.6 * self._speed)) + self._adversary_actor, plan=self._plan, target_speed=self._speed)) cut_in_behavior.add_child(cut_in_movement) - cut_in_behavior.add_child(Idle(self._adversary_end_distance / self._speed)) + cut_in_behavior.add_child(Idle(30)) # One minute timeout in case a collision happened sequence.add_child(cut_in_behavior) diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index 676a7f230..c6f5376e5 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -59,7 +59,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._other_lane_time = 3 self._change_time = 2 self._speed_perc = 80 - self._cut_in_distance = 15 + self._cut_in_distance = 7 self._extra_space = 170 self._start_location = convert_dict_to_location(config.other_parameters['other_actor_location']) diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index ad5eb06f9..c30e7fde5 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -13,6 +13,7 @@ import math import py_trees import carla +from math import floor from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, @@ -352,7 +353,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._num_lane_changes = 0 self._adversary_speed = 2.0 # Speed of the adversary [m/s] - self._reaction_time = 2.1 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout @@ -367,6 +367,10 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals if self._direction not in ('left', 'right'): raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") + # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.2 + self._reaction_time += 0.1 * floor(self._crossing_angle / 5) + super().__init__("ParkingCrossingPedestrian", ego_vehicles, config, @@ -420,6 +424,7 @@ def _initialize_actors(self, config): # Get the adversary transform and spawn it self._blocker_transform = self._get_blocker_transform(blocker_wp) + self._remove_parked_vehicles(self._blocker_transform.location) blocker = CarlaDataProvider.request_new_actor( 'vehicle.*', self._blocker_transform, attribute_filter=self._bp_attributes) if blocker is None: @@ -435,6 +440,7 @@ def _initialize_actors(self, config): # Get the adversary transform and spawn it self._walker_transform = self._get_walker_transform(walker_wp) + self._remove_parked_vehicles(self._walker_transform.location) walker = CarlaDataProvider.request_new_actor('walker.*', self._walker_transform) if walker is None: raise ValueError("Couldn't spawn the adversary") @@ -442,6 +448,15 @@ def _initialize_actors(self, config): self._collision_wp = walker_wp + def _remove_parked_vehicles(self, actor_location): + """Removes the parked vehicles that might have conflicts with the scenario""" + parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) + vehicles_to_destroy = set() + for v in parked_vehicles: + if v.transform.location.distance(actor_location) < 10: + vehicles_to_destroy.add(v) + self.world.enable_environment_objects(vehicles_to_destroy, False) + def _create_behavior(self): """ After invoking this scenario, cyclist will wait for the user diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index 6e77ea1dd..770a2970f 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -74,6 +74,8 @@ def _initialize_actors(self, config): else: parking_wp = self._blocker_wp.get_right_lane() + self._remove_parked_vehicles(parking_wp.transform.location) + self._blocker_actor = CarlaDataProvider.request_new_actor( 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) if not self._blocker_actor: @@ -95,6 +97,8 @@ def _initialize_actors(self, config): else: parking_wp = self._collision_wp.get_right_lane() + self._remove_parked_vehicles(parking_wp.transform.location) + self._parked_actor = CarlaDataProvider.request_new_actor( 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) if not self._parked_actor: @@ -104,6 +108,15 @@ def _initialize_actors(self, config): side_location = self._get_displaced_location(self._parked_actor, parking_wp) self._parked_actor.set_location(side_location) + def _remove_parked_vehicles(self, actor_location): + """Removes the parked vehicles that might have conflicts with the scenario""" + parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) + vehicles_to_destroy = set() + for v in parked_vehicles: + if v.transform.location.distance(actor_location) < 10: + vehicles_to_destroy.add(v) + self.world.enable_environment_objects(vehicles_to_destroy, False) + def _get_displaced_location(self, actor, wp): """ Calculates the location such that the actor is at the sidemost part of the lane diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 1f886a472..dc3fc7c07 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -109,6 +109,7 @@ def _initialize_actors(self, config): """ Custom initialization """ + # Spawn the actor in front of the ego front_points = self._parking_waypoint.next( self._front_vehicle_distance) @@ -116,6 +117,8 @@ def _initialize_actors(self, config): raise ValueError( "Couldn't find viable position for the vehicle in front of the parking point") + self._remove_parked_vehicles(front_points[0].transform.location) + actor_front = CarlaDataProvider.request_new_actor( 'vehicle.*', front_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_front is None: @@ -135,6 +138,8 @@ def _initialize_actors(self, config): raise ValueError( "Couldn't find viable position for the vehicle behind the parking point") + self._remove_parked_vehicles(behind_points[0].transform.location) + actor_behind = CarlaDataProvider.request_new_actor( 'vehicle.*', behind_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_behind is None: @@ -149,8 +154,9 @@ def _initialize_actors(self, config): actor_behind.set_location(side_location) # Move the ego to its side position - self._ego_transform = self._get_displaced_location(self.ego_vehicles[0], self._parking_waypoint) - self.ego_vehicles[0].set_location(self._ego_transform) + self._ego_location = self._get_displaced_location(self.ego_vehicles[0], self._parking_waypoint) + self._remove_parked_vehicles(self._ego_location) + self.ego_vehicles[0].set_location(self._ego_location) # Spawn the actor at the side of the ego actor_side = CarlaDataProvider.request_new_actor( @@ -164,6 +170,15 @@ def _initialize_actors(self, config): self._end_side_location = self.ego_vehicles[0].get_transform() self._end_side_location.location.z -= 500 + def _remove_parked_vehicles(self, actor_location): + """Removes the parked vehicles that might have conflicts with the scenario""" + parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) + vehicles_to_destroy = set() + for v in parked_vehicles: + if v.transform.location.distance(actor_location) < 10: + vehicles_to_destroy.add(v) + self.world.enable_environment_objects(vehicles_to_destroy, False) + def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index f48b2619f..0e2d34c8d 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -56,14 +56,14 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._adversary_speed = 1.1 # Speed of the adversary [m/s] + self._adversary_speed = 1.3 # Speed of the adversary [m/s] self._reaction_time = 4.0 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 12.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout self._crosswalk_dist = 1 - self._crosswalk_right_dist = 1.5 + self._crosswalk_right_dist = 1 super().__init__("PedestrianCrossing", ego_vehicles, @@ -101,29 +101,9 @@ def _initialize_actors(self, config): spawn_loc = start_wp.transform.location + carla.Location( self._crosswalk_dist * start_vec.x + self._crosswalk_right_dist * start_right_vec.x, self._crosswalk_dist * start_vec.y + self._crosswalk_right_dist * start_right_vec.y, - self._crosswalk_dist * start_vec.z + self._crosswalk_right_dist * start_right_vec.z + 0.5 + self._crosswalk_dist * start_vec.z + self._crosswalk_right_dist * start_right_vec.z + 1.0 ) - # left_wp = get_opposite_dir_lanes(collision_wp)[-1] - - # # Get the crosswalk end point - # end_wp = left_wp - # while end_wp.lane_type != carla.LaneType.Sidewalk: - # wp = end_wp.get_right_lane() - # if wp is None: - # raise ValueError("Couldn't find a waypoint to start the flow") - # end_wp = wp - - # # Displace it to the crosswalk. Move backwards towards the crosswalk - # end_vec = end_wp.transform.get_forward_vector() - # end_right_vec = end_wp.transform.get_right_vector() - - # end_loc = end_wp.transform.location + carla.Location( - # - self._crosswalk_dist * end_vec.x + self._crosswalk_right_dist * end_right_vec.x, - # - self._crosswalk_dist * end_vec.y + self._crosswalk_right_dist * end_right_vec.y, - # - self._crosswalk_dist * end_vec.z + self._crosswalk_right_dist * end_right_vec.z - # ) - spawn_rotation = start_wp.transform.rotation spawn_rotation.yaw += 270 spawn_transform = carla.Transform(spawn_loc, spawn_rotation) diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index eed6b1ff5..bf658d260 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -90,12 +90,12 @@ def _spawn_side_prop(self, wp): if self._direction == "right": wp = prop_wp.get_right_lane() else: - wp = prop_wp.get_right_lane() + wp = prop_wp.get_left_lane() if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): break prop_wp = wp - displacement = prop_wp.lane_width / 2 + displacement = 0.3 * prop_wp.lane_width r_vec = prop_wp.transform.get_right_vector() if self._direction == 'left': r_vec *= -1 @@ -106,7 +106,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - # signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(True) self.other_actors.append(signal_prop) def _spawn_obstacle(self, wp, blueprint, accident_actor=False): @@ -313,12 +313,12 @@ def _spawn_side_prop(self, wp): if self._direction == "right": wp = prop_wp.get_right_lane() else: - wp = prop_wp.get_right_lane() + wp = prop_wp.get_left_lane() if wp is None or wp.lane_type not in (carla.LaneType.Driving, carla.LaneType.Parking): break prop_wp = wp - displacement = prop_wp.lane_width / 2 + displacement = 0.3 * prop_wp.lane_width r_vec = prop_wp.transform.get_right_vector() if self._direction == 'left': r_vec *= -1 @@ -329,7 +329,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - # signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(True) self.other_actors.append(signal_prop) def _spawn_obstacle(self, wp, blueprint): diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index a91ca60d4..fb278574d 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -91,6 +91,8 @@ def _initialize_actors(self, config): if parked_wp is None: raise ValueError("Couldn't find a spot to place the adversary vehicle") + self._remove_parked_vehicles(parked_wp.transform.location) + self._parked_actor = CarlaDataProvider.request_new_actor( "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) if not self._parked_actor: @@ -136,6 +138,15 @@ def _create_behavior(self): return sequence + def _remove_parked_vehicles(self, actor_location): + """Removes the parked vehicles that might have conflicts with the scenario""" + parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) + vehicles_to_destroy = set() + for v in parked_vehicles: + if v.transform.location.distance(actor_location) < 10: + vehicles_to_destroy.add(v) + self.world.enable_environment_objects(vehicles_to_destroy, False) + def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py index 75dec88e9..8520f132c 100644 --- a/srunner/scenarios/yield_to_emergency_vehicle.py +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -40,16 +40,16 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._ev_drive_time = 12 # seconds + self._ev_drive_time = 10 # seconds # km/h. How much the EV is expected to be faster than the EGO - self._speed_increment = 15 + self._speed_increment = 10 if 'distance' in config.other_parameters: self._distance = float( config.other_parameters['distance']['value']) else: - self._distance = 100 # m + self._distance = 80 # m self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint( @@ -115,15 +115,12 @@ def _create_behavior(self): self.other_actors[0], self._ev_start_transform)) # Emergency Vehicle runs for self._ev_drive_time seconds - ev_end_condition = py_trees.composites.Parallel("Waiting for emergency vehicle driving for a certein distance", - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - - ev_end_condition.add_child(Idle(self._ev_drive_time)) - - ev_end_condition.add_child(AdaptiveConstantVelocityAgentBehavior( + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(Idle(self._ev_drive_time)) + main_behavior.add_child(AdaptiveConstantVelocityAgentBehavior( self.other_actors[0], self.ego_vehicles[0], speed_increment=self._speed_increment)) - sequence.add_child(ev_end_condition) + sequence.add_child(main_behavior) sequence.add_child(ActorDestroy(self.other_actors[0])) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 65e239737..50d4c0a0b 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -261,7 +261,7 @@ def get_traffic_light_from_osc_name(name): # Given by id if name.startswith("id="): tl_id = int(name[3:]) - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + for carla_tl in CarlaDataProvider.get_all_actors().filter('traffic.traffic_light'): if carla_tl.id == tl_id: traffic_light = carla_tl break @@ -269,7 +269,7 @@ def get_traffic_light_from_osc_name(name): elif name.startswith("pos="): tl_pos = name[4:] pos = tl_pos.split(",") - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + for carla_tl in CCarlaDataProvider.get_all_actors().filter('traffic.traffic_light'): carla_tl_location = carla_tl.get_transform().location distance = carla_tl_location.distance(carla.Location(float(pos[0]), float(pos[1]), @@ -621,7 +621,7 @@ def convert_position_to_transform(position, actor_list=None): obj_actor = actor actor_transform = actor.transform else: - for actor in CarlaDataProvider.get_world().get_actors(): + for actor in CarlaDataProvider.get_all_actors(): if 'role_name' in actor.attributes and actor.attributes['role_name'] == obj: obj_actor = actor actor_transform = obj_actor.get_transform() diff --git a/srunner/tools/scenario_helper.py b/srunner/tools/scenario_helper.py index ab8e98a38..b144f0d81 100644 --- a/srunner/tools/scenario_helper.py +++ b/srunner/tools/scenario_helper.py @@ -492,8 +492,7 @@ def detect_lane_obstacle(actor, extension_factor=3, margin=1.02): """ This function identifies if an obstacle is present in front of the reference actor """ - world = CarlaDataProvider.get_world() - world_actors = world.get_actors().filter('vehicle.*') + world_actors = CarlaDataProvider.get_all_actors().filter('vehicle.*') actor_bbox = actor.bounding_box actor_transform = actor.get_transform() actor_location = actor_transform.location @@ -607,7 +606,7 @@ def get_closest_traffic_light(waypoint, traffic_lights=None): Checks all traffic lights part of 'traffic_lights', or all the town ones, if None are passed. """ if not traffic_lights: - traffic_lights = CarlaDataProvider.get_world().get_actors().filter('*traffic_light*') + traffic_lights = CarlaDataProvider.get_all_actors().filter('*traffic_light*') closest_dist = float('inf') closest_tl = None From 7ccbe74f1e9239f866bdfcbefc4740bb356f6d1b Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 1 Aug 2022 17:34:19 +0200 Subject: [PATCH 44/86] Scenarios no longer spawn old vehicles --- .../scenarioatomics/atomic_criteria.py | 1 - srunner/scenarios/actor_flow.py | 1 + srunner/scenarios/background_activity.py | 19 ++++++++++++++----- srunner/scenarios/parking_exit.py | 2 +- srunner/scenarios/route_obstacles.py | 4 ++-- 5 files changed, 18 insertions(+), 9 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index a90e38e89..9befbdef2 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -2108,7 +2108,6 @@ def terminate(self, new_status): self.events.append(traffic_event) self._terminated = True - print(f"ACTUAL VALUE: {self.actual_value}") super().terminate(new_status) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index fc235d6a2..7e57a6731 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -490,6 +490,7 @@ def _get_entry_exit_route_lanes(self, wp, route): index = route_index dist = route_dist + reached_junction = False for i in range(index, len(route)): route_transform, road_option = route[i] diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 5c8c28170..2d3215e1c 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -22,7 +22,8 @@ JUNCTION_ENTRY = 'entry' JUNCTION_MIDDLE = 'middle' JUNCTION_EXIT = 'exit' -JUNCTION_ROAD = 'road' +JUNCTION_EXIT_ROAD = 'exit_road' +JUNCTION_EXIT_INACTIVE = 'exit_inactive' EGO_JUNCTION = 'junction' EGO_ROAD = 'road' @@ -218,7 +219,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._active_junctions = [] # List of all the active junctions self._junction_sources_dist = 40 # Distance from the entry sources to the junction [m] - self._junction_sources_max_actors = 4 # Maximum vehicles alive at the same time per source + self._junction_sources_max_actors = 6 # Maximum vehicles alive at the same time per source self._junction_spawn_dist = 15 # Distance between spawned vehicles [m] self._junction_source_perc = 70 # Probability [%] of the source being created @@ -2060,7 +2061,7 @@ def _extent_road_exit_space(self, space): actors = junction.exit_dict[exit_lane_key]['actors'] self._move_actors_forward(actors, space) for actor in actors: - if junction.actor_dict[actor]['state'] == JUNCTION_ROAD: + if junction.actor_dict[actor]['state'] == JUNCTION_EXIT_ROAD: self._actors_speed_perc[actor] = 100 junction.actor_dict[actor]['state'] = JUNCTION_EXIT @@ -2383,13 +2384,21 @@ def _update_junction_actors(self): elif state == JUNCTION_EXIT: distance = location.distance(exit_dict[exit_lane_key]['ref_wp'].transform.location) if distance > exit_dict[exit_lane_key]['max_distance']: - actor_dict[actor]['state'] = JUNCTION_ROAD + if exit_lane_key in junction.route_exit_keys: + actor_dict[actor]['state'] = JUNCTION_EXIT_ROAD + else: + self._actors_speed_perc[actor] = 0 + actor_dict[actor]['state'] = JUNCTION_EXIT_INACTIVE # Set them ready to move so that the ego can smoothly cross the junction - elif state == JUNCTION_ROAD: + elif state == JUNCTION_EXIT_ROAD: self._set_road_actor_speed(location, actor, multiplier=1.5) pass + # Wait + elif state == JUNCTION_EXIT_INACTIVE: + pass + def _update_opposite_actors(self): """ Updates the opposite actors. This involves tracking their position, diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index dc3fc7c07..f41bddd60 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -71,7 +71,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl CarlaDataProvider.get_traffic_manager_port()) self.timeout = timeout - self._bp_attributes = {'base_type': 'car'} + self._bp_attributes = {'base_type': 'car', 'generation': 2} self._side_end_distance = 50 self._front_vehicle_distance = get_value_parameter(config, 'front_vehicle_distance', float, 20) diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index bf658d260..586887376 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -122,7 +122,7 @@ def _spawn_obstacle(self, wp, blueprint, accident_actor=False): spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=1) if accident_actor: actor = CarlaDataProvider.request_new_actor( - blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car'}) + blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car', 'generation': 2}) else: actor = CarlaDataProvider.request_new_actor( blueprint, spawn_transform, rolename='scenario') @@ -344,7 +344,7 @@ def _spawn_obstacle(self, wp, blueprint): spawn_transform = wp.transform spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=1) actor = CarlaDataProvider.request_new_actor( - blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car'}) + blueprint, spawn_transform, rolename='scenario no lights', attribute_filter={'base_type': 'car', 'generation': 2}) if not actor: raise ValueError("Couldn't spawn an obstacle actor") From d63f73d03e96c0e32e7a85d9fc68e0b9acd61dc2 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 3 Aug 2022 17:42:18 +0200 Subject: [PATCH 45/86] Added variable flow to route obstacles --- .../scenarioatomics/atomic_behaviors.py | 289 +++++++++++++++- .../atomic_trigger_conditions.py | 55 ++- srunner/scenarios/background_activity.py | 5 +- .../scenarios/construction_crash_vehicle.py | 127 ++++--- srunner/scenarios/invading_turn.py | 157 +++++---- srunner/scenarios/object_crash_vehicle.py | 7 +- srunner/scenarios/route_obstacles.py | 317 +++++++++--------- .../signalized_junction_left_turn.py | 4 +- srunner/scenarios/vehicle_opens_door.py | 221 +++++------- srunner/tools/background_manager.py | 56 ++++ 10 files changed, 794 insertions(+), 444 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 0c51d0c8d..dc36a8eb7 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2147,6 +2147,28 @@ def update(self): return new_status +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 + + Alternatively, a parallel termination behavior has to be used to stop it. + """ + + def __init__(self, name="WaitForever"): + """ + Setup actor + """ + super().__init__(name) + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def update(self): + """ + wait forever + """ + return py_trees.common.Status.RUNNING + class WaypointFollower(AtomicBehavior): @@ -2743,6 +2765,7 @@ class ActorFlow(AtomicBehavior): - spawn_distance: Distance between spawned actors - sink_distance: Actors closer to the sink than this distance will be deleted - actors_speed: Speed of the actors part of the flow [m/s] + - add_initial_actors: Populates all the flow trajectory at the start """ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, @@ -2750,7 +2773,7 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, """ Setup class members """ - super(ActorFlow, self).__init__(name) + 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()) @@ -2805,21 +2828,23 @@ def _spawn_actor(self, transform): self._tm.set_desired_speed(actor, 3.6 * self._speed) self._tm.update_vehicle_lights(actor, True) + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + 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 - self._actor_list.append(actor) - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) + sensor.listen(lambda _: self.stop_constant_velocity()) - sensor = self._world.spawn_actor(self._collision_bp, carla.Transform(), attach_to=actor) - sensor.listen(lambda _: self.stop_constant_velocity()) self._collision_sensor_list.append(sensor) + self._actor_list.append(actor) def update(self): """Controls the created actors and creaes / removes other when needed""" # Control the vehicles, removing them when needed - for actor in list(self._actor_list): + for actor, sensor in zip(list(self._actor_list), list(self._collision_sensor_list)): location = CarlaDataProvider.get_location(actor) if not location: continue @@ -2827,16 +2852,17 @@ def update(self): if sink_distance < self._sink_dist: actor.destroy() self._actor_list.remove(actor) + if sensor is not None: + sensor.stop() + sensor.destroy() + self._collision_sensor_list.remove(sensor) # Spawn new actors if needed if len(self._actor_list) == 0: distance = self._spawn_dist + 1 else: actor_location = CarlaDataProvider.get_location(self._actor_list[-1]) - if actor_location is None: - distance = 0 - else: - distance = self._source_location.distance(actor_location) + distance = self._source_location.distance(actor_location) if actor_location else 0 if distance > self._spawn_dist: self._spawn_actor(self._source_transform) @@ -2860,6 +2886,249 @@ def terminate(self, new_status): except RuntimeError: pass # Actor was already destroyed + for sensor in self._collision_sensor_list: + if sensor is None: + continue + try: + sensor.stop() + sensor.destroy() + except RuntimeError: + pass # Actor was already destroyed + + +class OppositeActorFlow(AtomicBehavior): + """ + Similar to ActorFlow, but this is meant as an actor flow in the opposite direction. + As such, some configurations are different and for clarity, another behavior has been created + + Important parameters: + - source_wp (carla.Waypoint): Waypoint at which actors will be spawned + - sink_wp (carla.Waypoint): Waypoint at which actors will be deleted + - spawn_dist_interval: Distance interval between spawned actors + - sink_dist: Actors closer to the sink than this distance will be deleted + - actors_speed: Speed of the actors part of the flow [m/s] + - offset: offset from the center lane of the actors + """ + + def __init__(self, reference_wp, reference_actor, spawn_dist_interval, + time_distance=3.5, 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._reference_wp = reference_wp + self._reference_actor = reference_actor + self._time_distance = time_distance + + self._min_spawn_dist = spawn_dist_interval[0] + self._max_spawn_dist = spawn_dist_interval[1] + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + self._sink_dist = sink_dist + + # Opposite direction needs earlier vehicle detection + self._opt_dict = {'base_vehicle_threshold': 10, 'detection_speed_ratio': 1.6} + + self._actor_list = [] + self._grp = CarlaDataProvider.get_global_route_planner() + self._map = CarlaDataProvider.get_map() + + def _move_waypoint_forward(self, wp, distance): + """Moves forward a certain distance, stopping at junctions""" + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + + return next_wp + + def _move_waypoint_backwards(self, wp, distance): + """Moves backwards a certain distance, stopping at junctions""" + dist = 0 + prev_wp = wp + while dist < distance: + prev_wps = prev_wp.previous(1) + if prev_wps[0].is_junction: + break + prev_wp = prev_wps[0] + dist += 1 + + return prev_wp + + def initialise(self): + """Get the actor flow source and sink, depending on the reference actor speed""" + self._speed = self._reference_actor.get_speed_limit() + self._flow_distance = self._time_distance * self._speed + + 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 + self._sink_location = self._sink_wp.transform.location + + self._route = self._grp.trace_route(self._source_location, self._sink_location) + + return super().initialise() + + def _spawn_actor(self): + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._source_transform, rolename='scenario', + attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + ) + if actor is None: + return py_trees.common.Status.RUNNING + + controller = BasicAgent(actor, 3.6 * self._speed, self._opt_dict, self._map, self._grp) + controller.set_global_plan(self._route) + self._actor_list.append([actor, controller]) + + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + + def update(self): + """Controls the created actors and creates / removes other when needed""" + # Control the vehicles, removing them when needed + for actor_data in list(self._actor_list): + actor, controller = actor_data + location = CarlaDataProvider.get_location(actor) + if not location: + continue + sink_distance = self._sink_location.distance(location) + if sink_distance < self._sink_dist: + actor.destroy() + self._actor_list.remove(actor_data) + else: + actor.apply_control(controller.run_step()) + + # Spawn new actors if needed + if len(self._actor_list) == 0: + 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 + if distance > self._spawn_dist: + self._spawn_actor() + + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + """ + Default terminate. Can be extended in derived class + """ + for actor, _ in self._actor_list: + try: + actor.destroy() + except RuntimeError: + pass # Actor was already destroyed + + +class InvadingActorFlow(AtomicBehavior): + """ + Similar to ActorFlow, but this is meant as an actor flow in the opposite direction that invades the lane. + As such, some configurations are different and for clarity, another behavior has been created + + Important parameters: + - source_wp (carla.Waypoint): Waypoint at which actors will be spawned + - sink_wp (carla.Waypoint): Waypoint at which actors will be deleted + - spawn_dist_interval: Distance interval between spawned actors + - sink_dist: Actors closer to the sink than this distance will be deleted + - actors_speed: Speed of the actors part of the flow [m/s] + - 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"): + """ + Setup class members + """ + super().__init__(name) + self._world = CarlaDataProvider.get_world() + self._tm = CarlaDataProvider.get_client().get_trafficmanager(CarlaDataProvider.get_traffic_manager_port()) + + self._reference_actor = reference_actor + + self._source_wp = source_wp + self._source_transform = self._source_wp.transform + self._source_location = self._source_transform.location + + self._sink_wp = sink_wp + self._sink_location = self._sink_wp.transform.location + + self._spawn_dist = spawn_dist + + self._sink_dist = sink_dist + + 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._grp = CarlaDataProvider.get_global_route_planner() + self._map = CarlaDataProvider.get_map() + + def initialise(self): + """Get the actor flow source and sink, depending on the reference actor speed""" + self._speed = self._reference_actor.get_speed_limit() + self._route = self._grp.trace_route(self._source_location, self._sink_location) + return super().initialise() + + def _spawn_actor(self): + actor = CarlaDataProvider.request_new_actor( + 'vehicle.*', self._source_transform, rolename='scenario', + attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + ) + if actor is None: + return py_trees.common.Status.RUNNING + + controller = BasicAgent(actor, 3.6 * self._speed, self._opt_dict, self._map, self._grp) + controller.set_global_plan(self._route) + self._actor_list.append([actor, controller]) + + def update(self): + """Controls the created actors and creates / removes other when needed""" + # Control the vehicles, removing them when needed + for actor_data in list(self._actor_list): + actor, controller = actor_data + location = CarlaDataProvider.get_location(actor) + if not location: + continue + sink_distance = self._sink_location.distance(location) + if sink_distance < self._sink_dist: + actor.destroy() + self._actor_list.remove(actor_data) + else: + actor.apply_control(controller.run_step()) + + # Spawn new actors if needed + if len(self._actor_list) == 0: + 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 + if distance > self._spawn_dist: + self._spawn_actor() + + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + """ + Default terminate. Can be extended in derived class + """ + for actor, _ in self._actor_list: + try: + actor.destroy() + except RuntimeError: + pass # Actor was already destroyed + class BicycleFlow(AtomicBehavior): """ diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index 9eb0867fa..e1a81af1f 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -1045,9 +1045,8 @@ def update(self): # Wait for the vehicle to be in front other_dir = other_next_waypoint.transform.get_forward_vector() act_other_dir = actor_location - other_next_waypoint.transform.location - dot_ve_wp = other_dir.x * act_other_dir.x + other_dir.y * act_other_dir.y + other_dir.z * act_other_dir.z - if dot_ve_wp > 0.0: + if other_dir.dot(act_other_dir) > 0.0: in_front = True # Wait for it to be close-by @@ -1062,6 +1061,58 @@ def update(self): return new_status +class WaitUntilInFrontPosition(AtomicCondition): + + """ + Behavior that support the creation of cut ins. It waits until the actor has passed another actor + + Parameters: + - actor: the one getting in front of the other actor + - transform: the reference transform that the actor will have to get in front of + """ + + def __init__(self, actor, transform, check_distance=True, name="WaitUntilInFrontPosition"): + """ + Init + """ + super().__init__(name) + + self._actor = actor + self._ref_transform = transform + self._ref_location = transform.location + self._ref_vector = transform.get_forward_vector() + self._check_distance = check_distance + + self._distance = 10 + + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def update(self): + """ + Checks if the two actors meet the requirements + """ + new_status = py_trees.common.Status.RUNNING + + # Is the actor in front? + location = CarlaDataProvider.get_location(self._actor) + if location is None: + return new_status + + actor_dir = location - self._ref_location + 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: + close_by = True + else: + close_by = False + + if in_front and close_by: + new_status = py_trees.common.Status.SUCCESS + + return new_status + + class DriveDistance(AtomicCondition): """ diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 2d3215e1c..acb2669f4 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -148,7 +148,7 @@ def __init__(self, junction, junction_id, route_entry_index=None, route_exit_ind self.inactive_exit_keys = [] def contains_wp(self, wp): - """Checks whether or not a carla.Junction is part of the class""" + """Checks whether or not a carla.Waypoint is inside the junction""" if not wp.is_junction: return False other_id = wp.get_junction().id @@ -229,8 +229,8 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._opposite_sources = [] self._opposite_route_index = 0 - self._opposite_sources_dist = 100 # Distance from the ego to the opposite sources [m] self._opposite_spawn_dist = 40 # Distance between spawned vehicles [m] + self._opposite_sources_dist = 80 # Distance from the ego to the opposite sources [m]. Twice the spawn distance self._active_opposite_sources = True # Flag to (de)activate all opposite sources @@ -1580,6 +1580,7 @@ def _update_parameters(self): self._opposite_sources_dist = source_dist if spawn_dist is not None: self._opposite_spawn_dist = spawn_dist + self._opposite_sources_dist = 2 * spawn_dist if active is not None: self._active_opposite_sources = active for source in self._opposite_sources: diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index de2094290..6999d8518 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -18,14 +18,16 @@ ActorTransformSetter, SwitchWrongDirectionTest, ScenarioTimeout, - Idle) -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTriggerDistanceToLocation + Idle, WaitForever, + OppositeActorFlow) +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + WaitUntilInFrontPosition) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import (ChangeOppositeBehavior, - RemoveRoadLane, +from srunner.tools.background_manager import (RemoveRoadLane, ReAddRoadLane, - SetMaxSpeed) + SetMaxSpeed, + ChangeOppositeBehavior) def get_value_parameter(config, name, p_type, default): @@ -34,6 +36,14 @@ def get_value_parameter(config, name, p_type, default): else: return default +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default class ConstructionObstacle(BasicScenario): """ @@ -56,10 +66,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._trigger_distance = 30 - self._takeover_max_dist = 60 - self._drive_distance = self._trigger_distance + self._takeover_max_dist - self._opposite_wait_duration = 5 + self._end_distance = 50 self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location) self._construction_wp = None @@ -85,6 +93,19 @@ def _initialize_actors(self, config): self._construction_wp = wps[0] self._create_construction_setup(self._construction_wp.transform, self._reference_waypoint.lane_width) + self._end_wp = self._move_waypoint_forward(self._construction_wp, self._end_distance) + + def _move_waypoint_forward(self, wp, distance): + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if not next_wps or next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + return next_wp + def _spawn_side_prop(self, wp): """Spawn the accident indication signal""" prop_wp = wp @@ -144,7 +165,7 @@ def _create_construction_setup(self, start_transform, lane_width): _setup = {'lengths': [6, 3], 'offsets': [2, 1]} _z_increment = 0.1 - # Traffic warning and debris + # Traffic warning and debris for key, value in _initial_offset.items(): if key == 'cones': continue @@ -198,52 +219,37 @@ def _create_behavior(self): Remove the lane that would collide with the construction and add the construction props. Wait until the ego is close to the construction (and a bit more) before changing the side traffic Readd the traffic at the end - - Sequence: - - RemoveRoadLane - - TimeoutParallel (Parallel): - - ScenarioTimeout - - Behavior (Sequence): - - ActorTransformSetter (1 for each prop) - - InTriggerDistanceToLocation - - Idle - - BA stuff - - ActorDestory - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - ReAddRoadLane """ root = py_trees.composites.Sequence(name="ConstructionObstacle") if self.route_mode: root.add_child(RemoveRoadLane(self._reference_waypoint)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + for actor, transform in self._construction_transforms: + root.add_child(ActorTransformSetter(actor, transform, True)) + + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() - for actor, transform in self._construction_transforms: - behavior.add_child(ActorTransformSetter(actor, transform, True)) behavior.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], self._construction_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._opposite_wait_duration)) - if self.route_mode: behavior.add_child(SetMaxSpeed(self._max_speed)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + behavior.add_child(WaitForever()) - timeout_parallel.add_child(behavior) - - root.add_child(timeout_parallel) - for i, _ in enumerate(self.other_actors): - root.add_child(ActorDestroy(self.other_actors[i])) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: - root.add_child(SetMaxSpeed(self._max_speed)) + root.add_child(SetMaxSpeed(0)) root.add_child(ReAddRoadLane(0)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root - def _create_test_criteria(self): """ A list of all test criteria will be created that is later used @@ -267,8 +273,7 @@ class ConstructionObstacleTwoWays(ConstructionObstacle): """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=60): - self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) - + self._opposite_interval = get_interval_parameter(config, 'frequency', float, [20, 100]) super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): @@ -276,49 +281,39 @@ def _create_behavior(self): Remove the lane that would collide with the construction and add the construction props. Wait until the ego is close to the construction (and a bit more) before changing the opposite traffic Readd the traffic at the end, and allow the ego to invade the opposite lane by deactivating the criteria - - Sequence: - - RemoveRoadLane - - TimeoutParallel (Parallel): - - ScenarioTimeout - - Behavior (Sequence): - - ActorTransformSetter (1 for each prop) - - InTriggerDistanceToLocation - - Idle - - BA stuff - - ActorDestory - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - ReAddRoadLane """ - root = py_trees.composites.Sequence(name="ConstructionObstacle") + reference_wp = self._construction_wp.get_left_lane() + if not reference_wp: + raise ValueError("Couldnt find a left lane to spawn the opposite traffic") + + root = py_trees.composites.Sequence(name="ConstructionObstacleTwoWays") if self.route_mode: root.add_child(RemoveRoadLane(self._reference_waypoint)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + for actor, transform in self._construction_transforms: + root.add_child(ActorTransformSetter(actor, transform, True)) + + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() - for actor, transform in self._construction_transforms: - behavior.add_child(ActorTransformSetter(actor, transform, True)) behavior.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], self._construction_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._opposite_wait_duration)) - if self.route_mode: behavior.add_child(SwitchWrongDirectionTest(False)) - behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - - timeout_parallel.add_child(behavior) + behavior.add_child(ChangeOppositeBehavior(active=False)) + behavior.add_child(OppositeActorFlow(reference_wp, self.ego_vehicles[0], self._opposite_interval)) - root.add_child(timeout_parallel) - for i, _ in enumerate(self.other_actors): - root.add_child(ActorDestroy(self.other_actors[i])) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=40)) + root.add_child(ChangeOppositeBehavior(active=True)) root.add_child(ReAddRoadLane(0)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py index a36a83178..303cad363 100644 --- a/srunner/scenarios/invading_turn.py +++ b/srunner/scenarios/invading_turn.py @@ -16,14 +16,14 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (InvadingActorFlow, + ScenarioTimeout, ActorDestroy, - BasicAgentBehavior, - ScenarioTimeout) -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance + ActorTransformSetter) +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitUntilInFrontPosition from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import RemoveRoadLane, ChangeOppositeBehavior, ReAddRoadLane def convert_dict_to_location(actor_dict): @@ -38,6 +38,13 @@ def convert_dict_to_location(actor_dict): return location +def get_value_parameter(config, name, p_type, default): + if name in config.other_parameters: + return p_type(config.other_parameters[name]['value']) + else: + return default + + class InvadingTurn(BasicScenario): """ This class holds everything required for a scenario in which the ego is about to turn right @@ -60,92 +67,100 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._reference_waypoint = self._map.get_waypoint( self._trigger_location) - self._speed = 30 # Km/h - - # Distance between the trigger point and the start location of the adversary - if 'distance' in config.other_parameters: - self._distance = float( - config.other_parameters['distance']['value']) - else: - self._distance = 120 # m - - self._adversary_end = self._reference_waypoint.get_left_lane().transform.location - - # The width (m) of the vehicle invading the opposite lane. - if 'offset' in config.other_parameters: - self._offset = float( - config.other_parameters['offset']['value']) - else: - self._offset = 0.5 + self._flow_frequency = 40 # m + self._source_dist = 30 # Distance between source and end point + self._distance = get_value_parameter(config, 'distance', float, 100) + self._offset = get_value_parameter(config, 'offset', float, 0.5) # meters invaded in the opposite direction self._scenario_timeout = 240 - super(InvadingTurn, self).__init__("InvadingTurn", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + self._obstacle_transforms = [] + + super().__init__("InvadingTurn", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ # Spawn adversary actor - self._adversary_start_waypoint = self._reference_waypoint.next(self._distance)[ - 0] - self._adversary_start_waypoint = self._adversary_start_waypoint.get_left_lane() - if self._adversary_start_waypoint: - self._adversary_start_transform = self._adversary_start_waypoint.transform - else: - raise Exception( - "Couldn't find viable position for the adversary vehicle") - spawn_transform = carla.Transform( - self._adversary_start_transform.location, self._adversary_start_transform.rotation) - - spawn_transform.location.z += 5 # Avoid colliding with BA actors - actor = CarlaDataProvider.request_new_actor( - "vehicle.*", spawn_transform, rolename='scenario', attribute_filter={'base_type': 'car', 'has_lights': True}) - if actor is None: - raise Exception( - "Couldn't spawn the adversary vehicle") - - # Remove its physics so that it doesn't fall - actor.set_simulate_physics(False) - # Move the actor underground - new_location = actor.get_location() - new_location.z -= 500 - actor.set_location(new_location) - - self.other_actors.append(actor) - - # Calculate the real offset - lane_width = self._adversary_start_waypoint.lane_width - car_width = actor.bounding_box.extent.y*2 - self._offset = -1*(self._offset + 0.5*lane_width - 0.5*car_width) + next_wps = self._reference_waypoint.next(self._distance + self._source_dist) + if not next_wps: + raise ValueError("Couldn't find the source location for the actor flow") + self._forward_wp = next_wps[0] + self._source_wp = self._forward_wp.get_left_lane() + if not self._source_wp: + raise ValueError("Couldn't find the source location for the actor flow") + + self._sink_wp = self._reference_waypoint.get_left_lane() + if not self._sink_wp: + raise ValueError("Couldn't find the sink location for the actor flow") + + # Lane offset + self._offset_constant = 0.7 # Ideally, half the vehicle lane width + self._true_offset = self._offset + self._sink_wp.lane_width / 2 - self._offset_constant + self._true_offset *= -1 # Cause left direction + + self._create_obstacle() + + def _create_obstacle(self): + + next_wp = self._source_wp.next(10)[0] + obstacle_distance = 0.5 * self._distance + dist = 0 + while dist < obstacle_distance: + next_wp = next_wp.next(5)[0] + + displacement = 0.8 * next_wp.lane_width / 2 + r_vec = next_wp.transform.get_right_vector() + spawn_transform = next_wp.transform + spawn_transform.location += carla.Location(x=displacement * r_vec.x, y=displacement * r_vec.y, z=0.3) + + cone = CarlaDataProvider.request_new_actor('*constructioncone*', spawn_transform) + self.other_actors.append(cone) + + self._obstacle_transforms.append([cone, spawn_transform]) + + transform = carla.Transform(spawn_transform.location, spawn_transform.rotation) + transform.location.z -= 200 + cone.set_transform(transform) + cone.set_simulate_physics(False) + + dist += 5 + + self._obstacle_transforms.reverse() # So that the closest cones are spawned first def _create_behavior(self): """ The adversary vehicle will go to the target place while invading another lane. """ + sequence = py_trees.composites.Sequence("InvadingTurn") - sequence = py_trees.composites.Sequence() + for actor, transform in self._obstacle_transforms: + sequence.add_child(ActorTransformSetter(actor, transform)) if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._distance)) + sequence.add_child(RemoveRoadLane(self._reference_waypoint)) + sequence.add_child(ChangeOppositeBehavior(active=False)) - # Teleport adversary - sequence.add_child(ActorTransformSetter( - self.other_actors[0], self._adversary_start_transform)) + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(InvadingActorFlow( + self._source_wp, self._sink_wp, self.ego_vehicles[0], self._flow_frequency, offset=self._true_offset)) - behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - behavior.add_child(BasicAgentBehavior( - self.other_actors[0], self._adversary_end, target_speed=self._speed, opt_dict={'offset': self._offset})) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._distance)) - behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + main_behavior.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._forward_wp.transform, True)) + main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + sequence.add_child(main_behavior) + if self.route_mode: + sequence.add_child(ReAddRoadLane(0)) + sequence.add_child(ChangeOppositeBehavior(active=True)) - sequence.add_child(behavior) - sequence.add_child(ActorDestroy(self.other_actors[0])) + for actor in self.other_actors: + sequence.add_child(ActorDestroy(actor)) return sequence diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index c30e7fde5..8499ffc1e 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -164,7 +164,9 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._collision_wp = None self._adversary_speed = 2.0 # Speed of the adversary [m/s] - self._reaction_time = 2.1 # Time the agent has to react to avoid the collision [s] + self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) + self._reaction_time = 2.3 # Time the agent has to react to avoid the collision [s] + self._reaction_time += 0.1 * floor(self._crossing_angle / 5) self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout @@ -173,7 +175,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._distance = get_value_parameter(config, 'distance', float, 12) self._blocker_model = get_value_parameter(config, 'blocker_model', str, 'static.prop.vendingmachine') - self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) if abs(self._crossing_angle) > 90: raise ValueError("'crossing_angle' must be between -90 and 90º for the pedestrian to cross the road") self._direction = get_value_parameter(config, 'direction', str, 'right') @@ -368,7 +369,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") # Time the agent has to react to avoid the collision [s] - self._reaction_time = 2.2 + self._reaction_time = 2.3 self._reaction_time += 0.1 * floor(self._crossing_angle / 5) super().__init__("ParkingCrossingPedestrian", diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 586887376..6d50960e1 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -20,15 +20,17 @@ SwitchWrongDirectionTest, BasicAgentBehavior, ScenarioTimeout, - Idle, - HandBrakeVehicle) + Idle, WaitForever, + HandBrakeVehicle, + OppositeActorFlow) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (DriveDistance, InTriggerDistanceToLocation, InTriggerDistanceToVehicle, - WaitUntilInFront) + WaitUntilInFront, + WaitUntilInFrontPosition) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior, SetMaxSpeed +from srunner.tools.background_manager import LeaveSpaceInFront, SetMaxSpeed, ChangeOppositeBehavior def get_value_parameter(config, name, p_type, default): @@ -37,6 +39,15 @@ def get_value_parameter(config, name, p_type, default): else: return default +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default + class Accident(BasicScenario): """ @@ -54,17 +65,16 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._offset = 0.6 + self._first_distance = 10 self._second_distance = 6 self._trigger_distance = 30 - self._takeover_max_dist = self._first_distance + self._second_distance + 40 - self._drive_distance = self._trigger_distance + self._takeover_max_dist - + self._end_distance = 50 self._wait_duration = 5 + self._offset = 0.6 - self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 + self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 | carla.VehicleLightState.Position self._distance = get_value_parameter(config, 'distance', float, 120) self._direction = get_value_parameter(config, 'direction', str, 'right') @@ -78,10 +88,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals "Accident", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) def _move_waypoint_forward(self, wp, distance): - next_wps = wp.next(distance) - if not next_wps: - raise ValueError("Couldn't find a viable position to set up an accident actor") - return next_wps[0] + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if not next_wps or next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + return next_wp def _spawn_side_prop(self, wp): # Spawn the accident indication signal @@ -152,52 +167,53 @@ def _initialize_actors(self, config): self.other_actors.append(police_car) # Create the first vehicle that has been in the accident - first_vehicle_wp = self._move_waypoint_forward(self._accident_wp, self._first_distance) - first_actor = self._spawn_obstacle(first_vehicle_wp, 'vehicle.*', True) + self._first_vehicle_wp = self._move_waypoint_forward(self._accident_wp, self._first_distance) + first_actor = self._spawn_obstacle(self._first_vehicle_wp, 'vehicle.*', True) # Set its initial conditions first_actor.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(first_actor) # Create the second vehicle that has been in the accident - second_vehicle_wp = self._move_waypoint_forward(first_vehicle_wp, self._second_distance) + second_vehicle_wp = self._move_waypoint_forward(self._first_vehicle_wp, self._second_distance) second_actor = self._spawn_obstacle(second_vehicle_wp, 'vehicle.*', True) + self._accident_wp = second_vehicle_wp + self._end_wp = self._move_waypoint_forward(second_vehicle_wp, self._end_distance) + # Set its initial conditions second_actor.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(second_actor) def _create_behavior(self): """ - The vehicle has to drive the whole predetermined distance. + The vehicle has to drive the reach a specific point but an accident is in the middle of the road, + blocking its route and forcing it to lane change. """ - root = py_trees.composites.Sequence() + root = py_trees.composites.Sequence(name="Accident") if self.route_mode: total_dist = self._distance + self._first_distance + self._second_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() behavior.add_child(InTriggerDistanceToLocation( - self.ego_vehicles[0], self._accident_wp.transform.location, self._trigger_distance)) + self.ego_vehicles[0], self._first_vehicle_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SetMaxSpeed(self._max_speed)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + behavior.add_child(WaitForever()) - timeout_parallel.add_child(behavior) - - root.add_child(timeout_parallel) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: - behavior.add_child(SetMaxSpeed(0)) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) - root.add_child(ActorDestroy(self.other_actors[2])) - root.add_child(ActorDestroy(self.other_actors[3])) + root.add_child(SetMaxSpeed(0)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root @@ -224,8 +240,7 @@ class AccidentTwoWays(Accident): """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) - + self._opposite_interval = get_interval_parameter(config, 'frequency', float, [20, 100]) super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): @@ -233,38 +248,39 @@ def _create_behavior(self): The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to let the ego invade the opposite lane. """ - root = py_trees.composites.Sequence() + reference_wp = self._accident_wp.get_left_lane() + if not reference_wp: + raise ValueError("Couldnt find a left lane to spawn the opposite traffic") + + root = py_trees.composites.Sequence(name="AccidentTwoWays") if self.route_mode: total_dist = self._distance + self._first_distance + self._second_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() behavior.add_child(InTriggerDistanceToLocation( - self.ego_vehicles[0], self._accident_wp.transform.location, self._trigger_distance)) + self.ego_vehicles[0], self._first_vehicle_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SwitchWrongDirectionTest(False)) - behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - - timeout_parallel.add_child(behavior) + behavior.add_child(ChangeOppositeBehavior(active=False)) + behavior.add_child(OppositeActorFlow(reference_wp, self.ego_vehicles[0], self._opposite_interval)) - root.add_child(timeout_parallel) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: - root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=40)) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) - root.add_child(ActorDestroy(self.other_actors[2])) + behavior.add_child(SwitchWrongDirectionTest(True)) + behavior.add_child(ChangeOppositeBehavior(active=True)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root - class ParkedObstacle(BasicScenario): """ Scenarios in which a parked vehicle is incorrectly parked, @@ -282,12 +298,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._trigger_distance = 30 - self._drive_distance = self._trigger_distance + 40 - self._offset = 0.7 - + self._end_distance = 50 self._wait_duration = 5 + self._offset = 0.7 - self._lights = carla.VehicleLightState.RightBlinker | carla.VehicleLightState.LeftBlinker + self._lights = carla.VehicleLightState.RightBlinker | carla.VehicleLightState.LeftBlinker | carla.VehicleLightState.Position self._distance = get_value_parameter(config, 'distance', float, 120) self._direction = get_value_parameter(config, 'direction', str, 'right') @@ -301,10 +316,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals "ParkedObstacle", ego_vehicles, config, world, randomize, debug_mode, criteria_enable=criteria_enable) def _move_waypoint_forward(self, wp, distance): - next_wps = wp.next(distance) - if not next_wps: - raise ValueError("Couldn't find a viable position to set up an accident actor") - return next_wps[0] + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if not next_wps or next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + return next_wp def _spawn_side_prop(self, wp): # Spawn the accident indication signal @@ -354,13 +374,13 @@ def _initialize_actors(self, config): """ Custom initialization """ - starting_wp = self._map.get_waypoint(config.trigger_points[0].location) + self._starting_wp = self._map.get_waypoint(config.trigger_points[0].location) # Create the side prop - self._spawn_side_prop(starting_wp) + self._spawn_side_prop(self._starting_wp) # Create the first vehicle that has been in the accident - self._vehicle_wp = self._move_waypoint_forward(starting_wp, self._distance) + self._vehicle_wp = self._move_waypoint_forward(self._starting_wp, self._distance) parked_actor = self._spawn_obstacle(self._vehicle_wp, 'vehicle.*') lights = parked_actor.get_light_state() @@ -369,36 +389,36 @@ def _initialize_actors(self, config): parked_actor.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(parked_actor) + self._end_wp = self._move_waypoint_forward(self._vehicle_wp, self._end_distance) def _create_behavior(self): """ The vehicle has to drive the whole predetermined distance. """ - root = py_trees.composites.Sequence() + root = py_trees.composites.Sequence(name="ParkedObstacle") if self.route_mode: total_dist = self._distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() behavior.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], self._vehicle_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SetMaxSpeed(self._max_speed)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) - - timeout_parallel.add_child(behavior) + behavior.add_child(WaitForever()) - root.add_child(timeout_parallel) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: - behavior.add_child(SetMaxSpeed(0)) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(ActorDestroy(self.other_actors[1])) + root.add_child(SetMaxSpeed(0)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root @@ -424,8 +444,8 @@ class ParkedObstacleTwoWays(ParkedObstacle): Variation of the ParkedObstacle scenario but the ego now has to invade the opposite lane """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + self._opposite_interval = get_interval_parameter(config, 'frequency', float, [20, 100]) super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) def _create_behavior(self): @@ -433,32 +453,36 @@ def _create_behavior(self): The vehicle has to drive the whole predetermined distance. Adapt the opposite flow to let the ego invade the opposite lane. """ - root = py_trees.composites.Sequence() + reference_wp = self._vehicle_wp.get_left_lane() + if not reference_wp: + raise ValueError("Couldnt find a left lane to spawn the opposite traffic") + + root = py_trees.composites.Sequence(name="ParkedObstacleTwoWays") if self.route_mode: total_dist = self._distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) behavior = py_trees.composites.Sequence() behavior.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], self._vehicle_wp.transform.location, self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SwitchWrongDirectionTest(False)) - behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._drive_distance)) + behavior.add_child(ChangeOppositeBehavior(active=False)) + behavior.add_child(OppositeActorFlow(reference_wp, self.ego_vehicles[0], self._opposite_interval)) - timeout_parallel.add_child(behavior) - - root.add_child(timeout_parallel) + end_condition.add_child(behavior) + root.add_child(end_condition) if self.route_mode: root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=40)) - root.add_child(ActorDestroy(self.other_actors[0])) + root.add_child(ChangeOppositeBehavior(active=True)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) return root @@ -505,10 +529,15 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals criteria_enable=criteria_enable) def _move_waypoint_forward(self, wp, distance): - next_wps = wp.next(distance) - if not next_wps: - raise ValueError("Couldn't find a viable position to set up an accident actor") - return next_wps[0] + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if not next_wps or next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + return next_wp def _spawn_obstacle(self, wp, blueprint): """ @@ -562,58 +591,44 @@ def _create_behavior(self): """ Activate the bicycles and wait for the ego to be close-by before changing the side traffic. End condition is based on the ego behind in front of the bicycles, or timeout based. - - Sequence: - - LeaveSpaceInFront - - Parallel - - ScenarioTimeout - - Bicycles (Sequences) - - BasicAgentBehavior - - HandBrakeVehicle - - Idle - - Behavior (Sequence) - - InTriggerDistanceToVehicle - - Idle - - SetMaxSpeed - - WaitUntilInFront - - DriveDistance - - SetMaxSpeed - - ActorDestroys - """ - root = py_trees.composites.Sequence() + """ + root = py_trees.composites.Sequence(name="HazardAtSideLane") if self.route_mode: total_dist = self._distance + self._obstacle_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + # End condition + end_condition = py_trees.composites.Sequence(name="End Condition") + end_condition.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + main_behavior.add_child(end_condition) - # Bicycle movement. Move them for a set distance, then + # Bicycle movement. Move them for a set distance, then stop offset = self._offset * self._starting_wp.lane_width / 2 opt_dict = {'offset': offset} for actor, target_loc in zip(self.other_actors, self._target_locs): - bicycle = py_trees.composites.Sequence() + bicycle = py_trees.composites.Sequence(name="Bicycle behavior") bicycle.add_child(BasicAgentBehavior(actor, target_loc, target_speed=self._bicycle_speed, opt_dict=opt_dict)) bicycle.add_child(HandBrakeVehicle(actor, 1)) # In case of collisions - bicycle.add_child(Idle()) # Don't make the bicycle stop the parallel behavior - timeout_parallel.add_child(bicycle) + bicycle.add_child(WaitForever()) # Don't make the bicycle stop the parallel behavior + main_behavior.add_child(bicycle) - behavior = py_trees.composites.Sequence() + behavior = py_trees.composites.Sequence(name="Side lane behavior") behavior.add_child(InTriggerDistanceToVehicle( self.ego_vehicles[0], self.other_actors[0], self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SetMaxSpeed(self._max_speed)) - behavior.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + behavior.add_child(WaitForever()) - timeout_parallel.add_child(behavior) - - root.add_child(timeout_parallel) + main_behavior.add_child(behavior) + root.add_child(main_behavior) if self.route_mode: - behavior.add_child(SetMaxSpeed(0)) + root.add_child(SetMaxSpeed(0)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) @@ -651,65 +666,47 @@ def _create_behavior(self): """ Activate the bicycles and wait for the ego to be close-by before changing the opposite traffic. End condition is based on the ego behind in front of the bicycles, or timeout based. - - Sequence: - - LeaveSpaceInFront - - Parallel - - ScenarioTimeout - - Bicycles (Sequences) - - BasicAgentBehavior - - HandBrakeVehicle - - Idle - - Behavior (Sequence) - - InTriggerDistanceToVehicle - - Idle - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - WaitUntilInFront - - DriveDistance - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - ActorDestroys - """ - root = py_trees.composites.Sequence() + """ + + root = py_trees.composites.Sequence(name="HazardAtSideLaneTwoWays") if self.route_mode: total_dist = self._distance + self._obstacle_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) - timeout_parallel = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - # Bicycle movement. Move them for a set distance, then + # End condition + end_condition = py_trees.composites.Sequence(name="End Condition") + end_condition.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) + end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + main_behavior.add_child(end_condition) + + # Bicycle movement. Move them for a set distance, then stop offset = self._offset * self._starting_wp.lane_width / 2 opt_dict = {'offset': offset} for actor, target_loc in zip(self.other_actors, self._target_locs): - bicycle = py_trees.composites.Sequence() - movement = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - movement.add_child(BasicAgentBehavior(actor, target_speed=self._bicycle_speed, opt_dict=opt_dict)) - movement.add_child(InTriggerDistanceToLocation(actor, target_loc, 5)) - bicycle.add_child(movement) + bicycle = py_trees.composites.Sequence(name="Bicycle behavior") + bicycle.add_child(BasicAgentBehavior(actor, target_loc, target_speed=self._bicycle_speed, opt_dict=opt_dict)) bicycle.add_child(HandBrakeVehicle(actor, 1)) # In case of collisions - bicycle.add_child(Idle()) # Don't make the bicycle stop the parallel behavior - timeout_parallel.add_child(bicycle) + bicycle.add_child(WaitForever()) # Don't make the bicycle stop the parallel behavior + main_behavior.add_child(bicycle) - behavior = py_trees.composites.Sequence() + behavior = py_trees.composites.Sequence(name="Side lane behavior") behavior.add_child(InTriggerDistanceToVehicle( self.ego_vehicles[0], self.other_actors[0], self._trigger_distance)) behavior.add_child(Idle(self._wait_duration)) - if self.route_mode: behavior.add_child(SwitchWrongDirectionTest(False)) behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - behavior.add_child(WaitUntilInFront(self.ego_vehicles[0], self.other_actors[-1], check_distance=False)) - behavior.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + behavior.add_child(WaitForever()) - timeout_parallel.add_child(behavior) - - root.add_child(timeout_parallel) + main_behavior.add_child(behavior) + root.add_child(main_behavior) if self.route_mode: - root.add_child(SwitchWrongDirectionTest(True)) - root.add_child(ChangeOppositeBehavior(spawn_dist=40)) + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(spawn_dist=40)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index d84b7d370..b869d3f89 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -72,8 +72,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._scenario_timeout = 240 # The faster the flow, the further they are spawned, leaving time to react to them - self._source_dist = 5 * self._flow_speed - self._sink_dist = 3 * self._flow_speed + self._source_dist = 4 * self._flow_speed + self._sink_dist = 2.5 * self._flow_speed super().__init__("JunctionLeftTurn", ego_vehicles, diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index fb278574d..f30c7d486 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -22,13 +22,15 @@ OpenVehicleDoor, SwitchWrongDirectionTest, ScenarioTimeout, - Idle) + Idle, + OppositeActorFlow) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, - DriveDistance) + DriveDistance, + WaitUntilInFrontPosition) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior, SetMaxSpeed +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior def get_value_parameter(config, name, p_type, default): @@ -38,12 +40,21 @@ def get_value_parameter(config, name, p_type, default): return default -class VehicleOpensDoor(BasicScenario): +def get_interval_parameter(config, name, p_type, default): + if name in config.other_parameters: + return [ + p_type(config.other_parameters[name]['from']), + p_type(config.other_parameters[name]['to']) + ] + else: + return default + + +class VehicleOpensDoorTwoWays(BasicScenario): """ This class holds everything required for a scenario in which another vehicle parked at the side lane - opens the door, forcing the ego to lane change. + opens the door, forcing the ego to lane change, invading the opposite lane """ - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): """ @@ -54,11 +65,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._takeover_distance = 60 self._min_trigger_dist = 10 self._reaction_time = 3.0 self._opposite_wait_duration = 5 + self._end_distance = 50 self._parked_distance = get_value_parameter(config, 'distance', float, 50) self._direction = get_value_parameter(config, 'direction', str, 'right') @@ -68,7 +79,45 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._max_speed = get_value_parameter(config, 'speed', float, 60) self._scenario_timeout = 240 - super().__init__("VehicleOpensDoor", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) + self._opposite_interval = get_interval_parameter(config, 'frequency', float, [20, 100]) + + super().__init__("VehicleOpensDoorTwoWays", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) + + def _remove_parked_vehicles(self, actor_location): + """Removes the parked vehicles that might have conflicts with the scenario""" + parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) + vehicles_to_destroy = set() + for v in parked_vehicles: + if v.transform.location.distance(actor_location) < 10: + vehicles_to_destroy.add(v) + self.world.enable_environment_objects(vehicles_to_destroy, False) + + def _get_displaced_location(self, actor, wp): + """ + Calculates the transforming such that the actor is at the sidemost part of the lane + """ + # Move the actor to the edge of the lane, or the open door might not reach the ego vehicle + displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 + displacement_vector = wp.transform.get_right_vector() + if self._direction == 'right': + displacement_vector *= -1 + + new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, + y=displacement*displacement_vector.y, + z=displacement*displacement_vector.z) + new_location.z += 0.05 # Just in case, avoid collisions with the ground + return new_location + + def _move_waypoint_forward(self, wp, distance): + dist = 0 + next_wp = wp + while dist < distance: + next_wps = next_wp.next(1) + if not next_wps or next_wps[0].is_junction: + break + next_wp = next_wps[0] + dist += 1 + return next_wp def _initialize_actors(self, config): """ @@ -84,84 +133,76 @@ def _initialize_actors(self, config): self._front_wp = front_wps[0] if self._direction == 'left': - parked_wp = self._front_wp.get_left_lane() + self._parked_wp = self._front_wp.get_left_lane() else: - parked_wp = self._front_wp.get_right_lane() + self._parked_wp = self._front_wp.get_right_lane() - if parked_wp is None: + if self._parked_wp is None: raise ValueError("Couldn't find a spot to place the adversary vehicle") - self._remove_parked_vehicles(parked_wp.transform.location) + self._remove_parked_vehicles(self._parked_wp.transform.location) self._parked_actor = CarlaDataProvider.request_new_actor( - "*vehicle.*", parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) + "*vehicle.*", self._parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) if not self._parked_actor: raise ValueError("Couldn't spawn the parked vehicle") self.other_actors.append(self._parked_actor) # And move it to the side - side_location = self._get_displaced_location(self._parked_actor, parked_wp) + side_location = self._get_displaced_location(self._parked_actor, self._parked_wp) self._parked_actor.set_location(side_location) self._parked_actor.apply_control(carla.VehicleControl(hand_brake=True)) + self._end_wp = self._move_waypoint_forward(self._front_wp, self._end_distance) + def _create_behavior(self): """ - Leave space in front, as the TM doesn't detect open doors + Leave space in front, as the TM doesn't detect open doors, and change the opposite frequency + so that the ego can pass """ - sequence = py_trees.composites.Sequence(name="VehicleOpensDoor") + reference_wp = self._parked_wp.get_left_lane() + if not reference_wp: + raise ValueError("Couldnt find a left lane to spawn the opposite traffic") + root = py_trees.composites.Sequence(name="VehicleOpensDoorTwoWays") if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._parked_distance)) - sequence.add_child(SetMaxSpeed(self._max_speed)) + total_dist = self._parked_distance + 20 + root.add_child(LeaveSpaceInFront(total_dist)) - collision_location = self._front_wp.transform.location + end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + end_condition.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._end_wp.transform, False)) + + behavior = py_trees.composites.Sequence(name="Main Behavior") # Wait until ego is close to the adversary + collision_location = self._front_wp.transform.location trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerOpenDoor") trigger_adversary.add_child(InTimeToArrivalToLocation( self.ego_vehicles[0], self._reaction_time, collision_location)) trigger_adversary.add_child(InTriggerDistanceToLocation( self.ego_vehicles[0], collision_location, self._min_trigger_dist)) - sequence.add_child(trigger_adversary) + behavior.add_child(trigger_adversary) door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL - sequence.add_child(OpenVehicleDoor(self._parked_actor, door)) - end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) - end_condition.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - sequence.add_child(end_condition) - sequence.add_child(ActorDestroy(self._parked_actor)) - + behavior.add_child(OpenVehicleDoor(self._parked_actor, door)) + behavior.add_child(Idle(self._opposite_wait_duration)) if self.route_mode: - sequence.add_child(SetMaxSpeed(0)) + behavior.add_child(SwitchWrongDirectionTest(False)) + behavior.add_child(ChangeOppositeBehavior(active=False)) + behavior.add_child(OppositeActorFlow(reference_wp, self.ego_vehicles[0], self._opposite_interval)) - return sequence + end_condition.add_child(behavior) + root.add_child(end_condition) - def _remove_parked_vehicles(self, actor_location): - """Removes the parked vehicles that might have conflicts with the scenario""" - parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) - vehicles_to_destroy = set() - for v in parked_vehicles: - if v.transform.location.distance(actor_location) < 10: - vehicles_to_destroy.add(v) - self.world.enable_environment_objects(vehicles_to_destroy, False) - - def _get_displaced_location(self, actor, wp): - """ - Calculates the transforming such that the actor is at the sidemost part of the lane - """ - # Move the actor to the edge of the lane, or the open door might not reach the ego vehicle - displacement = (wp.lane_width - actor.bounding_box.extent.y) / 4 - displacement_vector = wp.transform.get_right_vector() - if self._direction == 'right': - displacement_vector *= -1 + if self.route_mode: + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(active=True)) + for actor in self.other_actors: + root.add_child(ActorDestroy(actor)) - new_location = wp.transform.location + carla.Location(x=displacement*displacement_vector.x, - y=displacement*displacement_vector.y, - z=displacement*displacement_vector.z) - new_location.z += 0.05 # Just in case, avoid collisions with the ground - return new_location + return root def _create_test_criteria(self): """ @@ -178,79 +219,3 @@ def __del__(self): Remove all actors and traffic lights upon deletion """ self.remove_all_actors() - - -class VehicleOpensDoorTwoWays(VehicleOpensDoor): - """ - Variation of VehicleOpensDoor wher ethe vehicle has to invade an opposite lane - """ - - def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, - timeout=180): - - self._opposite_frequency = get_value_parameter(config, 'frequency', float, 100) - - super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) - - def _create_behavior(self): - """ - Leave space in front, as the TM doesn't detect open doors, and change the opposite frequency - so that the ego can pass - - Sequence: - - LeaveSpaceInFront - - Parallel: - - ScenarioTimeout: - - Sequence - - Trigger adversary - - OpenVehicleDoor - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - DriveDistance - - SwitchWrongDirectionTest - - ChangeOppositeBehavior - - ActorDestroy - """ - - sequence = py_trees.composites.Sequence(name="VehicleOpensDoorTwoWays") - - if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._parked_distance)) - - main_behavior = py_trees.composites.Sequence() - - # Wait until ego is close to the adversary - collision_location = self._front_wp.transform.location - trigger_adversary = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerOpenDoor") - trigger_adversary.add_child(InTimeToArrivalToLocation( - self.ego_vehicles[0], self._reaction_time, collision_location)) - trigger_adversary.add_child(InTriggerDistanceToLocation( - self.ego_vehicles[0], collision_location, self._min_trigger_dist)) - main_behavior.add_child(trigger_adversary) - - door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL - main_behavior.add_child(OpenVehicleDoor(self._parked_actor, door)) - main_behavior.add_child(Idle(self._opposite_wait_duration)) - - if self.route_mode: - main_behavior.add_child(SwitchWrongDirectionTest(False)) - main_behavior.add_child(ChangeOppositeBehavior(spawn_dist=self._opposite_frequency)) - - main_behavior.add_child(DriveDistance(self.ego_vehicles[0], self._takeover_distance)) - - # Add the main behavior to the scenario timeout - timeout_parallel = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - - timeout_parallel.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - timeout_parallel.add_child(main_behavior) - - sequence.add_child(timeout_parallel) - - if self.route_mode: - sequence.add_child(SwitchWrongDirectionTest(True)) - sequence.add_child(ChangeOppositeBehavior(spawn_dist=40)) - sequence.add_child(ActorDestroy(self._parked_actor)) - - return sequence diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 0c2b39afe..2700faaea 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -11,6 +11,8 @@ import py_trees from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AtomicBehavior +from srunner.scenariomanager.timer import GameTime +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider class ChangeRoadBehavior(AtomicBehavior): @@ -286,3 +288,57 @@ def update(self): self._remove_exits, self._stop_entries, self._extend_road_exit], overwrite=True) return py_trees.common.Status.SUCCESS + + +class AddOppositeVariabilityBehavior(AtomicBehavior): + """ + Updates the blackboard to periodically change the parameters opposite road spawn distance. + + Args: + interval (list): Two element list with the minimum and maximum distance of the opposite lane + """ + + def __init__(self, interval, end_value, name="AddOppositeVariabilityBehavior"): + self._min_spawn_dist = interval[0] + self._max_spawn_dist = interval[1] + self._end_value = end_value + + self._spawn_dist = 0 + self._change_time = 0 + self._change_frequency = 0 + self._rng = CarlaDataProvider.get_random_seed() + + self._terminated = False + super().__init__(name) + + def initialise(self): + """Change the opposite frequency and start the timer""" + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._change_time = GameTime.get_time() + self._change_frequency = self._spawn_dist / 4 + + py_trees.blackboard.Blackboard().set( + "BA_ChangeOppositeBehavior", [None, self._spawn_dist, None], overwrite=True + ) + super().initialise() + + def update(self): + """Periodically change the frequency of the opposite traffic""" + if GameTime.get_time() - self._change_time > self._change_frequency: + self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) + self._change_time = GameTime.get_time() + self._change_frequency = self._spawn_dist / 4 + py_trees.blackboard.Blackboard().set( + "BA_ChangeOppositeBehavior", [None, self._spawn_dist, None], overwrite=True + ) + + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + + if not self._terminated: + py_trees.blackboard.Blackboard().set( + "BA_ChangeOppositeBehavior", [None, self._end_value, None], overwrite=True + ) + self._terminated = True + return super().terminate(new_status) From ce3ae655886d1a1b50840cad27434c134b2ed029 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 8 Aug 2022 17:10:25 +0200 Subject: [PATCH 46/86] Added VehicleTurningRotue variations --- srunner/scenarios/actor_flow.py | 140 ++++++++++------ srunner/scenarios/background_activity.py | 79 +++++++--- srunner/scenarios/blocked_intersection.py | 122 ++++---------- .../scenarios/object_crash_intersection.py | 149 +++++++++++++++++- srunner/scenarios/object_crash_vehicle.py | 3 +- srunner/scenarios/pedestrian_crossing.py | 95 +++++++---- srunner/tools/scenario_helper.py | 10 +- 7 files changed, 392 insertions(+), 206 deletions(-) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 7e57a6731..c9af8d807 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -457,15 +457,17 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reference_wp = self._map.get_waypoint(config.trigger_points[0].location) - self._middle_entry_wp, exit_wp = self._get_entry_exit_route_lanes(self._reference_wp, config.route) - exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match - self._other_entry_wp = exit_wp.get_left_lane() - if not self._other_entry_wp or self._other_entry_wp.lane_type != carla.LaneType.Driving: + route_entry_wp, route_exit_wp = self._get_entry_exit_route_lanes(self._reference_wp, config.route) + route_exit_wp = route_exit_wp.next(8)[0] # Just in case the junction maneuvers don't match + other_entry_wp = route_exit_wp.get_left_lane() + if not other_entry_wp or other_entry_wp.lane_type != carla.LaneType.Driving: raise ValueError("Couldn't find an end position") self._source_wp = self._map.get_waypoint(self._start_actor_flow) self._sink_wp = self._map.get_waypoint(self._end_actor_flow) + self._remove_entries = [route_entry_wp, other_entry_wp, self._source_wp] + super().__init__("InterurbanActorFlow", ego_vehicles, config, @@ -497,11 +499,13 @@ def _get_entry_exit_route_lanes(self, wp, route): # Enter the junction if not reached_junction and (road_option in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): reached_junction = True - entry_wp = self._map.get_waypoint(route[i - 1][0].location) + entry_wp = self._map.get_waypoint(route[i-1][0].location) + entry_wp = entry_wp.previous(2)[0] # Just in case # End condition for the behavior, at the end of the junction if reached_junction and (road_option not in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)): exit_wp = self._map.get_waypoint(route_transform.location) + exit_wp = exit_wp.next(2)[0] # Just in case break return (entry_wp, exit_wp) @@ -524,8 +528,8 @@ def _create_behavior(self): if self.route_mode: sequence.add_child(HandleJunctionScenario( clear_junction=False, - clear_ego_entry=False, - remove_entries=[self._source_wp, self._middle_entry_wp, self._other_entry_wp], + clear_ego_entry=True, + remove_entries=self._remove_entries, remove_exits=[], stop_entries=False, extend_road_exit=0 @@ -590,68 +594,108 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals debug_mode, criteria_enable=criteria_enable) - def _create_behavior(self): - """ - the ego vehicle mergers into a slow traffic flow from the freeway entrance. - """ - source_wp_1 = self._map.get_waypoint(self._start_actor_flow_1) - sink_wp_1 = self._map.get_waypoint(self._end_actor_flow_1) - - source_wp_2 = sink_wp_1.get_left_lane() - if not source_wp_2 or source_wp_2.lane_type != carla.LaneType.Driving: + def get_lane_key(self, waypoint): + return str(waypoint.road_id) + '*' + str(waypoint.lane_id) + + def _get_junction_entry_wp(self, entry_wp): + while entry_wp.is_junction: + entry_wps = entry_wp.previous(0.2) + if len(entry_wps) == 0: + return None # Stop when there's no prev + entry_wp = entry_wps[0] + return entry_wp + + def _get_junction_exit_wp(self, exit_wp): + while exit_wp.is_junction: + exit_wps = exit_wp.next(0.2) + if len(exit_wps) == 0: + return None # Stop when there's no prev + exit_wp = exit_wps[0] + return exit_wp + + def _initialize_actors(self, config): + + self._source_wp_1 = self._map.get_waypoint(self._start_actor_flow_1) + self._sink_wp_1 = self._map.get_waypoint(self._end_actor_flow_1) + + self._source_wp_2 = self._sink_wp_1.get_left_lane() + if not self._source_wp_2 or self._source_wp_2.lane_type != carla.LaneType.Driving: raise ValueError("Couldn't find a position for the actor flow") - sink_wp_2 = source_wp_1.get_left_lane() - if not sink_wp_2 or sink_wp_2.lane_type != carla.LaneType.Driving: + self._sink_wp_2 = self._source_wp_1.get_left_lane() + if not self._sink_wp_2 or self._sink_wp_2.lane_type != carla.LaneType.Driving: raise ValueError("Couldn't find a position for the actor flow") - root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp_2.transform.location, self._sink_distance)) - root.add_child(ActorFlow( - source_wp_1, sink_wp_1, self._source_dist_interval, self._sink_distance, self._flow_speed)) - root.add_child(ActorFlow( - source_wp_2, sink_wp_2, self._source_dist_interval, self._sink_distance, self._flow_speed)) - root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) - - sequence = py_trees.composites.Sequence() if self.route_mode: - grp = CarlaDataProvider.get_global_route_planner() - route = grp.trace_route(source_wp_2.transform.location, sink_wp_2.transform.location) - extra_space = 0 + route = grp.trace_route(self._source_wp_2.transform.location, self._sink_wp_2.transform.location) + self._extra_space = 10 + route_exit_wp = None for i in range(-2, -len(route)-1, -1): current_wp = route[i][0] - extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + self._extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) if current_wp.is_junction: + junction = current_wp.get_junction() break + route_exit_wp = current_wp - # Get the junction entry lane (1) - entry_wp_1 = source_wp_1 - while True: - next_wps = entry_wp_1.next(1) - if not next_wps: - break - if next_wps[0].is_junction: - break - entry_wp_1 = next_wps[0] + route_exit_key = self.get_lane_key(route_exit_wp) - # Get the junction entry lane (1) - entry_wp_2 = source_wp_2 + # Get the route entry waypoint + route_entry_wp = self._reference_wp while True: - next_wps = entry_wp_2.next(1) + next_wps = route_entry_wp.next(1) if not next_wps: break if next_wps[0].is_junction: break - entry_wp_2 = next_wps[0] + route_entry_wp = next_wps[0] + route_entry_key = self.get_lane_key(route_entry_wp) + + entry_wps = [] + entry_keys = [] + exit_wps = [] + exit_keys = [] + + for entry_wp, exit_wp in junction.get_waypoints(carla.LaneType.Driving): + + entry_wp = self._get_junction_entry_wp(entry_wp) + entry_key = self.get_lane_key(entry_wp) + if entry_key != route_entry_key and entry_key not in entry_keys: + entry_wps.append(entry_wp) + entry_keys.append(entry_key) + + exit_wp = self._get_junction_exit_wp(exit_wp) + exit_key = self.get_lane_key(exit_wp) + if exit_key != route_exit_key and exit_key not in exit_keys: + exit_wps.append(exit_wp) + exit_keys.append(exit_key) + + self._remove_entries = entry_wps + self._remove_exits = exit_wps + + def _create_behavior(self): + """ + the ego vehicle mergers into a slow traffic flow from the freeway entrance. + """ + root = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self._sink_wp_2.transform.location, self._sink_distance)) + root.add_child(ActorFlow( + self._source_wp_1, self._sink_wp_1, self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(ActorFlow( + self._source_wp_2, self._sink_wp_2, self._source_dist_interval, self._sink_distance, self._flow_speed)) + root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + sequence = py_trees.composites.Sequence() + if self.route_mode: sequence.add_child(HandleJunctionScenario( clear_junction=True, clear_ego_entry=True, - remove_entries=[entry_wp_1, entry_wp_2], - remove_exits=[self._exit_wp], + remove_entries=self._remove_entries, + remove_exits=self._remove_exits, stop_entries=False, - extend_road_exit=extra_space + extend_road_exit=self._extra_space )) sequence.add_child(SwitchRouteSources(False)) sequence.add_child(ChangeOppositeBehavior(active=False)) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index acb2669f4..bd807286d 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -207,6 +207,8 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._road_extra_front_actors = 0 # For cases where we want more space but not more vehicles self._road_spawn_dist = 15 # Distance between spawned vehicles [m] + self._active_road_sources = True + self._base_min_radius = 0 self._base_max_radius = 0 self._min_radius = 0 @@ -242,8 +244,6 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._scenario_removed_lane = False # Flag indicating a scenario has removed a lane self._scenario_remove_lane_offset = 0 - self._route_sources_active = True - def _get_route_data(self, route): """Extract the information from the route""" self._route = [] # Transform the route into a list of waypoints @@ -831,17 +831,25 @@ def _switch_to_junction_mode(self, junction): self._ego_state = EGO_JUNCTION for lane in self._road_dict: for actor in self._road_dict[lane].actors: - # TODO: Map the actors to the junction entry to have full control of them. This should remove the 'at_oppo_entry_lane' + # TODO: Map the actors to the junction entry to have full control of them. + # This should remove the 'at_oppo_entry_lane'. self._add_actor_dict_element(junction.actor_dict, actor) if actor not in self._scenario_stopped_actors: self._actors_speed_perc[actor] = 100 for lane_key in self._road_dict: source = self._road_dict[lane_key] - if get_lane_key(source.wp) in junction.route_entry_keys: + source_key = get_lane_key(source.wp) + if source_key in junction.route_entry_keys: junction.entry_sources.append(Source( - source.wp, source.actors, entry_lane_wp=source.wp, active=self._route_sources_active) + source.wp, source.actors, entry_lane_wp=source.wp, active=source.active) ) + if source_key in junction.inactive_entry_keys: + for actor in source.actors: + self._destroy_actor(actor) + source.active = False + junction.inactive_entry_keys.remove(source_key) + # TODO: Else should map the source to the entry and add it self._road_dict.clear() @@ -861,14 +869,14 @@ def _end_junction_behavior(self, junction): if not self._active_junctions: for wp in junction.exit_wps: if get_lane_key(wp) in route_exit_keys: - self._road_dict[get_lane_key(wp)] = Source(wp, [], active=self._route_sources_active) + self._road_dict[get_lane_key(wp)] = Source(wp, [], active=self._active_road_sources) else: for wp in junction.exit_wps: if get_lane_key(wp) in route_exit_keys: # TODO: entry_lane_wp isn't really this one (for cases with road changes) self._active_junctions[0].entry_sources.append( - Source(wp, [], entry_lane_wp=wp, active=self._route_sources_active) + Source(wp, [], entry_lane_wp=wp, active=self._active_road_sources) ) # Handle the actors @@ -1061,6 +1069,8 @@ def _update_road_sources(self): if not source.active: continue + if not self._is_location_behind_ego(source.wp.transform.location): + continue # Stop the sources in front of the ego (created by new lanes) if len(source.actors) >= self._road_back_vehicles + self._road_front_vehicles: continue @@ -1127,7 +1137,7 @@ def _initialise_road_behavior(self, road_wps): actors = self._spawn_actors(spawn_wps) self._road_dict[get_lane_key(wp)] = Source( - prev_wp, actors, active=self._route_sources_active + prev_wp, actors, active=self._active_road_sources ) def _initialise_opposite_sources(self): @@ -1373,16 +1383,33 @@ def get_source_wp(wp): route_move_dist = max(new_accum_dist - prev_accum_dist, 0.1) if len(new_wps) > len(old_wps): - for new_wp in list(new_wps): - prev_wps = new_wp.previous(2 * route_move_dist) # x2, just in case - if prev_wps: - continue + # Don't add anything in front if the junction entry has to be empty + if not self._scenario_junction_entry: - # Found the new lane, add a Source - source_wp = get_source_wp(new_wp) - if not source_wp: - continue - self._road_dict[get_lane_key(new_wp)] = Source(source_wp, [], active=self._route_sources_active) + for new_wp in list(new_wps): + + prev_wps = new_wp.previous(2 * route_move_dist) # x2, just in case + if prev_wps: + continue + + # Found the new lane, add the actors and source. + # Don't spawn actors while the source is in front of the ego + source_wp = get_source_wp(new_wp) + if not source_wp: + continue + + next_wp = source_wp + spawn_wps = [] + spawn_dist = self._road_front_vehicles + CarlaDataProvider.get_velocity(self._ego_actor) + for _ in range(self._road_front_vehicles): + next_wps = next_wp.next(spawn_dist) + if len(next_wps) != 1 or self._is_junction(next_wps[0]): + break # Stop when there's no next or found a junction + next_wp = next_wps[0] + spawn_wps.insert(0, next_wp) + + actors = self._spawn_actors(spawn_wps) + self._road_dict[get_lane_key(source_wp)] = Source(source_wp, actors, active=self._active_road_sources) elif len(new_wps) < len(old_wps): for old_wp in list(old_wps): @@ -1768,7 +1795,7 @@ def _switch_route_sources(self, enabled): """ Disables all sources that are part of the ego's route """ - self._route_sources_active = enabled + self._active_road_sources = enabled for lane in self._road_dict: self._road_dict[lane].active = enabled @@ -1807,9 +1834,9 @@ def _leave_crossing_space(self, collision_wp): stop_dist = 15 opposite_wp = collision_wp.get_left_lane() - opposite_loc = opposite_wp.transform.location if not opposite_wp: return # Nothing else to do + opposite_loc = opposite_wp.transform.location for actor in list(self._opposite_actors): location = actor.get_location() @@ -1863,7 +1890,7 @@ def _readd_road_lane(self, lane_offset): return ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - spawn_dist = self._road_spawn_dist + 2.5 * ego_speed + spawn_dist = self._road_spawn_dist + 2 * ego_speed spawn_wps = [] @@ -1893,7 +1920,7 @@ def _readd_road_lane(self, lane_offset): actor.set_target_velocity(spawn_wp.transform.get_forward_vector() * ego_speed) actors.append(actor) - self._road_dict[add_lane_key] = Source(prev_wp, actors, active=self._route_sources_active) + self._road_dict[add_lane_key] = Source(prev_wp, actors, active=self._active_road_sources) self._scenario_removed_lane = False self._scenario_remove_lane_offset = 0 @@ -1941,8 +1968,7 @@ def _clear_junction_middle(self): def _clear_ego_entry(self): """ - Remove all actors in front of the vehicle, and stops those behind, - letting the ego smoothly enter the junction. + Remove all actors in front of the vehicle. """ def handle_actors(actor_list): for actor in list(actor_list): @@ -1950,6 +1976,7 @@ def handle_actors(actor_list): if location and not self._is_location_behind_ego(location): self._destroy_actor(actor) + # This will make the actors behind never overtake the ego self._scenario_junction_entry = True if self._active_junctions: @@ -1957,7 +1984,9 @@ def handle_actors(actor_list): handle_actors(source.actors) else: for lane_key in list(self._road_dict): - handle_actors(self._road_dict[lane_key].actors) + source = self._road_dict[lane_key] + handle_actors(source.actors) + source.active = False def _remove_junction_entries(self, wps): """Removes a list of entries of the closest junction (or marks them so that they aren't spawned)""" @@ -2125,7 +2154,7 @@ def _spawn_actors(self, spawn_wps, ego_dist=0): return actors - def _spawn_source_actor(self, source, ego_dist=0): + def _spawn_source_actor(self, source, ego_dist=20): """Given a source, spawns an actor at that source""" ego_location = CarlaDataProvider.get_location(self._ego_actor) source_transform = source.wp.transform diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 679190cf2..f13ebfa72 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -14,15 +14,16 @@ import carla import py_trees from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ( - ActorDestroy, Idle) -from srunner.scenariomanager.scenarioatomics.atomic_criteria import \ - CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import \ - InTriggerDistanceToVehicle + +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, Idle) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle + from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import HandleJunctionScenario +from srunner.tools.scenario_helper import generate_target_waypoint_in_route + def convert_dict_to_location(actor_dict): """ @@ -38,12 +39,8 @@ def convert_dict_to_location(actor_dict): class BlockedIntersection(BasicScenario): """ - This class holds everything required for a scenario in which with low visibility, + This class holds everything required for a scenario in which, the ego performs a turn only to find out that the end is blocked by another vehicle. - The ego is expected to not see the blockage until far into the junction, resulting in a hard brake. - - User needs to specify the location of the blocker. - This scenario is expected to spawn obstacles on the sidewalk. """ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enable=True, @@ -57,100 +54,45 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self.timeout = timeout self._trigger_location = config.trigger_points[0].location - self._reference_waypoint = self._map.get_waypoint( - self._trigger_location) - - self._blocker_location = convert_dict_to_location( - config.other_parameters['blocker_point']) - self._blocker_waypoint = self._map.get_waypoint(self._blocker_location) - self._block_distance = 10 # m. Will stop blocking when ego is within this distance - self._block_time = 5 # s - - if 'obstacle_model' in config.other_parameters: - self._obstacle_model = config.other_parameters['obstacle_model']['value'] - else: - self._obstacle_model = "static.prop.trampoline" - - if 'obstacle_gap' in config.other_parameters: - self._obstacle_gap = int( - config.other_parameters['obstacle_gap']['value']) - else: - self._obstacle_gap = 2 - - # Extra obstacles are not included. One obstacle by default. - self._obstacle_amount = 1 - - # The amount of obstacles that invade the road - if 'extra_obstacle' in config.other_parameters: - self._extra_obstacle = int( - config.other_parameters['extra_obstacle']['value']) - else: - self._extra_obstacle = 2 - - super(BlockedIntersection, self).__init__("BlockedIntersection", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + self._reference_waypoint = self._map.get_waypoint(self._trigger_location) + + self._blocker_distance = 9 + self._trigger_distance = 13 + self._stop_time = 10 + + super().__init__("BlockedIntersection", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ - # Spawn obstacles to block the view - - # Move to the right sidewalk - sidewalk_waypoint = self._reference_waypoint - - while sidewalk_waypoint.lane_type != carla.LaneType.Sidewalk: - right_wp = sidewalk_waypoint.get_right_lane() - if right_wp is None: - raise RuntimeError("Can't find sidewalk to spawn obstacles") - sidewalk_waypoint = right_wp - - sidewalk_points = sidewalk_waypoint.next_until_lane_end( - self._obstacle_gap) - sidewalk_transforms = [wp.transform for wp in sidewalk_points] - - # Add some obstacles to invade the road - for _ in range(self._extra_obstacle): - end_transform_1 = sidewalk_transforms[-1] - end_transform_2 = sidewalk_transforms[-2] - delta_location = carla.Location(x=end_transform_1.location.x-end_transform_2.location.x, - y=end_transform_1.location.y-end_transform_2.location.y, - z=end_transform_1.location.z-end_transform_2.location.z) - extra_location = end_transform_1.location + delta_location - extra_transform = carla.Transform(extra_location, carla.Rotation()) - sidewalk_transforms.append(extra_transform) - - obs_transforms = sidewalk_transforms[-1 * - min(len(sidewalk_transforms), self._obstacle_amount + self._extra_obstacle):] - - # Spawn obstacles - actors = CarlaDataProvider.request_new_batch_actors( - self._obstacle_model, len(obs_transforms), obs_transforms, rolename='scenario') - self.other_actors += actors + waypoint = generate_target_waypoint_in_route(self._reference_waypoint, config.route) + waypoint = waypoint.next(self._blocker_distance)[0] # Spawn the blocker vehicle actor = CarlaDataProvider.request_new_actor( - "vehicle.*.*", self._blocker_waypoint.transform, rolename='scenario') + "vehicle.*.*", waypoint.transform, + attribute_filter={'base_type': 'car', 'has_lights': True} + ) if actor is None: - raise Exception( - "Couldn't spawn the blocker vehicle") + raise Exception("Couldn't spawn the blocker vehicle") self.other_actors.append(actor) def _create_behavior(self): """ - When ego arrives behind the blocker, idel for a while, then clear the blocker. + Just wait for a while after the ego closes in on the blocker, then remove it. """ - - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="BlockedIntersection") if self.route_mode: sequence.add_child(HandleJunctionScenario( clear_junction=True, - clear_ego_entry=True, + clear_ego_entry=False, remove_entries=[], remove_exits=[], stop_entries=True, @@ -158,8 +100,8 @@ def _create_behavior(self): )) # Ego go behind the blocker sequence.add_child(InTriggerDistanceToVehicle( - self.other_actors[-1], self.ego_vehicles[0], self._block_distance)) - sequence.add_child(Idle(self._block_time)) + self.other_actors[-1], self.ego_vehicles[0], self._trigger_distance)) + sequence.add_child(Idle(self._stop_time)) sequence.add_child(ActorDestroy(self.other_actors[-1])) return sequence @@ -170,7 +112,7 @@ def _create_test_criteria(self): in parallel behavior tree. """ if self.route_mode: - return[] + return [] return [CollisionTest(self.ego_vehicles[0])] diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 9509ae6f5..86e2fcead 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -24,20 +24,21 @@ InTimeToArrivalToLocation, DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route +from srunner.tools.scenario_helper import (generate_target_waypoint, + generate_target_waypoint_in_route, + get_same_dir_lanes, + get_opposite_dir_lanes) from srunner.tools.background_manager import LeaveCrossingSpace -def get_sidewalk_transform(waypoint): +def get_sidewalk_transform(waypoint, offset): """ Processes the waypoint transform to find a suitable spawning one at the sidewalk. It first rotates the transform so that it is pointing towards the road and then moves a bit to the side waypoint that aren't part of sidewalks, as they might be invading the road """ - offset = {"yaw": -90, "z": 0.2, "k": 1.5} - new_rotation = waypoint.transform.rotation new_rotation.yaw += offset['yaw'] @@ -83,11 +84,13 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_transform = None self._collision_wp = None - self._adversary_speed = 2.0 # Speed of the adversary [m/s] - self._reaction_time = 1.6 # Time the agent has to react to avoid the collision [s] + self._adversary_speed = 1.8 # Speed of the adversary [m/s] + self._reaction_time = 1.8 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 + self._offset = {"yaw": 270, "z": 0.2, "k": 1.5} + self.timeout = timeout super(BaseVehicleTurning, self).__init__( name, ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) @@ -127,7 +130,7 @@ def _initialize_actors(self, config): sidewalk_waypoint = right_wp # Get the adversary transform and spawn it - self._adversary_transform = get_sidewalk_transform(sidewalk_waypoint) + self._adversary_transform = get_sidewalk_transform(sidewalk_waypoint, self._offset) adversary = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', self._adversary_transform) if adversary is None: self._number_of_attempts -= 1 @@ -252,3 +255,135 @@ def _create_test_criteria(self): Empty, the route already has a collision criteria """ return [] + + +class VehicleTurningRoutePedestrian(BasicScenario): + + """ + This class holds everything required for a simple object crash + with prior vehicle action involving a vehicle and a cyclist. + The ego vehicle is passing through a road and encounters + a cyclist after taking a turn. + + This is a single ego vehicle scenario + """ + _subtype = None + + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, + timeout=60, name="VehicleTurningRoutePedestrian"): + """ + Setup all relevant parameters and create scenario + """ + + self._wmap = CarlaDataProvider.get_map() + self._trigger_location = config.trigger_points[0].location + self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + self._ego_route = config.route + + self._collision_wp = None + self._adversary_speed = 1.8 # Speed of the adversary [m/s] + self._reaction_time = 1.8 # Time the agent has to react to avoid the collision [s] + self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] + self._ego_end_distance = 40 + + self._offset = {"yaw": 270, "z": 0.2, "k": 1.5} + + self.timeout = timeout + super().__init__(name, ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) + + def _initialize_actors(self, config): + """ + Custom initialization + """ + # Get the waypoint right after the junction + waypoint = generate_target_waypoint_in_route(self._reference_waypoint, self._ego_route) + self._collision_wp = waypoint.next(0.5)[0] # Some wps are still part of the junction + + # Get the right waypoint at the sidewalk + same_dir_wps = get_same_dir_lanes(self._collision_wp) + right_wp = same_dir_wps[0] + while right_wp.lane_type != carla.LaneType.Sidewalk: + side_wp = right_wp.get_right_lane() + if side_wp is None: + break + right_wp = side_wp + + # Get the left waypoint at the sidewalk + other_dir_wps = get_opposite_dir_lanes(self._collision_wp) + if other_dir_wps: + # With opposite lane + left_wp = other_dir_wps[-1] + while left_wp.lane_type != carla.LaneType.Sidewalk: + side_wp = left_wp.get_right_lane() + if side_wp is None: + break + left_wp = side_wp + else: + # Without opposite lane + self._offset['yaw'] = 90 + left_wp = same_dir_wps[-1] + while left_wp.lane_type != carla.LaneType.Sidewalk: + side_wp = left_wp.get_left_lane() + if side_wp is None: + break + left_wp = side_wp + + self._adversary_distance = right_wp.transform.location.distance(left_wp.transform.location) + + entry_vec = self._reference_waypoint.transform.get_forward_vector() + exit_vec = waypoint.transform.get_forward_vector() + cross_prod = entry_vec.cross(exit_vec) + spawn_wp = right_wp if cross_prod.z < 0 else left_wp + + # Get the adversary transform and spawn it + spawn_transform = get_sidewalk_transform(spawn_wp, self._offset) + adversary = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) + if adversary is None: + raise ValueError("Couldn't spawn adversary") + + self.other_actors.append(adversary) + + def _create_behavior(self): + """ + """ + sequence = py_trees.composites.Sequence(name="VehicleTurningRoutePedestrian") + collision_location = self._collision_wp.transform.location + + # Adversary trigger behavior + trigger_adversary = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") + trigger_adversary.add_child(InTimeToArrivalToLocation( + self.ego_vehicles[0], self._reaction_time, collision_location)) + trigger_adversary.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], collision_location, self._min_trigger_dist)) + + sequence.add_child(trigger_adversary) + if self.route_mode: + sequence.add_child(LeaveCrossingSpace(self._collision_wp)) + + # Move the adversary. + speed_distance = self._adversary_distance + speed_duration = self._adversary_distance / self._adversary_speed + sequence.add_child(KeepVelocity( + self.other_actors[0], self._adversary_speed, True, + speed_duration, speed_distance, name="AdversaryCrossing") + ) + + # Remove everything + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) + sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) + + return sequence + + def _create_test_criteria(self): + """ + A list of all test criteria will be created that is later used + in parallel behavior tree. + """ + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() \ No newline at end of file diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 8499ffc1e..e0cb095f4 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -287,7 +287,8 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence(name="CrossingActor") if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._distance)) + total_dist = self._distance + 10 + sequence.add_child(LeaveSpaceInFront(total_dist)) collision_location = self._collision_wp.transform.location diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index 0e2d34c8d..ec60a3b6b 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -12,7 +12,7 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity, WaitForever, Idle from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -21,8 +21,6 @@ from srunner.tools.background_manager import HandleJunctionScenario -from srunner.tools.scenario_helper import get_same_dir_lanes, get_opposite_dir_lanes - def convert_dict_to_location(actor_dict): """ @@ -55,6 +53,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._wmap = CarlaDataProvider.get_map() self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) + self._rng = CarlaDataProvider.get_random_seed() self._adversary_speed = 1.3 # Speed of the adversary [m/s] self._reaction_time = 4.0 # Time the agent has to react to avoid the collision [s] @@ -62,8 +61,15 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._ego_end_distance = 40 self.timeout = timeout - self._crosswalk_dist = 1 - self._crosswalk_right_dist = 1 + self._walker_data = [ + {'x': 0.4, 'y': 1.5, 'z': 1, 'yaw': 270}, + {'x': 1, 'y': 2.5, 'z': 1, 'yaw': 270}, + {'x': 1.6, 'y': 0.5, 'z': 1, 'yaw': 270} + ] + + for walker_data in self._walker_data: + walker_data['idle_time'] = self._rng.uniform(0, 1.5) + walker_data['speed'] = self._rng.uniform(1.3, 2.0) super().__init__("PedestrianCrossing", ego_vehicles, @@ -72,6 +78,26 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl debug_mode, criteria_enable=criteria_enable) + def _get_walker_transform(self, wp, displacement): + disp_x = displacement['x'] + disp_y = displacement['y'] + disp_z = displacement['z'] + disp_yaw = displacement['yaw'] + + # Displace it to the crosswalk. Move forwards towards the crosswalk + start_vec = wp.transform.get_forward_vector() + start_right_vec = wp.transform.get_right_vector() + + spawn_loc = wp.transform.location + carla.Location( + disp_x * start_vec.x + disp_y * start_right_vec.x, + disp_x * start_vec.y + disp_y * start_right_vec.y, + disp_x * start_vec.z + disp_y * start_right_vec.z + disp_z + ) + + spawn_rotation = wp.transform.rotation + spawn_rotation.yaw += disp_yaw + return carla.Transform(spawn_loc, spawn_rotation) + def _initialize_actors(self, config): # Get the start point of the initial pedestrian @@ -94,27 +120,22 @@ def _initialize_actors(self, config): raise ValueError("Couldn't find a waypoint to start the flow") start_wp = wp - # Displace it to the crosswalk. Move forwards towards the crosswalk - start_vec = start_wp.transform.get_forward_vector() - start_right_vec = start_wp.transform.get_right_vector() - - spawn_loc = start_wp.transform.location + carla.Location( - self._crosswalk_dist * start_vec.x + self._crosswalk_right_dist * start_right_vec.x, - self._crosswalk_dist * start_vec.y + self._crosswalk_right_dist * start_right_vec.y, - self._crosswalk_dist * start_vec.z + self._crosswalk_right_dist * start_right_vec.z + 1.0 - ) - - spawn_rotation = start_wp.transform.rotation - spawn_rotation.yaw += 270 - spawn_transform = carla.Transform(spawn_loc, spawn_rotation) + # Spawn the walkers + for walker_data in self._walker_data: + spawn_transform = self._get_walker_transform(start_wp, walker_data) + walker = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) + if walker is None: + for walker in self.other_actors: + walker.destroy() + raise ValueError("Failed to spawn an adversary") + self.other_actors.append(walker) - adversary = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) - if adversary is None: - raise ValueError("Failed to spawn an adversary") + collision_dist = spawn_transform.location.distance(self._collision_wp.transform.location) - self._collision_dist = spawn_transform.location.distance(self._collision_wp.transform.location) - - self.other_actors.append(adversary) + # Distance and duration to cross the whole road + a bit more (supposing symetry in both directions) + move_dist = 2.3 * collision_dist + walker_data['distance'] = move_dist + walker_data['duration'] = move_dist / walker_data['speed'] def _create_behavior(self): """ @@ -123,7 +144,7 @@ def _create_behavior(self): the cyclist starts crossing the road once the condition meets, then after 60 seconds, a timeout stops the scenario """ - sequence = py_trees.composites.Sequence(name="CrossingActor") + sequence = py_trees.composites.Sequence(name="PedestrianCrossing") if self.route_mode: sequence.add_child(HandleJunctionScenario( clear_junction=False, @@ -145,16 +166,24 @@ def _create_behavior(self): self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) - # Move the adversary - move_distance = 2 * self._collision_dist # Cross the whole road (supposing symetry in both directions) - move_duration = move_distance / self._adversary_speed - sequence.add_child(KeepVelocity( - self.other_actors[0], self._adversary_speed, - duration=move_duration, distance=move_distance, name="AdversaryCrossing")) + # Move the walkers + main_behavior = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="WalkerMovement") + + for walker_actor, walker_data in zip(self.other_actors, self._walker_data): + walker_sequence = py_trees.composites.Sequence(name="WalkerCrossing") + walker_sequence.add_child(Idle(walker_data['idle_time'])) + walker_sequence.add_child(KeepVelocity( + walker_actor, walker_data['speed'], False, walker_data['duration'], walker_data['distance'])) + walker_sequence.add_child(ActorDestroy(walker_actor, name="DestroyAdversary")) + walker_sequence.add_child(WaitForever()) + + main_behavior.add_child(walker_sequence) + + main_behavior.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) + sequence.add_child(main_behavior) # Remove everything - sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) - sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) return sequence diff --git a/srunner/tools/scenario_helper.py b/srunner/tools/scenario_helper.py index b144f0d81..4aa03b641 100644 --- a/srunner/tools/scenario_helper.py +++ b/srunner/tools/scenario_helper.py @@ -738,7 +738,10 @@ def get_distance_between_actors(current, target, distance_type="euclidianDistanc def get_same_dir_lanes(waypoint): - """Gets all the lanes with the same direction of the road of a wp""" + """ + Gets all the lanes with the same direction of the road of a wp. + Ordered from the edge lane to the center one (from outwards to inwards) + """ same_dir_wps = [waypoint] # Check roads on the right @@ -765,7 +768,10 @@ def get_same_dir_lanes(waypoint): def get_opposite_dir_lanes(waypoint): - """Gets all the lanes with opposite direction of the road of a wp""" + """ + Gets all the lanes with opposite direction of the road of a wp + Ordered from the center lane to the edge one (from inwards to outwards) + """ other_dir_wps = [] other_dir_wp = None From da0da43e903c2300bc07612971760c496b72e306 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 12 Aug 2022 09:57:08 +0200 Subject: [PATCH 47/86] Added noise to control loss --- .../scenarioatomics/atomic_behaviors.py | 56 ++++- .../scenarioatomics/atomic_criteria.py | 237 +++++++++--------- .../atomic_trigger_conditions.py | 11 +- srunner/scenarios/blocked_intersection.py | 30 ++- srunner/scenarios/control_loss.py | 81 ++++-- .../scenarios/cut_in_with_static_vehicle.py | 3 + srunner/scenarios/parking_exit.py | 2 +- .../scenarios/yield_to_emergency_vehicle.py | 106 +++++--- 8 files changed, 321 insertions(+), 205 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index dc36a8eb7..5bb1c79f4 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1873,6 +1873,52 @@ def update(self): 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. + + Important parameters: + - actor: CARLA actor to execute the behavior + - steer_value: Applied steering noise in [0,1] + - throttle_value: Applied throttle noise in [0,1] + + The behavior terminates after setting the new actor controls + """ + + def __init__(self, actor, throttle_mean, throttle_std, steer_mean, steer_std, name="AddNoiseToVehicle"): + """ + Setup actor , maximum steer value and throttle value + """ + super().__init__(name, actor) + self._throttle_mean = throttle_mean + self._throttle_std = throttle_std + self._steer_mean = steer_mean + self._steer_std = steer_std + + self._rng = CarlaDataProvider.get_random_seed() + + def update(self): + """ + Set steer to steer_value and throttle to throttle_value until reaching full stop + """ + new_status = py_trees.common.Status.RUNNING + + control = py_trees.blackboard.Blackboard().get("AV_control") + + throttle_noise = random.normal(self._throttle_mean, self._throttle_std) + control.throttle = max(-1, min(1, control.throttle + throttle_noise)) + + 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._actor.apply_control(control) + + return new_status + + class ChangeNoiseParameters(AtomicBehavior): """ @@ -2054,29 +2100,27 @@ class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): 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="ConstantVelocityAgentBehavior"): + def __init__(self, actor, reference_actor, target_location=None, speed_increment=10, + opt_dict=None, name="AdaptiveConstantVelocityAgentBehavior"): """ Set up actor and local planner """ - super(AdaptiveConstantVelocityAgentBehavior, self).__init__(name, actor) + super().__init__(name, actor) self._speed_increment = speed_increment self._reference_actor = reference_actor - self._map = CarlaDataProvider.get_map() self._target_location = target_location self._opt_dict = opt_dict if opt_dict else {} self._control = carla.VehicleControl() self._agent = None self._plan = None + self._map = CarlaDataProvider.get_map() self._grp = CarlaDataProvider.get_global_route_planner() def initialise(self): """Initialises the agent""" # Get target speed self._target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 - py_trees.blackboard.Blackboard().set( - "ACVAB_speed_{}".format(self._reference_actor.id), self._target_speed, overwrite=True) self._agent = ConstantVelocityAgent(self._actor, self._target_speed, opt_dict=self._opt_dict, map_inst=self._map, grp_inst=self._grp) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 9befbdef2..794e75ddc 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -987,10 +987,10 @@ class OutsideRouteLanesTest(Criterion): optional (bool): If True, the result is not considered for an overall pass/fail result """ - ALLOWED_OUT_DISTANCE = 1.3 # At least 0.5, due to the mini-shoulder between lanes and sidewalks - MAX_ALLOWED_VEHICLE_ANGLE = 120.0 # Maximum angle between the yaw and waypoint lane - MAX_ALLOWED_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) + 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) def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): """ @@ -1005,7 +1005,7 @@ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"): self._route_transforms, _ = zip(*self._route) self._map = CarlaDataProvider.get_map() - self._pre_ego_waypoint = self._map.get_waypoint(self.actor.get_location()) + self._last_ego_waypoint = self._map.get_waypoint(self.actor.get_location()) self._outside_lane_active = False self._wrong_lane_active = False @@ -1097,27 +1097,25 @@ def _set_traffic_event(self): self._traffic_event.set_frame(GameTime.get_frame()) - def _is_outside_driving_lanes(self, location): """ Detects if the ego_vehicle is outside driving lanes """ + driving_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving) + parking_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Parking) - current_driving_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving, project_to_road=True) - current_parking_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Parking, project_to_road=True) - - driving_distance = location.distance(current_driving_wp.transform.location) - if current_parking_wp is not None: # Some towns have no parking - parking_distance = location.distance(current_parking_wp.transform.location) + driving_distance = location.distance(driving_wp.transform.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') if driving_distance >= parking_distance: distance = parking_distance - lane_width = current_parking_wp.lane_width + lane_width = parking_wp.lane_width else: distance = driving_distance - lane_width = current_driving_wp.lane_width + lane_width = driving_wp.lane_width self._outside_lane_active = bool(distance > (lane_width / 2 + self.ALLOWED_OUT_DISTANCE)) @@ -1125,49 +1123,41 @@ def _is_at_wrong_lane(self, location): """ Detects if the ego_vehicle has invaded a wrong lane """ - current_waypoint = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving, project_to_road=True) - current_lane_id = current_waypoint.lane_id - current_road_id = current_waypoint.road_id + waypoint = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving) + lane_id = waypoint.lane_id + road_id = waypoint.road_id # Lanes and roads are too chaotic at junctions - if current_waypoint.is_junction: + if waypoint.is_junction: self._wrong_lane_active = False - elif self._last_road_id != current_road_id or self._last_lane_id != current_lane_id: - - # Route direction can be considered continuous, except after exiting a junction. - if self._pre_ego_waypoint.is_junction: - yaw_waypt = current_waypoint.transform.rotation.yaw % 360 - yaw_actor = self.actor.get_transform().rotation.yaw % 360 + elif self._last_road_id != road_id or self._last_lane_id != lane_id: - vehicle_lane_angle = (yaw_waypt - yaw_actor) % 360 + 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 vehicle_lane_angle < self.MAX_ALLOWED_VEHICLE_ANGLE \ - or vehicle_lane_angle > (360 - self.MAX_ALLOWED_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 else: - # Check for a big gap in waypoint directions. - yaw_pre_wp = self._pre_ego_waypoint.transform.rotation.yaw % 360 - yaw_cur_wp = current_waypoint.transform.rotation.yaw % 360 + # Route direction can be considered continuous, check for a big gap. + last_wp_yaw = self._last_ego_waypoint.transform.rotation.yaw % 360 + wp_yaw = waypoint.transform.rotation.yaw % 360 + angle = (last_wp_yaw - wp_yaw) % 360 - waypoint_angle = (yaw_pre_wp - yaw_cur_wp) % 360 - - if waypoint_angle >= self.MAX_ALLOWED_WAYPOINT_ANGLE \ - and waypoint_angle <= (360 - self.MAX_ALLOWED_WAYPOINT_ANGLE): # pylint: disable=chained-comparison + 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) - else: - - # Changing to a lane with the same direction - self._wrong_lane_active = False # Remember the last state - self._last_lane_id = current_lane_id - self._last_road_id = current_road_id - self._pre_ego_waypoint = current_waypoint + self._last_lane_id = lane_id + self._last_road_id = road_id + self._last_ego_waypoint = waypoint class WrongLaneTest(Criterion): @@ -1180,7 +1170,7 @@ class WrongLaneTest(Criterion): - optional [optional]: If True, the result is not considered for an overall pass/fail result """ MAX_ALLOWED_ANGLE = 120.0 - MAX_ALLOWED_WAYPOINT_ANGLE = 150.0 + MAX_WAYPOINT_ANGLE = 150.0 def __init__(self, actor, optional=False, name="WrongLaneTest"): """ @@ -1232,7 +1222,7 @@ def update(self): 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_ALLOWED_WAYPOINT_ANGLE and self._in_lane: + if waypoint_angle > self.MAX_WAYPOINT_ANGLE and self._in_lane: self.test_status = "FAILURE" self._in_lane = False @@ -1673,10 +1663,10 @@ 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, location.z), 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, location.z), 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: @@ -1721,9 +1711,9 @@ def update(self): lane_width = wp.lane_width location_wp = wp.transform.location - lft_lane_wp = self.rotate_point(carla.Vector3D(0.4 * lane_width, 0.0, location_wp.z), 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.4 * lane_width, 0.0, location_wp.z), 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? @@ -1809,9 +1799,9 @@ 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 = 5.0 # meters - SPEED_THRESHOLD = 0.1 - EXTENT_MULTIPLIER = 1.5 + 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] + WAYPOINT_STEP = 0.5 # m def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): """ @@ -1823,14 +1813,12 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): self._target_stop_sign = None self._stop_completed = False - all_actors = CarlaDataProvider.get_all_actors() - for _actor in all_actors: + for _actor in CarlaDataProvider.get_all_actors(): if 'traffic.stop' in _actor.type_id: self._list_stop_signs.append(_actor) def point_inside_boundingbox(self, point, bb_center, bb_extent): """Checks whether or not a point is inside a bounding box""" - bb_extent = self.EXTENT_MULTIPLIER * bb_extent # pylint: disable=invalid-name A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y) @@ -1848,40 +1836,66 @@ def point_inside_boundingbox(self, point, bb_center, bb_extent): return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad # pylint: disable=chained-comparison - def is_actor_affected_by_stop(self, location, stop): + def is_actor_affected_by_stop(self, wp_list, stop): """ - Check if the given actor is affected by the stop + Check if the given actor is affected by the stop. + Without using waypoints, a stop might not be detected if the actor is moving at the lane edge. """ # Quick distance test - stop_transform = stop.get_transform() - if stop_transform.location.distance(location) > self.PROXIMITY_THRESHOLD: + stop_location = stop.get_transform().transform(stop.trigger_volume.location) + actor_location = wp_list[0].transform.location + if stop_location.distance(actor_location) > self.PROXIMITY_THRESHOLD: return False - # Check if the actor is inside the stop's bounding box - stop_t = stop.get_transform() - transformed_tv = stop_t.transform(stop.trigger_volume.location) - if self.point_inside_boundingbox(location, transformed_tv, stop.trigger_volume.extent): - return True + # Check if the any of the actor wps is inside the stop's bounding box. + # 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): + return True return False - def _scan_for_stop_sign(self, location): - ve_tra = CarlaDataProvider.get_transform(self.actor) - ve_dir = ve_tra.get_forward_vector() + def _scan_for_stop_sign(self, actor_transform, wp_list): + """ + Check the stop signs to see if any of them affect the actor. + Ignore all checks when going backwards or through an opposite direction""" - wp = self._map.get_waypoint(ve_tra.location) - wp_dir = wp.transform.get_forward_vector() + actor_direction = actor_transform.get_forward_vector() - if ve_dir.dot(wp_dir) < 0: # Ignore all when going in a wrong lane + # Ignore all when going backwards + actor_velocity = self.actor.get_velocity() + if actor_velocity.dot(actor_direction) < -0.17: # 100º, just in case return None - ve_vec = self.actor.get_velocity() - if ve_vec.dot(wp_dir) < 0: # Ignore all when going backwards + # Ignore all when going in the opposite direction + lane_direction = wp_list[0].transform.get_forward_vector() + if actor_direction.dot(lane_direction) < -0.17: # 100º, just in case return None - for stop_sign in self._list_stop_signs: - if self.is_actor_affected_by_stop(location, stop_sign): - return stop_sign # This stop sign is affecting the vehicle + for stop in self._list_stop_signs: + if self.is_actor_affected_by_stop(wp_list, stop): + return stop + + def _get_waypoints(self, actor): + """Returns a list of waypoints starting from the ego location and a set amount forward""" + wp_list = [] + steps = int(self.PROXIMITY_THRESHOLD / self.WAYPOINT_STEP) + + # Add the actor location + wp = self._map.get_waypoint(actor.get_location()) + wp_list.append(wp) + + # And its forward waypoints + next_wp = wp + for _ in range(steps): + next_wps = next_wp.next(self.WAYPOINT_STEP) + if not next_wps: + break + next_wp = next_wps[0] + wp_list.append(next_wp) + + return wp_list def update(self): """ @@ -1889,12 +1903,11 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING - location = self.actor.get_location() - if location is None: - return new_status + actor_transform = self.actor.get_transform() + check_wps = self._get_waypoints(self.actor) if not self._target_stop_sign: - self._target_stop_sign = self._scan_for_stop_sign(location) + self._target_stop_sign = self._scan_for_stop_sign(actor_transform, check_wps) return new_status if not self._stop_completed: @@ -1902,7 +1915,7 @@ def update(self): if current_speed < self.SPEED_THRESHOLD: self._stop_completed = True - if not self.is_actor_affected_by_stop(location, self._target_stop_sign): + if not self.is_actor_affected_by_stop(check_wps, self._target_stop_sign): if not self._stop_completed: # did we stop? self.actual_value += 1 @@ -2026,7 +2039,8 @@ def terminate(self, new_status): class YieldToEmergencyVehicleTest(Criterion): """ - Atomic Criterion to detect if the actor yields its lane to the emergency vehicle behind it + 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 Args: actor (carla.Actor): CARLA actor to be used for this test @@ -2040,71 +2054,44 @@ def __init__(self, actor, ev, optional=False, name="YieldToEmergencyVehicleTest" """ Constructor """ - super().__init__(name, actor, optional) - self.units = "%" - self.success_value = 95 - self.actual_value = 0 self._ev = ev - self._target_speed = None - self._ev_speed_log = [] - self._map = CarlaDataProvider.get_map() - - self.initialized = False self._terminated = False - - def initialise(self): - self.initialized = True - return super().initialise() + super().__init__(name, actor, optional) def update(self): """ - Collect ev's actual speed on each time-step + Monitor that the EV ends up in front of the actor returns: py_trees.common.Status.RUNNING """ new_status = py_trees.common.Status.RUNNING - # Get target speed from Blackboard - # The value is expected to be set by AdaptiveConstantVelocityAgentBehavior - if self._target_speed is None: - target_speed = py_trees.blackboard.Blackboard().get("ACVAB_speed_{}".format(self.actor.id)) - if target_speed is not None: - self._target_speed = target_speed - py_trees.blackboard.Blackboard().set("ACVAB_speed_{}".format(self.actor.id), None, overwrite=True) - else: - return new_status + actor_location = CarlaDataProvider.get_location(self.actor) + if not actor_location: + return new_status + ev_location = CarlaDataProvider.get_location(self._ev) + if not ev_location: + return new_status - if self._ev.is_alive: - ev_speed = get_speed(self._ev) - # Record ev's speed in this moment - self._ev_speed_log.append(ev_speed) + ev_direction = CarlaDataProvider.get_transform(self._ev).get_forward_vector() + actor_ev_vector = actor_location - ev_location + + if ev_direction.dot(actor_ev_vector) > 0: + self.test_status = "FAILURE" + else: + self.test_status = "SUCCESS" self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status def terminate(self, new_status): - """Set the traffic event to the according value if needed""" - + """Set the traffic event, if needed""" # Terminates are called multiple times. Do this only once - if not self._terminated and self.initialized: - if not len(self._ev_speed_log): - self.actual_value = 100 - else: - mean_speed = sum(self._ev_speed_log) / len(self._ev_speed_log) - self.actual_value = mean_speed / self._target_speed *100 - self.actual_value = round(self.actual_value, 2) - - if self.actual_value >= self.success_value: - self.test_status = "SUCCESS" - else: - self.test_status = "FAILURE" - + if not self._terminated: if self.test_status == "FAILURE": - traffic_event = TrafficEvent(event_type=TrafficEventType.YIELD_TO_EMERGENCY_VEHICLE, frame=GameTime.get_frame()) - traffic_event.set_dict({'percentage': self.actual_value}) - traffic_event.set_message( - f"Agent failed to yield to an emergency vehicle, slowing it to {self.actual_value}% of its velocity)") + 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 diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index e1a81af1f..31a832b5f 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -31,7 +31,7 @@ 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 +from srunner.tools.scenario_helper import get_distance_along_route, get_distance_between_actors import srunner.tools as sr_tools @@ -586,7 +586,7 @@ def __init__(self, reference_actor, actor, distance, comparison_operator=operato self._comparison_operator = comparison_operator if distance_type == "longitudinal": - self._global_rp = GlobalRoutePlanner(CarlaDataProvider.get_world().get_map(), 1.0) + self._global_rp = CarlaDataProvider.get_global_route_planner() else: self._global_rp = None @@ -602,11 +602,8 @@ def update(self): if location is None or reference_location is None: return new_status - distance = sr_tools.scenario_helper.get_distance_between_actors(self._actor, - self._reference_actor, - distance_type=self._distance_type, - freespace=self._freespace, - global_planner=self._global_rp) + distance = get_distance_between_actors( + 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 diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index f13ebfa72..2b2b909e8 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -15,8 +15,8 @@ import py_trees from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, Idle) -from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, Idle, ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle from srunner.scenarios.basic_scenario import BasicScenario @@ -56,10 +56,12 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint(self._trigger_location) - self._blocker_distance = 9 - self._trigger_distance = 13 + self._blocker_distance = 7 + self._trigger_distance = 12 self._stop_time = 10 + self._scenario_timeout = 240 + super().__init__("BlockedIntersection", ego_vehicles, config, @@ -99,9 +101,17 @@ def _create_behavior(self): extend_road_exit=0 )) # Ego go behind the blocker - sequence.add_child(InTriggerDistanceToVehicle( + main_behavior = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) + + behavior = py_trees.composites.Sequence(name="Approach and Wait") + behavior.add_child(InTriggerDistanceToVehicle( self.other_actors[-1], self.ego_vehicles[0], self._trigger_distance)) - sequence.add_child(Idle(self._stop_time)) + behavior.add_child(Idle(self._stop_time)) + main_behavior.add_child(behavior) + + sequence.add_child(main_behavior) sequence.add_child(ActorDestroy(self.other_actors[-1])) return sequence @@ -111,10 +121,10 @@ def _create_test_criteria(self): A list of all test criteria will be created that is later used in parallel behavior tree. """ - if self.route_mode: - return [] - - return [CollisionTest(self.ego_vehicles[0])] + criteria = [ScenarioTimeoutTest(self.ego_vehicles[0], self.config.name)] + if not self.route_mode: + criteria.append(CollisionTest(self.ego_vehicles[0])) + return criteria def __del__(self): """ diff --git a/srunner/scenarios/control_loss.py b/srunner/scenarios/control_loss.py index d78209ecc..454b50430 100644 --- a/srunner/scenarios/control_loss.py +++ b/srunner/scenarios/control_loss.py @@ -14,11 +14,14 @@ from numpy import random import py_trees +import operator + import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import AddNoiseToRouteEgo from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTriggerDistanceToLocation from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.scenario_helper import get_waypoint_in_distance @@ -38,22 +41,25 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals """ self.timeout = timeout self._randomize = randomize + self._rng = CarlaDataProvider.get_random_seed() self._map = CarlaDataProvider.get_map() self._end_distance = 110 - super(ControlLoss, self).__init__("ControlLoss", - ego_vehicles, - config, - world, - debug_mode, - criteria_enable=criteria_enable) + # Friction loss tends to have a much stronger steering compoenent then a throttle one + self._throttle_mean = 0.035 + self._throttle_std = 0.035 + self._steer_mean = 0.07 + self._steer_std = 0.07 + + self._trigger_dist = 2 + + super().__init__("ControlLoss", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) def _initialize_actors(self, config): """ Custom initialization """ - self._rng = CarlaDataProvider.get_random_seed() if self._randomize: self._distance = list(self._rng.randint(low=10, high=80, size=3)) self._distance = sorted(self._distance) @@ -68,19 +74,16 @@ def _initialize_actors(self, config): first_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[0]) first_ground_loc = self.world.ground_projection(first_wp.transform.location + carla.Location(z=1), 2) first_loc = first_ground_loc.location if first_ground_loc else first_wp.transform.location - first_loc = self._get_offset_location(first_loc, first_wp.transform.get_right_vector(), self._offset[0]) self.first_transform = carla.Transform(first_loc, first_wp.transform.rotation) second_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[1]) second_ground_loc = self.world.ground_projection(second_wp.transform.location + carla.Location(z=1), 2) second_loc = second_ground_loc.location if second_ground_loc else second_wp.transform.location - second_loc = self._get_offset_location(second_loc, second_wp.transform.get_right_vector(), self._offset[1]) self.second_transform = carla.Transform(second_loc, second_wp.transform.rotation) third_wp, _ = get_waypoint_in_distance(self._reference_waypoint, self._distance[2]) third_ground_loc = self.world.ground_projection(third_wp.transform.location + carla.Location(z=1), 2) third_loc = third_ground_loc.location if third_ground_loc else third_wp.transform.location - third_loc = self._get_offset_location(third_loc, third_wp.transform.get_right_vector(), self._offset[2]) self.third_transform = carla.Transform(third_loc, third_wp.transform.rotation) # Spawn the debris @@ -100,17 +103,61 @@ def _initialize_actors(self, config): self.other_actors.append(second_debris) self.other_actors.append(third_debris) - def _get_offset_location(self, location, direction, distance): - """Offset the debris a bit to the side. Debris in the center rarely affect the ego wheels""" - return location + carla.Location(x=distance*direction.x, y=distance*direction.y, z=distance*direction.z) + def _get_noise_parameters(self): + """Randomizes the mean to be either positive or negative""" + return [ + self._rng.choice([self._throttle_mean, -self._throttle_mean]), + self._throttle_std, + self._rng.choice([self._steer_mean, -self._steer_mean]), + self._steer_std + ] def _create_behavior(self): """ The scenario defined after is a "control loss vehicle" scenario. """ - sequence = py_trees.composites.Sequence("ControlLoss") - sequence.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) - return sequence + root = py_trees.composites.Parallel("ControlLoss", py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + sequence = py_trees.composites.Sequence() + + # First debris behavior + sequence.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.first_transform.location, self._trigger_dist)) + + noise_1 = self._get_noise_parameters() + noise_behavior_1 = py_trees.composites.Parallel("Add Noise 1", py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + noise_behavior_1.add_child(AddNoiseToRouteEgo(self.ego_vehicles[0], *noise_1)) + noise_behavior_1.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.first_transform.location, self._trigger_dist, operator.gt)) + sequence.add_child(noise_behavior_1) + + # Second debris behavior + sequence.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.second_transform.location, self._trigger_dist)) + + noise_2 = self._get_noise_parameters() + noise_behavior_2 = py_trees.composites.Parallel("Add Noise 2", py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + noise_behavior_2.add_child(AddNoiseToRouteEgo(self.ego_vehicles[0], *noise_2)) + noise_behavior_2.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.second_transform.location, self._trigger_dist, operator.gt)) + sequence.add_child(noise_behavior_2) + + # Third debris behavior + sequence.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.third_transform.location, self._trigger_dist)) + + noise_3 = self._get_noise_parameters() + noise_behavior_3 = py_trees.composites.Parallel("Add Noise 3", py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + noise_behavior_3.add_child(AddNoiseToRouteEgo(self.ego_vehicles[0], *noise_3)) + noise_behavior_3.add_child(InTriggerDistanceToLocation( + self.ego_vehicles[0], self.third_transform.location, self._trigger_dist, operator.gt)) + sequence.add_child(noise_behavior_3) + + end_distance = self._end_distance - self._distance[-1] + sequence.add_child(DriveDistance(self.ego_vehicles[0], end_distance)) + + root.add_child(sequence) + root.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) + return root def _create_test_criteria(self): """ diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 2c35b2d96..5dc40f10d 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -157,6 +157,9 @@ def _initialize_actors(self, config): self._side_transforms.append([self._adversary_actor, side_wp.transform]) self.other_actors.append(self._adversary_actor) + # This starts the engine, to allow the adversary to instantly move + self._adversary_actor.apply_control(carla.VehicleControl(throttle=1.0, brake=1.0)) + # Move to the front next_wps = blocker_wp.next(self._vehicle_gap) if not next_wps: diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index f41bddd60..72a3e7b86 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -82,7 +82,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._flow_distance = get_value_parameter(config, 'flow_distance', float, 25) self._max_speed = get_value_parameter(config, 'speed', float, 60) - self._scenario_timeout = 120 + self._scenario_timeout = 240 self._end_distance = self._front_vehicle_distance + 15 diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py index 8520f132c..0a9445e5c 100644 --- a/srunner/scenarios/yield_to_emergency_vehicle.py +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -15,8 +15,14 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorTransformSetter, ActorDestroy, Idle, AdaptiveConstantVelocityAgentBehavior +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter, + ActorDestroy, + Idle, + AdaptiveConstantVelocityAgentBehavior) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, YieldToEmergencyVehicleTest +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToVehicle, + WaitUntilInFront, + DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import RemoveRoadLane, ReAddRoadLane @@ -40,21 +46,31 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._world = world self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._ev_drive_time = 10 # seconds + self._ev_idle_time = 10 # seconds # km/h. How much the EV is expected to be faster than the EGO - self._speed_increment = 10 + self._speed_increment = 25 + + self._trigger_distance = 50 if 'distance' in config.other_parameters: - self._distance = float( - config.other_parameters['distance']['value']) + self._distance = float(config.other_parameters['distance']['value']) else: - self._distance = 80 # m + self._distance = 140 # m + + # Change some of the parameters to adapt its behavior. + # 1) ConstantVelocityAgent = infinite acceleration -> reduce the detection radius to pressure the ego + # 2) Always use the bb check to ensure the EV doesn't run over the ego when it is lane changing + # 3) Add more wps to improve BB detection + self._opt_dict = { + 'base_vehicle_threshold': 15, 'detection_speed_ratio': 0.2, 'use_bbs_detection': True, + 'base_min_distance': 1, 'distance_ratio': 0.2 + } self._trigger_location = config.trigger_points[0].location - self._reference_waypoint = self._map.get_waypoint( - self._trigger_location) - self._ev_start_transform = None + self._reference_waypoint = self._map.get_waypoint(self._trigger_location) + + self._end_distance = 50 super().__init__("YieldToEmergencyVehicle", ego_vehicles, @@ -68,57 +84,70 @@ def _initialize_actors(self, config): Custom initialization """ # Spawn emergency vehicle - ev_points = self._reference_waypoint.previous( - self._distance) - if ev_points: - self._ev_start_transform = ev_points[0].transform - else: - raise Exception( - "Couldn't find viable position for the emergency vehicle") + ev_points = self._reference_waypoint.previous(self._distance) + if not ev_points: + raise ValueError("Couldn't find viable position for the emergency vehicle") + + self._ev_start_transform = ev_points[0].transform actor = CarlaDataProvider.request_new_actor( - "vehicle.*.*", self._ev_start_transform, rolename='scenario', attribute_filter={'special_type': 'emergency'}) + "vehicle.*.*", self._ev_start_transform, attribute_filter={'special_type': 'emergency'}) if actor is None: - raise Exception( - "Couldn't spawn the emergency vehicle") - # Remove its physics so that it doesn't fall + raise Exception("Couldn't spawn the emergency vehicle") + + # Move the actor underground and remove its physics so that it doesn't fall actor.set_simulate_physics(False) - # Move the actor underground new_location = actor.get_location() new_location.z -= 500 actor.set_location(new_location) + # Turn on special lights actor.set_light_state(carla.VehicleLightState( carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) + self.other_actors.append(actor) def _create_behavior(self): """ - - Remove BA from current lane - - Teleport Emergency Vehicle(EV) behind the ego - - [Parallel SUCCESS_ON_ONE] - - Idle(20 seconds) + Spawn the EV behind and wait for it to be close-by. After it has approached, + give the ego a certain amount of time to yield to it. + + Sequence: + - RemoveRoadLane + - ActorTransformSetter + - Parallel: - AdaptiveConstantVelocityAgentBehavior - - Destroy EV - - [Parallel SUCCESS_ON_ONE] - - DriveDistance(ego, 30) - - Recover BA + - Sequence: (End condition 1) + - InTriggerDistanceToVehicle: + - Idle + - Sequence: (End condition 2) + - WaitUntilInFront + - DriveDistance + - ReAddRoadLane """ - - sequence = py_trees.composites.Sequence() + sequence = py_trees.composites.Sequence(name="YieldToEmergencyVehicle") if self.route_mode: sequence.add_child(RemoveRoadLane(self._reference_waypoint)) - # Teleport EV behind the ego - sequence.add_child(ActorTransformSetter( - self.other_actors[0], self._ev_start_transform)) + sequence.add_child(ActorTransformSetter(self.other_actors[0], self._ev_start_transform)) - # Emergency Vehicle runs for self._ev_drive_time seconds main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - main_behavior.add_child(Idle(self._ev_drive_time)) + + end_condition_1 = py_trees.composites.Sequence() + end_condition_1.add_child(InTriggerDistanceToVehicle( + self.ego_vehicles[0], self.other_actors[0], self._trigger_distance)) + end_condition_1.add_child(Idle(self._ev_idle_time)) + + end_condition_2 = py_trees.composites.Sequence() + end_condition_2.add_child(WaitUntilInFront(self.other_actors[0], self.ego_vehicles[0])) + end_condition_2.add_child(DriveDistance(self.other_actors[0], self._end_distance)) + + main_behavior.add_child(end_condition_1) + main_behavior.add_child(end_condition_2) + main_behavior.add_child(AdaptiveConstantVelocityAgentBehavior( - self.other_actors[0], self.ego_vehicles[0], speed_increment=self._speed_increment)) + self.other_actors[0], self.ego_vehicles[0], speed_increment=self._speed_increment, opt_dict=self._opt_dict)) sequence.add_child(main_behavior) @@ -135,8 +164,7 @@ def _create_test_criteria(self): in parallel behavior tree. """ criterias = [] - criterias.append(YieldToEmergencyVehicleTest( - self.ego_vehicles[0], self.other_actors[0])) + criterias.append(YieldToEmergencyVehicleTest(self.ego_vehicles[0], self.other_actors[0])) if not self.route_mode: criterias.append(CollisionTest(self.ego_vehicles[0])) From d8a251d1ce0efe549c535d916545a37f7b6f614c Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 17 Aug 2022 10:54:19 +0200 Subject: [PATCH 48/86] Route improvements --- srunner/scenariomanager/lights_sim.py | 25 ++++++++++++----- .../scenarioatomics/atomic_behaviors.py | 3 +++ .../scenarioatomics/atomic_criteria.py | 5 ++++ srunner/scenariomanager/timer.py | 4 +-- srunner/scenarios/basic_scenario.py | 2 ++ srunner/scenarios/blocked_intersection.py | 2 +- .../scenarios/object_crash_intersection.py | 10 ++++--- srunner/scenarios/object_crash_vehicle.py | 6 ++++- srunner/scenarios/parking_exit.py | 27 ++++++++----------- srunner/scenarios/pedestrian_crossing.py | 8 +++++- 10 files changed, 62 insertions(+), 30 deletions(-) diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py index 0ec451c06..17498d923 100644 --- a/srunner/scenariomanager/lights_sim.py +++ b/srunner/scenariomanager/lights_sim.py @@ -27,9 +27,12 @@ class RouteLightsBehavior(py_trees.behaviour.Behaviour): SUN_ALTITUDE_THRESHOLD_2 = 165 # For higher fog and cloudness values, the amount of light in scene starts to rapidly decrease - CLOUDINESS_THRESHOLD = 95 + CLOUDINESS_THRESHOLD = 80 FOG_THRESHOLD = 40 + # In cases where more than one weather conditition is active, decrease the thresholds + COMBINED_THRESHOLD = 10 + def __init__(self, ego_vehicle, radius=50, name="LightsBehavior"): """ Setup parameters @@ -65,13 +68,23 @@ def update(self): def _get_night_mode(self, weather): """Check wheather or not the street lights need to be turned on""" - if weather.sun_altitude_angle <= self.SUN_ALTITUDE_THRESHOLD_1 \ - or weather.sun_altitude_angle >= self.SUN_ALTITUDE_THRESHOLD_2: - return True - if weather.cloudiness >= self.CLOUDINESS_THRESHOLD: + 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) + cloudiness_dist = self.CLOUDINESS_THRESHOLD - weather.cloudiness + fog_density_dist = self.FOG_THRESHOLD - weather.fog_density + + # Check each parameter independetly + if altitude_dist < 0 or cloudiness_dist < 0 or fog_density_dist < 0: return True - if weather.fog_density >= self.FOG_THRESHOLD: + + # Check if two or more values are close to their threshold + joined_threshold = int(altitude_dist < self.COMBINED_THRESHOLD) + joined_threshold += int(cloudiness_dist < self.COMBINED_THRESHOLD) + joined_threshold += int(fog_density_dist < self.COMBINED_THRESHOLD) + + if joined_threshold >= 2: return True + return False def _turn_close_lights_on(self, location): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 5bb1c79f4..334ea8bfe 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1906,6 +1906,9 @@ def update(self): new_status = py_trees.common.Status.RUNNING 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") + return new_status throttle_noise = random.normal(self._throttle_mean, self._throttle_std) control.throttle = max(-1, min(1, control.throttle + throttle_noise)) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 794e75ddc..c37212ee3 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -294,6 +294,7 @@ 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"): @@ -377,6 +378,10 @@ def _count_collisions(self, event): # pylint: disable=too-many-return-statem if distance_vector.length() <= self.COLLISION_RADIUS: return + # If the actor speed is 0, the collision isn't its fault + if CarlaDataProvider.get_velocity(self.actor) < self.EPSILON: + return + # The collision is valid, save the data self.test_status = "FAILURE" self.actual_value += 1 diff --git a/srunner/scenariomanager/timer.py b/srunner/scenariomanager/timer.py index a17ba218a..de5d202bd 100644 --- a/srunner/scenariomanager/timer.py +++ b/srunner/scenariomanager/timer.py @@ -164,8 +164,8 @@ 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 = 180 - TIMEOUT_ROUTE_PERC = 15 + MIN_TIMEOUT = 300 + TIMEOUT_ROUTE_PERC = 10 def __init__(self, ego_vehicle, route, debug=False, name="RouteTimeoutBehavior"): """ diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index 2c512d4d0..de14f6675 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -309,6 +309,8 @@ def remove_all_actors(self): """ Remove all actors """ + if not hasattr(self, 'other_actors'): + return for i, _ in enumerate(self.other_actors): if self.other_actors[i] is not None: if CarlaDataProvider.actor_id_exists(self.other_actors[i].id): diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 2b2b909e8..4f0a98ca1 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -94,7 +94,7 @@ def _create_behavior(self): if self.route_mode: sequence.add_child(HandleJunctionScenario( clear_junction=True, - clear_ego_entry=False, + clear_ego_entry=True, remove_entries=[], remove_exits=[], stop_entries=True, diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 86e2fcead..52f7075ac 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -18,7 +18,8 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, HandBrakeVehicle, - KeepVelocity) + KeepVelocity, + ActorTransformSetter) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -336,17 +337,20 @@ def _initialize_actors(self, config): spawn_wp = right_wp if cross_prod.z < 0 else left_wp # Get the adversary transform and spawn it - spawn_transform = get_sidewalk_transform(spawn_wp, self._offset) - adversary = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) + self._spawn_transform = get_sidewalk_transform(spawn_wp, self._offset) + adversary = CarlaDataProvider.request_new_actor('walker.*', self._spawn_transform) if adversary is None: raise ValueError("Couldn't spawn adversary") self.other_actors.append(adversary) + adversary.set_location(self._spawn_transform.location + carla.Location(z=-200)) def _create_behavior(self): """ """ sequence = py_trees.composites.Sequence(name="VehicleTurningRoutePedestrian") + sequence.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_transform, True)) + collision_location = self._collision_wp.transform.location # Adversary trigger behavior diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index e0cb095f4..f6c31d898 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -18,7 +18,8 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, KeepVelocity, - Idle) + Idle, + ActorTransformSetter) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -278,6 +279,8 @@ def _initialize_actors(self, config): self.other_actors.append(adversary) self.other_actors.append(blocker) + adversary.set_location(self._adversary_transform.location + carla.Location(z=-200)) + def _create_behavior(self): """ After invoking this scenario, cyclist will wait for the user @@ -290,6 +293,7 @@ def _create_behavior(self): total_dist = self._distance + 10 sequence.add_child(LeaveSpaceInFront(total_dist)) + sequence.add_child(ActorTransformSetter(self.other_actors[0], self._adversary_transform, True)) collision_location = self._collision_wp.transform.location # Wait until ego is close to the adversary diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 72a3e7b86..788aad3d9 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -17,7 +17,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, ActorTransformSetter, - Idle, + WaitForever, ChangeAutoPilot, ScenarioTimeout) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest @@ -114,16 +114,14 @@ def _initialize_actors(self, config): front_points = self._parking_waypoint.next( self._front_vehicle_distance) if not front_points: - raise ValueError( - "Couldn't find viable position for the vehicle in front of the parking point") + raise ValueError("Couldn't find viable position for the vehicle in front of the parking point") self._remove_parked_vehicles(front_points[0].transform.location) actor_front = CarlaDataProvider.request_new_actor( 'vehicle.*', front_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_front is None: - raise ValueError( - "Couldn't spawn the vehicle in front of the parking point") + raise ValueError("Couldn't spawn the vehicle in front of the parking point") actor_front.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(actor_front) @@ -135,8 +133,7 @@ def _initialize_actors(self, config): behind_points = self._parking_waypoint.previous( self._behind_vehicle_distance) if not behind_points: - raise ValueError( - "Couldn't find viable position for the vehicle behind the parking point") + raise ValueError("Couldn't find viable position for the vehicle behind the parking point") self._remove_parked_vehicles(behind_points[0].transform.location) @@ -144,8 +141,7 @@ def _initialize_actors(self, config): 'vehicle.*', behind_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) if actor_behind is None: actor_front.destroy() - raise ValueError( - "Couldn't spawn the vehicle behind the parking point") + raise ValueError("Couldn't spawn the vehicle behind the parking point") actor_behind.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(actor_behind) @@ -160,15 +156,14 @@ def _initialize_actors(self, config): # Spawn the actor at the side of the ego actor_side = CarlaDataProvider.request_new_actor( - 'vehicle.*', self._reference_waypoint.transform, rolename='scenario', attribute_filter=self._bp_attributes) + 'vehicle.*', self._reference_waypoint.transform, attribute_filter=self._bp_attributes) if actor_side is None: - raise ValueError( - "Couldn't spawn the vehicle at the side of the parking point") + raise ValueError("Couldn't spawn the vehicle at the side of the parking point") self.other_actors.append(actor_side) self._tm.update_vehicle_lights(actor_side, True) - self._end_side_location = self.ego_vehicles[0].get_transform() - self._end_side_location.location.z -= 500 + self._end_side_transform = self.ego_vehicles[0].get_transform() + self._end_side_transform.location.z -= 500 def _remove_parked_vehicles(self, actor_location): """Removes the parked vehicles that might have conflicts with the scenario""" @@ -209,8 +204,8 @@ def _create_behavior(self): side_actor_behavior = py_trees.composites.Sequence() side_actor_behavior.add_child(ChangeAutoPilot(self.other_actors[2], True)) side_actor_behavior.add_child(DriveDistance(self.other_actors[2], self._side_end_distance)) - side_actor_behavior.add_child(ActorTransformSetter(self.other_actors[2], self._end_side_location, False)) - side_actor_behavior.add_child(Idle()) + side_actor_behavior.add_child(ActorTransformSetter(self.other_actors[2], self._end_side_transform, False)) + side_actor_behavior.add_child(WaitForever()) root.add_child(side_actor_behavior) end_condition = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index ec60a3b6b..21add4142 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -12,7 +12,7 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity, WaitForever, Idle +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity, WaitForever, Idle, ActorTransformSetter from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -130,10 +130,13 @@ def _initialize_actors(self, config): raise ValueError("Failed to spawn an adversary") self.other_actors.append(walker) + walker.set_location(spawn_transform + carla.Location(z=-200)) + collision_dist = spawn_transform.location.distance(self._collision_wp.transform.location) # Distance and duration to cross the whole road + a bit more (supposing symetry in both directions) move_dist = 2.3 * collision_dist + walker_data['transform'] = spawn_transform walker_data['distance'] = move_dist walker_data['duration'] = move_dist / walker_data['speed'] @@ -155,6 +158,9 @@ def _create_behavior(self): extend_road_exit=0 )) + for walker_actor, walker_data in zip(self.other_actors, self._walker_data): + sequence.add_child(ActorTransformSetter(walker_actor, walker_data['transform'], True)) + collision_location = self._collision_wp.transform.location # Wait until ego is close to the adversary From 4e93320c29425fb887afa1ff6965cd3f7b0c256f Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 19 Aug 2022 10:54:15 +0200 Subject: [PATCH 49/86] MinSpeed criteria checkpoints + Pedestrian patch --- .../scenarioatomics/atomic_behaviors.py | 42 ++++++ .../scenarioatomics/atomic_criteria.py | 123 ++++++++++++------ srunner/scenarios/basic_scenario.py | 5 +- .../scenarios/object_crash_intersection.py | 37 +++++- srunner/scenarios/object_crash_vehicle.py | 39 +++++- srunner/scenarios/pedestrian_crossing.py | 51 +++++++- srunner/scenarios/route_scenario.py | 4 +- 7 files changed, 246 insertions(+), 55 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 334ea8bfe..7e6f0ef5c 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -4208,3 +4208,45 @@ def terminate(self, new_status): 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"): + """ + Setup actor + """ + super().__init__(name) + self._actor = actor + self._reference_actor = reference_actor + self._distance = distance + self._displacement = displacement + + added_location = carla.Location(x=self._displacement, z=-self._distance) + self._actor.set_location(self._reference_actor.get_location() + added_location) + + self._start_time = 0 + self._teleport_time = 5 + + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def initialise(self): + """ + Set start time + """ + self._start_time = GameTime.get_time() + added_location = carla.Location(x=self._displacement, z=-self._distance) + self._actor.set_location(self._reference_actor.get_location() + added_location) + super().initialise() + + def update(self): + """ + Keep running until termination condition is satisfied + """ + new_status = py_trees.common.Status.RUNNING + + 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._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 c37212ee3..5523e7b7c 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1949,7 +1949,7 @@ def update(self): return new_status -class MinSpeedRouteTest(Criterion): +class MinimumSpeedRouteTest(Criterion): """ Check at which stage of the route is the actor at each tick @@ -1961,21 +1961,38 @@ class MinSpeedRouteTest(Criterion): """ WINDOWS_SIZE = 2 - # Thresholds to return that a route has been completed - MULTIPLIER = 1.5 # % - - def __init__(self, actor, name="MinSpeedRouteTest", 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 - self._world = CarlaDataProvider.get_world() + self.actual_value = 100 + + self._route = route + self._accum_dist = [] + prev_trans = None + for trans, _ in route: + if prev_trans: + dist = trans.location.distance(prev_trans.location) + self._accum_dist.append(dist + self._accum_dist[-1]) + else: + self._accum_dist.append(0) + prev_trans = trans + self._route_length = len(self._route) + + self._checkpoints = checkpoints + self._checkpoint_dist = self._accum_dist[-1] / self._checkpoints + self._mean_speed = 0 self._actor_speed = 0 self._speed_points = 0 - self._active = True + self._current_dist = 0 + self._checkpoint_values = [] + + self._index = 0 + def update(self): """ @@ -1983,61 +2000,87 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING + if self._terminate_on_failure and (self.test_status == "FAILURE"): + new_status = py_trees.common.Status.FAILURE + + # 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)): + # Get the dot product to know if it has passed this location + route_transform = self._route[index][0] + route_location = route_transform.location + wp_dir = route_transform.get_forward_vector() + wp_veh = location - route_location + + if wp_veh.dot(wp_dir) > 0: + self._index = index + + if self._accum_dist[self._index] - self._current_dist > self._checkpoint_dist: + self._set_traffic_event() + self._current_dist = self._accum_dist[self._index] + self._mean_speed = 0 + self._actor_speed = 0 + self._speed_points = 0 + # Get the actor speed velocity = CarlaDataProvider.get_velocity(self.actor) if velocity is None: return new_status - set_speed_data = py_trees.blackboard.Blackboard().get('BA_MinSpeedRouteTest') - if set_speed_data is not None: - self._active = set_speed_data - py_trees.blackboard.Blackboard().set('BA_MinSpeedRouteTest', None, True) + # 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'] - if self._active: - # 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'] + if background_vehicles: + frame_mean_speed = 0 + for vehicle in background_vehicles: + frame_mean_speed += CarlaDataProvider.get_velocity(vehicle) + frame_mean_speed /= len(background_vehicles) - if background_vehicles: - frame_mean_speed = 0 - for vehicle in background_vehicles: - frame_mean_speed += CarlaDataProvider.get_velocity(vehicle) - frame_mean_speed /= len(background_vehicles) - - # Record the data - self._mean_speed += frame_mean_speed - self._actor_speed += velocity - self._speed_points += 1 + # Record the data + self._mean_speed += frame_mean_speed + self._actor_speed += velocity + self._speed_points += 1 self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status - def terminate(self, new_status): - """ - Set the actual value as a percentage of the two mean speeds, - the test status to failure if not successful and terminate - """ - if self._mean_speed == 0: - self.actual_value = 0 - elif self._speed_points > 0: + 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 - self.actual_value = round(self._actor_speed / self._mean_speed * 100, 2) + checkpoint_value = round(self._actor_speed / self._mean_speed * 100, 2) else: - self.actual_value = 100 + checkpoint_value = 100 - if self.actual_value >= self.success_value: + if checkpoint_value >= self.success_value: self.test_status = "SUCCESS" else: self.test_status = "FAILURE" - if self.test_status == "FAILURE": - self._traffic_event = TrafficEvent(event_type=TrafficEventType.MIN_SPEED_INFRACTION, frame=GameTime.get_frame()) - self._traffic_event.set_dict({'percentage': self.actual_value}) - self._traffic_event.set_message("Average agent speed is {} of the surrounding traffic's one".format(self.actual_value)) + 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) + + def terminate(self, new_status): + """ + Set the actual value as a percentage of the two mean speeds, + the test status to failure if not successful and terminate + """ + # Routes end at around 99%, so make sure the last checkpoint is recorded + if self._accum_dist[self._index] / self._accum_dist[-1] > 0.95: + self._set_traffic_event() + + if len(self._checkpoint_values): + self.actual_value = sum(self._checkpoint_values) / len(self._checkpoint_values) super().terminate(new_status) diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index de14f6675..a75d49f95 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -18,6 +18,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (WaitForBlackboardVariable, InTimeToArrivalToLocation) +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import WaitForever from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.timer import TimeOut from srunner.scenariomanager.scenarioatomics.atomic_behaviors import UpdateAllActorControls @@ -194,8 +195,8 @@ def _setup_scenario_end(self, config): if not self.route_mode: return None - set_name = "Reset Blackboard Variable: {} ".format(config.route_var_name) - return py_trees.blackboard.SetBlackboardVariable(set_name, config.route_var_name, False) + # At routes, scenarios should never stop the route + return WaitForever() def _create_behavior(self): """ diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 52f7075ac..1c6313761 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -19,7 +19,8 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, HandBrakeVehicle, KeepVelocity, - ActorTransformSetter) + ActorTransformSetter, + MovePedestrianWithEgo) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -342,8 +343,9 @@ def _initialize_actors(self, config): if adversary is None: raise ValueError("Couldn't spawn adversary") - self.other_actors.append(adversary) adversary.set_location(self._spawn_transform.location + carla.Location(z=-200)) + adversary = self._replace_walker(adversary) + self.other_actors.append(adversary) def _create_behavior(self): """ @@ -390,4 +392,33 @@ def __del__(self): """ Remove all actors upon deletion """ - self.remove_all_actors() \ No newline at end of file + self.remove_all_actors() + + # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, + # so all functions below are meant to patch it until the fix is done + def _replace_walker(self, adversary): + """As the adversary is probably, replace it with another one""" + type_id = adversary.type_id + adversary.destroy() + spawn_transform = self._reference_waypoint.transform + spawn_transform.location.z += -100 + adversary = CarlaDataProvider.request_new_actor(type_id, spawn_transform) + if not adversary: + raise ValueError("Couldn't spawn the walker substitute") + adversary.set_simulate_physics(False) + return adversary + + def _setup_scenario_trigger(self, config): + """Normal scenario trigger but in parallel, a behavior that ensures the pedestrian stays active""" + trigger_tree = super()._setup_scenario_trigger(config) + + if not self.route_mode: + return trigger_tree + + parallel = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="ScenarioTrigger") + + parallel.add_child(MovePedestrianWithEgo(self.ego_vehicles[0], self.other_actors[0], 100)) + + parallel.add_child(trigger_tree) + return parallel diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index f6c31d898..80da15700 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -19,7 +19,8 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, KeepVelocity, Idle, - ActorTransformSetter) + ActorTransformSetter, + MovePedestrianWithEgo) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -275,12 +276,13 @@ def _initialize_actors(self, config): if self._number_of_attempts == 0: raise Exception("Couldn't find viable position for the adversary and blocker actors") - blocker.set_simulate_physics(enabled=False) + blocker.set_simulate_physics(False) + adversary.set_location(self._adversary_transform.location + carla.Location(z=-200)) + adversary = self._replace_walker(adversary) + self.other_actors.append(adversary) self.other_actors.append(blocker) - adversary.set_location(self._adversary_transform.location + carla.Location(z=-200)) - def _create_behavior(self): """ After invoking this scenario, cyclist will wait for the user @@ -336,6 +338,35 @@ def __del__(self): """ self.remove_all_actors() + # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, + # so all functions below are meant to patch it until the fix is done + def _replace_walker(self, adversary): + """As the adversary is probably, replace it with another one""" + type_id = adversary.type_id + adversary.destroy() + spawn_transform = self._reference_waypoint.transform + spawn_transform.location.z += -100 + adversary = CarlaDataProvider.request_new_actor(type_id, spawn_transform) + if not adversary: + raise ValueError("Couldn't spawn the walker substitute") + adversary.set_simulate_physics(False) + return adversary + + def _setup_scenario_trigger(self, config): + """Normal scenario trigger but in parallel, a behavior that ensures the pedestrian stays active""" + trigger_tree = super()._setup_scenario_trigger(config) + + if not self.route_mode: + return trigger_tree + + parallel = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="ScenarioTrigger") + + parallel.add_child(MovePedestrianWithEgo(self.ego_vehicles[0], self.other_actors[0], 100)) + + parallel.add_child(trigger_tree) + return parallel + class ParkingCrossingPedestrian(BasicScenario): diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index 21add4142..e32edf371 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -12,7 +12,12 @@ import carla from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorDestroy, KeepVelocity, WaitForever, Idle, ActorTransformSetter +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + KeepVelocity, + WaitForever, + Idle, + ActorTransformSetter, + MovePedestrianWithEgo) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation, @@ -128,9 +133,11 @@ def _initialize_actors(self, config): for walker in self.other_actors: walker.destroy() raise ValueError("Failed to spawn an adversary") - self.other_actors.append(walker) - walker.set_location(spawn_transform + carla.Location(z=-200)) + walker.set_location(spawn_transform.location + carla.Location(z=-200)) + walker = self._replace_walker(walker) + + self.other_actors.append(walker) collision_dist = spawn_transform.location.distance(self._collision_wp.transform.location) @@ -200,4 +207,40 @@ def _create_test_criteria(self): """ if self.route_mode: return [] - return [CollisionTest(self.ego_vehicles[0])] \ No newline at end of file + return [CollisionTest(self.ego_vehicles[0])] + + def __del__(self): + """ + Remove all actors upon deletion + """ + self.remove_all_actors() + + # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, + # so all functions below are meant to patch it until the fix is done + def _replace_walker(self, walker): + """As the adversary is probably, replace it with another one""" + type_id = walker.type_id + walker.destroy() + spawn_transform = self._reference_waypoint.transform + spawn_transform.location.z += -100 + walker = CarlaDataProvider.request_new_actor(type_id, spawn_transform) + if not walker: + raise ValueError("Couldn't spawn the walker substitute") + walker.set_simulate_physics(False) + return walker + + def _setup_scenario_trigger(self, config): + """Normal scenario trigger but in parallel, a behavior that ensures the pedestrian stays active""" + trigger_tree = super()._setup_scenario_trigger(config) + + if not self.route_mode: + return trigger_tree + + parallel = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="ScenarioTrigger") + + for walker in self.other_actors: + parallel.add_child(MovePedestrianWithEgo(self.ego_vehicles[0], walker, 100)) + + parallel.add_child(trigger_tree) + return parallel diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 189bbff9b..70d3f78b8 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -36,7 +36,7 @@ RunningRedLightTest, RunningStopTest, ActorBlockedTest, - MinSpeedRouteTest) + MinimumSpeedRouteTest) from srunner.scenarios.basic_scenario import BasicScenario from srunner.scenarios.background_activity import BackgroundBehavior @@ -303,7 +303,7 @@ def _create_test_criteria(self): criteria.add_child(CollisionTest(self.ego_vehicles[0], name="CollisionTest")) criteria.add_child(RunningRedLightTest(self.ego_vehicles[0])) criteria.add_child(RunningStopTest(self.ego_vehicles[0])) - criteria.add_child(MinSpeedRouteTest(self.ego_vehicles[0], name="MinSpeedTest")) + criteria.add_child(MinimumSpeedRouteTest(self.ego_vehicles[0], route=self.route, checkpoints=4, name="MinSpeedTest")) # These stop the route early to save computational time criteria.add_child(InRouteTest( From ee68a08c8dc355dcd8f7edd8dba01abc07156f39 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 19 Aug 2022 15:37:27 +0200 Subject: [PATCH 50/86] Fix bug PedestrianCrossing --- srunner/scenarios/pedestrian_crossing.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index e32edf371..60884eee1 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -126,7 +126,7 @@ def _initialize_actors(self, config): start_wp = wp # Spawn the walkers - for walker_data in self._walker_data: + for i, walker_data in enumerate(self._walker_data): spawn_transform = self._get_walker_transform(start_wp, walker_data) walker = CarlaDataProvider.request_new_actor('walker.*', spawn_transform) if walker is None: @@ -135,7 +135,8 @@ def _initialize_actors(self, config): raise ValueError("Failed to spawn an adversary") walker.set_location(spawn_transform.location + carla.Location(z=-200)) - walker = self._replace_walker(walker) + distance = 100 + 5 * i + walker = self._replace_walker(walker, distance) self.other_actors.append(walker) @@ -217,12 +218,12 @@ def __del__(self): # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, # so all functions below are meant to patch it until the fix is done - def _replace_walker(self, walker): + def _replace_walker(self, walker, distance): """As the adversary is probably, replace it with another one""" type_id = walker.type_id walker.destroy() spawn_transform = self._reference_waypoint.transform - spawn_transform.location.z += -100 + spawn_transform.location.z -= distance walker = CarlaDataProvider.request_new_actor(type_id, spawn_transform) if not walker: raise ValueError("Couldn't spawn the walker substitute") From b90c3d946d8aa95dc36ec7bbaba427c722ced0fe Mon Sep 17 00:00:00 2001 From: joel-mb Date: Sun, 4 Sep 2022 19:34:23 +0200 Subject: [PATCH 51/86] Fixed bug pedestrians and parked vehicles (#933) * Added parking removal to crossing actors * Fixed bug pedestrians and parked vehicles * Improved pedestrian code Co-authored-by: Guillermo --- srunner/scenarios/basic_scenario.py | 7 ++ .../scenarios/object_crash_intersection.py | 25 ++++++- srunner/scenarios/object_crash_vehicle.py | 71 ++++++++++++++----- .../opposite_vehicle_taking_priority.py | 1 + srunner/scenarios/parking_cut_in.py | 13 +--- srunner/scenarios/parking_exit.py | 15 +--- srunner/scenarios/pedestrian_crossing.py | 18 ++--- srunner/scenarios/vehicle_opens_door.py | 11 +-- 8 files changed, 98 insertions(+), 63 deletions(-) diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index a75d49f95..c423c5da8 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -40,6 +40,7 @@ def __init__(self, name, ego_vehicles, config, world, self.name = name self.ego_vehicles = ego_vehicles self.other_actors = [] + self.parking_slots = [] self.config = config self.world = world self.debug_mode = debug_mode @@ -318,3 +319,9 @@ def remove_all_actors(self): CarlaDataProvider.remove_actor_by_id(self.other_actors[i].id) self.other_actors[i] = None self.other_actors = [] + + def get_parking_slots(self): + """ + Returns occupied parking slots. + """ + return self.parking_slots diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 1c6313761..95c97903a 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -119,6 +119,8 @@ def _initialize_actors(self, config): waypoint = self._get_target_waypoint() move_dist = self._start_distance while self._number_of_attempts > 0: + parking_location = None + # Move to the front waypoint = waypoint.next(move_dist)[0] self._collision_wp = waypoint @@ -130,6 +132,8 @@ def _initialize_actors(self, config): if right_wp is None: break # No more right lanes sidewalk_waypoint = right_wp + if sidewalk_waypoint.lane_type == carla.LaneType.Parking: + parking_location = sidewalk_waypoint.transform.location # Get the adversary transform and spawn it self._adversary_transform = get_sidewalk_transform(sidewalk_waypoint, self._offset) @@ -146,6 +150,9 @@ def _initialize_actors(self, config): if self._number_of_attempts == 0: raise ValueError("Couldn't find viable position for the adversary") + if parking_location: + self.parking_slots.append(parking_location) + if isinstance(adversary, carla.Vehicle): adversary.apply_control(carla.VehicleControl(hand_brake=True)) self.other_actors.append(adversary) @@ -288,7 +295,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 - self._offset = {"yaw": 270, "z": 0.2, "k": 1.5} + self._offset = {"yaw": 270, "z": 1.2, "k": 1.5} self.timeout = timeout super().__init__(name, ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) @@ -298,6 +305,7 @@ def _initialize_actors(self, config): Custom initialization """ # Get the waypoint right after the junction + parking_location = None waypoint = generate_target_waypoint_in_route(self._reference_waypoint, self._ego_route) self._collision_wp = waypoint.next(0.5)[0] # Some wps are still part of the junction @@ -309,6 +317,8 @@ def _initialize_actors(self, config): if side_wp is None: break right_wp = side_wp + if right_wp.lane_type == carla.LaneType.Parking: + parking_location = right_wp.transform.location # Get the left waypoint at the sidewalk other_dir_wps = get_opposite_dir_lanes(self._collision_wp) @@ -320,6 +330,8 @@ def _initialize_actors(self, config): if side_wp is None: break left_wp = side_wp + if left_wp.lane_type == carla.LaneType.Parking: + parking_location = left_wp.transform.location else: # Without opposite lane self._offset['yaw'] = 90 @@ -329,6 +341,8 @@ def _initialize_actors(self, config): if side_wp is None: break left_wp = side_wp + if left_wp.lane_type == carla.LaneType.Parking: + parking_location = left_wp.transform.location self._adversary_distance = right_wp.transform.location.distance(left_wp.transform.location) @@ -345,6 +359,10 @@ def _initialize_actors(self, config): adversary.set_location(self._spawn_transform.location + carla.Location(z=-200)) adversary = self._replace_walker(adversary) + + if parking_location: + self.parking_slots.append(parking_location) + self.other_actors.append(adversary) def _create_behavior(self): @@ -400,12 +418,13 @@ def _replace_walker(self, adversary): """As the adversary is probably, replace it with another one""" type_id = adversary.type_id adversary.destroy() - spawn_transform = self._reference_waypoint.transform - spawn_transform.location.z += -100 + spawn_transform = self.ego_vehicles[0].get_transform() + spawn_transform.location.z -= 50 adversary = CarlaDataProvider.request_new_actor(type_id, spawn_transform) if not adversary: raise ValueError("Couldn't spawn the walker substitute") adversary.set_simulate_physics(False) + adversary.set_location(spawn_transform.location + carla.Location(z=-50)) return adversary def _setup_scenario_trigger(self, config): diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 80da15700..b9c9a8132 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -221,7 +221,9 @@ def _initialize_actors(self, config): # Get the waypoint in front of the ego. move_dist = self._distance waypoint = self._reference_waypoint + while self._number_of_attempts > 0: + parking_location = None self._collision_dist = 0 # Move to the front @@ -239,6 +241,8 @@ def _initialize_actors(self, config): if side_wp is None: break # No more side lanes sidewalk_waypoint = side_wp + if side_wp.lane_type == carla.LaneType.Parking: + parking_location = side_wp.transform.location # Get the blocker transform and spawn it offset = {"yaw": 0 if 'vehicle' in self._blocker_model else 90, "z": 0.0, "k": 1.5} @@ -258,7 +262,7 @@ def _initialize_actors(self, config): raise ValueError("Couldn't find a location to spawn the adversary") walker_wp = wps[0] - offset = {"yaw": 270 - self._crossing_angle, "z": 0.5, "k": 1.2} + offset = {"yaw": 270 - self._crossing_angle, "z": 1.2, "k": 1.2} self._adversary_transform = self._get_sidewalk_transform(walker_wp, offset) adversary = CarlaDataProvider.request_new_actor('walker.*', self._adversary_transform) if adversary is None: @@ -280,6 +284,9 @@ def _initialize_actors(self, config): adversary.set_location(self._adversary_transform.location + carla.Location(z=-200)) adversary = self._replace_walker(adversary) + if parking_location: + self.parking_slots.append(parking_location) + self.other_actors.append(adversary) self.other_actors.append(blocker) @@ -344,12 +351,13 @@ def _replace_walker(self, adversary): """As the adversary is probably, replace it with another one""" type_id = adversary.type_id adversary.destroy() - spawn_transform = self._reference_waypoint.transform - spawn_transform.location.z += -100 + spawn_transform = self.ego_vehicles[0].get_transform() + spawn_transform.location.z -= 50 adversary = CarlaDataProvider.request_new_actor(type_id, spawn_transform) if not adversary: raise ValueError("Couldn't spawn the walker substitute") adversary.set_simulate_physics(False) + adversary.set_location(spawn_transform.location + carla.Location(z=-50)) return adversary def _setup_scenario_trigger(self, config): @@ -445,7 +453,7 @@ def _get_walker_transform(self, waypoint): offset_location = carla.Location(waypoint.lane_width * vector.x, waypoint.lane_width * vector.y) new_location = waypoint.transform.location + offset_location - new_location.z += 0.5 + new_location.z += 1.2 return carla.Transform(new_location, new_rotation) @@ -461,7 +469,7 @@ def _initialize_actors(self, config): # Get the adversary transform and spawn it self._blocker_transform = self._get_blocker_transform(blocker_wp) - self._remove_parked_vehicles(self._blocker_transform.location) + self.parking_slots.append(self._blocker_transform.location) blocker = CarlaDataProvider.request_new_actor( 'vehicle.*', self._blocker_transform, attribute_filter=self._bp_attributes) if blocker is None: @@ -477,22 +485,18 @@ def _initialize_actors(self, config): # Get the adversary transform and spawn it self._walker_transform = self._get_walker_transform(walker_wp) - self._remove_parked_vehicles(self._walker_transform.location) + self.parking_slots.append(self._walker_transform.location) + walker = CarlaDataProvider.request_new_actor('walker.*', self._walker_transform) if walker is None: raise ValueError("Couldn't spawn the adversary") + + walker.set_location(self._walker_transform.location + carla.Location(z=-200)) + walker = self._replace_walker(walker) + self.other_actors.append(walker) - - self._collision_wp = walker_wp - def _remove_parked_vehicles(self, actor_location): - """Removes the parked vehicles that might have conflicts with the scenario""" - parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) - vehicles_to_destroy = set() - for v in parked_vehicles: - if v.transform.location.distance(actor_location) < 10: - vehicles_to_destroy.add(v) - self.world.enable_environment_objects(vehicles_to_destroy, False) + self._collision_wp = walker_wp def _create_behavior(self): """ @@ -505,6 +509,7 @@ def _create_behavior(self): if self.route_mode: sequence.add_child(LeaveSpaceInFront(self._distance)) + sequence.add_child(ActorTransformSetter(self.other_actors[1], self._walker_transform, True)) collision_location = self._collision_wp.transform.location # Wait until ego is close to the adversary @@ -525,8 +530,8 @@ def _create_behavior(self): duration=duration, distance=distance, name="AdversaryCrossing")) # Remove everything - sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) - sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyAdversary")) + sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyBlocker")) sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition")) return sequence @@ -545,3 +550,33 @@ def __del__(self): Remove all actors upon deletion """ self.remove_all_actors() + + # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, + # so all functions below are meant to patch it until the fix is done + def _replace_walker(self, walker): + """As the adversary is probably, replace it with another one""" + type_id = walker.type_id + walker.destroy() + spawn_transform = self.ego_vehicles[0].get_transform() + spawn_transform.location.z -= 50 + walker = CarlaDataProvider.request_new_actor(type_id, spawn_transform) + if not walker: + raise ValueError("Couldn't spawn the walker substitute") + walker.set_simulate_physics(False) + walker.set_location(spawn_transform.location + carla.Location(z=-50)) + return walker + + def _setup_scenario_trigger(self, config): + """Normal scenario trigger but in parallel, a behavior that ensures the pedestrian stays active""" + trigger_tree = super()._setup_scenario_trigger(config) + + if not self.route_mode: + return trigger_tree + + parallel = py_trees.composites.Parallel( + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="ScenarioTrigger") + + parallel.add_child(MovePedestrianWithEgo(self.ego_vehicles[0], self.other_actors[1], 100)) + + parallel.add_child(trigger_tree) + return parallel diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 602d686d9..87007bccc 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -116,6 +116,7 @@ def _initialize_actors(self, config): source_transform.location + carla.Location(z=0.1), source_transform.rotation ) + self.parking_slots.append(source_transform.location) # Spawn the actor and move it below ground opposite_actor = CarlaDataProvider.request_new_actor( diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index 770a2970f..0939b5bb8 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -74,7 +74,7 @@ def _initialize_actors(self, config): else: parking_wp = self._blocker_wp.get_right_lane() - self._remove_parked_vehicles(parking_wp.transform.location) + self.parking_slots.append(parking_wp.transform.location) self._blocker_actor = CarlaDataProvider.request_new_actor( 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) @@ -97,7 +97,7 @@ def _initialize_actors(self, config): else: parking_wp = self._collision_wp.get_right_lane() - self._remove_parked_vehicles(parking_wp.transform.location) + self.parking_slots.append(parking_wp.transform.location) self._parked_actor = CarlaDataProvider.request_new_actor( 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) @@ -108,15 +108,6 @@ def _initialize_actors(self, config): side_location = self._get_displaced_location(self._parked_actor, parking_wp) self._parked_actor.set_location(side_location) - def _remove_parked_vehicles(self, actor_location): - """Removes the parked vehicles that might have conflicts with the scenario""" - parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) - vehicles_to_destroy = set() - for v in parked_vehicles: - if v.transform.location.distance(actor_location) < 10: - vehicles_to_destroy.add(v) - self.world.enable_environment_objects(vehicles_to_destroy, False) - def _get_displaced_location(self, actor, wp): """ Calculates the location such that the actor is at the sidemost part of the lane diff --git a/srunner/scenarios/parking_exit.py b/srunner/scenarios/parking_exit.py index 788aad3d9..acbb69643 100644 --- a/srunner/scenarios/parking_exit.py +++ b/srunner/scenarios/parking_exit.py @@ -116,7 +116,7 @@ def _initialize_actors(self, config): if not front_points: raise ValueError("Couldn't find viable position for the vehicle in front of the parking point") - self._remove_parked_vehicles(front_points[0].transform.location) + self.parking_slots.append(front_points[0].transform.location) actor_front = CarlaDataProvider.request_new_actor( 'vehicle.*', front_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) @@ -135,7 +135,7 @@ def _initialize_actors(self, config): if not behind_points: raise ValueError("Couldn't find viable position for the vehicle behind the parking point") - self._remove_parked_vehicles(behind_points[0].transform.location) + self.parking_slots.append(behind_points[0].transform.location) actor_behind = CarlaDataProvider.request_new_actor( 'vehicle.*', behind_points[0].transform, rolename='scenario no lights', attribute_filter=self._bp_attributes) @@ -151,7 +151,7 @@ def _initialize_actors(self, config): # Move the ego to its side position self._ego_location = self._get_displaced_location(self.ego_vehicles[0], self._parking_waypoint) - self._remove_parked_vehicles(self._ego_location) + self.parking_slots.append(self._ego_location) self.ego_vehicles[0].set_location(self._ego_location) # Spawn the actor at the side of the ego @@ -165,15 +165,6 @@ def _initialize_actors(self, config): self._end_side_transform = self.ego_vehicles[0].get_transform() self._end_side_transform.location.z -= 500 - def _remove_parked_vehicles(self, actor_location): - """Removes the parked vehicles that might have conflicts with the scenario""" - parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) - vehicles_to_destroy = set() - for v in parked_vehicles: - if v.transform.location.distance(actor_location) < 10: - vehicles_to_destroy.add(v) - self.world.enable_environment_objects(vehicles_to_destroy, False) - def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index 60884eee1..ee749510a 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -67,9 +67,9 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self.timeout = timeout self._walker_data = [ - {'x': 0.4, 'y': 1.5, 'z': 1, 'yaw': 270}, - {'x': 1, 'y': 2.5, 'z': 1, 'yaw': 270}, - {'x': 1.6, 'y': 0.5, 'z': 1, 'yaw': 270} + {'x': 0.4, 'y': 1.5, 'z': 1.2, 'yaw': 270}, + {'x': 1, 'y': 2.5, 'z': 1.2, 'yaw': 270}, + {'x': 1.6, 'y': 0.5, 'z': 1.2, 'yaw': 270} ] for walker_data in self._walker_data: @@ -135,8 +135,7 @@ def _initialize_actors(self, config): raise ValueError("Failed to spawn an adversary") walker.set_location(spawn_transform.location + carla.Location(z=-200)) - distance = 100 + 5 * i - walker = self._replace_walker(walker, distance) + walker = self._replace_walker(walker) self.other_actors.append(walker) @@ -218,16 +217,17 @@ def __del__(self): # TODO: Pedestrian have an issue with large maps were setting them to dormant breaks them, # so all functions below are meant to patch it until the fix is done - def _replace_walker(self, walker, distance): + def _replace_walker(self, walker): """As the adversary is probably, replace it with another one""" type_id = walker.type_id walker.destroy() - spawn_transform = self._reference_waypoint.transform - spawn_transform.location.z -= distance + spawn_transform = self.ego_vehicles[0].get_transform() + spawn_transform.location.z -= 50 walker = CarlaDataProvider.request_new_actor(type_id, spawn_transform) if not walker: raise ValueError("Couldn't spawn the walker substitute") walker.set_simulate_physics(False) + walker.set_location(spawn_transform.location + carla.Location(z=-50)) return walker def _setup_scenario_trigger(self, config): @@ -240,7 +240,7 @@ def _setup_scenario_trigger(self, config): parallel = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="ScenarioTrigger") - for walker in self.other_actors: + for i, walker in enumerate(reversed(self.other_actors)): parallel.add_child(MovePedestrianWithEgo(self.ego_vehicles[0], walker, 100)) parallel.add_child(trigger_tree) diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index f30c7d486..69e9f5029 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -83,15 +83,6 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals super().__init__("VehicleOpensDoorTwoWays", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable) - def _remove_parked_vehicles(self, actor_location): - """Removes the parked vehicles that might have conflicts with the scenario""" - parked_vehicles = self.world.get_environment_objects(carla.CityObjectLabel.Vehicles) - vehicles_to_destroy = set() - for v in parked_vehicles: - if v.transform.location.distance(actor_location) < 10: - vehicles_to_destroy.add(v) - self.world.enable_environment_objects(vehicles_to_destroy, False) - def _get_displaced_location(self, actor, wp): """ Calculates the transforming such that the actor is at the sidemost part of the lane @@ -140,7 +131,7 @@ def _initialize_actors(self, config): if self._parked_wp is None: raise ValueError("Couldn't find a spot to place the adversary vehicle") - self._remove_parked_vehicles(self._parked_wp.transform.location) + self.parking_slots.append(self._parked_wp.transform.location) self._parked_actor = CarlaDataProvider.request_new_actor( "*vehicle.*", self._parked_wp.transform, attribute_filter={'has_dynamic_doors': True, 'base_type': 'car'}) From dbf92a123fd2ede20fda36fb01ac370fff8b405c Mon Sep 17 00:00:00 2001 From: Guillermo Date: Thu, 15 Sep 2022 10:53:57 +0200 Subject: [PATCH 52/86] Removed old test routes --- srunner/routes/leaderboard_2.0_testing.xml | 280 --------------------- 1 file changed, 280 deletions(-) delete mode 100644 srunner/routes/leaderboard_2.0_testing.xml diff --git a/srunner/routes/leaderboard_2.0_testing.xml b/srunner/routes/leaderboard_2.0_testing.xml deleted file mode 100644 index 21a0b4842..000000000 --- a/srunner/routes/leaderboard_2.0_testing.xml +++ /dev/null @@ -1,280 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 13cf802ff8121a97c51ec21bb1bdf2469e13d748 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 16 Sep 2022 13:17:50 +0200 Subject: [PATCH 53/86] Fixed bug with YieldtoEV criteria --- srunner/scenarios/basic_scenario.py | 16 ++++++++++------ srunner/scenarios/object_crash_vehicle.py | 3 ++- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index c423c5da8..6ff5a7595 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -179,7 +179,7 @@ def _setup_scenario_trigger(self, config): return None # Scenario is not part of a route, wait for the ego to move - if not self.route_mode: + if not self.route_mode or config.route_var_name is None: return InTimeToArrivalToLocation(self.ego_vehicles[0], 2.0, start_location) # Scenario is part of a route. @@ -189,15 +189,19 @@ def _setup_scenario_trigger(self, config): def _setup_scenario_end(self, config): """ This function adds and additional behavior to the scenario, which is triggered - after it has ended. - + after it has ended. The Blackboard variable is set to False to indicate the scenario has ended. The function can be overloaded by a user implementation inside the user-defined scenario class. """ - if not self.route_mode: + if not self.route_mode or config.route_var_name is None: return None - # At routes, scenarios should never stop the route - return WaitForever() + # Scenario is part of a route. + end_sequence = py_trees.composites.Sequence() + name = "Reset Blackboard Variable: {} ".format(config.route_var_name) + end_sequence.add_child(py_trees.blackboard.SetBlackboardVariable(name, config.route_var_name, False)) + end_sequence.add_child(WaitForever()) # scenario can't stop the route + + return end_sequence def _create_behavior(self): """ diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index b9c9a8132..99fce9c53 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -507,7 +507,8 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence(name="ParkingCrossingPedestrian") if self.route_mode: - sequence.add_child(LeaveSpaceInFront(self._distance)) + total_dist = self._distance + 15 + sequence.add_child(LeaveSpaceInFront(total_dist)) sequence.add_child(ActorTransformSetter(self.other_actors[1], self._walker_transform, True)) collision_location = self._collision_wp.transform.location From e1f82c5225cdff53263153a732c2ff21fec334be Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 19 Sep 2022 16:05:05 +0200 Subject: [PATCH 54/86] Minor parameter changes --- srunner/scenarios/object_crash_vehicle.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 99fce9c53..626eb316a 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -167,7 +167,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_speed = 2.0 # Speed of the adversary [m/s] self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) - self._reaction_time = 2.3 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.15 # Time the agent has to react to avoid the collision [s] self._reaction_time += 0.1 * floor(self._crossing_angle / 5) self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 @@ -413,7 +413,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals raise ValueError(f"'direction' must be either 'right' or 'left' but {self._direction} was given") # Time the agent has to react to avoid the collision [s] - self._reaction_time = 2.3 + self._reaction_time = 2.15 self._reaction_time += 0.1 * floor(self._crossing_angle / 5) super().__init__("ParkingCrossingPedestrian", From d7065352d98879485d7007fbf553d8e3600e1398 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 11 Oct 2022 15:45:28 +0200 Subject: [PATCH 55/86] Improved scenarios --- .../scenarioatomics/atomic_behaviors.py | 69 +++++++++++++++---- srunner/scenarios/actor_flow.py | 69 ++++++++++++++++--- srunner/scenarios/background_activity.py | 13 ++-- srunner/scenarios/blocked_intersection.py | 27 +++++--- .../scenarios/construction_crash_vehicle.py | 4 +- srunner/scenarios/cross_bicycle_flow.py | 2 +- .../scenarios/cut_in_with_static_vehicle.py | 16 +++-- srunner/scenarios/invading_turn.py | 9 ++- .../opposite_vehicle_taking_priority.py | 5 +- srunner/scenarios/route_obstacles.py | 29 ++++---- .../signalized_junction_left_turn.py | 4 +- .../signalized_junction_right_turn.py | 8 +-- srunner/tools/background_manager.py | 59 +--------------- 13 files changed, 188 insertions(+), 126 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 7e6f0ef5c..48958f740 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2123,9 +2123,9 @@ def __init__(self, actor, reference_actor, target_location=None, speed_increment def initialise(self): """Initialises the agent""" # Get target speed - self._target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 + target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 - self._agent = ConstantVelocityAgent(self._actor, self._target_speed, opt_dict=self._opt_dict, + 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: @@ -2137,6 +2137,8 @@ def initialise(self): def update(self): """Moves the actor and waits for it to finish the plan""" new_status = py_trees.common.Status.RUNNING + target_speed = get_speed(self._reference_actor) + self._speed_increment * 3.6 + self._agent.set_target_speed(target_speed) if self._agent.done(): new_status = py_trees.common.Status.SUCCESS @@ -2153,7 +2155,7 @@ def terminate(self, new_status): self._control.throttle = 0.0 self._control.brake = 0.0 self._actor.apply_control(self._control) - super(AdaptiveConstantVelocityAgentBehavior, self).terminate(new_status) + super().terminate(new_status) class Idle(AtomicBehavior): @@ -2630,7 +2632,7 @@ class ActorTransformSetter(AtomicBehavior): Important parameters: - actor: CARLA actor to execute the behavior - transform: New target transform (position + orientation) of the actor - - physics [optional]: If physics is true, the actor physics will be reactivated upon success + - physics [optional]: Change the physics of the actors to true / false. To not change the physics, use None. The behavior terminates when actor is set to the new actor transform (closer than 1 meter) @@ -2674,6 +2676,43 @@ def update(self): return new_status +class BatchActorTransformSetter(AtomicBehavior): + + """ + This class contains an atomic behavior to set the transform + of an actor. + + Important parameters: + - actor_transform_list: list [carla.Actor, carla.Transform] + - physics [optional]: Change the physics of the actors to true / false. To not change the physics, use None. + + The behavior terminates immediately + """ + + def __init__(self, actor_transform_list, physics=True, name="BatchActorTransformSetter"): + """ + Init + """ + super().__init__(name) + self._actor_transform_list = actor_transform_list + self._physics = physics + self.logger.debug("%s.__init__()" % (self.__class__.__name__)) + + def update(self): + """ + Transform actor + """ + + for actor, transform in self._actor_transform_list: + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0)) + actor.set_transform(transform) + if self._physics is not None: + actor.set_simulate_physics(self._physics) + + return py_trees.common.Status.SUCCESS + + class TrafficLightStateSetter(AtomicBehavior): """ @@ -2812,11 +2851,11 @@ class ActorFlow(AtomicBehavior): - spawn_distance: Distance between spawned actors - sink_distance: Actors closer to the sink than this distance will be deleted - actors_speed: Speed of the actors part of the flow [m/s] - - add_initial_actors: Populates all the flow trajectory at the start + - 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, add_initial_actors=False, name="ActorFlow"): + actor_speed=20 / 3.6, initial_actors=False, initial_junction=False, name="ActorFlow"): """ Setup class members """ @@ -2837,7 +2876,8 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._sink_dist = sink_dist self._speed = actor_speed - self._add_initial_actors = add_initial_actors + self._initial_actors = initial_actors + self._initial_junction = initial_junction self._min_spawn_dist = spawn_dist_interval[0] self._max_spawn_dist = spawn_dist_interval[1] @@ -2847,13 +2887,13 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._collision_sensor_list = [] def initialise(self): - if self._add_initial_actors: + if self._initial_actors: grp = CarlaDataProvider.get_global_route_planner() plan = grp.trace_route(self._source_location, self._sink_location) ref_loc = plan[0][0].transform.location for wp, _ in plan: - if wp.is_junction: + if wp.is_junction and not self._initial_junction: continue # Spawning at junctions might break the path, so don't if wp.transform.location.distance(ref_loc) < self._spawn_dist: continue @@ -2885,6 +2925,8 @@ def _spawn_actor(self, transform): 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) + self._tm.ignore_signs_percentage(actor, 100) self._collision_sensor_list.append(sensor) self._actor_list.append(actor) @@ -3188,11 +3230,11 @@ class BicycleFlow(AtomicBehavior): - spawn_distance_interval (list(float, float)): Distance between spawned actors - sink_distance (float): Actors at this distance from the sink will be deleted - actors_speed (float): Speed of the actors part of the flow [m/s] - - add_initial_actors (bool): Boolean to initialy populate all the flow with bicycles + - 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, add_initial_actors=False, name="BicycleFlow"): + actor_speed=20 / 3.6, initial_actors=False, name="BicycleFlow"): """ Setup class members """ @@ -3211,7 +3253,7 @@ def __init__(self, plan, 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._add_initial_actors = add_initial_actors + self._initial_actors = initial_actors self._opt_dict = {"ignore_traffic_lights": True, "ignore_vehicles": True} @@ -3219,7 +3261,7 @@ def __init__(self, plan, spawn_dist_interval, sink_dist=2, self._grp = CarlaDataProvider.get_global_route_planner() def initialise(self): - if self._add_initial_actors: + if self._initial_actors: ref_loc = self._plan[0][0].transform.location for wp, _ in self._plan: if wp.is_junction: @@ -3261,6 +3303,7 @@ def _spawn_actor(self, transform): 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)) self._actor_data.append([actor, controller]) self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index c9af8d807..424bd650b 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -18,7 +18,7 @@ from agents.navigation.local_planner import RoadOption from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout, WaitForever from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToLocation, WaitEndIntersection from srunner.scenarios.basic_scenario import BasicScenario @@ -109,14 +109,15 @@ def _create_behavior(self): for source_wp, sink_wp in zip(source_wps, sink_wps): root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, + self._flow_speed, initial_actors=True, initial_junction=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence = py_trees.composites.Sequence() if self.route_mode: grp = CarlaDataProvider.get_global_route_planner() route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) - extra_space = 0 + extra_space = 20 for i in range(-2, -len(route)-1, -1): current_wp = route[i][0] extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) @@ -173,7 +174,8 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, + self._flow_speed, initial_actors=True, initial_junction=True)) for sink_wp in sink_wps: root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -181,6 +183,29 @@ def _create_behavior(self): exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match + if self.route_mode: + grp = CarlaDataProvider.get_global_route_planner() + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + self._extra_space = 20 + for i in range(-2, -len(route)-1, -1): + current_wp = route[i][0] + self._extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + if current_wp.is_junction: + break + + sequence_2 = py_trees.composites.Sequence() + sequence_2.add_child(WaitEndIntersection(self.ego_vehicles[0])) + sequence_2.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=False, + remove_entries=[], + remove_exits=[], + stop_entries=False, + extend_road_exit=self._extra_space + )) + sequence_2.add_child(WaitForever()) + root.add_child(sequence_2) + sequence = py_trees.composites.Sequence() if self.route_mode: sequence.add_child(HandleJunctionScenario( @@ -255,7 +280,8 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, + self._flow_speed, initial_actors=True, initial_junction=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) root.add_child(WaitEndIntersection(self.ego_vehicles[0], junction_id)) @@ -339,7 +365,8 @@ def _create_behavior(self): for wp in sink_wps: root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], wp.transform.location, self._sink_distance)) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, + self._flow_speed, initial_actors=True, initial_junction=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence = py_trees.composites.Sequence() @@ -360,7 +387,7 @@ def _create_behavior(self): remove_entries=[source_wp], remove_exits=[], stop_entries=False, - extend_road_exit=extra_space + extend_road_exit=extra_space + 20 )) sequence.add_child(SwitchRouteSources(False)) sequence.add_child(root) @@ -403,7 +430,8 @@ def _create_behavior(self): root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(ActorFlow( - source_wp, sink_wp, self._source_dist_interval, self._sink_distance, self._flow_speed, add_initial_actors=True)) + source_wp, sink_wp, self._source_dist_interval, self._sink_distance, + self._flow_speed, initial_actors=True, initial_junction=True)) for sink_wp in sink_wps: root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], sink_wp.transform.location, self._sink_distance)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -411,6 +439,29 @@ def _create_behavior(self): exit_wp = generate_target_waypoint_in_route(self._reference_waypoint, self.config.route) exit_wp = exit_wp.next(10)[0] # just in case the junction maneuvers don't match + if self.route_mode: + grp = CarlaDataProvider.get_global_route_planner() + route = grp.trace_route(source_wp.transform.location, sink_wp.transform.location) + self._extra_space = 20 + for i in range(-2, -len(route)-1, -1): + current_wp = route[i][0] + self._extra_space += current_wp.transform.location.distance(route[i+1][0].transform.location) + if current_wp.is_junction: + break + + sequence_2 = py_trees.composites.Sequence() + sequence_2.add_child(WaitEndIntersection(self.ego_vehicles[0])) + sequence_2.add_child(HandleJunctionScenario( + clear_junction=False, + clear_ego_entry=False, + remove_entries=[], + remove_exits=[], + stop_entries=False, + extend_road_exit=self._extra_space + )) + sequence_2.add_child(WaitForever()) + root.add_child(sequence_2) + sequence = py_trees.composites.Sequence() if self.route_mode: sequence.add_child(HandleJunctionScenario( @@ -628,7 +679,7 @@ def _initialize_actors(self, config): if self.route_mode: grp = CarlaDataProvider.get_global_route_planner() route = grp.trace_route(self._source_wp_2.transform.location, self._sink_wp_2.transform.location) - self._extra_space = 10 + self._extra_space = 20 route_exit_wp = None for i in range(-2, -len(route)-1, -1): current_wp = route[i][0] diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index bd807286d..c2cd86357 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -206,6 +206,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._road_extra_front_actors = 0 # For cases where we want more space but not more vehicles self._road_spawn_dist = 15 # Distance between spawned vehicles [m] + self._road_extra_space = 0 # Extra space for the road vehicles self._active_road_sources = True @@ -271,8 +272,8 @@ def _get_road_radius(self): stopped. Between the two, the velocity decreases linearly""" self._base_min_radius = (self._road_front_vehicles + self._road_extra_front_actors) * self._road_spawn_dist self._base_max_radius = (self._road_front_vehicles + self._road_extra_front_actors + 1) * self._road_spawn_dist - self._min_radius = self._base_min_radius - self._max_radius = self._base_max_radius + self._min_radius = self._base_min_radius + self._road_extra_space + self._max_radius = self._base_max_radius + self._road_extra_space def initialise(self): """Creates the background activity actors. Pressuposes that the ego is at a road""" @@ -1586,13 +1587,15 @@ def _update_parameters(self): # Road behavior road_behavior_data = py_trees.blackboard.Blackboard().get('BA_ChangeRoadBehavior') if road_behavior_data is not None: - num_front_vehicles, num_back_vehicles, spawn_dist = road_behavior_data + num_front_vehicles, num_back_vehicles, spawn_dist, extra_space = road_behavior_data if num_front_vehicles is not None: self._road_front_vehicles = num_front_vehicles if num_back_vehicles is not None: self._road_back_vehicles = num_back_vehicles if spawn_dist is not None: self._road_spawn_dist = spawn_dist + if extra_space is not None: + self._road_extra_space = extra_space self._get_road_radius() py_trees.blackboard.Blackboard().set('BA_ChangeRoadBehavior', None, True) @@ -1703,8 +1706,8 @@ def _update_parameters(self): def _compute_parameters(self): """Computes the parameters that are dependent on the speed of the ego. """ ego_speed = CarlaDataProvider.get_velocity(self._ego_actor) - self._min_radius = self._base_min_radius + self._radius_increase_ratio * ego_speed - self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed + self._min_radius = self._base_min_radius + self._radius_increase_ratio * ego_speed + self._road_extra_space + self._max_radius = self._base_max_radius + self._radius_increase_ratio * ego_speed + self._road_extra_space self._detection_dist = self._base_junction_detection + self._detection_ratio * ego_speed def _stop_road_front_vehicles(self): diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 4f0a98ca1..14bfb73d7 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -15,7 +15,10 @@ import py_trees from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, Idle, ScenarioTimeout) +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, + Idle, + ScenarioTimeout, + ActorTransformSetter) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle @@ -56,12 +59,14 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._map.get_waypoint(self._trigger_location) - self._blocker_distance = 7 - self._trigger_distance = 12 + self._blocker_distance = 5 + self._trigger_distance = 13 self._stop_time = 10 self._scenario_timeout = 240 + self._blocker_transform = None + super().__init__("BlockedIntersection", ego_vehicles, config, @@ -76,14 +81,19 @@ def _initialize_actors(self, config): waypoint = generate_target_waypoint_in_route(self._reference_waypoint, config.route) waypoint = waypoint.next(self._blocker_distance)[0] + self._blocker_transform = waypoint.transform + # Spawn the blocker vehicle - actor = CarlaDataProvider.request_new_actor( - "vehicle.*.*", waypoint.transform, + blocker = CarlaDataProvider.request_new_actor( + "vehicle.*.*", self._blocker_transform, attribute_filter={'base_type': 'car', 'has_lights': True} ) - if actor is None: + if blocker is None: raise Exception("Couldn't spawn the blocker vehicle") - self.other_actors.append(actor) + self.other_actors.append(blocker) + + blocker.set_simulate_physics(False) + blocker.set_location(self._blocker_transform.location + carla.Location(z=-200)) def _create_behavior(self): """ @@ -106,13 +116,14 @@ def _create_behavior(self): main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) behavior = py_trees.composites.Sequence(name="Approach and Wait") + behavior.add_child(ActorTransformSetter(self.other_actors[0], self._blocker_transform, True)) behavior.add_child(InTriggerDistanceToVehicle( self.other_actors[-1], self.ego_vehicles[0], self._trigger_distance)) behavior.add_child(Idle(self._stop_time)) main_behavior.add_child(behavior) sequence.add_child(main_behavior) - sequence.add_child(ActorDestroy(self.other_actors[-1])) + sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index 6999d8518..d02b1bbef 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -129,7 +129,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningconstruction', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(False) self.other_actors.append(signal_prop) def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_length=0, cone_offset=0): @@ -155,7 +155,7 @@ def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_leng def _create_construction_setup(self, start_transform, lane_width): """Create construction setup""" - _initial_offset = {'cones': {'yaw': 270, 'k': lane_width / 2.0}, + _initial_offset = {'cones': {'yaw': 270, 'k': 0.75 * lane_width / 2.0}, 'warning_sign': {'yaw': 180, 'k': 5, 'z': 0}, 'debris': {'yaw': 0, 'k': 2, 'z': 1}} _prop_names = {'warning_sign': 'static.prop.trafficwarning', diff --git a/srunner/scenarios/cross_bicycle_flow.py b/srunner/scenarios/cross_bicycle_flow.py index e7930420f..fd13bf310 100644 --- a/srunner/scenarios/cross_bicycle_flow.py +++ b/srunner/scenarios/cross_bicycle_flow.py @@ -183,7 +183,7 @@ def _create_behavior(self): clear_ego_entry=True, remove_entries=[], remove_exits=[], - stop_entries=False, + stop_entries=True, extend_road_exit=0 )) sequence.add_child(root) diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 5dc40f10d..02396af54 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -15,7 +15,7 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, - ActorTransformSetter, + BatchActorTransformSetter, CutIn, BasicAgentBehavior, Idle) @@ -23,7 +23,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, InTimeToArrivalToLocation) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import RemoveRoadLane, LeaveSpaceInFront, ReAddRoadLane +from srunner.tools.background_manager import RemoveRoadLane, LeaveSpaceInFront, ReAddRoadLane, ChangeRoadBehavior def get_value_parameter(config, name, p_type, default): @@ -61,6 +61,8 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_end_distance = 70 + self._extra_space = 30 # Leave extra space as a vehicle is invading the ego's lane (BA parameter) + self._side_transforms = [] self._side_wp = None @@ -210,8 +212,7 @@ def _create_behavior(self): total_dist += self._vehicle_gap * (self._back_vehicles + self._front_vehicles + 1) sequence.add_child(LeaveSpaceInFront(total_dist)) - for actor, transform in self._side_transforms: - sequence.add_child(ActorTransformSetter(actor, transform)) + sequence.add_child(BatchActorTransformSetter(self._side_transforms)) collision_location = self._collision_wp.transform.location @@ -224,6 +225,8 @@ def _create_behavior(self): self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) + if self.route_mode: + sequence.add_child(ChangeRoadBehavior(extra_space=self._extra_space)) if self.route_mode: sequence.add_child(RemoveRoadLane(self._side_wp)) @@ -239,14 +242,15 @@ def _create_behavior(self): self._adversary_actor, plan=self._plan, target_speed=self._speed)) cut_in_behavior.add_child(cut_in_movement) - cut_in_behavior.add_child(Idle(30)) # One minute timeout in case a collision happened + cut_in_behavior.add_child(Idle(30)) # Timeout in case a collision happened sequence.add_child(cut_in_behavior) for actor in self.other_actors: sequence.add_child(ActorDestroy(actor)) - + if self.route_mode: + sequence.add_child(ChangeRoadBehavior(extra_space=0)) sequence.add_child(ReAddRoadLane(1 if self._direction == 'right' else -1)) return sequence diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py index 303cad363..ac838962d 100644 --- a/srunner/scenarios/invading_turn.py +++ b/srunner/scenarios/invading_turn.py @@ -19,7 +19,7 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (InvadingActorFlow, ScenarioTimeout, ActorDestroy, - ActorTransformSetter) + BatchActorTransformSetter) from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import WaitUntilInFrontPosition from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenarios.basic_scenario import BasicScenario @@ -71,7 +71,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._source_dist = 30 # Distance between source and end point self._distance = get_value_parameter(config, 'distance', float, 100) - self._offset = get_value_parameter(config, 'offset', float, 0.5) # meters invaded in the opposite direction + self._offset = get_value_parameter(config, 'offset', float, 0.25) # meters invaded in the opposite direction self._scenario_timeout = 240 self._obstacle_transforms = [] @@ -140,13 +140,12 @@ def _create_behavior(self): """ sequence = py_trees.composites.Sequence("InvadingTurn") - for actor, transform in self._obstacle_transforms: - sequence.add_child(ActorTransformSetter(actor, transform)) - if self.route_mode: sequence.add_child(RemoveRoadLane(self._reference_waypoint)) sequence.add_child(ChangeOppositeBehavior(active=False)) + sequence.add_child(BatchActorTransformSetter(self._obstacle_transforms)) + main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) main_behavior.add_child(InvadingActorFlow( self._source_wp, self._sink_wp, self.ego_vehicles[0], self._flow_frequency, offset=self._true_offset)) diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index 87007bccc..f59b9899d 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -216,7 +216,6 @@ def _create_behavior(self): sequence.add_child(trigger_adversary) - end_location = self._sink_wp.transform.location start_location = self._spawn_wp.transform.location time = start_location.distance(end_location) / self._adversary_speed @@ -231,6 +230,8 @@ def _create_behavior(self): main_behavior.add_child(Idle(time)) sequence.add_child(main_behavior) + sequence.add_child(ActorDestroy(self.other_actors[0])) + sequence.add_child(WaitEndIntersection(self.ego_vehicles[0])) tls_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) tls_behavior.add_child(TrafficLightFreezer(self._tl_dict)) @@ -248,8 +249,6 @@ def _create_behavior(self): )) root.add_child(ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(tls_behavior) - root.add_child(ActorDestroy(self.other_actors[0])) - root.add_child(WaitEndIntersection(self.ego_vehicles[0])) return root diff --git a/srunner/scenarios/route_obstacles.py b/srunner/scenarios/route_obstacles.py index 6d50960e1..8a0a3dc11 100644 --- a/srunner/scenarios/route_obstacles.py +++ b/srunner/scenarios/route_obstacles.py @@ -30,7 +30,7 @@ WaitUntilInFront, WaitUntilInFrontPosition) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, SetMaxSpeed, ChangeOppositeBehavior +from srunner.tools.background_manager import LeaveSpaceInFront, SetMaxSpeed, ChangeOppositeBehavior, ChangeRoadBehavior def get_value_parameter(config, name, p_type, default): @@ -69,7 +69,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._first_distance = 10 self._second_distance = 6 - self._trigger_distance = 30 + self._trigger_distance = 50 self._end_distance = 50 self._wait_duration = 5 self._offset = 0.6 @@ -121,7 +121,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(False) self.other_actors.append(signal_prop) def _spawn_obstacle(self, wp, blueprint, accident_actor=False): @@ -274,8 +274,8 @@ def _create_behavior(self): root.add_child(end_condition) if self.route_mode: - behavior.add_child(SwitchWrongDirectionTest(True)) - behavior.add_child(ChangeOppositeBehavior(active=True)) + root.add_child(SwitchWrongDirectionTest(True)) + root.add_child(ChangeOppositeBehavior(active=True)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) @@ -297,7 +297,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._trigger_distance = 30 + self._trigger_distance = 50 self._end_distance = 50 self._wait_duration = 5 self._offset = 0.7 @@ -349,7 +349,7 @@ def _spawn_side_prop(self, wp): signal_prop = CarlaDataProvider.request_new_actor('static.prop.warningaccident', spawn_transform) if not signal_prop: raise ValueError("Couldn't spawn the indication prop asset") - signal_prop.set_simulate_physics(True) + signal_prop.set_simulate_physics(False) self.other_actors.append(signal_prop) def _spawn_obstacle(self, wp, blueprint): @@ -504,8 +504,9 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._obstacle_distance = 9 - self._trigger_distance = 30 - self._end_distance = 40 + self._trigger_distance = 50 + self._end_distance = 50 + self._extra_space = 30 self._offset = 0.55 self._wait_duration = 5 @@ -596,6 +597,7 @@ def _create_behavior(self): if self.route_mode: total_dist = self._distance + self._obstacle_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(ChangeRoadBehavior(extra_space=self._extra_space)) main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -629,6 +631,7 @@ def _create_behavior(self): root.add_child(main_behavior) if self.route_mode: root.add_child(SetMaxSpeed(0)) + root.add_child(ChangeRoadBehavior(extra_space=0)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) @@ -658,7 +661,7 @@ class HazardAtSideLaneTwoWays(HazardAtSideLane): """ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=180): - self._opposite_frequency = get_value_parameter(config, 'frequency', float, 200) + self._opposite_frequency = get_value_parameter(config, 'frequency', float, 100) super().__init__(world, ego_vehicles, config, randomize, debug_mode, criteria_enable, timeout) @@ -672,6 +675,7 @@ def _create_behavior(self): if self.route_mode: total_dist = self._distance + self._obstacle_distance + 20 root.add_child(LeaveSpaceInFront(total_dist)) + root.add_child(ChangeRoadBehavior(extra_space=self._extra_space)) main_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) @@ -705,8 +709,9 @@ def _create_behavior(self): root.add_child(main_behavior) if self.route_mode: - behavior.add_child(SwitchWrongDirectionTest(False)) - behavior.add_child(ChangeOppositeBehavior(spawn_dist=40)) + root.add_child(SwitchWrongDirectionTest(False)) + root.add_child(ChangeOppositeBehavior(spawn_dist=40)) + root.add_child(ChangeRoadBehavior(extra_space=0)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) diff --git a/srunner/scenarios/signalized_junction_left_turn.py b/srunner/scenarios/signalized_junction_left_turn.py index b869d3f89..0bb4ae308 100644 --- a/srunner/scenarios/signalized_junction_left_turn.py +++ b/srunner/scenarios/signalized_junction_left_turn.py @@ -200,7 +200,7 @@ def _create_behavior(self): remove_entries=get_same_dir_lanes(self._source_wp), remove_exits=get_same_dir_lanes(self._sink_wp), stop_entries=False, - extend_road_exit=self._sink_dist + extend_road_exit=self._sink_dist + 20 )) sequence.add_child(ChangeOppositeBehavior(active=False)) @@ -250,7 +250,7 @@ def _create_behavior(self): remove_entries=get_same_dir_lanes(self._source_wp), remove_exits=get_same_dir_lanes(self._sink_wp), stop_entries=True, - extend_road_exit=self._sink_dist + extend_road_exit=self._sink_dist + 20 )) sequence.add_child(ChangeOppositeBehavior(active=False)) diff --git a/srunner/scenarios/signalized_junction_right_turn.py b/srunner/scenarios/signalized_junction_right_turn.py index ced01b498..f8843ec5d 100644 --- a/srunner/scenarios/signalized_junction_right_turn.py +++ b/srunner/scenarios/signalized_junction_right_turn.py @@ -201,7 +201,7 @@ def _create_behavior(self): remove_entries=get_same_dir_lanes(self._source_wp), remove_exits=[], stop_entries=False, - extend_road_exit=self._sink_dist + extend_road_exit=self._sink_dist + 20 )) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) @@ -210,7 +210,7 @@ def _create_behavior(self): end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) root.add_child(end_condition) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, add_initial_actors=True)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, initial_actors=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) tl_freezer_sequence = py_trees.composites.Sequence("Traffic Light Behavior") @@ -244,7 +244,7 @@ def _create_behavior(self): remove_entries=get_same_dir_lanes(self._source_wp), remove_exits=[], stop_entries=True, - extend_road_exit=self._sink_dist + extend_road_exit=self._sink_dist + 20 )) root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) @@ -253,7 +253,7 @@ def _create_behavior(self): end_condition.add_child(DriveDistance(self.ego_vehicles[0], self._end_distance)) root.add_child(end_condition) root.add_child(ActorFlow( - self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, add_initial_actors=True)) + self._source_wp, self._sink_wp, self._source_dist_interval, 2, self._flow_speed, initial_actors=True)) root.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence.add_child(root) diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 2700faaea..4200f21f8 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -26,15 +26,16 @@ class ChangeRoadBehavior(AtomicBehavior): switch_source (bool): (De)activatea the road sources. """ - def __init__(self, num_front_vehicles=None, num_back_vehicles=None, spawn_dist=None, name="ChangeRoadBehavior"): + def __init__(self, num_front_vehicles=None, num_back_vehicles=None, spawn_dist=None, extra_space=None, name="ChangeRoadBehavior"): self._num_front = num_front_vehicles self._num_back = num_back_vehicles self._spawn_dist = spawn_dist + self._extra_space = extra_space super().__init__(name) def update(self): py_trees.blackboard.Blackboard().set( - "BA_ChangeRoadBehavior", [self._num_front, self._num_back, self._spawn_dist], overwrite=True + "BA_ChangeRoadBehavior", [self._num_front, self._num_back, self._spawn_dist, self._extra_space], overwrite=True ) return py_trees.common.Status.SUCCESS @@ -288,57 +289,3 @@ def update(self): self._remove_exits, self._stop_entries, self._extend_road_exit], overwrite=True) return py_trees.common.Status.SUCCESS - - -class AddOppositeVariabilityBehavior(AtomicBehavior): - """ - Updates the blackboard to periodically change the parameters opposite road spawn distance. - - Args: - interval (list): Two element list with the minimum and maximum distance of the opposite lane - """ - - def __init__(self, interval, end_value, name="AddOppositeVariabilityBehavior"): - self._min_spawn_dist = interval[0] - self._max_spawn_dist = interval[1] - self._end_value = end_value - - self._spawn_dist = 0 - self._change_time = 0 - self._change_frequency = 0 - self._rng = CarlaDataProvider.get_random_seed() - - self._terminated = False - super().__init__(name) - - def initialise(self): - """Change the opposite frequency and start the timer""" - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - self._change_time = GameTime.get_time() - self._change_frequency = self._spawn_dist / 4 - - py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", [None, self._spawn_dist, None], overwrite=True - ) - super().initialise() - - def update(self): - """Periodically change the frequency of the opposite traffic""" - if GameTime.get_time() - self._change_time > self._change_frequency: - self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) - self._change_time = GameTime.get_time() - self._change_frequency = self._spawn_dist / 4 - py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", [None, self._spawn_dist, None], overwrite=True - ) - - return py_trees.common.Status.RUNNING - - def terminate(self, new_status): - - if not self._terminated: - py_trees.blackboard.Blackboard().set( - "BA_ChangeOppositeBehavior", [None, self._end_value, None], overwrite=True - ) - self._terminated = True - return super().terminate(new_status) From 5b3c267efd6309fe5dddd63455d4262da91ae088 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 11 Oct 2022 17:28:44 +0200 Subject: [PATCH 56/86] Added hand brake to BlockedIntersection --- srunner/scenarios/blocked_intersection.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 14bfb73d7..bdce66bdf 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -18,7 +18,8 @@ from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorDestroy, Idle, ScenarioTimeout, - ActorTransformSetter) + ActorTransformSetter, + HandBrakeVehicle) from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle @@ -117,6 +118,7 @@ def _create_behavior(self): behavior = py_trees.composites.Sequence(name="Approach and Wait") behavior.add_child(ActorTransformSetter(self.other_actors[0], self._blocker_transform, True)) + behavior.add_child(HandBrakeVehicle(self.other_actors[0], 1)) behavior.add_child(InTriggerDistanceToVehicle( self.other_actors[-1], self.ego_vehicles[0], self._trigger_distance)) behavior.add_child(Idle(self._stop_time)) From 155c416a41d015676c79e320d0f5506c4fd0fe71 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Thu, 13 Oct 2022 08:28:49 +0200 Subject: [PATCH 57/86] Revereted some of the constructions changes --- srunner/scenarios/construction_crash_vehicle.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index d02b1bbef..d9ed0eecf 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -155,7 +155,7 @@ def _create_cones_side(self, start_transform, forward_vector, z_inc=0, cone_leng def _create_construction_setup(self, start_transform, lane_width): """Create construction setup""" - _initial_offset = {'cones': {'yaw': 270, 'k': 0.75 * lane_width / 2.0}, + _initial_offset = {'cones': {'yaw': 270, 'k': 0.85 * lane_width / 2.0}, 'warning_sign': {'yaw': 180, 'k': 5, 'z': 0}, 'debris': {'yaw': 0, 'k': 2, 'z': 1}} _prop_names = {'warning_sign': 'static.prop.trafficwarning', From 30d2e2ca49f87d9c0a2c9579c7e8f66b6554116c Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 14 Oct 2022 11:33:05 +0200 Subject: [PATCH 58/86] Some parameter changes --- .../scenariomanager/scenarioatomics/atomic_criteria.py | 8 ++++++-- srunner/scenarios/construction_crash_vehicle.py | 2 +- srunner/scenarios/control_loss.py | 8 ++++---- srunner/scenarios/cut_in_with_static_vehicle.py | 2 +- srunner/scenarios/object_crash_vehicle.py | 2 +- srunner/scenarios/parking_cut_in.py | 2 +- srunner/scenarios/pedestrian_crossing.py | 2 +- 7 files changed, 15 insertions(+), 11 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 5523e7b7c..409d07684 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1818,6 +1818,8 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): self._target_stop_sign = None self._stop_completed = False + self._last_failed_stop = None + for _actor in CarlaDataProvider.get_all_actors(): if 'traffic.stop' in _actor.type_id: self._list_stop_signs.append(_actor) @@ -1921,7 +1923,7 @@ 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: + 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" @@ -1937,6 +1939,8 @@ def update(self): self.events.append(running_stop_event) + self._last_failed_stop = self._target_stop_sign.id + # Reset state self._target_stop_sign = None self._stop_completed = False @@ -2065,7 +2069,7 @@ def _set_traffic_event(self): 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.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) diff --git a/srunner/scenarios/construction_crash_vehicle.py b/srunner/scenarios/construction_crash_vehicle.py index d9ed0eecf..da1cc7769 100644 --- a/srunner/scenarios/construction_crash_vehicle.py +++ b/srunner/scenarios/construction_crash_vehicle.py @@ -162,7 +162,7 @@ def _create_construction_setup(self, start_transform, lane_width): 'debris': 'static.prop.dirtdebris02'} _perp_angle = 90 - _setup = {'lengths': [6, 3], 'offsets': [2, 1]} + _setup = {'lengths': [4, 3], 'offsets': [2, 1]} _z_increment = 0.1 # Traffic warning and debris diff --git a/srunner/scenarios/control_loss.py b/srunner/scenarios/control_loss.py index 454b50430..7d83b7b78 100644 --- a/srunner/scenarios/control_loss.py +++ b/srunner/scenarios/control_loss.py @@ -47,10 +47,10 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._end_distance = 110 # Friction loss tends to have a much stronger steering compoenent then a throttle one - self._throttle_mean = 0.035 - self._throttle_std = 0.035 - self._steer_mean = 0.07 - self._steer_std = 0.07 + self._throttle_mean = 0.03 + self._throttle_std = 0.01 + self._steer_mean = 0.055 + self._steer_std = 0.015 self._trigger_dist = 2 diff --git a/srunner/scenarios/cut_in_with_static_vehicle.py b/srunner/scenarios/cut_in_with_static_vehicle.py index 02396af54..01d345bfd 100644 --- a/srunner/scenarios/cut_in_with_static_vehicle.py +++ b/srunner/scenarios/cut_in_with_static_vehicle.py @@ -50,7 +50,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._trigger_location = config.trigger_points[0].location self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location) - self._reaction_time = 3.1 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.7 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 15.0 # Min distance to the collision location that triggers the adversary [m] self._back_vehicles = 2 diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 626eb316a..1309dbf42 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -167,7 +167,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._adversary_speed = 2.0 # Speed of the adversary [m/s] self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) - self._reaction_time = 2.15 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.1 # Time the agent has to react to avoid the collision [s] self._reaction_time += 0.1 * floor(self._crossing_angle / 5) self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index 0939b5bb8..fa6f268c9 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -42,7 +42,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._blocker_distance = 28 self._adversary_speed = 13.0 # Speed of the adversary [m/s] - self._reaction_time = 2.5 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.35 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 10.0 # Min distance to the collision location that triggers the adversary [m] self._end_distance = 30 self.timeout = timeout diff --git a/srunner/scenarios/pedestrian_crossing.py b/srunner/scenarios/pedestrian_crossing.py index ee749510a..6cb70c2de 100644 --- a/srunner/scenarios/pedestrian_crossing.py +++ b/srunner/scenarios/pedestrian_crossing.py @@ -61,7 +61,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._rng = CarlaDataProvider.get_random_seed() self._adversary_speed = 1.3 # Speed of the adversary [m/s] - self._reaction_time = 4.0 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 3.5 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 12.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 self.timeout = timeout From 5cbc9b6efd2c407063ab4c4b64820d9c066c8285 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 14 Oct 2022 15:13:35 +0200 Subject: [PATCH 59/86] Removed unused code --- srunner/scenarios/background_activity.py | 52 ------------------- .../opposite_vehicle_taking_priority.py | 2 +- srunner/tools/background_manager.py | 31 ----------- 3 files changed, 1 insertion(+), 84 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index c2cd86357..ac35fe3a2 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1652,19 +1652,6 @@ def _update_parameters(self): self._start_road_front_vehicles() py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) - # Handles road accident scenario - handle_start_accident_data = py_trees.blackboard.Blackboard().get('BA_StartObstacleScenario') - if handle_start_accident_data is not None: - accident_wp, distance, direction, stop_back_vehicles = handle_start_accident_data - self._handle_lanechange_scenario(accident_wp, distance, direction, stop_back_vehicles) - py_trees.blackboard.Blackboard().set('BA_StartObstacleScenario', None, True) - - # Handles road accident scenario - handle_end_accident_data = py_trees.blackboard.Blackboard().get('BA_EndObstacleScenario') - if handle_end_accident_data is not None: - self._road_extra_front_actors = 0 - py_trees.blackboard.Blackboard().set('BA_EndObstacleScenario', None, True) - # Leave space in front leave_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveSpaceInFront') if leave_space_data is not None: @@ -1755,45 +1742,6 @@ def _move_actors_forward(self, actors, space): else: self._destroy_actor(actor) - def _handle_lanechange_scenario(self, accident_wp, distance, direction, stop_back_vehicles=False): - """ - Handles the scenarios in which the BA has to change lane, - generally due to an obstacle in the road (accident / construction / stopped vehicle...) - """ - ego_wp = self._route[self._route_index] - lane_change_actors = self._road_dict[get_lane_key(ego_wp)].actors - - if direction == 'left': - first_wp = accident_wp.get_left_lane().next(distance / 4)[0] - second_wp = first_wp.next(distance / 2)[0] - third_wp = second_wp.get_right_lane().next(distance / 4)[0] - else: - first_wp = accident_wp.get_right_lane().next(distance / 4)[0] - second_wp = first_wp.next(distance / 2)[0] - third_wp = second_wp.get_left_lane().next(distance / 4)[0] - - vehicle_path = [first_wp.transform.location, - second_wp.transform.location, - third_wp.transform.location] - - for i, actor in enumerate(lane_change_actors): - - location = CarlaDataProvider.get_location(actor) - - if i == 0: - # First actor might get blocked by the accident, so teleport it - distance = location.distance(third_wp.transform.location) - if distance > 0: - self._move_actors_forward([actor], distance) - - elif not self._is_location_behind_ego(location): - # The others can just lane change, which will also teach the ego what to do - self._tm.set_path(actor, vehicle_path) - self._road_extra_front_actors += 1 - elif stop_back_vehicles: - # Stop the vehicles behind - self._actors_speed_perc[actor] = 0 - def _switch_route_sources(self, enabled): """ Disables all sources that are part of the ego's route diff --git a/srunner/scenarios/opposite_vehicle_taking_priority.py b/srunner/scenarios/opposite_vehicle_taking_priority.py index f59b9899d..3bf945d61 100644 --- a/srunner/scenarios/opposite_vehicle_taking_priority.py +++ b/srunner/scenarios/opposite_vehicle_taking_priority.py @@ -63,7 +63,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self.timeout = timeout self._sync_time = 2.2 # Time the agent has to react to avoid the collision [s] - self._min_trigger_dist = 9.0 # Min distance to the collision location that triggers the adversary [m] + self._min_trigger_dist = 12.0 # Min distance to the collision location that triggers the adversary [m] self._lights = carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2 diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 4200f21f8..08862cb8b 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -158,37 +158,6 @@ def update(self): return py_trees.common.Status.SUCCESS -class StartObstacleScenario(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that the road behavior has to be initialized - """ - def __init__(self, accident_wp, distance, direction='left', stop_back_vehicles=False, name="StartObstacleScenario"): - self._accident_wp = accident_wp - self._distance = distance - self._direction = direction - self._stop_back_vehicles = stop_back_vehicles - super().__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_StartObstacleScenario", - [self._accident_wp, self._distance, self._direction, self._stop_back_vehicles], overwrite=True) - return py_trees.common.Status.SUCCESS - - -class EndObstacleScenario(AtomicBehavior): - """ - Updates the blackboard to tell the background activity that the road behavior has to be initialized - """ - def __init__(self, name="EndObstacleScenario"): - super().__init__(name) - - def update(self): - """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_EndObstacleScenario", True, overwrite=True) - return py_trees.common.Status.SUCCESS - - class RemoveRoadLane(AtomicBehavior): """ Updates the blackboard to tell the background activity to remove its actors from the given lane From 27070a44a603a2e21f521003f6a79b833eead529 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 17 Oct 2022 13:01:15 +0200 Subject: [PATCH 60/86] Prettyfied some criteria --- .../scenarioatomics/atomic_criteria.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 409d07684..0a10b1fbd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1824,13 +1824,13 @@ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False): if 'traffic.stop' in _actor.type_id: self._list_stop_signs.append(_actor) - def point_inside_boundingbox(self, point, bb_center, bb_extent): - """Checks whether or not a point is inside a bounding box""" + 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 - bb_extent.x, bb_center.y - bb_extent.y) - B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y) - D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + 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 @@ -2084,7 +2084,7 @@ def terminate(self, new_status): self._set_traffic_event() if len(self._checkpoint_values): - self.actual_value = sum(self._checkpoint_values) / len(self._checkpoint_values) + self.actual_value = round(sum(self._checkpoint_values) / len(self._checkpoint_values), 2) super().terminate(new_status) From cf8590e9659ba3c0b86400a4ba2e41f8e050fec7 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Mon, 17 Oct 2022 15:20:30 +0200 Subject: [PATCH 61/86] Fixed units bug --- .../scenariomanager/scenarioatomics/atomic_behaviors.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 48958f740..9c23b48a7 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3054,7 +3054,7 @@ 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() + self._speed = self._reference_actor.get_speed_limit() # Km / h self._flow_distance = self._time_distance * self._speed self._sink_wp = self._move_waypoint_forward(self._reference_wp, self._flow_distance) @@ -3076,7 +3076,7 @@ def _spawn_actor(self): if actor is None: return py_trees.common.Status.RUNNING - controller = BasicAgent(actor, 3.6 * 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]) @@ -3166,7 +3166,7 @@ def __init__(self, source_wp, sink_wp, reference_actor, spawn_dist, def initialise(self): """Get the actor flow source and sink, depending on the reference actor speed""" - self._speed = self._reference_actor.get_speed_limit() + self._speed = self._reference_actor.get_speed_limit() # Km / h self._route = self._grp.trace_route(self._source_location, self._sink_location) return super().initialise() @@ -3178,7 +3178,7 @@ def _spawn_actor(self): if actor is None: return py_trees.common.Status.RUNNING - controller = BasicAgent(actor, 3.6 * 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]) From 97c7908aa78a92168b2854671ef0a9f3679e1593 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 18 Oct 2022 15:03:46 +0200 Subject: [PATCH 62/86] Parameter change --- srunner/scenarios/object_crash_intersection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srunner/scenarios/object_crash_intersection.py b/srunner/scenarios/object_crash_intersection.py index 95c97903a..8c1d65e00 100644 --- a/srunner/scenarios/object_crash_intersection.py +++ b/srunner/scenarios/object_crash_intersection.py @@ -291,7 +291,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._collision_wp = None self._adversary_speed = 1.8 # Speed of the adversary [m/s] - self._reaction_time = 1.8 # Time the agent has to react to avoid the collision [s] + self._reaction_time = 2.2 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 6.0 # Min distance to the collision location that triggers the adversary [m] self._ego_end_distance = 40 From 65384e8d42f2a0a661d9a7bffe7df9d2d290f682 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 18 Oct 2022 15:45:46 +0200 Subject: [PATCH 63/86] Changed opposite flow parameters --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 6 +++--- srunner/scenarios/actor_flow.py | 6 ++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 9c23b48a7..d6746d0ba 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3000,7 +3000,7 @@ class OppositeActorFlow(AtomicBehavior): """ def __init__(self, reference_wp, reference_actor, spawn_dist_interval, - time_distance=3.5, sink_dist=2, name="OppositeActorFlow"): + time_distance=2, base_distance=25, sink_dist=2, name="OppositeActorFlow"): """ Setup class members """ @@ -3012,7 +3012,7 @@ def __init__(self, reference_wp, reference_actor, spawn_dist_interval, self._reference_wp = reference_wp self._reference_actor = reference_actor self._time_distance = time_distance - + self._base_distance = base_distance self._min_spawn_dist = spawn_dist_interval[0] self._max_spawn_dist = spawn_dist_interval[1] self._spawn_dist = self._rng.uniform(self._min_spawn_dist, self._max_spawn_dist) @@ -3055,7 +3055,7 @@ 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._flow_distance = self._time_distance * self._speed + 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) diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 424bd650b..104008e76 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -20,7 +20,9 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout, WaitForever from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest -from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToLocation, WaitEndIntersection +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, + WaitEndIntersection, + WaitUntilInFrontPosition) from srunner.scenarios.basic_scenario import BasicScenario from srunner.tools.background_manager import (SwitchRouteSources, @@ -730,7 +732,7 @@ def _create_behavior(self): """ root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) - root.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self._sink_wp_2.transform.location, self._sink_distance)) + root.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._sink_wp_2.transform)) root.add_child(ActorFlow( self._source_wp_1, self._sink_wp_1, self._source_dist_interval, self._sink_distance, self._flow_speed)) root.add_child(ActorFlow( From e719626e4b7488f739c6e9e66076fce6613e9e85 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 19 Oct 2022 16:05:37 +0200 Subject: [PATCH 64/86] Minor robustness changes --- .../scenarioatomics/atomic_behaviors.py | 2 +- .../scenarioatomics/atomic_trigger_conditions.py | 5 ++--- srunner/scenarios/blocked_intersection.py | 4 ++++ srunner/scenarios/invading_turn.py | 4 +++- srunner/scenarios/object_crash_vehicle.py | 2 +- srunner/scenarios/parking_cut_in.py | 16 +++++++++++++--- 6 files changed, 24 insertions(+), 9 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index d6746d0ba..bbb70a82f 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3000,7 +3000,7 @@ class OppositeActorFlow(AtomicBehavior): """ def __init__(self, reference_wp, reference_actor, spawn_dist_interval, - time_distance=2, base_distance=25, sink_dist=2, name="OppositeActorFlow"): + time_distance=1.5, base_distance=30, sink_dist=2, name="OppositeActorFlow"): """ Setup class members """ diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index 31a832b5f..909851a08 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -1068,7 +1068,7 @@ 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, name="WaitUntilInFrontPosition"): + def __init__(self, actor, transform, check_distance=True, distance=10, name="WaitUntilInFrontPosition"): """ Init """ @@ -1079,8 +1079,7 @@ def __init__(self, actor, transform, check_distance=True, name="WaitUntilInFront self._ref_location = transform.location self._ref_vector = transform.get_forward_vector() self._check_distance = check_distance - - self._distance = 10 + self._distance = distance self.logger.debug("%s.__init__()" % (self.__class__.__name__)) diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index bdce66bdf..9c6e4a902 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -96,6 +96,10 @@ def _initialize_actors(self, config): blocker.set_simulate_physics(False) blocker.set_location(self._blocker_transform.location + carla.Location(z=-200)) + lights = blocker.get_light_state() + lights |= carla.VehicleLightState.Brake + blocker.set_light_state(carla.VehicleLightState(lights)) + def _create_behavior(self): """ Just wait for a while after the ego closes in on the blocker, then remove it. diff --git a/srunner/scenarios/invading_turn.py b/srunner/scenarios/invading_turn.py index ac838962d..cda71841e 100644 --- a/srunner/scenarios/invading_turn.py +++ b/srunner/scenarios/invading_turn.py @@ -70,6 +70,8 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl self._flow_frequency = 40 # m self._source_dist = 30 # Distance between source and end point + self._check_distance = 50 + self._distance = get_value_parameter(config, 'distance', float, 100) self._offset = get_value_parameter(config, 'offset', float, 0.25) # meters invaded in the opposite direction self._scenario_timeout = 240 @@ -150,7 +152,7 @@ def _create_behavior(self): main_behavior.add_child(InvadingActorFlow( self._source_wp, self._sink_wp, self.ego_vehicles[0], self._flow_frequency, offset=self._true_offset)) - main_behavior.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._forward_wp.transform, True)) + main_behavior.add_child(WaitUntilInFrontPosition(self.ego_vehicles[0], self._forward_wp.transform, True, self._check_distance)) main_behavior.add_child(ScenarioTimeout(self._scenario_timeout, self.config.name)) sequence.add_child(main_behavior) diff --git a/srunner/scenarios/object_crash_vehicle.py b/srunner/scenarios/object_crash_vehicle.py index 1309dbf42..485705ade 100644 --- a/srunner/scenarios/object_crash_vehicle.py +++ b/srunner/scenarios/object_crash_vehicle.py @@ -402,7 +402,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._ego_end_distance = 40 self.timeout = timeout - self._bp_attributes = {'base_type': 'car', 'has_lights': False} + self._bp_attributes = {'base_type': 'car', 'generation': 2} self._distance = get_value_parameter(config, 'distance', float, 12) self._crossing_angle = get_value_parameter(config, 'crossing_angle', float, 0) diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index fa6f268c9..72fc6f082 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -20,7 +20,7 @@ InTimeToArrivalToLocation, DriveDistance) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeRoadBehavior class ParkingCutIn(BasicScenario): @@ -45,6 +45,10 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._reaction_time = 2.35 # Time the agent has to react to avoid the collision [s] self._min_trigger_dist = 10.0 # Min distance to the collision location that triggers the adversary [m] self._end_distance = 30 + self._extra_space = 20 + + self._bp_attributes = {'base_type': 'car', 'generation': 2} + self.timeout = timeout if 'direction' in config.other_parameters: @@ -77,7 +81,7 @@ def _initialize_actors(self, config): self.parking_slots.append(parking_wp.transform.location) self._blocker_actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + 'vehicle.*', parking_wp.transform, 'scenario no lights', attribute_filter={'base_type': 'car', 'generation': 2}) if not self._blocker_actor: raise ValueError("Couldn't spawn the parked actor") self._blocker_actor.apply_control(carla.VehicleControl(hand_brake=True)) @@ -100,7 +104,7 @@ def _initialize_actors(self, config): self.parking_slots.append(parking_wp.transform.location) self._parked_actor = CarlaDataProvider.request_new_actor( - 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter={'base_type': 'car', 'has_lights':True}) + 'vehicle.*', parking_wp.transform, 'scenario', attribute_filter=self._bp_attributes) if not self._parked_actor: raise ValueError("Couldn't spawn the parked actor") self.other_actors.append(self._parked_actor) @@ -144,6 +148,9 @@ def _create_behavior(self): self.ego_vehicles[0], collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) + if self.route_mode: + sequence.add_child(ChangeRoadBehavior(extra_space=self._extra_space)) + # Move the adversary cut_in = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Cut in behavior") @@ -155,6 +162,9 @@ def _create_behavior(self): sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary")) sequence.add_child(ActorDestroy(self.other_actors[1], name="DestroyBlocker")) + if self.route_mode: + sequence.add_child(ChangeRoadBehavior(extra_space=0)) + return sequence def _create_test_criteria(self): From 2f5dd3650804b14f86d8904cdc071b516ac1075b Mon Sep 17 00:00:00 2001 From: Guillermo Date: Thu, 27 Oct 2022 12:11:19 +0200 Subject: [PATCH 65/86] Removed police car from several scenarios --- .../scenarioatomics/atomic_behaviors.py | 12 +++++++++--- srunner/scenarios/background_activity.py | 6 ++++-- srunner/scenarios/blocked_intersection.py | 2 +- srunner/scenarios/parking_cut_in.py | 2 +- 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index bbb70a82f..009eefc46 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2883,6 +2883,8 @@ 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._actor_list = [] self._collision_sensor_list = [] @@ -2904,7 +2906,7 @@ def initialise(self): def _spawn_actor(self, transform): actor = CarlaDataProvider.request_new_actor( 'vehicle.*', transform, rolename='scenario', - attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + attribute_filter=self._attribute_filter, tick=False ) if actor is None: return py_trees.common.Status.RUNNING @@ -3019,6 +3021,8 @@ 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': ''} + # Opposite direction needs earlier vehicle detection self._opt_dict = {'base_vehicle_threshold': 10, 'detection_speed_ratio': 1.6} @@ -3071,7 +3075,7 @@ def initialise(self): def _spawn_actor(self): actor = CarlaDataProvider.request_new_actor( 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + attribute_filter=self._attribute_filter, tick=False ) if actor is None: return py_trees.common.Status.RUNNING @@ -3155,6 +3159,8 @@ 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._actor_list = [] # Opposite direction needs earlier vehicle detection @@ -3173,7 +3179,7 @@ def initialise(self): def _spawn_actor(self): actor = CarlaDataProvider.request_new_actor( 'vehicle.*', self._source_transform, rolename='scenario', - attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False + attribute_filter=self._attribute_filter, tick=False ) if actor is None: return py_trees.common.Status.RUNNING diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index ac35fe3a2..2b8445e8a 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -176,6 +176,8 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._tm.global_percentage_speed_difference(0.0) self._rng = CarlaDataProvider.get_random_seed() + self._attribute_filter = {'base_type': 'car', 'special_type': '', 'has_lights': True, } + # Global variables self._ego_actor = ego_actor self._ego_state = EGO_ROAD @@ -2095,7 +2097,7 @@ def _spawn_actors(self, spawn_wps, ego_dist=0): actors = CarlaDataProvider.request_new_batch_actors( 'vehicle.*', len(spawn_transforms), spawn_transforms, True, False, 'background', - attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False) + attribute_filter=self._attribute_filter, tick=False) if not actors: return actors @@ -2118,7 +2120,7 @@ def _spawn_source_actor(self, source, ego_dist=20): ) actor = CarlaDataProvider.request_new_actor( 'vehicle.*', new_transform, rolename='background', - autopilot=True, random_location=False, attribute_filter={'base_type': 'car', 'has_lights': True}, tick=False) + autopilot=True, random_location=False, attribute_filter=self._attribute_filter, tick=False) if not actor: return actor diff --git a/srunner/scenarios/blocked_intersection.py b/srunner/scenarios/blocked_intersection.py index 9c6e4a902..68b8f4f3e 100644 --- a/srunner/scenarios/blocked_intersection.py +++ b/srunner/scenarios/blocked_intersection.py @@ -87,7 +87,7 @@ def _initialize_actors(self, config): # Spawn the blocker vehicle blocker = CarlaDataProvider.request_new_actor( "vehicle.*.*", self._blocker_transform, - attribute_filter={'base_type': 'car', 'has_lights': True} + attribute_filter={'base_type': 'car', 'has_lights': True, 'special_type': ''} ) if blocker is None: raise Exception("Couldn't spawn the blocker vehicle") diff --git a/srunner/scenarios/parking_cut_in.py b/srunner/scenarios/parking_cut_in.py index 72fc6f082..30c8fee63 100644 --- a/srunner/scenarios/parking_cut_in.py +++ b/srunner/scenarios/parking_cut_in.py @@ -47,7 +47,7 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._end_distance = 30 self._extra_space = 20 - self._bp_attributes = {'base_type': 'car', 'generation': 2} + self._bp_attributes = {'base_type': 'car', 'generation': 2, 'special_type': ''} self.timeout = timeout From 292b717589cd292a0528ead1a92161a55463e5d4 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 28 Oct 2022 16:56:36 +0200 Subject: [PATCH 66/86] Updated highway cut in parameters --- srunner/scenarios/highway_cut_in.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/srunner/scenarios/highway_cut_in.py b/srunner/scenarios/highway_cut_in.py index c6f5376e5..1a0fac4f3 100644 --- a/srunner/scenarios/highway_cut_in.py +++ b/srunner/scenarios/highway_cut_in.py @@ -55,11 +55,11 @@ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=Fals self._map = CarlaDataProvider.get_map() self.timeout = timeout - self._same_lane_time = 0.5 + self._same_lane_time = 0.3 self._other_lane_time = 3 self._change_time = 2 self._speed_perc = 80 - self._cut_in_distance = 7 + self._cut_in_distance = 10 self._extra_space = 170 self._start_location = convert_dict_to_location(config.other_parameters['other_actor_location']) From 7b5894c8a1d6886405ff5c2126bbf5832b274275 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 4 Nov 2022 15:45:40 +0100 Subject: [PATCH 67/86] Final parameter changes + some minor BA changes --- srunner/scenariomanager/lights_sim.py | 5 ++-- srunner/scenarios/actor_flow.py | 4 +-- srunner/scenarios/background_activity.py | 28 +++++++++++++++++-- .../scenarios/yield_to_emergency_vehicle.py | 2 +- 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/srunner/scenariomanager/lights_sim.py b/srunner/scenariomanager/lights_sim.py index 17498d923..316def239 100644 --- a/srunner/scenariomanager/lights_sim.py +++ b/srunner/scenariomanager/lights_sim.py @@ -33,13 +33,14 @@ 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, name="LightsBehavior"): + def __init__(self, ego_vehicle, radius=50, radius_increase=15, name="LightsBehavior"): """ Setup parameters """ super().__init__(name) self._ego_vehicle = ego_vehicle self._radius = radius + self._radius_increase = radius_increase self._world = CarlaDataProvider.get_world() self._light_manager = self._world.get_lightmanager() self._light_manager.set_day_night_cycle(False) @@ -90,7 +91,7 @@ def _get_night_mode(self, weather): def _turn_close_lights_on(self, location): """Turns on the lights of all the objects close to the ego vehicle""" ego_speed = CarlaDataProvider.get_velocity(self._ego_vehicle) - radius = max(self._radius, 10 * ego_speed) + radius = max(self._radius, self._radius_increase * ego_speed) # Street lights on_lights = [] diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 104008e76..88a461f2c 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -127,7 +127,7 @@ def _create_behavior(self): break sequence.add_child(HandleJunctionScenario( - clear_junction=False, + clear_junction=True, clear_ego_entry=True, remove_entries=source_wps, remove_exits=[], @@ -384,7 +384,7 @@ def _create_behavior(self): break sequence.add_child(HandleJunctionScenario( - clear_junction=False, + clear_junction=True, clear_ego_entry=True, remove_entries=[source_wp], remove_exits=[], diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 2b8445e8a..e43dd01f3 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -187,7 +187,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._get_route_data(route) self._actors_speed_perc = {} # Dictionary actor - percentage self._all_actors = [] - self._lane_width_threshold = 2 # Used to stop some behaviors at narrow lanes to avoid problems [m] + self._lane_width_threshold = 2.25 # Used to stop some behaviors at narrow lanes to avoid problems [m] self._spawn_vertical_shift = 0.2 self._reuse_dist = 10 # When spawning actors, might reuse actors closer to this distance @@ -226,8 +226,9 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._junction_sources_dist = 40 # Distance from the entry sources to the junction [m] self._junction_sources_max_actors = 6 # Maximum vehicles alive at the same time per source self._junction_spawn_dist = 15 # Distance between spawned vehicles [m] + self._junction_minimum_source_dist = 15 # Minimum distance between sources and their junction - self._junction_source_perc = 70 # Probability [%] of the source being created + self._junction_source_perc = 80 # Probability [%] of the source being created # Opposite lane variables self._opposite_actors = [] @@ -1209,6 +1210,10 @@ def _initialise_junction_sources(self, junction): prev_wp = prev_wps[0] moved_dist += 5 + # Don't add junction sources too close to the junction + if moved_dist < self._junction_minimum_source_dist: + continue + source = Source(prev_wp, [], entry_lane_wp=wp) entry_lane_key = get_lane_key(wp) if entry_lane_key in junction.inactive_entry_keys: @@ -2155,10 +2160,27 @@ def _update_road_actors(self): string += '_[' + lane_key + ']' draw_string(self._world, location, string, DEBUG_ROAD, False) - # if actor in scenario_actors or self._is_location_behind_ego(location): + # Actors part of scenarios are their own category, ignore them if actor in self._scenario_stopped_actors: continue + # TODO: Lane changes are weird with the TM, so just stop them + actor_wp = self._map.get_waypoint(location) + if actor_wp.lane_width < self._lane_width_threshold: + + # Ensure only ending lanes are affected. not sure if it is needed though + next_wps = actor_wp.next(0.5) + if next_wps and next_wps[0].lane_width < actor_wp.lane_width: + actor.set_target_velocity(carla.Vector3D(0, 0, 0)) + self._actors_speed_perc[actor] = 0 + lights = actor.get_light_state() + lights |= carla.VehicleLightState.RightBlinker + lights |= carla.VehicleLightState.LeftBlinker + lights |= carla.VehicleLightState.Position + actor.set_light_state(carla.VehicleLightState(lights)) + actor.set_autopilot(False) + continue + self._set_road_actor_speed(location, actor) def _set_road_actor_speed(self, location, actor, multiplier=1): diff --git a/srunner/scenarios/yield_to_emergency_vehicle.py b/srunner/scenarios/yield_to_emergency_vehicle.py index 0a9445e5c..f77750e14 100644 --- a/srunner/scenarios/yield_to_emergency_vehicle.py +++ b/srunner/scenarios/yield_to_emergency_vehicle.py @@ -63,7 +63,7 @@ def __init__(self, world, ego_vehicles, config, debug_mode=False, criteria_enabl # 2) Always use the bb check to ensure the EV doesn't run over the ego when it is lane changing # 3) Add more wps to improve BB detection self._opt_dict = { - 'base_vehicle_threshold': 15, 'detection_speed_ratio': 0.2, 'use_bbs_detection': True, + 'base_vehicle_threshold': 10, 'detection_speed_ratio': 0.15, 'use_bbs_detection': True, 'base_min_distance': 1, 'distance_ratio': 0.2 } From f10a5b37456a70d84a843e7e786c07fb4cd51377 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 22 Feb 2023 15:31:09 +0100 Subject: [PATCH 68/86] Added vehicle opens door stop --- srunner/scenarios/background_activity.py | 44 ++++++++++++++++++++---- srunner/scenarios/vehicle_opens_door.py | 4 ++- srunner/tools/background_manager.py | 28 ++++++++++++++- 3 files changed, 67 insertions(+), 9 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index e43dd01f3..a9591a083 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -242,6 +242,7 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): # Scenario variables: self._scenario_stopped_actors = [] # Actors stopped by a hard break scenario + self._scenario_stopped_back_actors = [] # Actors stopped by a open doors scenario self._scenario_max_speed = 0 # Max speed of the Background Activity. Deactivated with a value of 0 self._scenario_junction_entry = False # Flag indicating the ego is entering a junction self._scenario_junction_entry_distance = self._road_spawn_dist # Min distance between vehicles and ego @@ -1659,13 +1660,25 @@ def _update_parameters(self): self._start_road_front_vehicles() py_trees.blackboard.Blackboard().set("BA_StartFrontVehicles", None, True) + # Stop back vehicles + stop_back_data = py_trees.blackboard.Blackboard().get('BA_StopBackVehicles') + if stop_back_data is not None: + self._stop_road_back_vehicles() + py_trees.blackboard.Blackboard().set('BA_StopBackVehicles', None, True) + + # Start back vehicles + start_back_data = py_trees.blackboard.Blackboard().get('BA_StartBackVehicles') + if start_back_data is not None: + self._start_road_back_vehicles() + py_trees.blackboard.Blackboard().set("BA_StartBackVehicles", None, True) + # Leave space in front leave_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveSpaceInFront') if leave_space_data is not None: self._leave_space_in_front(leave_space_data) py_trees.blackboard.Blackboard().set('BA_LeaveSpaceInFront', None, True) - # Leave space in front + # Leave crosssing space leave_crossing_space_data = py_trees.blackboard.Blackboard().get('BA_LeaveCrossingSpace') if leave_crossing_space_data is not None: self._leave_crossing_space(leave_crossing_space_data) @@ -1706,8 +1719,7 @@ def _compute_parameters(self): def _stop_road_front_vehicles(self): """ - Manages the break scenario, where all road vehicles in front of the ego suddenly stop, - wait for a bit, and start moving again. This will never trigger unless done so from outside. + Stops all road vehicles in front of the ego. Use `_start_road_front_vehicles` to make them move again. """ for lane in self._road_dict: for actor in self._road_dict[lane].actors: @@ -1722,8 +1734,7 @@ def _stop_road_front_vehicles(self): def _start_road_front_vehicles(self): """ - Manages the break scenario, where all road vehicles in front of the ego suddenly stop, - wait for a bit, and start moving again. This will never trigger unless done so from outside. + Restarts all road vehicles stopped by `_stop_road_front_vehicles`. """ for actor in self._scenario_stopped_actors: self._actors_speed_perc[actor] = 100 @@ -1733,6 +1744,25 @@ def _start_road_front_vehicles(self): actor.set_light_state(carla.VehicleLightState(lights)) self._scenario_stopped_actors = [] + def _stop_road_back_vehicles(self): + """ + Stops all road vehicles behind the ego. Use `_start_road_back_vehicles` to make them move again. + """ + for lane in self._road_dict: + for actor in self._road_dict[lane].actors: + location = CarlaDataProvider.get_location(actor) + if location and self._is_location_behind_ego(location): + self._actors_speed_perc[actor] = 0 + self._scenario_stopped_back_actors = [] + + def _start_road_back_vehicles(self): + """ + Restarts all road vehicles stopped by `_stop_road_back_vehicles`. + """ + for actor in self._scenario_stopped_actors: + self._actors_speed_perc[actor] = 100 + self._scenario_stopped_actors = [] + def _move_actors_forward(self, actors, space): """Teleports the actors forward a set distance""" for actor in list(actors): @@ -1788,8 +1818,8 @@ def _leave_space_in_front(self, space): def _leave_crossing_space(self, collision_wp): """Removes all vehicle in the middle of crossing trajectory and stops the nearby ones""" - destruction_dist = 10 - stop_dist = 15 + destruction_dist = 20 + stop_dist = 30 opposite_wp = collision_wp.get_left_lane() if not opposite_wp: diff --git a/srunner/scenarios/vehicle_opens_door.py b/srunner/scenarios/vehicle_opens_door.py index 69e9f5029..1f226e3b3 100644 --- a/srunner/scenarios/vehicle_opens_door.py +++ b/srunner/scenarios/vehicle_opens_door.py @@ -30,7 +30,7 @@ WaitUntilInFrontPosition) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior +from srunner.tools.background_manager import LeaveSpaceInFront, ChangeOppositeBehavior, StopBackVehicles, StartBackVehicles def get_value_parameter(config, name, p_type, default): @@ -178,6 +178,7 @@ def _create_behavior(self): door = carla.VehicleDoor.FR if self._direction == 'left' else carla.VehicleDoor.FL behavior.add_child(OpenVehicleDoor(self._parked_actor, door)) + behavior.add_child(StopBackVehicles()) behavior.add_child(Idle(self._opposite_wait_duration)) if self.route_mode: behavior.add_child(SwitchWrongDirectionTest(False)) @@ -192,6 +193,7 @@ def _create_behavior(self): root.add_child(ChangeOppositeBehavior(active=True)) for actor in self.other_actors: root.add_child(ActorDestroy(actor)) + root.add_child(StartBackVehicles()) return root diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 08862cb8b..5761f424d 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -129,6 +129,33 @@ def update(self): return py_trees.common.Status.SUCCESS +class StopBackVehicles(AtomicBehavior): + """ + Updates the blackboard to tell the background activity to stop the vehicles behind the ego as to + not interfere with the scenarios. This only works at roads, not junctions. + """ + def __init__(self, name="StopBackVehicles"): + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_StopBackVehicles", overwrite=True) + return py_trees.common.Status.SUCCESS + + +class StartBackVehicles(AtomicBehavior): + """ + Updates the blackboard to tell the background activity to restart the vehicles behind the ego. + """ + def __init__(self, name="StartBackVehicles"): + super().__init__(name) + + def update(self): + """Updates the blackboard and succeds""" + py_trees.blackboard.Blackboard().set("BA_StartBackVehicles", overwrite=True) + return py_trees.common.Status.SUCCESS + + class LeaveSpaceInFront(AtomicBehavior): """ Updates the blackboard to tell the background activity that the ego needs more space in front. @@ -224,7 +251,6 @@ def update(self): py_trees.blackboard.Blackboard().set("BA_LeaveCrossingSpace", self._collision_wp, overwrite=True) return py_trees.common.Status.SUCCESS - class HandleJunctionScenario(AtomicBehavior): """ Updates the blackboard to tell the background activity to adapt to a junction scenario From 3ea5c919c15298d25cbd7ff63780f4e59542ed69 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 3 Mar 2023 10:17:43 +0100 Subject: [PATCH 69/86] Fixed VehicleOpensDoor --- srunner/scenarios/background_activity.py | 11 +++++++---- srunner/tools/background_manager.py | 4 ++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index a9591a083..075d8da13 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -1753,15 +1753,15 @@ def _stop_road_back_vehicles(self): location = CarlaDataProvider.get_location(actor) if location and self._is_location_behind_ego(location): self._actors_speed_perc[actor] = 0 - self._scenario_stopped_back_actors = [] + self._scenario_stopped_back_actors.append(actor) def _start_road_back_vehicles(self): """ Restarts all road vehicles stopped by `_stop_road_back_vehicles`. """ - for actor in self._scenario_stopped_actors: + for actor in self._scenario_stopped_back_actors: self._actors_speed_perc[actor] = 100 - self._scenario_stopped_actors = [] + self._scenario_stopped_back_actors = [] def _move_actors_forward(self, actors, space): """Teleports the actors forward a set distance""" @@ -2178,6 +2178,7 @@ def _update_road_actors(self): Not applied to those behind it so that they can catch up it """ # Updates their speed + scenario_actors = self._scenario_stopped_actors + self._scenario_stopped_back_actors for lane_key in self._road_dict: for i, actor in enumerate(self._road_dict[lane_key].actors): location = CarlaDataProvider.get_location(actor) @@ -2191,7 +2192,7 @@ def _update_road_actors(self): draw_string(self._world, location, string, DEBUG_ROAD, False) # Actors part of scenarios are their own category, ignore them - if actor in self._scenario_stopped_actors: + if actor in scenario_actors: continue # TODO: Lane changes are weird with the TM, so just stop them @@ -2484,6 +2485,8 @@ def _remove_actor_info(self, actor): self._opposite_actors.remove(actor) if actor in self._scenario_stopped_actors: self._scenario_stopped_actors.remove(actor) + if actor in self._scenario_stopped_back_actors: + self._scenario_stopped_back_actors.remove(actor) for opposite_source in self._opposite_sources: if actor in opposite_source.actors: diff --git a/srunner/tools/background_manager.py b/srunner/tools/background_manager.py index 5761f424d..436ff4262 100644 --- a/srunner/tools/background_manager.py +++ b/srunner/tools/background_manager.py @@ -139,7 +139,7 @@ def __init__(self, name="StopBackVehicles"): def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_StopBackVehicles", overwrite=True) + py_trees.blackboard.Blackboard().set("BA_StopBackVehicles", True, overwrite=True) return py_trees.common.Status.SUCCESS @@ -152,7 +152,7 @@ def __init__(self, name="StartBackVehicles"): def update(self): """Updates the blackboard and succeds""" - py_trees.blackboard.Blackboard().set("BA_StartBackVehicles", overwrite=True) + py_trees.blackboard.Blackboard().set("BA_StartBackVehicles", True, overwrite=True) return py_trees.common.Status.SUCCESS From 38a686845298fad7060447b0c8ec021a734b2852 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 28 Mar 2023 16:47:00 +0200 Subject: [PATCH 70/86] Added ActroFlow patch --- .../scenarioatomics/atomic_behaviors.py | 62 +++++++++++++++++-- srunner/scenarios/background_activity.py | 53 +++++++++++++--- 2 files changed, 100 insertions(+), 15 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 009eefc46..9873a6314 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2888,6 +2888,8 @@ def __init__(self, source_wp, sink_wp, spawn_dist_interval, sink_dist=2, self._actor_list = [] self._collision_sensor_list = [] + self._terminated = False + def initialise(self): if self._initial_actors: grp = CarlaDataProvider.get_global_route_planner() @@ -2971,11 +2973,10 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ - for actor in self._actor_list: - try: - actor.destroy() - except RuntimeError: - pass # Actor was already destroyed + if self._terminated: + return + + self._terminated = True for sensor in self._collision_sensor_list: if sensor is None: @@ -2986,6 +2987,18 @@ def terminate(self, new_status): except RuntimeError: pass # Actor was already destroyed + for actor in self._actor_list: + # TODO: Actors spawned in the same frame as the behavior termination won't be removed. + # Patched by removing its movement + actor.disable_constant_velocity() + actor.set_autopilot(False) + actor.set_target_velocity(carla.Vector3D(0,0,0)) + actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) + try: + actor.destroy() + except RuntimeError: + pass # Actor was already destroyed + class OppositeActorFlow(AtomicBehavior): """ @@ -3030,6 +3043,8 @@ def __init__(self, reference_wp, reference_actor, spawn_dist_interval, self._grp = CarlaDataProvider.get_global_route_planner() self._map = CarlaDataProvider.get_map() + self._terminated = False + def _move_waypoint_forward(self, wp, distance): """Moves forward a certain distance, stopping at junctions""" dist = 0 @@ -3116,7 +3131,18 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ + if self._terminated: + return + + self._terminated = True + for actor, _ in self._actor_list: + # TODO: Actors spawned in the same frame as the behavior termination won't be removed. + # Patched by removing its movement + actor.disable_constant_velocity() + actor.set_autopilot(False) + actor.set_target_velocity(carla.Vector3D(0,0,0)) + actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) try: actor.destroy() except RuntimeError: @@ -3170,6 +3196,8 @@ def __init__(self, source_wp, sink_wp, reference_actor, spawn_dist, self._grp = CarlaDataProvider.get_global_route_planner() self._map = CarlaDataProvider.get_map() + self._terminated = False + 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 @@ -3218,7 +3246,18 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ + if self._terminated: + return + + self._terminated = True + for actor, _ in self._actor_list: + # TODO: Actors spawned in the same frame as the behavior termination won't be removed. + # Patched by removing its movement + actor.disable_constant_velocity() + actor.set_autopilot(False) + actor.set_target_velocity(carla.Vector3D(0,0,0)) + actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) try: actor.destroy() except RuntimeError: @@ -3266,6 +3305,8 @@ def __init__(self, plan, spawn_dist_interval, sink_dist=2, self._actor_data = [] self._grp = CarlaDataProvider.get_global_route_planner() + self._terminated = False + def initialise(self): if self._initial_actors: ref_loc = self._plan[0][0].transform.location @@ -3348,7 +3389,18 @@ def terminate(self, new_status): """ Default terminate. Can be extended in derived class """ + if self._terminated: + return + + self._terminated = True + for actor, _ in self._actor_data: + # TODO: Actors spawned in the same frame as the behavior termination won't be removed. + # Patched by removing its movement + actor.disable_constant_velocity() + actor.set_autopilot(False) + actor.set_target_velocity(carla.Vector3D(0,0,0)) + actor.set_target_angular_velocity(carla.Vector3D(0,0,0)) try: actor.destroy() except RuntimeError: diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 075d8da13..9fdd6df19 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -33,16 +33,20 @@ def get_lane_key(waypoint): object and used to compare waypoint lanes""" return '' if waypoint is None else get_road_key(waypoint) + '*' + str(waypoint.lane_id) - def get_road_key(waypoint): """Returns a key corresponding to the waypoint road. Equivalent to a 'Road' object and used to compare waypoint roads""" return '' if waypoint is None else str(waypoint.road_id) def is_lane_at_road(lane_key, road_key): - """Returns whther or not a lane is part of a road""" + """Returns whether or not a lane is part of a road""" return lane_key.startswith(road_key) +def get_lane_key_from_ids(road_id, lane_id): + """Returns the lane corresping to a given road and lane ids""" + return str(road_id) + '*' + str(lane_id) + + # Debug variables DEBUG_ROAD = 'road' DEBUG_OPPOSITE = 'opposite' @@ -195,6 +199,12 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self._fake_junction_ids = [] self._fake_lane_pair_keys = [] + # Initialisation values + self._vehicle_lane_change = False + self._vehicle_lights = True + self._vehicle_leading_distance = 10 + self._vehicle_offset = 0.1 + # Road variables self._road_dict = {} # Dictionary lane key -> actor source self._road_checker_index = 0 @@ -1418,7 +1428,17 @@ def get_source_wp(wp): spawn_wps.insert(0, next_wp) actors = self._spawn_actors(spawn_wps) - self._road_dict[get_lane_key(source_wp)] = Source(source_wp, actors, active=self._active_road_sources) + + if get_lane_key(source_wp) not in self._road_dict: + # Lanes created away from the center won't affect the ids of other lanes, so just add the new id + self._road_dict[get_lane_key(source_wp)] = Source(source_wp, actors, active=self._active_road_sources) + else: + # If the lane is inwards, all lanes have their id shifted by 1 outwards + # TODO: Doesn't work for more than one lane. + added_id = 1 if source_wp.lane_id > 0 else -1 + new_lane_key = get_lane_key_from_ids(source_wp.road_id, source_wp.lane_id + added_id) + self._road_dict[new_lane_key] = self._road_dict[get_lane_key(source_wp)] + self._road_dict[get_lane_key(source_wp)] = Source(source_wp, actors, active=self._active_road_sources) elif len(new_wps) < len(old_wps): for old_wp in list(old_wps): @@ -2090,11 +2110,10 @@ def _initialise_actor(self, actor): """ Save the actor into the needed structures, disable its lane changes and set the leading distance. """ - self._tm.auto_lane_change(actor, False) - self._tm.update_vehicle_lights(actor, True) - self._tm.distance_to_leading_vehicle(actor, 10) - self._tm.vehicle_lane_offset(actor, 0.1) - self._actors_speed_perc[actor] = 100 + self._tm.auto_lane_change(actor, self._vehicle_lane_change) + self._tm.update_vehicle_lights(actor, self._vehicle_lights) + self._tm.distance_to_leading_vehicle(actor, self._vehicle_leading_distance) + self._tm.vehicle_lane_offset(actor, self._vehicle_offset) self._all_actors.append(actor) def _spawn_actor(self, spawn_wp, ego_dist=0): @@ -2284,7 +2303,7 @@ def is_remapping_needed(current_wp, prev_wp): if get_road_key(prev_wp) != get_road_key(current_wp): return True - # Some roads have ending lanes in the middle. Remap if that is detected + # Some roads have starting / ending lanes in the middle. Remap if that is detected prev_wps = get_same_dir_lanes(prev_wp) current_wps = get_same_dir_lanes(current_wp) if len(prev_wps) != len(current_wps): @@ -2292,6 +2311,17 @@ def is_remapping_needed(current_wp, prev_wp): return False + def is_road_dict_unchanging(wp_pairs): + """Sometimes 'monitor_topology_changes' has already done the necessary changes""" + road_dict_keys = list(self._road_dict) + if len(wp_pairs) != len(road_dict_keys): + return False + + for _, new_wp in wp_pairs: + if get_lane_key(new_wp) not in road_dict_keys: + return False + return True + if prev_route_index == self._route_index: return route_wp = self._route[self._route_index] @@ -2305,7 +2335,10 @@ def is_remapping_needed(current_wp, prev_wp): new_wps = get_lane_waypoints(route_wp, 0) check_dist = 1.1 * route_wp.transform.location.distance(prev_route_wp.transform.location) wp_pairs = get_wp_pairs(old_wps, new_wps, check_dist) - # TODO: these pairs are sometimes wrong as some fake intersections have overlapping lanes (highway entries) + # TODO: These pairs are sometimes wrong as some fake intersections have overlapping lanes (highway entries) + + if is_road_dict_unchanging(wp_pairs): + return for old_wp, new_wp in wp_pairs: old_key = get_lane_key(old_wp) From 1e8de386813df3e456927fd24472d4afb2b6d830 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 31 Mar 2023 09:10:15 +0200 Subject: [PATCH 71/86] Minor sensor destruction change --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 9873a6314..1197e97fe 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2943,12 +2943,12 @@ def update(self): continue sink_distance = self._sink_location.distance(location) if sink_distance < self._sink_dist: - actor.destroy() - self._actor_list.remove(actor) if sensor is not None: sensor.stop() sensor.destroy() self._collision_sensor_list.remove(sensor) + actor.destroy() + self._actor_list.remove(actor) # Spawn new actors if needed if len(self._actor_list) == 0: From 32c6a4cfdf31c448af4ff8c4feaa5b7bba4edb4d Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 12 Apr 2023 10:46:50 +0200 Subject: [PATCH 72/86] Changes numpy version to match CARLA's --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 2df0106a9..07bd4900e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,5 @@ py-trees==0.8.3 +numpy==1.18.4; python_version >= '3.0' networkx==2.2 Shapely==1.7.1 psutil @@ -6,7 +7,6 @@ xmlschema==1.0.18 ephem tabulate opencv-python==4.2.0.32 -numpy matplotlib six simple-watchdog-timer From 8b5477bed78cea3cdb77b6ea1bddb9e631f28002 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 18 Apr 2023 12:00:52 +0200 Subject: [PATCH 73/86] Minor fix --- srunner/scenarios/background_activity.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 9fdd6df19..62316380e 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -2547,8 +2547,8 @@ def _remove_actor_info(self, actor): def _destroy_actor(self, actor): """Destroy the actor and all its references""" self._remove_actor_info(actor) - actor.set_autopilot(False) try: + actor.set_autopilot(False) actor.destroy() except RuntimeError: pass From d6385cbc6a2cd14ddd3a35874908e2c3d88803ba Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 19 Apr 2023 12:45:23 +0200 Subject: [PATCH 74/86] Added tm ports to BA --- srunner/scenarios/background_activity.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/srunner/scenarios/background_activity.py b/srunner/scenarios/background_activity.py index 62316380e..fdacf5663 100644 --- a/srunner/scenarios/background_activity.py +++ b/srunner/scenarios/background_activity.py @@ -175,8 +175,8 @@ def __init__(self, ego_actor, route, debug=False, name="BackgroundBehavior"): self.debug = debug self._map = CarlaDataProvider.get_map() self._world = CarlaDataProvider.get_world() - self._tm = CarlaDataProvider.get_client().get_trafficmanager( - CarlaDataProvider.get_traffic_manager_port()) + self._tm_port = CarlaDataProvider.get_traffic_manager_port() + self._tm = CarlaDataProvider.get_client().get_trafficmanager(self._tm_port) self._tm.global_percentage_speed_difference(0.0) self._rng = CarlaDataProvider.get_random_seed() @@ -2228,7 +2228,7 @@ def _update_road_actors(self): lights |= carla.VehicleLightState.LeftBlinker lights |= carla.VehicleLightState.Position actor.set_light_state(carla.VehicleLightState(lights)) - actor.set_autopilot(False) + actor.set_autopilot(False, self._tm_port) continue self._set_road_actor_speed(location, actor) @@ -2548,7 +2548,7 @@ def _destroy_actor(self, actor): """Destroy the actor and all its references""" self._remove_actor_info(actor) try: - actor.set_autopilot(False) + actor.set_autopilot(False, self._tm_port) actor.destroy() except RuntimeError: pass From f155a0a9dc76d9b9bb80132352bb4c4105da8fea Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 19 Apr 2023 12:52:36 +0200 Subject: [PATCH 75/86] Added the port to all set autopilots --- srunner/scenariomanager/carla_data_provider.py | 3 +-- .../scenarioatomics/atomic_behaviors.py | 10 +++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 95d782c0b..7a33d101c 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -722,8 +722,7 @@ def request_new_batch_actors(model, amount, spawn_points, autopilot=False, if spawn_point: batch.append(SpawnActor(blueprint, spawn_point).then( - SetAutopilot(FutureActor, autopilot, - CarlaDataProvider._traffic_manager_port))) + SetAutopilot(FutureActor, autopilot, CarlaDataProvider._traffic_manager_port))) actors = CarlaDataProvider.handle_actor_batch(batch, tick) for actor in actors: diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 1197e97fe..f14a8f6c2 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2913,7 +2913,7 @@ def _spawn_actor(self, transform): if actor is None: return py_trees.common.Status.RUNNING - actor.set_autopilot(True) + actor.set_autopilot(True, CarlaDataProvider.get_traffic_manager_port()) self._tm.set_path(actor, [self._sink_location]) self._tm.auto_lane_change(actor, False) self._tm.set_desired_speed(actor, 3.6 * self._speed) @@ -2991,7 +2991,7 @@ def terminate(self, new_status): # TODO: Actors spawned in the same frame as the behavior termination won't be removed. # Patched by removing its movement actor.disable_constant_velocity() - actor.set_autopilot(False) + 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)) try: @@ -3140,7 +3140,7 @@ def terminate(self, new_status): # TODO: Actors spawned in the same frame as the behavior termination won't be removed. # Patched by removing its movement actor.disable_constant_velocity() - actor.set_autopilot(False) + 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)) try: @@ -3255,7 +3255,7 @@ def terminate(self, new_status): # TODO: Actors spawned in the same frame as the behavior termination won't be removed. # Patched by removing its movement actor.disable_constant_velocity() - actor.set_autopilot(False) + 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)) try: @@ -3398,7 +3398,7 @@ def terminate(self, new_status): # TODO: Actors spawned in the same frame as the behavior termination won't be removed. # Patched by removing its movement actor.disable_constant_velocity() - actor.set_autopilot(False) + 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)) try: From 2775d2c745ded234e07faeba594cec839812ed95 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 21 Apr 2023 15:39:28 +0200 Subject: [PATCH 76/86] Minor improvement to result writer and debug --- srunner/scenariomanager/result_writer.py | 6 +++--- srunner/scenarios/route_scenario.py | 10 ++++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/srunner/scenariomanager/result_writer.py b/srunner/scenariomanager/result_writer.py index 7e7711ed8..7f358f7f7 100644 --- a/srunner/scenariomanager/result_writer.py +++ b/srunner/scenariomanager/result_writer.py @@ -92,9 +92,9 @@ def create_output_text(self): list_statistics = [["Start Time", "{}".format(self._start_time)]] list_statistics.extend([["End Time", "{}".format(self._end_time)]]) - list_statistics.extend([["Duration (System Time)", "{}s".format(system_time)]]) - list_statistics.extend([["Duration (Game Time)", "{}s".format(game_time)]]) - list_statistics.extend([["Ratio (System Time / Game Time)", "{}s".format(ratio)]]) + list_statistics.extend([["System Time", "{}s".format(system_time)]]) + 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 += "\n\n" diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 70d3f78b8..9835f1109 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -71,7 +71,7 @@ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeou self.timeout = self._estimate_route_timeout() if debug_mode: - self._draw_waypoints(world, self.route, vertical_shift=0.1, size=0.1, persistency=self.timeout) + self._draw_waypoints(world, self.route, vertical_shift=0.1, size=0.1, persistency=self.timeout, downsample=5) self._build_scenarios( world, ego_vehicle, sampled_scenario_definitions, timeout=self.timeout, debug=debug_mode > 0 @@ -142,12 +142,14 @@ def _estimate_route_timeout(self): return int(SECONDS_GIVEN_PER_METERS * route_length) - # pylint: disable=no-self-use - def _draw_waypoints(self, world, waypoints, vertical_shift, size, persistency=-1): + def _draw_waypoints(self, world, waypoints, vertical_shift, size, persistency=-1, downsample=1): """ Draw a list of waypoints at a certain height given in vertical_shift. """ - for w in waypoints: + for i, w in enumerate(waypoints): + if i % downsample != 0: + continue + wp = w[0].location + carla.Location(z=vertical_shift) if w[1] == RoadOption.LEFT: # Yellow From d35e3503ceb053204e2204116ed16a033fbe1409 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 21 Apr 2023 15:53:00 +0200 Subject: [PATCH 77/86] Added missing sensor destruction --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 4 ++++ srunner/scenariomanager/scenarioatomics/atomic_criteria.py | 1 + 2 files changed, 5 insertions(+) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index f14a8f6c2..a9b2d1dbc 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -2083,6 +2083,8 @@ def terminate(self, new_status): self._control.throttle = 0.0 self._control.brake = 0.0 self._actor.apply_control(self._control) + if self._agent: + self._agent.destroy_sensor() super(ConstantVelocityAgentBehavior, self).terminate(new_status) class AdaptiveConstantVelocityAgentBehavior(AtomicBehavior): @@ -2155,6 +2157,8 @@ def terminate(self, new_status): self._control.throttle = 0.0 self._control.brake = 0.0 self._actor.apply_control(self._control) + if self._agent: + self._agent.destroy_sensor() super().terminate(new_status) class Idle(AtomicBehavior): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 0a10b1fbd..5a9081a70 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -351,6 +351,7 @@ def terminate(self, new_status): Cleanup sensor """ if self._collision_sensor is not None: + self._collision_sensor.stop() self._collision_sensor.destroy() self._collision_sensor = None super(CollisionTest, self).terminate(new_status) From 5bbc79daf06186d06d2e71f49a5e83320337a3d5 Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Mon, 15 May 2023 16:08:02 +0800 Subject: [PATCH 78/86] fix duplicate env initialization during runtime --- srunner/scenarios/basic_scenario.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/srunner/scenarios/basic_scenario.py b/srunner/scenarios/basic_scenario.py index 6ff5a7595..6973e8a74 100644 --- a/srunner/scenarios/basic_scenario.py +++ b/srunner/scenarios/basic_scenario.py @@ -57,7 +57,10 @@ def __init__(self, name, ego_vehicles, config, world, if debug_mode: py_trees.logging.level = py_trees.logging.Level.DEBUG - self._initialize_environment(world) + if self.route_mode: + # Only init env for route mode, avoid duplicate initialization during runtime + self._initialize_environment(world) + self._initialize_actors(config) if CarlaDataProvider.is_sync_mode(): From d77099761ed792201e6c7dbaa92ba378ead8846b Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Thu, 18 May 2023 15:26:45 +0800 Subject: [PATCH 79/86] check location in DriveDistance --- .../scenarioatomics/atomic_trigger_conditions.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index 909851a08..65e613e0c 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -1142,6 +1142,9 @@ def update(self): """ new_status = py_trees.common.Status.RUNNING + if self._location is None: + return new_status + new_location = CarlaDataProvider.get_location(self._actor) self._distance += calculate_distance(self._location, new_location) self._location = new_location From ffffa6656b41f4240170f18e675b152061ac6d31 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Thu, 18 May 2023 09:38:30 +0200 Subject: [PATCH 80/86] Added spawn transforms to CDP actors --- scenario_runner.py | 2 +- .../scenariomanager/carla_data_provider.py | 21 ++++++++++++------- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/scenario_runner.py b/scenario_runner.py index a99707c1e..a655ceece 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -233,7 +233,7 @@ def _prepare_ego_vehicles(self, ego_vehicles): self.ego_vehicles[i].set_target_velocity(carla.Vector3D()) self.ego_vehicles[i].set_target_angular_velocity(carla.Vector3D()) self.ego_vehicles[i].apply_control(carla.VehicleControl()) - CarlaDataProvider.register_actor(self.ego_vehicles[i]) + CarlaDataProvider.register_actor(self.ego_vehicles[i], ego_vehicles[i].transform) # sync state if CarlaDataProvider.is_sync_mode(): diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 7a33d101c..c62b87a0c 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -68,7 +68,7 @@ class CarlaDataProvider(object): # pylint: disable=too-many-public-methods _grp = None @staticmethod - def register_actor(actor): + def register_actor(actor, transform=None): """ Add new actor to dictionaries If actor already exists, throw an exception @@ -82,6 +82,8 @@ def register_actor(actor): if actor in CarlaDataProvider._actor_location_map: raise KeyError( "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)) + elif transform: + CarlaDataProvider._actor_location_map[actor] = transform.location else: CarlaDataProvider._actor_location_map[actor] = None @@ -89,7 +91,7 @@ def register_actor(actor): raise KeyError( "Vehicle '{}' already registered. Cannot register twice!".format(actor.id)) else: - CarlaDataProvider._actor_transform_map[actor] = None + CarlaDataProvider._actor_transform_map[actor] = transform @staticmethod def update_osc_global_params(parameters): @@ -106,12 +108,15 @@ def get_osc_global_param_value(ref): return CarlaDataProvider._global_osc_parameters.get(ref.replace("$", "")) @staticmethod - def register_actors(actors): + def register_actors(actors, transforms=None): """ Add new set of actors to dictionaries """ - for actor in actors: - CarlaDataProvider.register_actor(actor) + if transforms is None: + transforms = [None] * len(actors) + + for actor, transform in zip(actors, transforms): + CarlaDataProvider.register_actor(actor, transform) @staticmethod def on_carla_tick(): @@ -602,7 +607,7 @@ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False, CarlaDataProvider._world.wait_for_tick() CarlaDataProvider._carla_actor_pool[actor.id] = actor - CarlaDataProvider.register_actor(actor) + CarlaDataProvider.register_actor(actor, spawn_point) return actor @staticmethod @@ -676,7 +681,7 @@ def request_new_actors(actor_list, attribute_filter=None, tick=True): if actor is None: continue CarlaDataProvider._carla_actor_pool[actor.id] = actor - CarlaDataProvider.register_actor(actor) + CarlaDataProvider.register_actor(actor, _spawn_point) return actors @@ -729,7 +734,7 @@ def request_new_batch_actors(model, amount, spawn_points, autopilot=False, if actor is None: continue CarlaDataProvider._carla_actor_pool[actor.id] = actor - CarlaDataProvider.register_actor(actor) + CarlaDataProvider.register_actor(actor, spawn_point) return actors From 076a7d44706ea94eb24de9399755e2bb48ee8a4e Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Tue, 13 Jun 2023 14:05:44 +0800 Subject: [PATCH 81/86] use log agent CDP --- srunner/scenariomanager/carla_data_provider.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index c62b87a0c..09834df54 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -247,6 +247,20 @@ def get_global_route_planner(): """ return CarlaDataProvider._grp + @staticmethod + def set_ego_route(route): + """ + set the ego route + """ + CarlaDataProvider._route = route + + @staticmethod + def get_ego_route(): + """ + @return the ego route + """ + return CarlaDataProvider._route + @staticmethod def get_all_actors(): """ From 3285e08794da8a592191d05c0c1c017bd613c6e2 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 13 Jun 2023 10:21:51 +0200 Subject: [PATCH 82/86] Added ratio to min speed --- srunner/scenariomanager/scenarioatomics/atomic_criteria.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index 5a9081a70..ebb422a1d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1965,6 +1965,7 @@ class MinimumSpeedRouteTest(Criterion): - 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): """ @@ -2059,7 +2060,7 @@ 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._mean_speed * 100, 2) + checkpoint_value = round(self._actor_speed / (self.RATIO * self._mean_speed * 100), 2) else: checkpoint_value = 100 From c8eaf6de3a9c9e22c864752d2961b97b0642ccda Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Tue, 13 Jun 2023 16:55:33 +0800 Subject: [PATCH 83/86] fix min_speed_infractions RATIO 0.8 --- srunner/scenariomanager/scenarioatomics/atomic_criteria.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py index ebb422a1d..bea681aa6 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_criteria.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_criteria.py @@ -1965,7 +1965,7 @@ class MinimumSpeedRouteTest(Criterion): - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test """ WINDOWS_SIZE = 2 - RATIO = 1 + RATIO = 0.8 def __init__(self, actor, route, checkpoints=1, name="MinimumSpeedRouteTest", terminate_on_failure=False): """ @@ -2060,7 +2060,7 @@ 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 From c38b50df966f84a58ffc5c1d17746511680c8c82 Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Tue, 28 May 2024 11:21:30 +0800 Subject: [PATCH 84/86] Add scenario name transfer --- srunner/scenariomanager/carla_data_provider.py | 16 ++++++++++++++++ .../scenarioatomics/atomic_behaviors.py | 4 +++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 09834df54..7623facb1 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -66,6 +66,7 @@ class CarlaDataProvider(object): # pylint: disable=too-many-public-methods _random_seed = 2000 _rng = random.RandomState(_random_seed) _grp = None + _latest_scenario = "" @staticmethod def register_actor(actor, transform=None): @@ -833,6 +834,20 @@ def set_traffic_manager_port(tm_port): """ CarlaDataProvider._traffic_manager_port = tm_port + @staticmethod + def set_latest_scenario(scenario_name): + """ + Set the latest scenario + """ + CarlaDataProvider._latest_scenario = scenario_name + + @staticmethod + def get_latest_scenario(): + """ + Get the latest scenario + """ + return CarlaDataProvider._latest_scenario + @staticmethod def cleanup(): """ @@ -870,3 +885,4 @@ def cleanup(): CarlaDataProvider._spawn_index = 0 CarlaDataProvider._rng = random.RandomState(CarlaDataProvider._random_seed) CarlaDataProvider._grp = None + CarlaDataProvider._latest_scenario = "" diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index a9b2d1dbc..dcfd26a33 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3899,7 +3899,7 @@ def update(self): # Check which scenarios can be triggered blackboard = py_trees.blackboard.Blackboard() - for black_var_name, scen_location in self._blackboard_list: + for black_var_name, scen_location, scen_name in self._blackboard_list: # Close enough scen_distance = route_location.distance(scen_location) @@ -3916,6 +3916,8 @@ def update(self): _ = blackboard.set(black_var_name, True) self._triggered_scenarios.append(black_var_name) + CarlaDataProvider.set_latest_scenario(scen_name) + if self._debug: self._world.debug.draw_point( scen_location + carla.Location(z=4), From d826a728489f1ad9305ed8560ce94ce28878826e Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Tue, 28 May 2024 16:11:53 +0800 Subject: [PATCH 85/86] Add ego route setter and getter --- srunner/scenariomanager/carla_data_provider.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 35a193849..bbe4cbd17 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -270,6 +270,20 @@ def get_global_route_planner(): """ return CarlaDataProvider._grp + @staticmethod + def set_ego_route(route): + """ + set the ego route + """ + CarlaDataProvider._ego_vehicle_route = route + + @staticmethod + def get_ego_route(): + """ + @return the ego route + """ + return CarlaDataProvider._ego_vehicle_route + @staticmethod def get_all_actors(): """ From 0fc813e3fc0030724a52a2e6ae87dec1bf92eb99 Mon Sep 17 00:00:00 2001 From: Songyang Yan Date: Mon, 3 Jun 2024 15:41:28 +0800 Subject: [PATCH 86/86] Fix duplicate var --- srunner/scenariomanager/carla_data_provider.py | 1 - 1 file changed, 1 deletion(-) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index bbe4cbd17..9e5e23d39 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -70,7 +70,6 @@ class CarlaDataProvider(object): # pylint: disable=too-many-public-methods _grp = None # type: GlobalRoutePlanner _runtime_init_flag = False _lock = threading.Lock() - _grp = None _latest_scenario = "" @staticmethod