p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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 2019-03-02 14:14:15 +05:30
parent 106d3e116c
commit b854ed0851
2 changed files with 190 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

404
PythonAPI/agents/navigation/global_route_planner.py
View File

@ -12,6 +12,7 @@ import networkx as nx
import carla import carla
from agents.navigation.local_planner import RoadOption from agents.navigation.local_planner import RoadOption
from agents.tools.misc import vector
class GlobalRoutePlanner(object): class GlobalRoutePlanner(object):
@ -29,6 +30,7 @@ class GlobalRoutePlanner(object):
self._topology = None self._topology = None
self._graph = None self._graph = None
self._id_map = None self._id_map = None
self._road_id_to_edge = None
def setup(self): def setup(self):
""" """
@ -37,59 +39,191 @@ class GlobalRoutePlanner(object):
""" """
self._topology = self._dao.get_topology() self._topology = self._dao.get_topology()
# Creating graph of the world map and also a maping from # Creating graph of the world map and also a maping from
# node co-ordinates to node id # node co-ordinates to node id along with a map from road id to edge
self._graph, self._id_map = self._build_graph() self._graph, self._id_map, self._road_id_to_edge = self._build_graph()
# self._lane_change_link() 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): def plan_route(self, origin, destination):
""" """
The following function generates the route plan based on The following function generates the route plan based on
origin : tuple containing x, y of the route's start position origin : carla.Location object of the route's start position
destination : tuple containing x, y of the route's end position destination : carla.Location object of the route's end position
return : list of turn by turn navigation decisions as return : list of turn by turn navigation decisions as
agents.navigation.local_planner.RoadOption elements 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) route = self._path_search(origin, destination)
plan = [] 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 # Compare current edge and next edge to decide on action
for i in range(len(route) - 2): for i in range(len(route) - 2):
current_edge = self._graph.edges[route[i], route[i + 1]] current_edge = self._graph.edges[route[i], route[i + 1]]
next_edge = self._graph.edges[route[i + 1], route[i + 2]] next_edge = self._graph.edges[route[i + 1], route[i + 2]]
cv = current_edge['exit_vector'] if next_edge['type'] != RoadOption.LANEFOLLOW:
nv = next_edge['net_vector'] plan.append(next_edge['type'])
cv, nv = cv + (0,), nv + (0,) # Making vectors 3D elif current_edge['type'] == RoadOption.LANEFOLLOW:
entry_edges = 0 cv, nv = current_edge['exit_vector'], next_edge['net_vector']
exit_edges = 0 entry_edges, exit_edges = 0, 0
cross_list = [] cross_list = []
# Accumulating cross products of all other paths # Accumulating cross products of all other paths
for neighbor in self._graph.successors(route[i+1]): for neighbor in self._graph.successors(route[i+1]):
entry_edges += 1 select_edge = self._graph.edges[route[i+1], neighbor]
if neighbor != route[i + 2]: if select_edge['type'] == RoadOption.LANEFOLLOW:
select_edge = self._graph.edges[route[i + 1], neighbor] entry_edges += 1
sv = select_edge['net_vector'] if neighbor != route[i + 2]:
cross_list.append(np.cross(cv, sv)[2]) sv = select_edge['net_vector']
for neighbor in self._graph.predecessors(route[i+2]): cross_list.append(np.cross(cv, sv)[2])
exit_edges += 1 for neighbor in self._graph.predecessors(route[i+2]): exit_edges += 1
if entry_edges == 1 and exit_edges > 1: if entry_edges == 1 and exit_edges > 1: cross_list.append(0)
cross_list.append(0) # Calculating turn decision
# Calculating turn decision elif next_edge['intersection'] and (entry_edges > 1 or exit_edges > 1):
if next_edge['intersection'] and (entry_edges > 1 or exit_edges > 1): next_cross = np.cross(cv, nv)[2]
next_cross = np.cross(cv, nv)[2] deviation = math.acos(np.dot(cv, nv) /\
deviation = math.acos(np.dot(cv, nv) / (np.linalg.norm(cv)*np.linalg.norm(nv)))
(np.linalg.norm(cv) * np.linalg.norm(nv))) if deviation < threshold: action = RoadOption.STRAIGHT
if deviation < threshold: elif next_cross < min(cross_list):
action = RoadOption.STRAIGHT action = RoadOption.LEFT
elif next_cross < min(cross_list): elif next_cross > max(cross_list):
action = RoadOption.LEFT action = RoadOption.RIGHT
elif next_cross > max(cross_list): plan.append(action)
action = RoadOption.RIGHT
plan.append(action)
return plan return plan
def verify_intersection(self, waypoint): def verify_intersection(self, waypoint):
""" """
This function recieves a waypoint and returns true This function recieves a waypoint and returns true
@ -99,197 +233,15 @@ class GlobalRoutePlanner(object):
is_intersection = False is_intersection = False
if waypoint.is_intersection : if waypoint.is_intersection :
x = waypoint.transform.location.x entry_node_id, exit_node_id = self._localise(waypoint.transform.location)
y = waypoint.transform.location.y segment = self._graph.edges[entry_node_id, exit_node_id]
segment = self._localise(x, y)
entry_node_id = self._id_map[segment['entry']]
exit_node_id = self._id_map[segment['exit']]
entry_edges, exit_edges = 0, 0 entry_edges, exit_edges = 0, 0
for _ in self._graph.successors(entry_node_id): entry_edges += 1 for _ in self._graph.successors(entry_node_id):
for _ in self._graph.predecessors(exit_node_id): exit_edges += 1 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 if entry_edges > 1 or exit_edges > 1: is_intersection = True
return is_intersection return is_intersection
def _distance_heuristic(self, n1, n2): pass
"""
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]

30
PythonAPI/agents/navigation/global_route_planner_dao.py
View File

@ -15,7 +15,7 @@ class GlobalRoutePlannerDAO(object):
""" """
def __init__(self, wmap): def __init__(self, wmap):
""" """get_topology
Constructor Constructor
wmap : carl world map object wmap : carl world map object
@ -36,35 +36,29 @@ class GlobalRoutePlannerDAO(object):
to exit to exit
intersection - Boolean indicating if the road segment intersection - Boolean indicating if the road segment
is an intersection is an intersection
roadid - unique id common for all lanes of a road segment
""" """
topology = [] topology = []
# Retrieving waypoints to construct a detailed topology # Retrieving waypoints to construct a detailed topology
for segment in self._wmap.get_topology(): for segment in self._wmap.get_topology():
x1 = segment[0].transform.location.x wp1, wp2 = segment[0], segment[1]
y1 = segment[0].transform.location.y l1, l2 = wp1.transform.location, wp2.transform.location
x2 = segment[1].transform.location.x 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)
y2 = segment[1].transform.location.y wp1.transform.location, wp2.transform.location = l1, l2
x1, y1, x2, y2 = np.round([x1, y1, x2, y2], 2)
seg_dict = dict() seg_dict = dict()
seg_dict['entry'] = (x1, y1) seg_dict['entry'] = wp1
seg_dict['exit'] = (x2, y2) seg_dict['exit'] = wp2
seg_dict['path'] = [] seg_dict['path'] = []
wp1 = segment[0]
wp2 = segment[1]
seg_dict['intersection'] = True if wp1.is_intersection else False
endloc = wp2.transform.location endloc = wp2.transform.location
w = wp1.next(1)[0] w = wp1.next(1)[0]
while w.transform.location.distance(endloc) > 1: while w.transform.location.distance(endloc) > 1:
x = w.transform.location.x seg_dict['path'].append(w)
y = w.transform.location.y
seg_dict['path'].append((x, y))
w = w.next(1)[0] w = w.next(1)[0]
topology.append(seg_dict) topology.append(seg_dict)
return topology 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)