def _get_target_waypoint(self):
"""
Gets the first waypoint after the junction.
This method depends on the subtype of VehicleTurning scenario
"""
if self._subtype == 'right':
return generate_target_waypoint(self._reference_waypoint, 1)
elif self._subtype == 'left':
return generate_target_waypoint(self._reference_waypoint, -1)
elif self._subtype == 'route':
return generate_target_waypoint_in_route(self._reference_waypoint, self._ego_route)
else:
raise ValueError("Trying to run a VehicleTurning scenario with a wrong subtype")
def _create_behavior(self):
"""
Hero vehicle is turning left in an urban area,
at a signalized intersection, while other actor coming straight
.The hero actor may turn left either before other actor
passes intersection or later, without any collision.
After 80 seconds, a timeout stops the scenario.
"""
sequence = py_trees.composites.Sequence("Sequence Behavior")
# Selecting straight path at intersection
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), 0)
# Generating waypoint list till next intersection
plan = []
wp_choice = target_waypoint.next(1.0)
while not wp_choice[0].is_intersection:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(1.0)
# adding flow of actors
actor_source = ActorSource(['vehicle.tesla.model3', 'vehicle.audi.tt'],
self._other_actor_transform, 15,
self._blackboard_queue_name)
# destroying flow of actors
actor_sink = ActorSink(plan[-1][0].transform.location, 10)
# follow waypoints untill next intersection
move_actor = WaypointFollower(
self.other_actors[0],
self._target_vel,
plan=plan,
blackboard_queue_name=self._blackboard_queue_name,
avoid_collision=True)
# wait
wait = DriveDistance(self.ego_vehicles[0], self._ego_distance)
# Behavior tree
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(wait)
root.add_child(actor_source)
root.add_child(actor_sink)
root.add_child(move_actor)
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(root)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _initialize_actors(self, config):
"""
Custom initialization
"""
waypoint = self._reference_waypoint
direction = self.SUBTYPE_INDEX_TRANSLATION[config.subtype]
waypoint = generate_target_waypoint(waypoint, direction)
_start_distance = 8
while True:
wp_next = waypoint.get_right_lane()
self._num_lane_changes += 1
if wp_next is not None:
_start_distance += 1
waypoint = wp_next
if waypoint.lane_type == carla.LaneType.Shoulder or waypoint.lane_type == carla.LaneType.Sidewalk:
last_waypoint_lane = waypoint.lane_type
break
else:
last_waypoint_lane = waypoint.lane_type
break
while True:
try:
self._other_actor_transform = get_opponent_transform(
_start_distance, waypoint, self._trigger_location,
last_waypoint_lane)
first_vehicle = CarlaActorPool.request_new_actor(
'vehicle.diamondback.century', self._other_actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
break
except RuntimeError as r:
# In the case there is an object just move a little bit and retry
print("Base transform is blocking objects ",
self._other_actor_transform)
_start_distance += 0.2
self._spawn_attempted += 1
if self._spawn_attempted >= self._number_of_attempts:
raise r
# Set the transform to -500 z after we are able to spawn it
actor_transform = carla.Transform(
carla.Location(self._other_actor_transform.location.x,
self._other_actor_transform.location.y,
self._other_actor_transform.location.z - 500),
self._other_actor_transform.rotation)
first_vehicle.set_transform(actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
self.other_actors.append(first_vehicle)
def _initialize_actors(self, config):
"""
Custom initialization
"""
# Get the waypoint right after the junction
waypoint = generate_target_waypoint(self._reference_waypoint, -1)
# Move a certain distance to the front
start_distance = 8
waypoint = waypoint.next(start_distance)[0]
# Get the last driving lane to the right
waypoint, self._num_lane_changes = get_right_driving_lane(waypoint)
# And for synchrony purposes, move to the front a bit
added_dist = self._num_lane_changes
while True:
# Try to spawn the actor
try:
self._other_actor_transform = get_opponent_transform(
added_dist, waypoint, self._trigger_location)
first_vehicle = CarlaDataProvider.request_new_actor(
'vehicle.diamondback.century', self._other_actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
break
# Move the spawning point a bit and try again
except RuntimeError as r:
# In the case there is an object just move a little bit and retry
print(" Base transform is blocking objects ",
self._other_actor_transform)
added_dist += 0.5
self._spawn_attempted += 1
if self._spawn_attempted >= self._number_of_attempts:
raise r
# Set the transform to -500 z after we are able to spawn it
actor_transform = carla.Transform(
carla.Location(self._other_actor_transform.location.x,
self._other_actor_transform.location.y,
self._other_actor_transform.location.z - 500),
self._other_actor_transform.rotation)
first_vehicle.set_transform(actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
self.other_actors.append(first_vehicle)
def _initialize_actors(self, config):
"""
Custom initialization
"""
# Get the waypoint left after the junction
waypoint = generate_target_waypoint(self._reference_waypoint, 1)
while True:
# Try to spawn the actor
try:
self._other_actor_transform = waypoint.transform
first_vehicle = CarlaDataProvider.request_new_actor(
'vehicle.*', self._other_actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
break
# Move the spawning point a bit and try again
except RuntimeError as r:
# In the case there is an object just move a little bit and retry
waypoint = waypoint.next(1)[0]
self._spawn_attempted += 1
if self._spawn_attempted >= self._number_of_attempts:
raise r
# Set the transform to -500 z after we are able to spawn it
actor_transform = carla.Transform(
carla.Location(self._other_actor_transform.location.x,
self._other_actor_transform.location.y,
self._other_actor_transform.location.z - 500),
self._other_actor_transform.rotation)
first_vehicle.set_transform(actor_transform)
first_vehicle.set_simulate_physics(enabled=False)
self.other_actors.append(first_vehicle)
def get_route(
self,
spawn_location,
distance: float = 10.,
turning_flag=-1, # left -> -1, straight -> 0, right -> 1
resolution: float = 1.):
"""
Generate a route for the scenario.
Route is determined by class attribute turning_flag.
params:
distance: distance after the junction
resolution: distance gap between waypoint in route.
"""
waypoint_route = []
transform_route = []
location_route = []
spawn_waypoint = self.map.get_waypoint(
spawn_location,
project_to_road=True, # must set to True, center of lane
)
# start waypoint: beginning waypoint of the route,
start_waypoint = spawn_waypoint.next(3.0)[
0] # assuming that next waypoint is not in junction
# exit_waypoint: the first waypoint after leaving the junction
exit_waypoint = generate_target_waypoint(waypoint=start_waypoint,
turn=turning_flag)
# end_waypoint: end waypoint of the route
end_waypoint, _ = get_waypoint_in_distance(exit_waypoint, distance)
# list of carla.Location for route generation
raw_route = [
start_waypoint.transform.location,
exit_waypoint.transform.location,
end_waypoint.transform.location,
]
waypoint_route = interpolate_trajectory(world=self.world,
waypoints_trajectory=raw_route,
hop_resolution=resolution)
# get route of transform
for wp, road_option in waypoint_route:
transform_route.append((wp.transform, road_option))
# get route of location
for wp, road_option in waypoint_route:
location_route.append((wp.transform.location, road_option))
# ==================== visualization ====================
key_waypoints = [
start_waypoint,
exit_waypoint,
end_waypoint,
]
for wp in key_waypoints:
draw_waypoint(self.world, wp, color=(red, red))
if self.verbose:
# visualize complete route
for wp, _ in waypoint_route:
draw_waypoint(self.world, wp, color=(magenta, magenta))
return waypoint_route, location_route, transform_route
def _create_behavior(self):
"""
Hero vehicle is turning right in an urban area,
at a signalized intersection, while other actor coming straight
from left.The hero actor may turn right either before other actor
passes intersection or later, without any collision.
After 80 seconds, a timeout stops the scenario.
"""
location_of_collision_dynamic = get_geometric_linear_intersection(
self.ego_vehicles[0], self.other_actors[0])
crossing_point_dynamic = get_crossing_point(self.other_actors[0])
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
location_of_collision_dynamic)
sync_arrival_stop = InTriggerDistanceToLocation(
self.other_actors[0], crossing_point_dynamic, 5)
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
# Selecting straight path at intersection
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), 0)
# Generating waypoint list till next intersection
plan = []
wp_choice = target_waypoint.next(1.0)
while not wp_choice[0].is_intersection:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(1.0)
move_actor = WaypointFollower(self.other_actors[0],
self._target_vel,
plan=plan)
waypoint_follower_end = InTriggerDistanceToLocation(
self.other_actors[0], plan[-1][0].transform.location, 10)
move_actor_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
move_actor_parallel.add_child(move_actor)
move_actor_parallel.add_child(waypoint_follower_end)
#manipulate traffic light
manipulate_traffic_light = TrafficLightManipulator(
self.ego_vehicles[0], "S7right")
move_actor_parallel.add_child(manipulate_traffic_light)
# stop other actor
stop = StopVehicle(self.other_actors[0], self._brake_value)
# end condition
end_condition = DriveDistance(self.ego_vehicles[0], self._ego_distance)
# Behavior tree
sequence = py_trees.composites.Sequence()
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(sync_arrival_parallel)
sequence.add_child(move_actor_parallel)
sequence.add_child(stop)
sequence.add_child(end_condition)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _initialize_actors(self, config):
"""
Custom initialization
"""
ego_location = config.trigger_points[0].location
ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location)
# Get the junction
starting_wp = ego_wp
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]
junction = starting_wp.get_junction()
# Get the opposite entry lane wp
possible_directions = ['right', 'left']
self._rng.shuffle(possible_directions)
for direction in possible_directions:
entry_wps, _ = get_junction_topology(junction)
source_entry_wps = filter_junction_wp_direction(
starting_wp, entry_wps, direction)
if source_entry_wps:
self._direction = direction
break
if not self._direction:
raise ValueError(
"Trying to find a lane to spawn the opposite actor but none was found"
)
# Get the source transform
spawn_wp = source_entry_wps[0]
added_dist = 0
while added_dist < self._source_dist:
spawn_wps = spawn_wp.previous(1.0)
if len(spawn_wps) == 0:
raise ValueError(
"Failed to find a source location as a waypoint with no previous was detected"
)
if spawn_wps[0].is_junction:
break
spawn_wp = spawn_wps[0]
added_dist += 1
self._spawn_wp = spawn_wp
source_transform = spawn_wp.transform
self._spawn_location = carla.Transform(
source_transform.location + carla.Location(z=0.1),
source_transform.rotation)
# Spawn the actor and move it below ground
opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards)
opposite_actor = CarlaDataProvider.request_new_actor(
opposite_bp_wildcard, self._spawn_location)
if not opposite_actor:
raise Exception("Couldn't spawn the actor")
opposite_actor.set_light_state(
carla.VehicleLightState(carla.VehicleLightState.Special1
| carla.VehicleLightState.Special2))
self.other_actors.append(opposite_actor)
opposite_transform = carla.Transform(
source_transform.location - carla.Location(z=500),
source_transform.rotation)
opposite_actor.set_transform(opposite_transform)
opposite_actor.set_simulate_physics(enabled=False)
# Get the sink location
sink_exit_wp = generate_target_waypoint(
self._map.get_waypoint(source_transform.location), 0)
sink_wps = sink_exit_wp.next(self._sink_dist)
if len(sink_wps) == 0:
raise ValueError(
"Failed to find a sink location as a waypoint with no next was detected"
)
self._sink_wp = sink_wps[0]
# get the collision location
self._collision_location = get_geometric_linear_intersection(
starting_wp.transform.location,
source_entry_wps[0].transform.location)
if not self._collision_location:
raise ValueError("Couldn't find an intersection point")
# Get the relevant traffic lights
tls = self._world.get_traffic_lights_in_junction(junction.id)
ego_tl = get_closest_traffic_light(ego_wp, tls)
source_tl = get_closest_traffic_light(
self._spawn_wp,
tls,
)
self._tl_dict = {}
for tl in tls:
if tl in (ego_tl, source_tl):
self._tl_dict[tl] = carla.TrafficLightState.Green
else:
self._tl_dict[tl] = carla.TrafficLightState.Red
def _initialize_actors(self, config):
"""
Default initialization of other actors.
Override this method in child class to provide custom initialization.
"""
ego_location = config.trigger_points[0].location
ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location)
# Get the junction
starting_wp = ego_wp
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]
junction = starting_wp.get_junction()
# Get the opposite entry lane wp
entry_wps, _ = get_junction_topology(junction)
source_entry_wps = filter_junction_wp_direction(
starting_wp, entry_wps, self._direction)
if not source_entry_wps:
raise ValueError(
"Trying to find a lane in the {} direction but none was found".
format(self._direction))
# Get the source transform
source_entry_wp = self._rng.choice(source_entry_wps)
# Get the source transform
source_wp = source_entry_wp
accum_dist = 0
while accum_dist < self._source_dist:
source_wps = source_wp.previous(5)
if len(source_wps) == 0:
raise ValueError(
"Failed to find a source location as a waypoint with no previous was detected"
)
if source_wps[0].is_junction:
break
source_wp = source_wps[0]
accum_dist += 5
self._source_wp = source_wp
source_transform = self._source_wp.transform
# Get the sink location
sink_exit_wp = generate_target_waypoint(
self._map.get_waypoint(source_transform.location), 0)
sink_wps = sink_exit_wp.next(self._sink_dist)
if len(sink_wps) == 0:
raise ValueError(
"Failed to find a sink location as a waypoint with no next was detected"
)
self._sink_wp = sink_wps[0]
# get traffic lights
tls = self._world.get_traffic_lights_in_junction(junction.id)
ego_tl = get_closest_traffic_light(ego_wp, tls)
source_tl = get_closest_traffic_light(self._source_wp, tls)
self._flow_tl_dict = {}
self._init_tl_dict = {}
for tl in tls:
if tl == ego_tl:
self._flow_tl_dict[tl] = carla.TrafficLightState.Green
self._init_tl_dict[tl] = carla.TrafficLightState.Red
elif tl == source_tl:
self._flow_tl_dict[tl] = carla.TrafficLightState.Green
self._init_tl_dict[tl] = carla.TrafficLightState.Green
else:
self._flow_tl_dict[tl] = carla.TrafficLightState.Red
self._init_tl_dict[tl] = carla.TrafficLightState.Red
