-
Notifications
You must be signed in to change notification settings - Fork 371
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* 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>
1 parent
3522896
commit d12d8bb
Showing
67 changed files
with
43,113 additions
and
21,094 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file was deleted.
Oops, something went wrong.
This file was deleted.
Oops, something went wrong.
This file was deleted.
Oops, something went wrong.
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
Large diffs are not rendered by default.
Oops, something went wrong.
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
1,377 changes: 1,313 additions & 64 deletions
1,377
srunner/scenariomanager/scenarioatomics/atomic_behaviors.py
Large diffs are not rendered by default.
Oops, something went wrong.
1,197 changes: 674 additions & 523 deletions
1,197
srunner/scenariomanager/scenarioatomics/atomic_criteria.py
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Large diffs are not rendered by default.
Oops, something went wrong.
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
Oops, something went wrong.