Global router overhauled
* Enabled support for 3d localization and routing * Added graph representation for lane change * Overhauled code to make use of carla objects as much as possible EOD commit
This commit is contained in:
Praveen Kumar
parent
106d3e116c
commit
b854ed0851
|
|
@ -12,6 +12,7 @@ import networkx as nx
|
|||
|
||||
import carla
|
||||
from agents.navigation.local_planner import RoadOption
|
||||
from agents.tools.misc import vector
|
||||
|
||||
|
||||
class GlobalRoutePlanner(object):
|
||||
|
|
@ -29,6 +30,7 @@ class GlobalRoutePlanner(object):
|
|||
self._topology = None
|
||||
self._graph = None
|
||||
self._id_map = None
|
||||
self._road_id_to_edge = None
|
||||
|
||||
def setup(self):
|
||||
"""
|
||||
|
|
@ -37,57 +39,189 @@ class GlobalRoutePlanner(object):
|
|||
"""
|
||||
self._topology = self._dao.get_topology()
|
||||
# Creating graph of the world map and also a maping from
|
||||
# node co-ordinates to node id
|
||||
self._graph, self._id_map = self._build_graph()
|
||||
# self._lane_change_link()
|
||||
# node co-ordinates to node id along with a map from road id to edge
|
||||
self._graph, self._id_map, self._road_id_to_edge = self._build_graph()
|
||||
self._lane_change_link()
|
||||
|
||||
def _build_graph(self):
|
||||
"""
|
||||
This function builds a networkx graph representation of topology.
|
||||
The topology is read from self._topology.
|
||||
graph node properties:
|
||||
vertex - carla.Location object of node's 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_wp = segment['entry']
|
||||
exit_wp = segment['exit']
|
||||
path = segment['path']
|
||||
intersection = entry_wp.is_intersection
|
||||
road_id, lane_id = entry_wp.road_id, entry_wp.lane_id
|
||||
|
||||
for vertex_wp in entry_wp, exit_wp:
|
||||
# Adding unique nodes and populating id_map
|
||||
location = vertex_wp.transform.location
|
||||
if (location.x, location.y, location.z) not in id_map:
|
||||
new_id = len(id_map)
|
||||
id_map[location.x, location.y, location.z] = new_id
|
||||
graph.add_node(new_id, vertex=location)
|
||||
n1 = id_map[entry_wp.transform.location.x, entry_wp.transform.location.y, entry_wp.transform.location.z]
|
||||
n2 = id_map[exit_wp.transform.location.x, exit_wp.transform.location.y, exit_wp.transform.location.z]
|
||||
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_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 _localise(self, location):
|
||||
"""
|
||||
This function finds the road segment closest to given waypoint
|
||||
x, y : co-rodinates to be localized in the graph
|
||||
return : pair node ids representing an edge in the graph
|
||||
"""
|
||||
waypoint = self._dao.get_waypoint(location)
|
||||
return self._road_id_to_edge[waypoint.road_id][waypoint.lane_id]
|
||||
|
||||
def _lane_change_link(self):
|
||||
"""
|
||||
This method places zero cost links in the topology graph
|
||||
representing availability of lane changes.
|
||||
"""
|
||||
|
||||
for segment in self._topology:
|
||||
left_found, right_found = False, False
|
||||
for waypoint in segment['path']:
|
||||
|
||||
next_waypoint = None
|
||||
next_road_option = None
|
||||
|
||||
if bool(waypoint.lane_change & carla.LaneChange.Right) and not right_found:
|
||||
next_waypoint = waypoint.get_right_lane()
|
||||
next_road_option = RoadOption.CHANGELANERIGHT
|
||||
next_segment = self._localise(next_waypoint.transform.location)
|
||||
next_edge = self._graph.edges[next_segment[0], next_segment[1]]
|
||||
sloc = segment['entry'].transform.location
|
||||
self._graph.add_edge(
|
||||
self._id_map[sloc.x, sloc.y, sloc.z], next_segment[1],
|
||||
length=next_edge['length'], type=next_road_option, change_waypoint = waypoint)
|
||||
right_found = True
|
||||
|
||||
if bool(waypoint.lane_change & carla.LaneChange.Left) and not left_found:
|
||||
next_waypoint = waypoint.get_left_lane()
|
||||
next_road_option = RoadOption.CHANGELANELEFT
|
||||
next_segment = self._localise(next_waypoint.transform.location)
|
||||
next_edge = self._graph.edges[next_segment[0], next_segment[1]]
|
||||
sloc = segment['entry'].transform.location
|
||||
self._graph.add_edge(
|
||||
self._id_map[sloc.x, sloc.y, sloc.z], next_segment[1],
|
||||
length=next_edge['length'], type=next_road_option, change_waypoint = waypoint)
|
||||
left_found = True
|
||||
|
||||
if left_found and right_found: break
|
||||
|
||||
def _distance_heuristic(self, n1, n2):
|
||||
"""
|
||||
Distance heuristic calculator for path searching
|
||||
in self._graph
|
||||
"""
|
||||
l1 = self._graph.nodes[n1]['vertex']
|
||||
l2 = self._graph.nodes[n2]['vertex']
|
||||
return l1.distance(l2)
|
||||
|
||||
def _path_search(self, origin, destination):
|
||||
"""
|
||||
This function finds the shortest path connecting origin and destination
|
||||
using A* search with distance heuristic.
|
||||
origin : carla.Location object of start position
|
||||
desitnation : carla.Location object of of end position
|
||||
return : path as list of node ids (as int) of the graph self._graph
|
||||
connecting origin and destination
|
||||
"""
|
||||
|
||||
start, end = self._localise(origin), self._localise(destination)
|
||||
|
||||
route = nx.astar_path(
|
||||
self._graph, source=start[0], target=end[1],
|
||||
heuristic=self._distance_heuristic, weight='length')
|
||||
|
||||
return route
|
||||
|
||||
def plan_route(self, origin, destination):
|
||||
"""
|
||||
The following function generates the route plan based on
|
||||
origin : tuple containing x, y of the route's start position
|
||||
destination : tuple containing x, y of the route's end position
|
||||
origin : carla.Location object of the route's start position
|
||||
destination : carla.Location object of the route's end position
|
||||
return : list of turn by turn navigation decisions as
|
||||
agents.navigation.local_planner.RoadOption elements
|
||||
Possible values (for now) are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID
|
||||
Possible values are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID
|
||||
CHANGELANELEFT, CHANGELANERIGHT
|
||||
"""
|
||||
|
||||
threshold = math.radians(4.0)
|
||||
threshold = math.radians(5.0)
|
||||
route = self._path_search(origin, destination)
|
||||
plan = []
|
||||
|
||||
if len(route) == 2 and self._graph.edges[route[0], route[1]]['type'] != RoadOption.LANEFOLLOW:
|
||||
plan.append(self._graph.edges[route[0], route[1]]['type'])
|
||||
|
||||
# Compare current edge and next edge to decide on action
|
||||
for i in range(len(route) - 2):
|
||||
current_edge = self._graph.edges[route[i], route[i + 1]]
|
||||
next_edge = self._graph.edges[route[i + 1], route[i + 2]]
|
||||
cv = current_edge['exit_vector']
|
||||
nv = next_edge['net_vector']
|
||||
cv, nv = cv + (0,), nv + (0,) # Making vectors 3D
|
||||
entry_edges = 0
|
||||
exit_edges = 0
|
||||
cross_list = []
|
||||
# Accumulating cross products of all other paths
|
||||
for neighbor in self._graph.successors(route[i+1]):
|
||||
entry_edges += 1
|
||||
if neighbor != route[i + 2]:
|
||||
select_edge = self._graph.edges[route[i + 1], neighbor]
|
||||
sv = select_edge['net_vector']
|
||||
cross_list.append(np.cross(cv, sv)[2])
|
||||
for neighbor in self._graph.predecessors(route[i+2]):
|
||||
exit_edges += 1
|
||||
if entry_edges == 1 and exit_edges > 1:
|
||||
cross_list.append(0)
|
||||
# Calculating turn decision
|
||||
if next_edge['intersection'] and (entry_edges > 1 or exit_edges > 1):
|
||||
next_cross = np.cross(cv, nv)[2]
|
||||
deviation = math.acos(np.dot(cv, nv) /
|
||||
(np.linalg.norm(cv) * np.linalg.norm(nv)))
|
||||
if deviation < threshold:
|
||||
action = RoadOption.STRAIGHT
|
||||
elif next_cross < min(cross_list):
|
||||
action = RoadOption.LEFT
|
||||
elif next_cross > max(cross_list):
|
||||
action = RoadOption.RIGHT
|
||||
plan.append(action)
|
||||
if next_edge['type'] != RoadOption.LANEFOLLOW:
|
||||
plan.append(next_edge['type'])
|
||||
elif current_edge['type'] == RoadOption.LANEFOLLOW:
|
||||
cv, nv = current_edge['exit_vector'], next_edge['net_vector']
|
||||
entry_edges, exit_edges = 0, 0
|
||||
cross_list = []
|
||||
# Accumulating cross products of all other paths
|
||||
for neighbor in self._graph.successors(route[i+1]):
|
||||
select_edge = self._graph.edges[route[i+1], neighbor]
|
||||
if select_edge['type'] == RoadOption.LANEFOLLOW:
|
||||
entry_edges += 1
|
||||
if neighbor != route[i + 2]:
|
||||
sv = select_edge['net_vector']
|
||||
cross_list.append(np.cross(cv, sv)[2])
|
||||
for neighbor in self._graph.predecessors(route[i+2]): exit_edges += 1
|
||||
if entry_edges == 1 and exit_edges > 1: cross_list.append(0)
|
||||
# Calculating turn decision
|
||||
elif next_edge['intersection'] and (entry_edges > 1 or exit_edges > 1):
|
||||
next_cross = np.cross(cv, nv)[2]
|
||||
deviation = math.acos(np.dot(cv, nv) /\
|
||||
(np.linalg.norm(cv)*np.linalg.norm(nv)))
|
||||
if deviation < threshold: action = RoadOption.STRAIGHT
|
||||
elif next_cross < min(cross_list):
|
||||
action = RoadOption.LEFT
|
||||
elif next_cross > max(cross_list):
|
||||
action = RoadOption.RIGHT
|
||||
plan.append(action)
|
||||
|
||||
return plan
|
||||
|
||||
def verify_intersection(self, waypoint):
|
||||
|
|
@ -99,197 +233,15 @@ class GlobalRoutePlanner(object):
|
|||
|
||||
is_intersection = False
|
||||
if waypoint.is_intersection :
|
||||
x = waypoint.transform.location.x
|
||||
y = waypoint.transform.location.y
|
||||
segment = self._localise(x, y)
|
||||
entry_node_id = self._id_map[segment['entry']]
|
||||
exit_node_id = self._id_map[segment['exit']]
|
||||
|
||||
entry_node_id, exit_node_id = self._localise(waypoint.transform.location)
|
||||
segment = self._graph.edges[entry_node_id, exit_node_id]
|
||||
entry_edges, exit_edges = 0, 0
|
||||
for _ in self._graph.successors(entry_node_id): entry_edges += 1
|
||||
for _ in self._graph.predecessors(exit_node_id): exit_edges += 1
|
||||
|
||||
for _ in self._graph.successors(entry_node_id):
|
||||
if segment['type'] == RoadOption.LANEFOLLOW: entry_edges += 1
|
||||
for _ in self._graph.predecessors(exit_node_id):
|
||||
if segment['type'] == RoadOption.LANEFOLLOW: exit_edges += 1
|
||||
if entry_edges > 1 or exit_edges > 1: is_intersection = True
|
||||
|
||||
return is_intersection
|
||||
|
||||
def _distance_heuristic(self, n1, n2):
|
||||
"""
|
||||
Distance heuristic calculator for path searching
|
||||
in self._graph
|
||||
"""
|
||||
(x1, y1) = self._graph.nodes[n1]['vertex']
|
||||
(x2, y2) = self._graph.nodes[n2]['vertex']
|
||||
return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5
|
||||
|
||||
def _path_search(self, origin, destination):
|
||||
"""
|
||||
This function finds the shortest path connecting origin and destination
|
||||
using A* search with distance heuristic.
|
||||
origin : tuple containing x, y co-ordinates of start position
|
||||
desitnation : tuple containing x, y co-ordinates of end position
|
||||
return : path as list of node ids (as int) of the graph self._graph
|
||||
connecting origin and destination
|
||||
"""
|
||||
xo, yo = origin
|
||||
xd, yd = destination
|
||||
start = self._localise(xo, yo)
|
||||
end = self._localise(xd, yd)
|
||||
|
||||
route = nx.astar_path(
|
||||
self._graph, source=self._id_map[start['entry']],
|
||||
target=self._id_map[end['exit']],
|
||||
heuristic=self._distance_heuristic,
|
||||
weight='length')
|
||||
|
||||
return route
|
||||
|
||||
def _localise(self, x, y):
|
||||
"""
|
||||
This function finds the road segment closest to (x, y)
|
||||
x, y : co-ordinates of the point to be localized
|
||||
return : pair of points, tuple of tuples containing co-ordinates
|
||||
of points that represents the road segment closest to x, y
|
||||
"""
|
||||
distance = float('inf')
|
||||
nearest = (distance, dict())
|
||||
|
||||
# Measuring distances from segment waypoints and (x, y)
|
||||
for segment in self._topology:
|
||||
entryxy = segment['entry']
|
||||
exitxy = segment['exit']
|
||||
path = segment['path']
|
||||
for xp, yp in [entryxy] + path + [exitxy]:
|
||||
new_distance = self._distance((xp, yp), (x, y))
|
||||
if new_distance < nearest[0]:
|
||||
nearest = (new_distance, segment)
|
||||
|
||||
segment = nearest[1]
|
||||
return segment
|
||||
|
||||
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) of node's position in world map
|
||||
num_edges - Number of exit edges from the node
|
||||
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) to node id
|
||||
"""
|
||||
graph = nx.DiGraph()
|
||||
# Map with structure {(x,y): id, ... }
|
||||
id_map = dict()
|
||||
|
||||
for segment in self._topology:
|
||||
|
||||
entryxy = segment['entry']
|
||||
exitxy = segment['exit']
|
||||
path = segment['path']
|
||||
intersection = segment['intersection']
|
||||
for vertex in entryxy, exitxy:
|
||||
# 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, n2 = id_map[entryxy], id_map[exitxy]
|
||||
# Adding edge with attributes
|
||||
graph.add_edge(
|
||||
n1, n2,
|
||||
length=len(path) + 1, path=path,
|
||||
entry_vector=self._unit_vector(
|
||||
entryxy, path[0] if len(path) > 0 else exitxy),
|
||||
exit_vector=self._unit_vector(
|
||||
path[-1] if len(path) > 0 else entryxy, exitxy),
|
||||
net_vector=self._unit_vector(entryxy, exitxy),
|
||||
intersection=intersection,
|
||||
type=RoadOption.LANEFOLLOW)
|
||||
|
||||
return graph, id_map
|
||||
|
||||
def _lane_change_link(self):
|
||||
"""
|
||||
This method places zero cost links in the topology graph
|
||||
representing availability of lane changes.
|
||||
"""
|
||||
|
||||
next_waypoint = None
|
||||
next_road_option = None
|
||||
|
||||
for segment in self._topology:
|
||||
lane_change_types = []
|
||||
for point in segment['path']:
|
||||
|
||||
waypoint = self._dao.get_waypoint(*point)
|
||||
if waypoint.lane_change & carla.LaneChange.Right \
|
||||
and RoadOption.CHANGELANERIGHT not in lane_change_types:
|
||||
next_waypoint = waypoint.right_lane()
|
||||
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
|
||||
next_road_option = RoadOption.CHANGELANERIGHT
|
||||
|
||||
elif waypoint.lane_change & carla.LaneChange.Left \
|
||||
and RoadOption.CHANGELANERIGHT not in lane_change_types:
|
||||
next_waypoint = waypoint.left_lane()
|
||||
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
|
||||
next_road_option = RoadOption.CHANGELANELEFT
|
||||
else: pass
|
||||
|
||||
if next_waypoint is not None:
|
||||
next_segment = self._localise(
|
||||
next_waypoint.transform.location.x, next_waypoint.transform.location.x)
|
||||
|
||||
self._graph.add_edge(
|
||||
self._id_map[segment['entry']],
|
||||
self._id_map[next_segment['exit']],
|
||||
type=next_road_option)
|
||||
|
||||
lane_change_types.append(next_road_option)
|
||||
|
||||
next_waypoint = None
|
||||
if len(lane_change_types) == 2:
|
||||
break
|
||||
|
||||
def _distance(self, point1, point2):
|
||||
"""
|
||||
returns the distance between point1 and point2
|
||||
point1 : (x,y) of first point
|
||||
point2 : (x,y) of second point
|
||||
return : distance from point1 to point2
|
||||
"""
|
||||
x1, y1 = point1
|
||||
x2, y2 = point2
|
||||
return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||
|
||||
def _unit_vector(self, point1, point2):
|
||||
"""
|
||||
This function returns the unit vector from point1 to point2
|
||||
point1 : (x,y) of first point
|
||||
point2 : (x,y) of second point
|
||||
return : tuple containing x and y components of unit vector
|
||||
from point1 to point2
|
||||
"""
|
||||
x1, y1 = point1
|
||||
x2, y2 = point2
|
||||
|
||||
vector = (x2 - x1, y2 - y1)
|
||||
vector_mag = math.sqrt(vector[0]**2 + vector[1]**2)
|
||||
vector = (vector[0] / vector_mag, vector[1] / vector_mag)
|
||||
|
||||
return vector
|
||||
|
||||
def _dot(self, vector1, vector2):
|
||||
"""
|
||||
This function returns the dot product of vector1 with vector2
|
||||
vector1 : x, y components of first vector
|
||||
vector2 : x, y components of second vector
|
||||
return : dot porduct scalar between vector1 and vector2
|
||||
"""
|
||||
return vector1[0] * vector2[0] + vector1[1] * vector2[1]
|
||||
pass
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ class GlobalRoutePlannerDAO(object):
|
|||
"""
|
||||
|
||||
def __init__(self, wmap):
|
||||
"""
|
||||
"""get_topology
|
||||
Constructor
|
||||
|
||||
wmap : carl world map object
|
||||
|
|
@ -36,35 +36,29 @@ class GlobalRoutePlannerDAO(object):
|
|||
to exit
|
||||
intersection - Boolean indicating if the road segment
|
||||
is an intersection
|
||||
roadid - unique id common for all lanes of a road segment
|
||||
"""
|
||||
topology = []
|
||||
# Retrieving waypoints to construct a detailed topology
|
||||
for segment in self._wmap.get_topology():
|
||||
x1 = segment[0].transform.location.x
|
||||
y1 = segment[0].transform.location.y
|
||||
x2 = segment[1].transform.location.x
|
||||
y2 = segment[1].transform.location.y
|
||||
x1, y1, x2, y2 = np.round([x1, y1, x2, y2], 2)
|
||||
wp1, wp2 = segment[0], segment[1]
|
||||
l1, l2 = wp1.transform.location, wp2.transform.location
|
||||
l1.x, l1.y, l1.z, l2.x, l2.y, l2.z = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 2)
|
||||
wp1.transform.location, wp2.transform.location = l1, l2
|
||||
seg_dict = dict()
|
||||
seg_dict['entry'] = (x1, y1)
|
||||
seg_dict['exit'] = (x2, y2)
|
||||
seg_dict['entry'] = wp1
|
||||
seg_dict['exit'] = wp2
|
||||
seg_dict['path'] = []
|
||||
wp1 = segment[0]
|
||||
wp2 = segment[1]
|
||||
seg_dict['intersection'] = True if wp1.is_intersection else False
|
||||
endloc = wp2.transform.location
|
||||
w = wp1.next(1)[0]
|
||||
while w.transform.location.distance(endloc) > 1:
|
||||
x = w.transform.location.x
|
||||
y = w.transform.location.y
|
||||
seg_dict['path'].append((x, y))
|
||||
seg_dict['path'].append(w)
|
||||
w = w.next(1)[0]
|
||||
|
||||
topology.append(seg_dict)
|
||||
return topology
|
||||
|
||||
def get_waypoint(self, x, y):
|
||||
def get_waypoint(self, location):
|
||||
"""
|
||||
The method returns waytpoint at x, y
|
||||
The method returns waytpoint at given location
|
||||
"""
|
||||
return self._wmap.get_waypoint(carla.Location(x=x, y=y))
|
||||
return self._wmap.get_waypoint(location)
|
||||
|
|
|
|||
Loading…
Reference in New Issue