def _check_town(self, world):
if CarlaDataProvider.get_map().name != self._town:
print("The CARLA server uses the wrong map!")
print("This scenario requires to use map {}".format(self._town))
raise Exception("The CARLA server uses the wrong map!")
def get_distance_along_route(route, target_location):
"""
Calculate the distance of the given location along the route
Note: If the location is not along the route, the route length will be returned
"""
wmap = CarlaDataProvider.get_map()
covered_distance = 0
prev_position = None
found = False
# Don't use the input location, use the corresponding wp as location
target_location_from_wp = wmap.get_waypoint(target_location).transform.location
for position, _ in route:
location = target_location_from_wp
# Don't perform any calculations for the first route point
if not prev_position:
prev_position = position
continue
# Calculate distance between previous and current route point
interval_length_squared = ((prev_position.x - position.x) ** 2) + ((prev_position.y - position.y) ** 2)
distance_squared = ((location.x - prev_position.x) ** 2) + ((location.y - prev_position.y) ** 2)
# Close to the current position? Stop calculation
if distance_squared < 0.01:
break
if distance_squared < 400 and not distance_squared < interval_length_squared:
# Check if a neighbor lane is closer to the route
# Do this only in a close distance to correct route interval, otherwise the computation load is too high
starting_wp = wmap.get_waypoint(location)
wp = starting_wp.get_left_lane()
while wp is not None:
new_location = wp.transform.location
new_distance_squared = ((new_location.x - prev_position.x) ** 2) + (
(new_location.y - prev_position.y) ** 2)
if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
break
if new_distance_squared < distance_squared:
distance_squared = new_distance_squared
location = new_location
else:
break
wp = wp.get_left_lane()
wp = starting_wp.get_right_lane()
while wp is not None:
new_location = wp.transform.location
new_distance_squared = ((new_location.x - prev_position.x) ** 2) + (
(new_location.y - prev_position.y) ** 2)
if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
break
if new_distance_squared < distance_squared:
distance_squared = new_distance_squared
location = new_location
else:
break
wp = wp.get_right_lane()
if distance_squared < interval_length_squared:
# The location could be inside the current route interval, if route/lane ids match
# Note: This assumes a sufficiently small route interval
# An alternative is to compare orientations, however, this also does not work for
# long route intervals
curr_wp = wmap.get_waypoint(position)
prev_wp = wmap.get_waypoint(prev_position)
wp = wmap.get_waypoint(location)
if prev_wp and curr_wp and wp:
if wp.road_id == prev_wp.road_id or wp.road_id == curr_wp.road_id:
# Roads match, now compare the sign of the lane ids
if (np.sign(wp.lane_id) == np.sign(prev_wp.lane_id) or
np.sign(wp.lane_id) == np.sign(curr_wp.lane_id)):
# The location is within the current route interval
covered_distance += math.sqrt(distance_squared)
found = True
break
covered_distance += math.sqrt(interval_length_squared)
prev_position = position
return covered_distance, found
def convert_position_to_transform(position, actor_list=None):
"""
Convert an OpenScenario position into a CARLA transform
Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
does not provide sufficient access to OpenDrive information
Also not supported is Route. This can be added by checking additional
route information
"""
if position.find('World') is not None:
world_pos = position.find('World')
x = float(world_pos.attrib.get('x', 0))
y = float(world_pos.attrib.get('y', 0))
z = float(world_pos.attrib.get('z', 0))
yaw = math.degrees(float(world_pos.attrib.get('h', 0)))
pitch = math.degrees(float(world_pos.attrib.get('p', 0)))
roll = math.degrees(float(world_pos.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
y = y * (-1.0)
yaw = yaw * (-1.0)
return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif ((position.find('RelativeWorld') is not None) or (position.find('RelativeObject') is not None) or
(position.find('RelativeLane') is not None)):
rel_pos = position.find('RelativeWorld') or position.find('RelativeObject') or position.find('RelativeLane')
# get relative object and relative position
obj = rel_pos.attrib.get('object')
obj_actor = None
actor_transform = None
if actor_list is not None:
for actor in actor_list:
if actor.rolename == obj:
obj_actor = actor
actor_transform = actor.transform
else:
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes['role_name'] == obj:
obj_actor = actor
actor_transform = obj_actor.get_transform()
break
if obj_actor is None:
raise AttributeError("Object '{}' provided as position reference is not known".format(obj))
# calculate orientation h, p, r
is_absolute = False
if rel_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
is_absolute = (orientation.attrib.get('type') == "absolute")
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
yaw = actor_transform.rotation.yaw
pitch = actor_transform.rotation.pitch
roll = actor_transform.rotation.roll
if not is_absolute:
yaw = yaw + dyaw
pitch = pitch + dpitch
roll = roll + droll
else:
yaw = dyaw
pitch = dpitch
roll = droll
# calculate location x, y, z
# dx, dy, dz
if (position.find('RelativeWorld') is not None) or (position.find('RelativeObject') is not None):
dx = float(rel_pos.attrib.get('dx', 0))
dy = float(rel_pos.attrib.get('dy', 0))
dz = float(rel_pos.attrib.get('dz', 0))
if not OpenScenarioParser.use_carla_coordinate_system:
dy = dy * (-1.0)
x = actor_transform.location.x + dx
y = actor_transform.location.y + dy
z = actor_transform.location.z + dz
# dLane, ds, offset
elif position.find('RelativeLane') is not None:
dlane = float(rel_pos.attrib.get('dLane'))
ds = float(rel_pos.attrib.get('ds'))
offset = float(rel_pos.attrib.get('offset'))
carla_map = CarlaDataProvider.get_map()
relative_waypoint = carla_map.get_waypoint(actor_transform.location)
if dlane == 0:
wp = relative_waypoint
elif dlane == -1:
wp = relative_waypoint.get_left_lane()
elif dlane == 1:
wp = relative_waypoint.get_right_lane()
if wp is None:
raise AttributeError("Object '{}' position with dLane={} is not valid".format(obj, dlane))
wp = wp.next(ds)[-1]
# Adapt transform according to offset
h = math.radians(wp.transform.rotation.yaw)
x_offset = math.sin(h) * offset
y_offset = math.cos(h) * offset
if OpenScenarioParser.use_carla_coordinate_system:
x_offset = x_offset * (-1.0)
y_offset = y_offset * (-1.0)
x = wp.transform.location.x + x_offset
y = wp.transform.location.y + y_offset
z = wp.transform.location.z
return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
# Not implemented
elif position.find('Road') is not None:
raise NotImplementedError("Road positions are not yet supported")
elif position.find('RelativeRoad') is not None:
raise NotImplementedError("RelativeRoad positions are not yet supported")
elif position.find('Lane') is not None:
lane_pos = position.find('Lane')
road_id = int(lane_pos.attrib.get('roadId', 0))
lane_id = int(lane_pos.attrib.get('laneId', 0))
offset = float(lane_pos.attrib.get('offset', 0))
s = float(lane_pos.attrib.get('s', 0))
is_absolute = True
waypoint = CarlaDataProvider.get_map().get_waypoint_xodr(road_id, lane_id, s)
if waypoint is None:
raise AttributeError("Lane position cannot be found")
transform = waypoint.transform
if lane_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
transform.rotation.yaw = transform.rotation.yaw + dyaw
transform.rotation.pitch = transform.rotation.pitch + dpitch
transform.rotation.roll = transform.rotation.roll + droll
if offset != 0:
forward_vector = transform.rotation.get_forward_vector()
orthogonal_vector = carla.Vector3D(x=-forward_vector.y, y=forward_vector.x, z=forward_vector.z)
transform.location.x = transform.location.x + offset * orthogonal_vector.x
transform.location.y = transform.location.y + offset * orthogonal_vector.y
return transform
elif position.find('Route') is not None:
raise NotImplementedError("Route positions are not yet supported")
else:
raise AttributeError("Unknown position")
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"],
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)
# actor_source2 = ActorSource(['vehicle.audi.tt'],
# self._other_actor_transform2, 10,
# self._blackboard_queue_name2)
# # destroying flow of actors
# actor_sink2 = ActorSink(carla.Location(x=-74.9, y=30), 20)
# # follow waypoints untill next intersection
# move_actor2 = WaypointFollower(
# self.other_actors[1],
# self._target_vel,
# blackboard_queue_name=self._blackboard_queue_name2,
# avoid_collision=True)
distance_to_vehicle = InTriggerDistanceToVehicle(
self.other_actors[1], self.ego_vehicles[0], 20)
actor1_drive = KeepVelocity(self.other_actors[1], 8, distance=90)
# Behavior tree
root1 = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# root1.add_child(wait)
root1.add_child(actor_source)
root1.add_child(actor_sink)
root1.add_child(move_actor)
# root2 = py_trees.composites.Parallel(
# policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root2 = py_trees.composites.Sequence(
"Sequence_dist",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
# root2.add_child(actor_source2)
# root2.add_child(actor_sink2)
# root2.add_child(move_actor2)
root2.add_child(distance_to_vehicle)
root2.add_child(actor1_drive)
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(root1)
root.add_child(root2)
root.add_child(wait)
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(root)
sequence.add_child(ActorDestroy(self.other_actors[0]))
sequence.add_child(ActorDestroy(self.other_actors[1]))
return sequence
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed = math.sqrt(self._actor.get_velocity().x**2 +
self._actor.get_velocity().y**2)
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 +
self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (
current_speed - current_speed_other)**2 / distance
target_speed = max(
acceleration * (current_time - self._last_update) +
current_speed, 0)
else:
target_speed = 0
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
new_yaw = CarlaDataProvider.get_map().get_waypoint(
next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
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)
# 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 __call__(self):
return {'opendrive': CarlaDataProvider.get_map().to_opendrive()}
def _load_and_wait_for_world(self, town, ego_vehicles=None):
"""
Load a new CARLA world and provide data to CarlaDataProvider
"""
if self._args.reloadWorld:
self.world = self.client.load_world(town)
else:
# if the world should not be reloaded, wait at least until all ego vehicles are ready
ego_vehicle_found = False
if self._args.waitForEgo:
while not ego_vehicle_found and not self._shutdown_requested:
vehicles = self.client.get_world().get_actors().filter(
'vehicle.*')
for ego_vehicle in ego_vehicles:
ego_vehicle_found = False
for vehicle in vehicles:
if vehicle.attributes[
'role_name'] == ego_vehicle.rolename:
ego_vehicle_found = True
break
if not ego_vehicle_found:
print("Not all ego vehicles ready. Waiting ... ")
time.sleep(1)
break
self.world = self.client.get_world()
for speed_sign in self.world.get_actors().filter(
'traffic.speed_limit.*'):
if speed_sign.type_id == 'traffic.speed_limit.30':
speed_sign.destroy()
if self._args.sync:
settings = self.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 1.0 / self.frame_rate
self.world.apply_settings(settings)
self.traffic_manager.set_synchronous_mode(True)
self.traffic_manager.set_random_device_seed(
int(self._args.trafficManagerSeed))
CarlaDataProvider.set_client(self.client)
CarlaDataProvider.set_world(self.world)
CarlaDataProvider.set_traffic_manager_port(
int(self._args.trafficManagerPort))
# Wait for the world to be ready
if CarlaDataProvider.is_sync_mode():
self.world.tick()
else:
self.world.wait_for_tick()
if CarlaDataProvider.get_map(
).name != town and CarlaDataProvider.get_map().name != "OpenDriveMap":
print("The CARLA server uses the wrong map: {}".format(
CarlaDataProvider.get_map().name))
print("This scenario requires to use map: {}".format(town))
return False
return True
def convert_maneuver_to_atomic(action, actor):
"""
Convert an OpenSCENARIO maneuver action into a Behavior atomic
Note not all OpenSCENARIO actions are currently supported
"""
maneuver_name = action.attrib.get('name', 'unknown')
if action.find('Global') is not None:
global_action = action.find('Global')
if global_action.find('Infrastructure') is not None:
infrastructure_action = global_action.find(
'Infrastructure').find('Signal')
if infrastructure_action.find('SetState') is not None:
traffic_light_action = infrastructure_action.find(
'SetState')
traffic_light_id = traffic_light_action.attrib.get('name')
traffic_light_state = traffic_light_action.attrib.get(
'state')
atomic = TrafficLightStateSetter(traffic_light_id,
traffic_light_state)
else:
raise NotImplementedError(
"TrafficLights can only be influenced via SetState")
else:
raise NotImplementedError(
"Global actions are not yet supported")
elif action.find('UserDefined') is not None:
user_defined_action = action.find('UserDefined')
if user_defined_action.find('Command') is not None:
command = user_defined_action.find('Command').text.replace(
" ", "")
if command == 'Idle':
atomic = Idle()
else:
raise AttributeError(
"Unknown user command action: {}".format(command))
else:
raise NotImplementedError(
"UserDefined script actions are not yet supported")
elif action.find('Private') is not None:
private_action = action.find('Private')
if private_action.find('Longitudinal') is not None:
private_action = private_action.find('Longitudinal')
if private_action.find('Speed') is not None:
long_maneuver = private_action.find('Speed')
# duration and distance
duration = float(
long_maneuver.find("Dynamics").attrib.get(
'time', float("inf")))
distance = float(
long_maneuver.find("Dynamics").attrib.get(
'distance', float("inf")))
# absolute velocity with given target speed
if long_maneuver.find("Target").find(
"Absolute") is not None:
target_speed = float(
long_maneuver.find("Target").find(
"Absolute").attrib.get('value', 0))
atomic = KeepVelocity(actor,
target_speed,
distance=distance,
duration=duration,
name=maneuver_name)
# relative velocity to given actor
if long_maneuver.find("Target").find(
"Relative") is not None:
relative_speed = long_maneuver.find("Target").find(
"Relative")
obj = relative_speed.attrib.get('object')
value = float(relative_speed.attrib.get('value', 0))
value_type = relative_speed.attrib.get('valueType')
continuous = relative_speed.attrib.get('continuous')
for traffic_actor in CarlaDataProvider.get_world(
).get_actors():
if 'role_name' in traffic_actor.attributes and traffic_actor.attributes[
'role_name'] == obj:
obj_actor = traffic_actor
atomic = SetRelativeOSCVelocity(
actor, obj_actor, value, value_type, continuous,
duration, distance)
elif private_action.find('Distance') is not None:
raise NotImplementedError(
"Longitudinal distance actions are not yet supported")
else:
raise AttributeError("Unknown longitudinal action")
elif private_action.find('Lateral') is not None:
private_action = private_action.find('Lateral')
if private_action.find('LaneChange') is not None:
lat_maneuver = private_action.find('LaneChange')
target_lane_rel = float(
lat_maneuver.find("Target").find(
"Relative").attrib.get('value', 0))
distance = float(
lat_maneuver.find("Dynamics").attrib.get(
'distance', float("inf")))
atomic = LaneChange(
actor,
None,
direction="left" if target_lane_rel < 0 else "right",
distance_lane_change=distance,
name=maneuver_name)
else:
raise AttributeError("Unknown lateral action")
elif private_action.find('Visibility') is not None:
raise NotImplementedError(
"Visibility actions are not yet supported")
elif private_action.find('Meeting') is not None:
raise NotImplementedError(
"Meeting actions are not yet supported")
elif private_action.find('Autonomous') is not None:
private_action = private_action.find('Autonomous')
activate = strtobool(private_action.attrib.get('activate'))
atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
elif private_action.find('Controller') is not None:
raise NotImplementedError(
"Controller actions are not yet supported")
elif private_action.find('Position') is not None:
position = private_action.find('Position')
atomic = ActorTransformSetterToOSCPosition(actor,
position,
name=maneuver_name)
elif private_action.find('Routing') is not None:
target_speed = 5.0
private_action = private_action.find('Routing')
if private_action.find('FollowRoute') is not None:
private_action = private_action.find('FollowRoute')
if private_action.find('Route') is not None:
route = private_action.find('Route')
plan = []
if route.find('ParameterDeclaration') is not None:
if route.find('ParameterDeclaration').find(
'Parameter') is not None:
parameter = route.find(
'ParameterDeclaration').find('Parameter')
if parameter.attrib.get('name') == "Speed":
target_speed = float(
parameter.attrib.get('value', 5.0))
for waypoint in route.iter('Waypoint'):
position = waypoint.find('Position')
transform = OpenScenarioParser.convert_position_to_transform(
position)
waypoint = CarlaDataProvider.get_map(
).get_waypoint(transform.location)
plan.append((waypoint, RoadOption.LANEFOLLOW))
atomic = WaypointFollower(actor,
target_speed=target_speed,
plan=plan,
name=maneuver_name)
elif private_action.find('CatalogReference') is not None:
raise NotImplementedError(
"CatalogReference private actions are not yet supported"
)
else:
raise AttributeError(
"Unknown private FollowRoute action")
elif private_action.find('FollowTrajectory') is not None:
raise NotImplementedError(
"Private FollowTrajectory actions are not yet supported"
)
elif private_action.find('AcquirePosition') is not None:
raise NotImplementedError(
"Private AcquirePosition actions are not yet supported"
)
else:
raise AttributeError("Unknown private routing action")
else:
raise AttributeError("Unknown private action")
else:
raise AttributeError("Unknown action")
return atomic
def __init__(self,
world,
ego_vehicles,
config,
test_num,
rep,
randomize=False,
debug_mode=False,
criteria_enable=True,
timeout=80):
self._map = CarlaDataProvider.get_map()
self._world = world
# specify test run number
self._test = int(test_num)
self._rep = int(rep)
# location of the ego vehicle
self._reference_waypoint = self._map.get_waypoint(
config.trigger_points[0].location)
self._other_actor_max_brake = 1.0
self._other_actor_stop_in_front_intersection = 10
self._other_actor_transform = []
self.timeout = timeout
# Timeout of scenario in seconds
self._num_scenarios = 3 #random.randint(1, 5)
self.scenario_list = []
# return all possible scenarios and trigger points
potential_scenarios = list_potential_scenarios()
potential_triggers = list_potential_triggers(self._map)
# if self._test >= 0 and self._test <= 4:
# if self._rep != 0:
# print("\nrunning repeated scenarios\n")#{}\n".format(str(self._test)))
# self.selected_scenarios = self.select_scenarios(potential_scenarios, self._test)
# self.selected_triggers = self.select_triggers(potential_triggers, self._test)
# else:
# i = 0
# while i < int(generate):
if self._rep == 0:
# i += 1
# print("\nrandomly generating scenario {}\n".format(i))
# randomly select specified number of scenarios and their trigger points
self.selected_scenarios = self.select_scenarios_random(
potential_scenarios, self._num_scenarios)
self.selected_triggers = random.sample(potential_triggers,
self._num_scenarios)
for i in range(len(self.selected_scenarios)):
trigger = self.selected_triggers[i]
scenario = self.selected_scenarios[i]
print(scenario + " | ")
print(str(trigger) + " | ")
print("\n")
if scenario == "VehiclesAhead":
actor_dict = LeadVehicleDict(trigger, self._map, None)
# elif scenario == "StationaryObstaclesAhead":
# actor_dict = StationaryObstaclesDict(trigger, self._map)
elif scenario == "CutIn":
actor_dict = CutInDict(trigger, self._map, None)
elif scenario == "DynamicObstaclesAhead":
off_highway_1 = self._map.get_waypoint(
carla.Location(203, -341, 3))
if (trigger != off_highway_1):
trigger = off_highway_1
actor_dict = DynamicObstaclesDict(trigger, self._map, None)
else:
print("actor dict class not implemented")
exit(1)
if not actor_dict.failed:
self.scenario_list.append((scenario, actor_dict))
# for trigger in self.selected_triggers:
# print(str(trigger) + " | ")
# print("\n")
# self.actor_list = dict()
# if randomize:
# self._first_vehicle_location = random.randint(20, 150)
data = "\n"
for ind in range(len(self.scenario_list)):
(scenario, actor_dict) = self.scenario_list[ind]
# scenario = self.selected_scenarios[ind]
# trigger = self.selected_triggers[ind]
x = actor_dict.start_transform.location.x
y = actor_dict.start_transform.location.y
z = actor_dict.start_transform.location.z
transform_str = "{}|{}|{}".format(x, y, z)
x = actor_dict.trigger.transform.location.x
y = actor_dict.trigger.transform.location.y
z = actor_dict.trigger.transform.location.z
trigger_str = "{}|{}|{}".format(x, y, z)
#test.rep.scenario
test_info = "{}.{}.{}".format(self._test, self._rep, ind)
if scenario == "VehiclesAhead":
data += "{},VehiclesAhead,{},{},{},{},{},{},{},".format(
test_info,
#actor_dict.start_transform.location,
#actor_dict.trigger.transform.location,
transform_str,
trigger_str,
actor_dict.start_dist,
actor_dict.speed,
actor_dict.stop,
"",
"")
elif scenario == "DynamicObstaclesAhead":
# self._initialize_dynamic_obstacles_ahead()
data += "{},DynamicObstaclesAhead,{},{},{},{},{},{},{},".format(
test_info,
#actor_dict.start_transform.location,
#actor_dict.trigger.transform.location,
transform_str,
trigger_str,
actor_dict.start_dist,
actor_dict.speed,
actor_dict.time_to_reach,
"",
"")
else: #CutIn
data += "{},CutIn,{},{},{},{},{},{},{},".format(
test_info,
#actor_dict.start_transform.location,
#actor_dict.trigger.transform.location,
transform_str,
trigger_str,
actor_dict.start_dist,
actor_dict.speed,
actor_dict.delta_speed,
actor_dict.trigger_distance,
actor_dict.direction)
# 20% chance that there is extreme weather
# extreme_weather = (random.randint(0, 5) < 1)
extreme_weather = False
if extreme_weather:
data += "True,"
else:
data += "False,"
# data += "\n"
# f = open('test.txt', 'a')
# f.write(data)
f = open('test_config.txt', 'w')
f.write(data)
f.close()
# agent = ""
# while not agent:
# f = open('test_agent.txt','r')
# agent = f.read()
# f.close()
# f = open('test_agent.txt','w')
# f.write("")
# f.close()
f = open('test_log.txt', 'a')
f.write(data)
f.close()
# if int(generate) < 0: # retrieve test config from test_config.txt
else:
f = open('test_config.txt', 'r')
config_info = f.read()
f.close()
# print("test {}".format(test_num))
# config_lines = file_content.split("\n")
# config_info = config_lines[test_num]
config_list = config_info.split(",")
# agent = ""
# while not agent:
# f = open('test_agent.txt','r')
# agent = f.read()
# f.close()
f = open('test_log.txt', 'a')
for i in range(self._num_scenarios):
test_info = config_list[0 + i * 9].split('.')
test = test_info[0]
rep = int(test_info[1])
scenario = test_info[2]
rep += 1 # rep += 1
test_info[1] = str(rep)
test_info_str = ".".join(test_info)
config_list[0 + i * 9] = test_info_str
config_info = ",".join(config_list)
f.write(config_info)
f.close()
# update scenario config file
f = open('test_config.txt', 'w')
f.write(config_info)
f.close()
for i in range(self._num_scenarios):
scenario_name = config_list[1 + i * 9]
# start_transform = config_list[3]
trigger = config_list[3 + i * 9].split(
"|") #NEEDS TO BE PARSED --> x.y.z
x = float(trigger[0])
y = float(trigger[1])
z = float(trigger[2])
trigger_waypoint = self._map.get_waypoint(
carla.Location(x, y, z))
print(scenario_name + " | ")
print(str(trigger_waypoint) + " | ")
print("\n")
start_dist = int(config_list[4 + i * 9])
speed = int(config_list[5 + i * 9])
if scenario_name == "VehiclesAhead":
parsed_dict = dict()
parsed_dict["start_dist"] = start_dist
parsed_dict["trigger"] = trigger_waypoint
parsed_dict["speed"] = speed
if config_list[6 + i * 9] == "True":
parsed_dict["stop"] = True
else:
parsed_dict["stop"] = False
actor_dict = LeadVehicleDict(None, self._map, parsed_dict)
elif scenario_name == "DynamicObstaclesAhead":
time_to_reach = config_list[6 + i * 9]
parsed_dict = dict()
parsed_dict["start_dist"] = start_dist
parsed_dict["trigger"] = trigger_waypoint
parsed_dict["speed"] = speed
parsed_dict["time_to_reach"] = int(time_to_reach)
actor_dict = DynamicObstaclesDict(None, self._map,
parsed_dict)
elif scenario_name == "CutIn":
delta_speed = int(config_list[6 + i * 9])
trigger_dist = int(config_list[7 + i * 9])
direction = config_list[8 + i * 9]
parsed_dict = dict()
parsed_dict["start_dist"] = start_dist
parsed_dict["trigger"] = trigger_waypoint
parsed_dict["speed"] = speed
parsed_dict["delta_speed"] = delta_speed
parsed_dict["trigger_distance"] = trigger_dist
parsed_dict["direction"] = direction
actor_dict = CutInDict(None, self._map, parsed_dict)
else:
print("ERROR: Invalid scenario name")
raise RuntimeError
self.scenario_list.append((scenario_name, actor_dict))
#PARSE CONFIG HERE
f = open('test.log', 'a')
# log test configurations
# row = []
# for ind in range(len(self.selected_scenarios)):
# scenario = self.selected_scenarios[ind]
# trigger = self.selected_triggers[ind]
# row.append(scenario + " at: \n" + str(trigger.transform.location))
# scenario_headers = [
# "Scenario One", "Scenario Two", "Scenario Three"
# ]
f.write(
"\n\n\n\n##########################################################\n"
+ "\nTest Number: {}\nRep: {}\n\n".format(test_num, rep) +
"##########################################################\n")
# f.write(tabulate([row], headers=scenario_headers))
# f.write("\n-----------------------------------------------------------------------------------------------------------------------------------------")
# f.write("\n<----- Test Configurations ----->")
for (scenario, actor_dict) in self.scenario_list:
log_header = [scenario]
row = actor_dict._print_config()
f.write(tabulate([row]))
f.write("\n")
f.close()
super(RandomTest, self).__init__("RandomTest",
ego_vehicles,
config,
world,
debug_mode,
criteria_enable=criteria_enable)
def _create_behavior(self):
"""
Scenario behavior:
The other vehicle waits until the ego vehicle is close enough to the
intersection and that its own traffic light is red. Then, it will start
driving and 'illegally' cross the intersection. After a short distance
it should stop again, outside of the intersection. The ego vehicle has
to avoid the crash, but continue driving after the intersection is clear.
If this does not happen within 120 seconds, a timeout stops the scenario
"""
crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0])
# start condition
startcondition = InTriggerDistanceToLocation(
self.ego_vehicles[0],
crossing_point_dynamic,
self._ego_distance_to_traffic_light,
name="Waiting for start position")
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
location_of_collision_dynamic = get_geometric_linear_intersection(
self.ego_vehicles[0], self.other_actors[0])
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
location_of_collision_dynamic)
sync_arrival_stop = InTriggerDistanceToNextIntersection(
self.other_actors[0], 5)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
# Generate plan for WaypointFollower
turn = 0 # drive straight ahead
plan = []
# generating waypoints until intersection (target_waypoint)
plan, target_waypoint = generate_target_waypoint_list(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), turn)
# Generating waypoint list till next intersection
wp_choice = target_waypoint.next(5.0)
while len(wp_choice) == 1:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(5.0)
continue_driving = py_trees.composites.Parallel(
"ContinueDriving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
continue_driving_waypoints = WaypointFollower(
self.other_actors[0],
self._other_actor_target_velocity,
plan=plan,
avoid_collision=False)
continue_driving_distance = DriveDistance(self.other_actors[0],
self._other_actor_distance,
name="Distance")
continue_driving_timeout = TimeOut(10)
continue_driving.add_child(continue_driving_waypoints)
continue_driving.add_child(continue_driving_distance)
continue_driving.add_child(continue_driving_timeout)
# finally wait that ego vehicle drove a specific distance
wait = DriveDistance(self.ego_vehicles[0],
self._ego_distance_to_drive,
name="DriveDistance")
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(startcondition)
sequence.add_child(sync_arrival_parallel)
sequence.add_child(continue_driving)
sequence.add_child(wait)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
"""
if self._cv_image is not None:
cv2.imshow("", self._cv_image)
cv2.waitKey(1)
if self._reached_goal:
# Reached the goal, so stop
velocity = carla.Vector3D(0, 0, 0)
self._actor.set_target_velocity(velocity)
return
self._reached_goal = False
if not self._waypoints:
# No waypoints are provided, so we have to create a list of waypoints internally
# get next waypoints from map, to avoid leaving the road
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(
CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(
self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wps = map_wp.next(3.0)
if map_wps:
self._generated_waypoint_list.append(map_wps[0].transform)
map_wp = map_wps[0]
else:
break
direction_norm = self._set_new_velocity(
self._generated_waypoint_list[0].location)
if direction_norm < 2.0:
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
else:
# When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
# it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
# reasonable control command. Therefore, we drop these waypoints first.
while self._waypoints and self._waypoints[0].location.distance(
self._actor.get_location()) < 0.5:
self._waypoints = self._waypoints[1:]
self._reached_goal = False
direction_norm = self._set_new_velocity(
self._waypoints[0].location)
if direction_norm < 4.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
If _consider_trafficlights is true, the vehicle will enforce a stop at a red
traffic light.
If _max_deceleration is set, the vehicle will reduce its speed according to the
given deceleration value.
If the vehicle reduces its speed, braking lights will be activated.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2)
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (current_speed - current_speed_other)**2 / distance
target_speed = max(acceleration * (current_time - self._last_update) + current_speed, 0)
else:
target_speed = 0
if self._consider_traffic_lights:
if (self._actor.is_at_traffic_light() and
self._actor.get_traffic_light_state() == carla.TrafficLightState.Red):
target_speed = 0
if target_speed < current_speed:
if not self._brake_lights_active:
self._brake_lights_active = True
light_state = self._actor.get_light_state()
light_state |= carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
if self._max_deceleration is not None:
target_speed = max(target_speed, current_speed - (current_time -
self._last_update) * self._max_deceleration)
else:
if self._brake_lights_active:
self._brake_lights_active = False
light_state = self._actor.get_light_state()
light_state &= ~carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
if self._max_acceleration is not None:
tmp_speed = min(target_speed, current_speed + (current_time -
self._last_update) * self._max_acceleration)
# If the tmp_speed is < 0.5 the vehicle may not properly accelerate.
# Therefore, we bump the speed to 0.5 m/s if target_speed allows.
target_speed = max(tmp_speed, min(0.5, target_speed))
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_target_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
# When we have a waypoint list, use the direction between the waypoints to calculate the heading (change)
# otherwise use the waypoint heading directly
if self._waypoints:
delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw
else:
new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_target_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
def run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
For further details see :func:`_set_new_velocity`
"""
if self._reached_goal:
# Reached the goal, so stop
velocity = carla.Vector3D(0, 0, 0)
self._actor.set_target_velocity(velocity)
return
if self._visualizer:
self._visualizer.render()
self._reached_goal = False
if not self._waypoints:
# No waypoints are provided, so we have to create a list of waypoints internally
# get next waypoints from map, to avoid leaving the road
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wps = map_wp.next(2.0)
if map_wps:
self._generated_waypoint_list.append(map_wps[0].transform)
map_wp = map_wps[0]
else:
break
# Remove all waypoints that are too close to the vehicle
while (self._generated_waypoint_list and
self._generated_waypoint_list[0].location.distance(self._actor.get_location()) < 0.5):
self._generated_waypoint_list = self._generated_waypoint_list[1:]
direction_norm = self._set_new_velocity(self._offset_waypoint(self._generated_waypoint_list[0]))
if direction_norm < 2.0:
self._generated_waypoint_list = self._generated_waypoint_list[1:]
else:
# When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
# it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
# reasonable control command. Therefore, we drop these waypoints first.
while self._waypoints and self._waypoints[0].location.distance(self._actor.get_location()) < 0.5:
self._waypoints = self._waypoints[1:]
self._reached_goal = False
if not self._waypoints:
self._reached_goal = True
else:
direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0]))
if direction_norm < 4.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
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
