Skip to content

Commit

Permalink
Release 0.9.15 merge (#1032)
Browse files Browse the repository at this point in the history
* 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

* Improve actor flow scenarios (#859)

* Improved actor flow behavior

* CHANGELOG

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* 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 <g.loepzdiest@gmail.com>

* Changed ActorFlow to use the TrafficManager

* Routes can now have scenario specific criterias

* Scenarios are now route specific

* Added urban bicycle flow prototype

* Added CHANGELOG (#870)

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added VehicleOpensDoor scenario

* Minor changes

* Fixed bug with new JucntionScenarioManager (#873)

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added dynamic weathers to routes

* Improved traffic event handling (#877)

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added HighwayExit scenario

* Added minimum speed criteria

* Pretty code

* New Parking Exit scenario

* General improvements (#884)

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Improve bycicle flow scenario

* Changed BA 'RemoveLane' for 'SwitchLane'

* New LB2.0 scenario improvements

* Refined BA's target speed

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added HighwayCutIn scenario

* Improved HighwayCutIn

* CHANGELOG

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added Idle (#898)

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Added ParkingCutIn scenario

* 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

* Improve night mode (#902)

* Added lights beahvior to rotues

* Improved lights and automatic TM lights

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>

* Add yield to emergency vehicle scenario

* Add new scenario PedestrianCrossing

* New Blocked intersection scenario

* Added InvadingTurn scenario

* Added Static cut in scenario

* Added BicycleAtSideLane scenario

* Improved route's timeout

* Leaderboard 2.0 scenario improvements

* Fixed several merge errors

* New scenario improvements

* Removed debug messages

* Added scenario timeouts

* Added frame to traffic events

* Added realtime to OutsideRouteLanesTest

* More Leaderboard 2.0 scenario improvements

* Route obstacles + parked vehicle destruction

* Scenarios no longer spawn old vehicles

* Added variable flow to route obstacles

* Added VehicleTurningRotue variations

* Added noise to control loss

* Route improvements

* MinSpeed criteria checkpoints + Pedestrian patch

* Fix bug PedestrianCrossing

* 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 <g.loepzdiest@gmail.com>

* Removed old test routes

* Fixed bug with YieldtoEV criteria

* Minor parameter changes

* Improved scenarios

* Added hand brake to BlockedIntersection

* Revereted some of the constructions changes

* Some parameter changes

* Removed unused code

* Prettyfied some criteria

* Fixed units bug

* Parameter change

* Changed opposite flow parameters

* Minor robustness changes

* Removed police car from several scenarios

* Updated highway cut in parameters

* Final parameter changes + some minor BA changes

* Added vehicle opens door stop

* Fixed VehicleOpensDoor

* Added ActroFlow patch

* Minor sensor destruction change

* Changes numpy version to match CARLA's

* Minor fix

* Added tm ports to BA

* Added the port to all set autopilots

* Minor improvement to result writer and debug

* Added missing sensor destruction

* Added spawn transforms to CDP actors

* Added ratio to min speed

* fix duplicate env initialization during runtime

* check location in DriveDistance

* add is_runtime_init_mode process

* fix weather

* Added ScenariOTrigger blackboard

* removed whitespaces

* Added a lock at CDP

* Added Leaderboard 2.0 changelog

* Added Leaderboard 2.0 routes

* Fixed bug with MinimumSpeedRouteTest

* Added Release 0.9.15

---------

Co-authored-by: Guillermo <g.loepzdiest@gmail.com>
Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>
Co-authored-by: Tay Yim <yan_sy@126.com>
Co-authored-by: yuting <88497445+13370724230@users.noreply.github.com>
Co-authored-by: dudu_fang <108377816+May-fang@users.noreply.github.com>
Co-authored-by: threewater-wang <60544237+threewater-wang@users.noreply.github.com>
Co-authored-by: joel-mb <joel.moriana@gmail.com>
8 people authored Nov 16, 2023
1 parent 3522896 commit d12d8bb
Showing 67 changed files with 43,113 additions and 21,094 deletions.
93 changes: 88 additions & 5 deletions Docs/CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
## Table of Contents
* [Latest Changes](#latest-changes)
* [CARLA ScenarioRunner 0.9.15](#carla-scenariorunner-0915)
* [CARLA ScenarioRunner 0.9.13](#carla-scenariorunner-0913)
* [CARLA ScenarioRunner 0.9.12](#carla-scenariorunner-0912)
* [CARLA ScenarioRunner 0.9.11](#carla-scenariorunner-0911)
@@ -12,18 +12,100 @@
* [CARLA ScenarioRunner 0.9.5](#carla-scenariorunner-095)
* [CARLA ScenarioRunner 0.9.2](#carla-scenariorunner-092)

## Latest changes
## CARLA ScenarioRunner 0.9.15
### :rocket: New Features
* 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
* Add waypoint reached threshold so that the precision of the actor reaching to waypoints can be adjusted based on object types.
* OpenSCENARIO support:
- Added both init and story support for `EntityAction`
- Added story support for `TrafficSignalControllerAction`
- Added both init and story support for `LateralDistanceAction`
- Added support for `TrafficSignalControllerCondition`
* Supported OpenSCENARIO 2.0 standard.
* New scenarios:
- InvadingTurn: vehicles at the opposite direction lane partially invade the ego's one, forcing it to leave space for them,moving slightly off-center.
- EnterActorFlow: the ego has to enter a highway lane filled with incoming traffic
- MergerIntoslowtraffic. variation of `EnterActorFlow` but with slow traffic.
- InterurbanActorFlow and InterurbanAdvancedActorFlow: actor flow scenarios for the new interurban intersections with dedicated lanes present at Town12 and Town13.
- Accident: the ego is met with an accident, forcing it to lane change to avoid it.
- AccidentTwoWays: same as `Accident` but having to invade an opposite direction lane.
- ParkedObstacle: similar to `Accident` but with a parked obstacle instead.
- ParkedObstacleTwoWays: same as `ParkedObstacle` but having to invade an opposite direction lane.
- HazardAtSideLane: similar to `Accident` but with a moving group of bicycles in the rightmost park of the lane
- HazardAtSideLaneTwoWays: same as `HazardAtSideLane` but having to invade an opposite direction lane.
- ConstructionObstacleTwoWays: same as `ConstructionObstacle` but having to invade an opposite direction lane.
- VehicleOpensDoorTwoWays: similar to `Accident` but this time the blockage is cause by a vehicle opening its door.
- StaticCutIn: the ego is meant with an adversary that exits a stopped lane, cutting in front of the ego.
- ParkingCutIn: similar to `StaticCutIn` but the adversary starts at a parking lane.
- HighwayCutIn: the ego is met with a vehicle that tries to enter the highway by cutting in front of it.
- ParkingExit: Only usable at the beginning of the routes, makes the ego start at a parking lane.
- HardBreakRoute: uses the BackgroundActivity to make all vehicles in front of the ego hard break.
- YieldToEmergencyVehicle: the ego finds an emergency vehicle behind, having to lane chane to give way.
- VehicleTurningRoutePedestrian: variation of `VehicleTurningRoute` but with a pedestrian crossing instead of a bycicle.
- BlockedIntersection: with low visibility, the ego performs a turn only to find out that the end is blocked by another vehicle.
- CrossingBicycleFlow: the ego has to do a turn at an intersection but it has to cross a bycicle lane full of traffic.
- PedestrianCrossing: a group of pedestrians crossing a crosswalk. Easier version of `DynamicObjectCrossing` with no occluder.
- ParkingCrossingPedestrian: variation of `DynamicObjectCrossing`, but using a parked vehicle as the occluder.
- OppositeVehicleTakingPriority: variation of `OppositeVehicleRunningRedLight` but without traffic lights.
- NonSignalizedJunctionLeftTurn: variation of `SignalizedJunctionLeftTurn` but without traffic lights.
- NonSignalizedJunctionRightTurn: variation of `SignalizedJunctionRightTurn` but without traffic lights.
- PriorityAtJunction: utility scenario during routes to add a green traffic light at the next intersection.
- NoSignalJunctionCrossingRoute: Does nothing but wait for the ego to exit an intersection.
* Improvements to old scenarios:
- ControlLoss: Added actual noise to the ego's control (currently only during routes).
- All VehicleTurning variations: more robustness and better synchronization.
- OppositeVehicleRunningRedLight: Improvement synchronization and the opposite vehicle's behavior.
- SignalizedJunctionLeftTurn: it is now an actor flow that ego has to cross.
- SignalizedJunctionRightTurn. it is now an actor flow that the ego has to merge into.
- Renamed `ConstructionSetupCrossing` to `ConstructionObstacle`, and prepared it for routes.
* Improvements to the CarlaDataProvider:
- Added a lock when checking the dictionaries to avoid issues in multithreading
- Added the `transform` argument to all register function to avoid returning None during the first frame
- Added the `get_global_route_planner` and `get_all_actors` to avoid repeating these costly calls more than necessary
- Added `set_runtime_init_mode` and `is_runtime_init_mode`, used by the Leaderboard to initialize scenarios during the simulation
- At the `create_blueprint` function, replaced the `safe` argument with the `attribute_filter`, for a more generic parsing of any of the blueprint attributes.
- Removed the `CarlaDataProvider.get_ego_vehicle_route()` and `CarlaDataProvider.set_ego_vehicle_route()` functions as this is now information available to all scenarios.
* Improvements to the routes:
- Scenarios are no longer position based, but instead part of a route's xml.
- Routes now also include the criteria of its scenarios.
- `waypoint` have been renamed to `position` and are part of the `waypoints` category.
- More than one `weather` are allowed, creating a dynamic one based on the ego vehicle's completed percentage of the route.
- Changed the timeout to also be dependent on the distance driven by the ego vehicle.
- Added the `RouteLightsBehavior` to control of all scene and vehicle lights during runtime
- Added a new criteria for routes, `CheckMinSpeed`, that checks the ego's speed and compares it with the rest of the traffic
- Separated the route argument into two, `route` for the file path, and `route-id`, for the name of route. the functionality remains unchanged.
- Simplified the overall parsing.
* The BackgroundActivity part of the routes has been completely remade, with the objective of creating the sensation of traffic around the ego will increasing the performance
* Added a Backgroundmanager to interact with the new BackgroundActivity, to allow it to adapt to incoming scenarios
* Added new atomic behaviors:
- SyncArrivalWithAgent
- CutIn
- AddNoiseToRouteEgo
- ConstantVelocityAgentBehavior
- AdaptiveConstantVelocityAgentBehavior
- WaitForever
- BatchActorTransformSetter
- OppositeActorFlow
- InvadingActorFlow
- BicycleFlow
- OpenVehicleDoor
- SwitchWrongDirectionTest
- SwitchMinSpeedCriteria
- WalkerFlow
- AIWalkerBehavior
- ScenarioTimeout
- MovePedestrianWithEgo
* Improved the Criterion class for a more comprehensive base criteria and easier use in the `results_writer` class.
* Added new atomic criteria:
- MinimumSpeedRouteTest
- YieldToEmergencyVehicleTest
- ScenarioTimeoutTest
* Added new atomic trigger conditions
- WaitUntilInFrontPosition
* Merged the `Scenario` class into the `BasicScenario` one.
* Scenarios can now have parameters as part of the their xml definition, which is saved as a dictionary at `config.other_parameters`
* Simplified and improved how routes are parsed.
* Added the `wait-for-repetitions` argument at the manual control for a smoother transition between scenarios / repetitions
* Updated numpy's version to avoid issues with newer version of Python 3

### :bug: Bug Fixes
* Fixed bug at OtherLeadingVehicle scenario causing the vehicles to move faster than intended
@@ -459,3 +541,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
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -21,6 +21,7 @@ branch contains the latest fixes and features, and may be required to use the la

It is important to also consider the release version that has to match the CARLA version.

* [Version 0.9.15](https://github.com/carla-simulator/scenario_runner/releases/tag/v0.9.15) and the 0.9.15 Branch: Compatible with [CARLA 0.9.15](https://github.com/carla-simulator/carla/releases/tag/0.9.15)
* [Version 0.9.13](https://github.com/carla-simulator/scenario_runner/releases/tag/v0.9.13) and the 0.9.13 Branch: Compatible with [CARLA 0.9.13](https://github.com/carla-simulator/carla/releases/tag/0.9.13)
* [Version 0.9.12](https://github.com/carla-simulator/scenario_runner/releases/tag/v0.9.12) and the 0.9.12 Branch: Compatible with [CARLA 0.9.12](https://github.com/carla-simulator/carla/releases/tag/0.9.12)
* [Version 0.9.11](https://github.com/carla-simulator/scenario_runner/releases/tag/v0.9.11) and the 0.9.11 Branch: Compatible with [CARLA 0.9.11](https://github.com/carla-simulator/carla/releases/tag/0.9.11)
31 changes: 13 additions & 18 deletions manual_control.py
Original file line number Diff line number Diff line change
@@ -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, args):

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, args):
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, args):

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')]
2 changes: 1 addition & 1 deletion requirements.txt
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
py-trees==0.8.3
numpy==1.18.4; python_version >= '3.0'
networkx==2.2
Shapely==1.7.1
psutil
xmlschema==1.0.18
ephem
tabulate
opencv-python==4.2.0.32
numpy
matplotlib
six
simple-watchdog-timer
35 changes: 15 additions & 20 deletions scenario_runner.py
Original file line number Diff line number Diff line change
@@ -93,8 +93,8 @@ def __init__(self, args):
self.client = carla.Client(args.host, int(args.port))
self.client.set_timeout(self.client_timeout)
dist = pkg_resources.get_distribution("carla")
if LooseVersion(dist.version) < LooseVersion('0.9.12'):
raise ImportError("CARLA version 0.9.12 or newer required. CARLA version found: {}".format(dist))
if LooseVersion(dist.version) < LooseVersion('0.9.15'):
raise ImportError("CARLA version 0.9.15 or newer required. CARLA version found: {}".format(dist))

# Load agent if requested via command line args
# If something goes wrong an exception will be thrown by importlib (ok here)
@@ -236,7 +236,10 @@ def _prepare_ego_vehicles(self, ego_vehicles):

for i, _ in enumerate(self.ego_vehicles):
self.ego_vehicles[i].set_transform(ego_vehicles[i].transform)
CarlaDataProvider.register_actor(self.ego_vehicles[i])
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], ego_vehicles[i].transform)

# sync state
if CarlaDataProvider.is_sync_mode():
@@ -401,11 +404,11 @@ def _load_and_run_scenario(self, config):
timeout=100000)
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()
@@ -471,15 +474,8 @@ def _run_route(self):
"""
result = False

if self._args.route:
routes = self._args.route[0]
scenario_file = self._args.route[1]
single_route = None
if len(self._args.route) > 2:
single_route = self._args.route[2]

# retrieve routes
route_configurations = RouteParser.parse_routes_file(routes, scenario_file, single_route)
route_configurations = RouteParser.parse_routes_file(self._args.route, self._args.route_id)

for config in route_configurations:
for _ in range(self._args.repetitions):
@@ -576,11 +572,10 @@ def main():
parser.add_argument('--openscenario', help='Provide an OpenSCENARIO definition')
parser.add_argument('--openscenarioparams', help='Overwrited for OpenSCENARIO ParameterDeclaration')
parser.add_argument('--openscenario2', help='Provide an openscenario2 definition')
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,scenario_file,[route id]))', nargs='+', type=str)

parser.add_argument(
'--agent', help="Agent used to execute the scenario. Currently only compatible with route-based scenarios.")
'--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')
15,013 changes: 0 additions & 15,013 deletions srunner/data/all_towns_traffic_scenarios.json

This file was deleted.

6 changes: 0 additions & 6 deletions srunner/data/no_scenarios.json

This file was deleted.

92 changes: 0 additions & 92 deletions srunner/data/routes_debug.xml

This file was deleted.

1,642 changes: 585 additions & 1,057 deletions srunner/data/routes_devtest.xml

Large diffs are not rendered by default.

57 changes: 57 additions & 0 deletions srunner/data/routes_town10.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
<routes>
<route id="0" town="Town10HD_Opt">
<!-- <weathers>
<weather route_percentage="0"
cloudiness="5.0" precipitation="0.0" precipitation_deposits="0.0" wetness="0.0"
wind_intensity="10.0" sun_azimuth_angle="-1.0" sun_altitude_angle="90.0" fog_density="2.0"/>
<weather route_percentage="100"
cloudiness="5.0" precipitation="0.0" precipitation_deposits="0.0" wetness="0.0"
wind_intensity="10.0" sun_azimuth_angle="-1.0" sun_altitude_angle="15.0" fog_density="2.0"/>
</weathers> -->
<waypoints>
<position x="85.7" y="66.4" z="0.0"/>
<position x="-2.8" y="66.2" z="0.0"/>
<position x="-41.7" y="47.7" z="0.0"/>
<position x="-41.7" y="-32.0" z="0.0"/>
<position x="12.7" y="-57.4" z="0.0"/>
<position x="-0.8" y="13.2" z="0.0"/>
</waypoints>
<scenarios>
<scenario name="ControlLoss_1" type="ControlLoss">
<trigger_point x="71.0" y="66.3" z="0.0" yaw="-179.9"/>
</scenario>
<scenario name="ControlLoss_2" type="ControlLoss">
<trigger_point x="19.4" y="66.3" z="0.0" yaw="-179.9"/>
</scenario>
<scenario name="DynamicObjectCrossing_1" type="DynamicObjectCrossing">
<trigger_point x="19.4" y="66.3" z="0.0" yaw="-179.9"/>
<distance value="35"/>
<direction value="right"/>
<blocker_model value="static.prop.advertisement"/>
<crossing_angle value="7"/>
</scenario>
<scenario name="ControlLoss_3" type="ControlLoss">
<trigger_point x="-41.7" y="49.4" z="0.0" yaw="-90.2"/>
</scenario>
<scenario name="DynamicObjectCrossing_2" type="DynamicObjectCrossing">
<trigger_point x="-41.8" y="-3.3" z="0.0" yaw="-90.2"/>
<distance value="35"/>
<direction value="right"/>
<blocker_model value="static.prop.advertisement"/>
<crossing_angle value="8"/>
</scenario>
<scenario name="HardBreakRoute_1" type="HardBreakRoute">
<trigger_point x="33.3" y="-57.4" z="0.0" yaw="360.0"/>
</scenario>
<scenario name="HardBreakRoute_2" type="HardBreakRoute">
<trigger_point x="88.3" y="-48.5" z="0.0" yaw="42.3"/>
</scenario>
<scenario name="BlockedIntersection_1" type="BlockedIntersection">
<trigger_point x="99.4" y="-15.6" z="0.0" yaw="89.8"/>
</scenario>
<scenario name="ControlLoss_4" type="ControlLoss">
<trigger_point x="15.2" y="13.2" z="0.0" yaw="180.2"/>
</scenario>
</scenarios>
</route>
</routes>
25,469 changes: 23,902 additions & 1,567 deletions srunner/data/routes_training.xml

Large diffs are not rendered by default.

8,398 changes: 8,398 additions & 0 deletions srunner/data/routes_validation.xml

Large diffs are not rendered by default.

10 changes: 10 additions & 0 deletions srunner/examples/ActorFlow.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
<?xml version="1.0"?>
<scenarios>
<scenario name="EnterActorFlow_1" type="EnterActorFlow" town="Town04">
<ego_vehicle x="86.9" y="144.0" z="1.9" yaw="170" model="vehicle.lincoln.mkz_2017" />
<start_actor_flow x="16.2" y="220.6" z="0.2"/>
<end_actor_flow x="15.50" y="35.0" z="0.2"/>
<flow_speed value="30"/>
<source_dist_interval from="35" to="50"/>
</scenario>
</scenarios>
7 changes: 7 additions & 0 deletions srunner/examples/HighwayCutIn.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<scenarios>
<scenario name="HighwayCutIn_1" type="HighwayCutIn" town="Highway">
<ego_vehicle x="-619.20" y="1055.3" z="0" yaw="-120" model="vehicle.lincoln.mkz_2017" />
<other_actor x="-515.70" y="1048.5" z="0" yaw="150" model="vehicle.tesla.model3" />
</scenario>
</scenarios>
2 changes: 1 addition & 1 deletion srunner/examples/ObjectCrossing.xml
Original file line number Diff line number Diff line change
@@ -51,7 +51,7 @@
<scenario name="DynamicObjectCrossing_9" type="DynamicObjectCrossing" town="Town03">
<ego_vehicle x="90" y="129" z="1" yaw="180" model="vehicle.lincoln.mkz_2017" />
</scenario>
<scenario name="ConstructionSetupCrossing" type="ConstructionSetupCrossing" town="Town02">
<scenario name="ConstructionObstacle" type="ConstructionObstacle" town="Town02">
<ego_vehicle x="7.63" y="109.91" z="1.22" yaw="359" model="vehicle.lincoln.mkz_2017" />
</scenario>

9 changes: 9 additions & 0 deletions srunner/examples/RouteObstacles.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<scenarios>
<scenario name="Accident_1" type="Accident" town="Town04">
<ego_vehicle x="30.39" y="-285.92" z="2.8" yaw="-70" model="vehicle.lincoln.mkz_2017" />
</scenario>
<scenario name="Construction_1" type="ConstructionObstacle" town="Town04">
<ego_vehicle x="30.39" y="-285.92" z="2.8" yaw="-70" model="vehicle.lincoln.mkz_2017" />
</scenario>
</scenarios>
7 changes: 7 additions & 0 deletions srunner/examples/VehicleOpensDoor.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<scenarios>
<scenario name="VehicleOpensDoorTwoWays_1" type="VehicleOpensDoorTwoWays" town="Town10HD_Opt">
<ego_vehicle x="-19.4" y="69.5" z="0.3" yaw="0" model="vehicle.lincoln.mkz_2017" />
<direction value="right"/>
</scenario>
</scenarios>
2 changes: 1 addition & 1 deletion srunner/metrics/examples/criteria_filter.py
Original file line number Diff line number Diff line change
@@ -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"]
}
}
)
2 changes: 2 additions & 0 deletions srunner/scenarioconfigs/openscenario_configuration.py
Original file line number Diff line number Diff line change
@@ -34,6 +34,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 {}
6 changes: 4 additions & 2 deletions srunner/scenarioconfigs/route_scenario_configuration.py
Original file line number Diff line number Diff line change
@@ -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.keypoints = None
self.scenario_configs = []
48 changes: 36 additions & 12 deletions srunner/scenarioconfigs/scenario_configuration.py
Original file line number Diff line number Diff line change
@@ -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, cloudiness=50)
self.friction = None
self.subtype = None
self.route_var_name = None
203 changes: 127 additions & 76 deletions srunner/scenariomanager/carla_data_provider.py

Large diffs are not rendered by default.

153 changes: 153 additions & 0 deletions srunner/scenariomanager/lights_sim.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
#!/usr/bin/env python

# Copyright (c) 2020 Intel Corporation
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""
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):

"""
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 = 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, 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)
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._get_night_mode(self._world.get_weather())
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 _get_night_mode(self, weather):
"""Check wheather or not the street lights need to be turned on"""
altitude_dist = weather.sun_altitude_angle - self.SUN_ALTITUDE_THRESHOLD_1
altitude_dist = min(altitude_dist, self.SUN_ALTITUDE_THRESHOLD_2 - weather.sun_altitude_angle)
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

# 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):
"""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, self._radius_increase * 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 = CarlaDataProvider.get_all_actors().filter('*vehicle.*')
scenario_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'scenario']

for vehicle in scenario_vehicles:
if vehicle.get_location().distance(location) > radius:
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 = CarlaDataProvider.get_all_actors().filter('*vehicle.*')
scenario_vehicles = [v for v in all_vehicles if v.attributes['role_name'] == 'scenario']

for vehicle in scenario_vehicles:
lights = vehicle.get_light_state()
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)
25 changes: 11 additions & 14 deletions srunner/scenariomanager/result_writer.py
Original file line number Diff line number Diff line change
@@ -92,32 +92,29 @@ 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"

# 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: {} = {}]]></failure>\n".format(
criterion.name, criterion.actual_value)
9 changes: 4 additions & 5 deletions srunner/scenariomanager/scenario_manager.py
Original file line number Diff line number Diff line change
@@ -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"):
1,377 changes: 1,313 additions & 64 deletions srunner/scenariomanager/scenarioatomics/atomic_behaviors.py

Large diffs are not rendered by default.

1,197 changes: 674 additions & 523 deletions srunner/scenariomanager/scenarioatomics/atomic_criteria.py

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -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

@@ -665,7 +665,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

@@ -681,11 +681,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
@@ -1124,9 +1121,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
@@ -1141,6 +1137,57 @@ 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, distance=10, 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 = distance

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):

"""
@@ -1174,6 +1221,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
@@ -1337,14 +1387,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):
@@ -1354,12 +1406,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
86 changes: 86 additions & 0 deletions srunner/scenariomanager/timer.py
Original file line number Diff line number Diff line change
@@ -14,6 +14,8 @@
import operator
import py_trees

from srunner.scenariomanager.carla_data_provider import CarlaDataProvider


class GameTime(object):

@@ -155,3 +157,87 @@ 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 = 300
TIMEOUT_ROUTE_PERC = 10

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
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
44 changes: 22 additions & 22 deletions srunner/scenariomanager/traffic_events.py
Original file line number Diff line number Diff line change
@@ -30,6 +30,9 @@ class TrafficEventType(Enum):
OUTSIDE_LANE_INFRACTION = 11
OUTSIDE_ROUTE_LANES_INFRACTION = 12
VEHICLE_BLOCKED = 13
MIN_SPEED_INFRACTION = 14
YIELD_TO_EMERGENCY_VEHICLE = 15
SCENARIO_TIMEOUT = 16


class TrafficEvent(object):
@@ -38,47 +41,44 @@ class TrafficEvent(object):
TrafficEvent definition
"""

def __init__(self, event_type, message=None, 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

def get_type(self):
"""
@return type
"""
"""return the type"""
return self._type

def get_message(self):
"""
@return message
"""
if self._message:
return self._message
def get_frame(self):
"""return the frame"""
return self._frame

return ""
def set_frame(self, frame):
"""return the frame"""
self._frame = frame

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
135 changes: 133 additions & 2 deletions srunner/scenariomanager/weather_sim.py
Original file line number Diff line number Diff line change
@@ -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,134 @@ 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]
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)

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
778 changes: 778 additions & 0 deletions srunner/scenarios/actor_flow.py

Large diffs are not rendered by default.

2,400 changes: 1,429 additions & 971 deletions srunner/scenarios/background_activity.py

Large diffs are not rendered by default.

89 changes: 89 additions & 0 deletions srunner/scenarios/background_activity_parametrizer.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
#!/usr/bin/env python

# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""
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)

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.
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 = 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 = 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 = 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,
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()

286 changes: 157 additions & 129 deletions srunner/scenarios/basic_scenario.py
Original file line number Diff line number Diff line change
@@ -16,11 +16,13 @@

import carla

import srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions as conditions
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.weather_sim import WeatherBehavior
from srunner.scenariomanager.scenarioatomics.atomic_behaviors import UpdateAllActorControls
from srunner.scenariomanager.scenarioatomics.atomic_criteria import Criterion


class BasicScenario(object):
@@ -35,53 +37,104 @@ 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.parking_slots = []
self.config = config
self.world = world
self.debug_mode = debug_mode
self.terminate_on_failure = terminate_on_failure
self.criteria_enable = criteria_enable

self.route_mode = bool(config.route)
self.behavior_tree = None
self.criteria_tree = None

# 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

self._initialize_environment(world)
if not self.route_mode:
# Only init env for route mode, avoid duplicate initialization during runtime
self._initialize_environment(world)

# Initializing adversarial actors
self._initialize_actors(config)
if CarlaDataProvider.is_sync_mode():

if CarlaDataProvider.is_runtime_init_mode():
world.wait_for_tick()
elif 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 = 'VehicleBehavior'
scenario_behavior = self._create_behavior()
if scenario_behavior is not None:
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)

# 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)

# 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("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)

# 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.scenario_tree.add_child(UpdateAllActorControls())

self.scenario_tree.setup(timeout=1)

def _initialize_environment(self, world):
"""
@@ -126,48 +179,35 @@ 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 or config.route_var_name is None:
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):
"""
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.
"""
ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route()
if not self.route_mode or config.route_var_name is None:
return None

# 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

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
return end_sequence

def _create_behavior(self):
"""
@@ -186,6 +226,29 @@ 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 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,
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
@@ -196,68 +259,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.
criteria = []
if not self.criteria_tree:
return criteria

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
criteria_nodes = self._extract_nodes_from_tree(self.criteria_tree)
for criterion in criteria_nodes:
if isinstance(criterion, Criterion):
criteria.append(criterion)

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

# 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())

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
"""
@@ -279,13 +295,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
@@ -307,3 +316,22 @@ 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
"""
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):
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
150 changes: 150 additions & 0 deletions srunner/scenarios/blocked_intersection.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,150 @@
#!/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 <https://opensource.org/licenses/MIT>.

"""
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,
ScenarioTimeout,
ActorTransformSetter,
HandBrakeVehicle)
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
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):
"""
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,
the ego performs a turn only to find out that the end is blocked by another vehicle.
"""

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_distance = 5
self._trigger_distance = 13
self._stop_time = 10

self._scenario_timeout = 240

self._blocker_transform = None

super().__init__("BlockedIntersection",
ego_vehicles,
config,
world,
debug_mode,
criteria_enable=criteria_enable)

def _initialize_actors(self, config):
"""
Custom initialization
"""
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
blocker = CarlaDataProvider.request_new_actor(
"vehicle.*.*", self._blocker_transform,
attribute_filter={'base_type': 'car', 'has_lights': True, 'special_type': ''}
)
if blocker is None:
raise Exception("Couldn't spawn the blocker vehicle")
self.other_actors.append(blocker)

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.
"""
sequence = py_trees.composites.Sequence(name="BlockedIntersection")

if self.route_mode:
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
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(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))
main_behavior.add_child(behavior)

sequence.add_child(main_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.
"""
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 and traffic lights upon deletion
"""
self.remove_all_actors()
306 changes: 230 additions & 76 deletions srunner/scenarios/construction_crash_vehicle.py

Large diffs are not rendered by default.

112 changes: 83 additions & 29 deletions srunner/scenarios/control_loss.py
Original file line number Diff line number Diff line change
@@ -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,46 +41,50 @@ 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.03
self._throttle_std = 0.01
self._steer_mean = 0.055
self._steer_std = 0.015

self._trigger_dist = 2

super().__init__("ControlLoss", ego_vehicles, config, world, debug_mode, criteria_enable=criteria_enable)

def _initialize_actors(self, config):
"""
Custom initialization
"""
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
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
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
self.third_transform = carla.Transform(third_loc, third_wp.transform.rotation)

# Spawn the debris
first_debris = CarlaDataProvider.request_new_actor(
@@ -87,10 +94,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,22 +103,73 @@ def _initialize_actors(self, config):
self.other_actors.append(second_debris)
self.other_actors.append(third_debris)

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)
if self.route_mode:
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)
if self.route_mode:
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)
if self.route_mode:
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):
"""
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):
"""
206 changes: 206 additions & 0 deletions srunner/scenarios/cross_bicycle_flow.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,206 @@
#!/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 <https://opensource.org/licenses/MIT>.

"""
Scenarios in which the ego has to cross a flow of bycicles
"""

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 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

from srunner.tools.background_manager import HandleJunctionScenario
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


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
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

self._signalized_junction = False

self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)

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,
config,
world,
debug_mode,
criteria_enable=criteria_enable)

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
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 = self._source_wp
while True:
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
wp = next_wp[0]
self._plan.append([next_wp[0], RoadOption.LANEFOLLOW])

if plan_step == 0 and wp.is_junction:
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

tls = self._world.get_traffic_lights_in_junction(junction.id)
if not tls:
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)
self._flow_tl_dict = {}
self._init_tl_dict = {}
for tl in tls:
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(self._plan, self._source_dist_interval, self._sink_distance, self._flow_speed, True))
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
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:
return root

sequence = py_trees.composites.Sequence()
sequence.add_child(HandleJunctionScenario(
clear_junction=True,
clear_ego_entry=True,
remove_entries=[],
remove_exits=[],
stop_entries=True,
extend_road_exit=0
))
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 and traffic lights upon deletion
"""
self.remove_all_actors()
271 changes: 271 additions & 0 deletions srunner/scenarios/cut_in_with_static_vehicle.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,271 @@
#!/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 <https://opensource.org/licenses/MIT>.

from __future__ import print_function

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,
BatchActorTransformSetter,
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, LeaveSpaceInFront, ReAddRoadLane, ChangeRoadBehavior


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
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.timeout = timeout

self._trigger_location = config.trigger_points[0].location
self._reference_waypoint = self._wmap.get_waypoint(self._trigger_location)

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
self._front_vehicles = 3
self._vehicle_gap = 11

self._speed = 60 # Km/h

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

self._attributes = {'base_type': 'car', 'has_lights': True}

self._blocker_distance = get_value_parameter(config, 'distance', float, 100)
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,
debug_mode,
criteria_enable=criteria_enable)

def _initialize_actors(self, config):
"""
Custom initialization
"""
# Spawn the blocker vehicle
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")
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))

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]

self._collision_wp = blocker_wp

# 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()
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)

# 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:
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="StaticCutIn")
if self.route_mode:
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(BatchActorTransformSetter(self._side_transforms))

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)
if self.route_mode:
sequence.add_child(ChangeRoadBehavior(extra_space=self._extra_space))

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, change_time=3, other_lane_time=2))
cut_in_movement.add_child(BasicAgentBehavior(
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)) # 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

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()
Loading

0 comments on commit d12d8bb

Please sign in to comment.