From b854ed085115cb218bfae0a723ea302ff2f586c4 Mon Sep 17 00:00:00 2001 From: Praveen Kumar Date: Sat, 2 Mar 2019 14:14:15 +0530 Subject: [PATCH] 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 --- .../agents/navigation/global_route_planner.py | 404 ++++++++---------- .../navigation/global_route_planner_dao.py | 30 +- 2 files changed, 190 insertions(+), 244 deletions(-) diff --git a/PythonAPI/agents/navigation/global_route_planner.py b/PythonAPI/agents/navigation/global_route_planner.py index 7149d81de..830c340d2 100644 --- a/PythonAPI/agents/navigation/global_route_planner.py +++ b/PythonAPI/agents/navigation/global_route_planner.py @@ -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,59 +39,191 @@ 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): """ This function recieves a waypoint and returns true @@ -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 diff --git a/PythonAPI/agents/navigation/global_route_planner_dao.py b/PythonAPI/agents/navigation/global_route_planner_dao.py index c02e2c2c0..c187793fb 100644 --- a/PythonAPI/agents/navigation/global_route_planner_dao.py +++ b/PythonAPI/agents/navigation/global_route_planner_dao.py @@ -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)