def choose_at_junction(current_waypoint, next_choices, direction=0):
"""
This function chooses the appropriate waypoint from next_choices based on direction
"""
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
current_vector = vector(current_location, projected_location)
cross_list = []
cross_to_waypoint = dict()
for waypoint in next_choices:
waypoint = waypoint.next(10)[0]
select_vector = vector(current_location, waypoint.transform.location)
cross = np.cross(current_vector, select_vector)[2]
cross_list.append(cross)
cross_to_waypoint[cross] = waypoint
select_cross = None
if direction > 0:
select_cross = max(cross_list)
elif direction < 0:
select_cross = min(cross_list)
else:
select_cross = min(cross_list, key=abs)
return cross_to_waypoint[select_cross]
def get_intersection(ego_actor, other_actor):
"""
Obtain a intersection point between two actor's location
@return the intersection location
"""
waypoint = CarlaDataProvider.get_map().get_waypoint(
ego_actor.get_location())
waypoint_other = CarlaDataProvider.get_map().get_waypoint(
other_actor.get_location())
max_dist = float("inf")
distance = float("inf")
while distance <= max_dist:
max_dist = distance
current_location = waypoint.transform.location
waypoint_choice = waypoint.next(1)
# Select the straighter path at intersection
if len(waypoint_choice) > 1:
max_dot = -1 * float('inf')
loc_projection = current_location + carla.Location(
x=math.cos(math.radians(waypoint.transform.rotation.yaw)),
y=math.sin(math.radians(waypoint.transform.rotation.yaw)))
v_current = vector(current_location, loc_projection)
for wp_select in waypoint_choice:
v_select = vector(current_location,
wp_select.transform.location)
dot_select = np.dot(v_current, v_select)
if dot_select > max_dot:
max_dot = dot_select
waypoint = wp_select
else:
waypoint = waypoint_choice[0]
distance = current_location.distance(waypoint_other.transform.location)
return current_location
def generate_target_waypoint_list(waypoint, turn=0):
"""
This method follow waypoints to a junction and choose path based on turn input.
Turn input: LEFT -> -1, RIGHT -> 1, STRAIGHT -> 0
@returns a waypoint list from the starting point to the end point according to turn input
"""
reached_junction = False
threshold = math.radians(0.1)
plan = []
while True:
wp_choice = waypoint.next(2)
if len(wp_choice) > 1:
reached_junction = True
waypoint = choose_at_junction(waypoint, wp_choice, turn)
else:
waypoint = wp_choice[0]
plan.append((waypoint, RoadOption.LANEFOLLOW))
# End condition for the behavior
if turn != 0 and reached_junction and len(plan) >= 3:
v_1 = vector(plan[-2][0].transform.location,
plan[-1][0].transform.location)
v_2 = vector(plan[-3][0].transform.location,
plan[-2][0].transform.location)
angle_wp = math.acos(
np.dot(v_1, v_2) / abs(
(np.linalg.norm(v_1) * np.linalg.norm(v_2))))
if angle_wp < threshold:
break
elif reached_junction and not plan[-1][0].is_intersection:
break
return plan, plan[-1][0]
def angle_between_transforms(location1, location2, location3):
v_1 = vector(location1, location2)
v_2 = vector(location2, location3)
vec_dots = np.dot(v_1, v_2)
cos_wp = vec_dots / abs((np.linalg.norm(v_1) * np.linalg.norm(v_2)))
angle_wp = math.acos(
min(1.0, cos_wp)
) # COS can't be larger than 1, it can happen due to float imprecision
return angle_wp
def _build_graph(self):
"""
This function builds a networkx graph representation of topology.
The topology is read from self._topology.
graph node properties:
vertex - (x,y,z) position in world map
graph edge properties:
entry_vector - unit vector along tangent at entry point
exit_vector - unit vector along tangent at exit point
net_vector - unit vector of the chord from entry to exit
intersection - boolean indicating if the edge belongs to an
intersection
return : graph -> networkx graph representing the world map,
id_map-> mapping from (x,y,z) to node id
road_id_to_edge-> map from road id to edge in the graph
"""
graph = nx.DiGraph()
id_map = dict() # Map with structure {(x,y,z): id, ... }
road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... }
for segment in self._topology:
entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz']
path = segment['path']
entry_wp, exit_wp = segment['entry'], segment['exit']
intersection = entry_wp.is_intersection
road_id, lane_id = entry_wp.road_id, entry_wp.lane_id
for vertex in entry_xyz, exit_xyz:
# Adding unique nodes and populating id_map
if vertex not in id_map:
new_id = len(id_map)
id_map[vertex] = new_id
graph.add_node(new_id, vertex=vertex)
n1 = id_map[entry_xyz]
n2 = id_map[exit_xyz]
if road_id not in road_id_to_edge:
road_id_to_edge[road_id] = dict()
road_id_to_edge[road_id][lane_id] = (n1, n2)
# Adding edge with attributes
graph.add_edge(
n1, n2,
length=len(path) + 1, path=path,
entry_waypoint=entry_wp, exit_waypoint=exit_wp,
entry_vector=vector(
entry_wp.transform.location,
path[0].transform.location if len(path) > 0 else exit_wp.transform.location),
exit_vector=vector(
path[-1].transform.location if len(path) > 0 else entry_wp.transform.location,
exit_wp.transform.location),
net_vector=vector(entry_wp.transform.location, exit_wp.transform.location),
intersection=intersection, type=RoadOption.LANEFOLLOW)
return graph, id_map, road_id_to_edge
def check_behind(t0, t1, t2):
unit_vec = NavigationSystem.get_loc(
misc.vector(t0.location, t1.location))
r_vec = NavigationSystem.get_loc(misc.vector(t1.location, t2.location))
dot = r_vec.x * unit_vec.x + r_vec.y * unit_vec.y
angle = math.degrees(np.arccos(dot))
# print(angle)
if dot < 0:
return True # (True,unit_vec,r_vec,dot)
else:
return False #,unit_vec,r_vec,dot)
def _build_graph(self):
"""
This function builds a networkx graph representation of topology, creating several class attributes:
- graph (networkx.DiGraph): networkx graph representing the world map, with:
Node properties:
vertex: (x,y,z) position in world map
Edge properties:
entry_vector: unit vector along tangent at entry point
exit_vector: unit vector along tangent at exit point
net_vector: unit vector of the chord from entry to exit
intersection: boolean indicating if the edge belongs to an intersection
- id_map (dictionary): mapping from (x,y,z) to node id
- road_id_to_edge (dictionary): map from road id to edge in the graph
"""
self._graph = nx.DiGraph()
self._id_map = dict() # Map with structure {(x,y,z): id, ... }
self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... }
for segment in self._topology:
entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz']
path = segment['path']
entry_wp, exit_wp = segment['entry'], segment['exit']
intersection = entry_wp.is_junction
road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id
for vertex in entry_xyz, exit_xyz:
# Adding unique nodes and populating id_map
if vertex not in self._id_map:
new_id = len(self._id_map)
self._id_map[vertex] = new_id
self._graph.add_node(new_id, vertex=vertex)
n1 = self._id_map[entry_xyz]
n2 = self._id_map[exit_xyz]
if road_id not in self._road_id_to_edge:
self._road_id_to_edge[road_id] = dict()
if section_id not in self._road_id_to_edge[road_id]:
self._road_id_to_edge[road_id][section_id] = dict()
self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2)
entry_carla_vector = entry_wp.transform.rotation.get_forward_vector()
exit_carla_vector = exit_wp.transform.rotation.get_forward_vector()
# Adding edge with attributes
self._graph.add_edge(
n1, n2,
length=len(path) + 1, path=path,
entry_waypoint=entry_wp, exit_waypoint=exit_wp,
entry_vector=np.array(
[entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]),
exit_vector=np.array(
[exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]),
net_vector=vector(entry_wp.transform.location, exit_wp.transform.location),
intersection=intersection, type=RoadOption.LANEFOLLOW)
def get_direction(self):
vehicle_pos = self.simulator.vehicle_variables.vehicle_location
rot = self.simulator.vehicle_variables.vehicle_waypoint.transform.rotation
forward_vector = rot.get_forward_vector()
forward_vector = [forward_vector.x,forward_vector.y,forward_vector.z]
obstacle_vector = np.array(misc.vector(vehicle_pos,self.location))
dot = obstacle_vector.dot(forward_vector)
arc_cos = np.degrees(np.arccos(dot))
self.angle = arc_cos
def check_waypoint_angle(self,next_waypoint,vehicle_transform,turn_angle=150):
vp = vehicle_transform
p2 = next_waypoint.transform.location
u2 =misc.vector(p2, vp.location)
u1 = np.array(misc.vector(vp.location,self.simulator.navigation_system.ideal_route[self.simulator.navigation_system.curr_pos].location))
angle = math.degrees(np.arccos(u1.dot(u2) ))
cnt =0
# print(angle)
while angle<turn_angle and cnt<10:
n = next_waypoint.next(0.6)
if n:
next_waypoint = n[0]
else:
break
p2 = next_waypoint.transform.location
u2 =misc.vector(p2, vp.location)
u1 = np.array(misc.vector(vp.location,self.simulator.navigation_system.ideal_route[self.simulator.navigation_system.curr_pos].location))
angle = math.degrees(np.arccos(u1.dot(u2) ))
# print(angle)
cnt+=1
return next_waypoint
def generate_target_waypoint(waypoint, turn=0):
"""
This method follow waypoints to a junction and choose path based on turn input.
Turn input: LEFT -> -1, RIGHT -> 1, STRAIGHT -> 0
@returns a waypoint list according to turn input
"""
sampling_radius = 1
reached_junction = False
wp_list = []
threshold = math.radians(0.1)
while True:
wp_choice = waypoint.next(sampling_radius)
# Choose path at intersection
if len(wp_choice) > 1:
reached_junction = True
waypoint = choose_at_junction(waypoint, wp_choice, turn)
else:
waypoint = wp_choice[0]
wp_list.append(waypoint)
# End condition for the behavior
if turn != 0 and reached_junction and len(wp_list) >= 3:
v_1 = vector(wp_list[-2].transform.location,
wp_list[-1].transform.location)
v_2 = vector(wp_list[-3].transform.location,
wp_list[-2].transform.location)
vec_dots = np.dot(v_1, v_2)
cos_wp = vec_dots / abs(
(np.linalg.norm(v_1) * np.linalg.norm(v_2)))
angle_wp = math.acos(
min(1.0, cos_wp)
) # COS can't be larger than 1, it can happen due to float imprecision
if angle_wp < threshold:
break
elif reached_junction and not wp_list[-1].is_intersection:
break
return wp_list[-1]
def _build_graph(self):
"""
This function builds a NetworkX graph representation of topology.
The topology is read from self._topology.
graph node properties:
vertex - (x,y,z) position in world map
graph edge properties:
entry_vector - unit vector along tangent at entry point
exit_vector - unit vector along tangent at exit point
net_vector - unit vector of the chord from entry to exit
intersection - boolean indicating if the edge belongs to an
intersection
return : graph -> NetworkX graph representing the world map,
id_map-> mapping from (x,y,z) to node id
road_id_to_edge-> map from road id to edge in the graph
"""
# Create NetworkX directed graph
graph = nx.DiGraph()
# Map with structure {wp_coordinate: id, ... } wp_coordinate: (x, y, z)
id_map = dict()
# Map with structure {road_id: {section_id: {lane_id: edge, ...}, ... }, ... }, edge: (wp1_id_1, wp2_id)
road_id_to_edge = dict()
# Each segment: tuple(carla.Waypoint, carla.Waypoint)
for segment in self._topology:
entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz']
path = segment['path']
entry_wp, exit_wp = segment['entry'], segment['exit']
intersection = entry_wp.is_junction
# id depends on entry waypoint (not exit one)
road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id
for vertex in entry_xyz, exit_xyz:
# Adding unique nodes and populating id_map
if vertex not in id_map:
new_id = len(id_map)
id_map[vertex] = new_id # id of id_map
graph.add_node(new_id, vertex=vertex)
# Adding edge to road_id_to_edge
id1 = id_map[entry_xyz]
id2 = id_map[exit_xyz]
if road_id not in road_id_to_edge:
road_id_to_edge[road_id] = dict()
if section_id not in road_id_to_edge[road_id]:
road_id_to_edge[road_id][section_id] = dict()
road_id_to_edge[road_id][section_id][lane_id] = (id1, id2)
# Adding edge with attributes to NetworkX graph
entry_carla_vector = entry_wp.transform.rotation.get_forward_vector(
)
exit_carla_vector = exit_wp.transform.rotation.get_forward_vector()
graph.add_edge(id1,
id2,
length=len(path) + 1,
path=path,
entry_waypoint=entry_wp,
exit_waypoint=exit_wp,
entry_vector=np.array([
entry_carla_vector.x, entry_carla_vector.y,
entry_carla_vector.z
]),
exit_vector=np.array([
exit_carla_vector.x, exit_carla_vector.y,
exit_carla_vector.z
]),
net_vector=vector(entry_wp.transform.location,
exit_wp.transform.location),
intersection=intersection,
type=RoadOption.LANEFOLLOW)
return graph, id_map, road_id_to_edge
def set_destination(self, location):
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
solution = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
route.append(RoadOption.VOID)
for action in route:
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location
) < self._hop_resolution:
break
if action == RoadOption.VOID: break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
for wp_select in wp_choice:
v_select = vector(current_location,
wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction * np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
while current_waypoint.is_intersection:
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
assert solution
self._current_plan = solution
self._local_planner.set_global_plan(self._current_plan)
def retrieve_route(self, actor_configuration, target_configuration,
lat_ref, lon_ref):
"""
Estimate a route from a starting position to a target position.
:param actor_configuration: ActorConfiguration object representing the ego vehicle
:param target_configuration: TargetConfiguration object representing the target point
:param lat_ref: map reference latitude
:param lon_ref: map reference longitude
:return: a tuple with two lists. Both lists represent the route.
The first one in world coordinates.
The second one in GPS coordinates.
"""
start_waypoint = self.world.get_map().get_waypoint(
actor_configuration.transform.location)
end_waypoint = self.world.get_map().get_waypoint(
target_configuration.transform.location)
solution = []
solution_gps = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self.world.get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
route.append(RoadOption.VOID)
for action in route:
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
gps_point = self._location_to_gps(
lat_ref, lon_ref, current_waypoint.transform.location)
solution.append((current_waypoint.transform.location,
RoadOption.LANEFOLLOW))
solution_gps.append((gps_point, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location
) < self._hop_resolution:
break
if action == RoadOption.VOID: break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(
current_transform.rotation.yaw)),
y=math.sin(math.radians(
current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
for wp_select in wp_choice:
v_select = vector(current_location,
wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction * np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
gps_point = self._location_to_gps(
lat_ref, lon_ref, current_waypoint.transform.location)
solution.append((current_waypoint.transform.location, action))
solution_gps.append((gps_point, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
while current_waypoint.is_intersection:
gps_point = self._location_to_gps(
lat_ref, lon_ref, current_waypoint.transform.location)
solution.append(
(current_waypoint.transform.location, action))
solution_gps.append((gps_point, action))
current_waypoint = \
current_waypoint.next(self._hop_resolution)[0]
# send plan to agent
self.agent_instance.set_global_plan(solution_gps)
return solution, solution_gps
def compute_route_waypoints(world_map,
start_waypoint,
end_waypoint,
resolution=1.0,
plan=None):
"""
Returns a list of (waypoint, RoadOption)-tuples that describes a route
starting at start_waypoint, ending at end_waypoint.
start_waypoint (carla.Waypoint):
Starting waypoint of the route
end_waypoint (carla.Waypoint):
Destination waypoint of the route
resolution (float):
Resolution, or lenght, of the steps between waypoints
(in meters)
plan (list(RoadOption) or None):
If plan is not None, generate a route that takes every option as provided
in the list for every intersections, in the given order.
(E.g. set plan=[RoadOption.STRAIGHT, RoadOption.LEFT, RoadOption.RIGHT]
to make the route go straight, then left, then right.)
If plan is None, we use the GlobalRoutePlanner to find a path between
start_waypoint and end_waypoint.
"""
if plan is None:
# Setting up global router
dao = GlobalRoutePlannerDAO(world_map, resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
route = grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
else:
# Compute route waypoints
route = []
current_waypoint = start_waypoint
for i, action in enumerate(plan):
# Generate waypoints to next junction
wp_choice = [current_waypoint]
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
route.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(resolution)
# Stop at destination
if i > 0 and current_waypoint.transform.location.distance(
end_waypoint.transform.location) < resolution:
break
if action == RoadOption.VOID:
break
# Make sure that next intersection waypoints are far enough
# from each other so we choose the correct path
step = resolution
while len(wp_choice) > 1:
wp_choice = current_waypoint.next(step)
wp0, wp1 = wp_choice[:2]
if wp0.transform.location.distance(
wp1.transform.location) < resolution:
step += resolution
else:
break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=np.cos(np.radians(current_transform.rotation.yaw)),
y=np.sin(np.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float("inf")
# Choose correct path
for wp_select in wp_choice:
v_select = vector(current_location,
wp_select.transform.location)
cross = float("inf")
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction * np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
route.append((current_waypoint, action))
current_waypoint = current_waypoint.next(resolution)[0]
while current_waypoint.is_intersection:
route.append((current_waypoint, action))
current_waypoint = current_waypoint.next(resolution)[0]
assert route
# Change action 5 wp before intersection
num_wp_to_extend_actions_with = 5
action = route[0][1]
for i in range(1, len(route)):
next_action = route[i][1]
if next_action != action:
if next_action != RoadOption.LANEFOLLOW:
for j in range(num_wp_to_extend_actions_with):
route[i - j - 1] = (route[i - j - 1][0], route[i][1])
action = next_action
return route
