From 6591cb3e1adcba1be3dee156040bf2a8beb80f56 Mon Sep 17 00:00:00 2001 From: Praveen Kumar Date: Mon, 15 Apr 2019 17:58:42 +0530 Subject: [PATCH 1/3] draft implementation of fix: * Improved decision logic after intersection * Fixed key error on lane change segments --- .../agents/navigation/global_route_planner.py | 94 +++++++++++++------ 1 file changed, 67 insertions(+), 27 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index a46b940a3..6bb5e8454 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/PythonAPI/carla/agents/navigation/global_route_planner.py @@ -31,6 +31,8 @@ class GlobalRoutePlanner(object): self._graph = None self._id_map = None self._road_id_to_edge = None + self._intersection_end_node = -1 + self._previous_decision = RoadOption.VOID def setup(self): """ @@ -234,6 +236,30 @@ class GlobalRoutePlanner(object): route.append(end[1]) return route + def _successive_last_intersection_edge(self, index, route): + """ + This method returns the last successive intersection edge + from a starting index on the route. + + This helps moving past tiny intersection edges to calculate + proper turn decisions. + """ + + last_intersection_edge = None + last_node = None + for node1, node2 in [(route[i], route[i+1]) for i in range(index, len(route)-1)]: + candidate_edge = self._graph.edges[node1, node2] + if node1 == route[index]: + last_intersection_edge = candidate_edge + if candidate_edge['type'] == RoadOption.LANEFOLLOW and \ + candidate_edge['intersection']: + last_intersection_edge = candidate_edge + last_node = node2 + else: + break + + return last_node, last_intersection_edge + def _turn_decision(self, index, route, threshold=math.radians(5)): """ This method returns the turn decision (RoadOption) for pair of edges @@ -246,35 +272,48 @@ class GlobalRoutePlanner(object): next_node = route[index+1] next_edge = self._graph.edges[current_node, next_node] if index > 0: - current_edge = self._graph.edges[previous_node, current_node] - calculate_turn = current_edge['type'].value == RoadOption.LANEFOLLOW.value and \ - not current_edge['intersection'] and \ - next_edge['type'].value == RoadOption.LANEFOLLOW.value and \ - next_edge['intersection'] - if calculate_turn: - cv, nv = current_edge['exit_vector'], next_edge['net_vector'] - cross_list = [] - for neighbor in self._graph.successors(current_node): - select_edge = self._graph.edges[current_node, neighbor] - if select_edge['type'].value == RoadOption.LANEFOLLOW.value: - if neighbor != route[index+1]: - sv = select_edge['net_vector'] - cross_list.append(np.cross(cv, sv)[2]) - next_cross = np.cross(cv, nv)[2] - deviation = math.acos(np.clip( - np.dot(cv, nv)/(np.linalg.norm(cv)*np.linalg.norm(nv)), -1.0, 1.0)) - if not cross_list: - cross_list.append(0) - if deviation < threshold: - decision = RoadOption.STRAIGHT - elif cross_list and next_cross < min(cross_list): - decision = RoadOption.LEFT - elif cross_list and next_cross > max(cross_list): - decision = RoadOption.RIGHT + if self._previous_decision != RoadOption.VOID and \ + self._intersection_end_node > 0 and \ + self._intersection_end_node != previous_node and \ + next_edge['type'] == RoadOption.LANEFOLLOW and \ + next_edge['intersection']: + decision = self._previous_decision else: - decision = next_edge['type'] + self._intersection_end_node = -1 + current_edge = self._graph.edges[previous_node, current_node] + calculate_turn = current_edge['type'].value == RoadOption.LANEFOLLOW.value and \ + not current_edge['intersection'] and \ + next_edge['type'].value == RoadOption.LANEFOLLOW.value and \ + next_edge['intersection'] + if calculate_turn: + last_node, tail_edge = self._successive_last_intersection_edge(index, route) + self._intersection_end_node = last_node + if tail_edge is not None: + next_edge = tail_edge + cv, nv = current_edge['exit_vector'], next_edge['net_vector'] + cross_list = [] + for neighbor in self._graph.successors(current_node): + select_edge = self._graph.edges[current_node, neighbor] + if select_edge['type'].value == RoadOption.LANEFOLLOW.value: + if neighbor != route[index+1]: + sv = select_edge['net_vector'] + cross_list.append(np.cross(cv, sv)[2]) + next_cross = np.cross(cv, nv)[2] + deviation = math.acos(np.clip( + np.dot(cv, nv)/(np.linalg.norm(cv)*np.linalg.norm(nv)), -1.0, 1.0)) + if not cross_list: + cross_list.append(0) + if deviation < threshold: + decision = RoadOption.STRAIGHT + elif cross_list and next_cross < min(cross_list): + decision = RoadOption.LEFT + elif cross_list and next_cross > max(cross_list): + decision = RoadOption.RIGHT + else: + decision = next_edge['type'] else: decision = next_edge['type'] + self._previous_decision = decision return decision @@ -312,7 +351,8 @@ class GlobalRoutePlanner(object): def trace_route(self, origin, destination): """ - This method returns list of (carla.Waypoint, RoadOption) from origin to destination + This method returns list of (carla.Waypoint, RoadOption) + from origin (carla.Location) to destination (carla.Location) """ route_trace = [] From ec4fdd71f55b096e602d594bfb2853e61b4e8ddb Mon Sep 17 00:00:00 2001 From: Praveen Kumar Date: Tue, 16 Apr 2019 09:45:30 +0530 Subject: [PATCH 2/3] Fixed false straights --- .../agents/navigation/global_route_planner.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index 6bb5e8454..ed3f0084e 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/PythonAPI/carla/agents/navigation/global_route_planner.py @@ -86,17 +86,17 @@ class GlobalRoutePlanner(object): road_id_to_edge[road_id][section_id] = dict() 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 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), + 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) @@ -309,6 +309,10 @@ class GlobalRoutePlanner(object): decision = RoadOption.LEFT elif cross_list and next_cross > max(cross_list): decision = RoadOption.RIGHT + elif next_cross < 0: + decision = RoadOption.LEFT + elif next_cross > 0: + decision = RoadOption.RIGHT else: decision = next_edge['type'] else: From a707c69db99550ea51e5445b381290edb679554f Mon Sep 17 00:00:00 2001 From: Praveen Kumar Date: Tue, 16 Apr 2019 14:50:52 +0530 Subject: [PATCH 3/3] Improved turn decision logic on lane changes. Fixed route duplication issue on lane change. --- .../carla/agents/navigation/global_route_planner.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index ed3f0084e..fc9023917 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/PythonAPI/carla/agents/navigation/global_route_planner.py @@ -362,6 +362,7 @@ class GlobalRoutePlanner(object): route_trace = [] route = self._path_search(origin, destination) current_waypoint = self._dao.get_waypoint(origin) + destination_waypoint = self._dao.get_waypoint(destination) resolution = self._dao.get_resolution() for i in range(len(route) - 1): @@ -371,6 +372,7 @@ class GlobalRoutePlanner(object): if edge['type'].value != RoadOption.LANEFOLLOW.value and \ edge['type'].value != RoadOption.VOID.value: + route_trace.append((current_waypoint, road_option)) exit_wp = edge['exit_waypoint'] n1, n2 = self._road_id_to_edge[exit_wp.road_id][exit_wp.section_id][exit_wp.lane_id] next_edge = self._graph.edges[n1, n2] @@ -391,5 +393,12 @@ class GlobalRoutePlanner(object): if len(route)-i <= 2 and \ waypoint.transform.location.distance(destination) < 2*resolution: break + elif len(route)-i <= 2 and \ + current_waypoint.road_id == destination_waypoint.road_id and \ + current_waypoint.section_id == destination_waypoint.section_id and \ + current_waypoint.lane_id == destination_waypoint.lane_id: + destination_index = self._find_closest_in_list(destination_waypoint, path) + if closest_index > destination_index: + break return route_trace