From 8e16e696b1c3ba39453e0d3886a767f6a41e5e6b Mon Sep 17 00:00:00 2001 From: Guillermo Date: Tue, 29 Jun 2021 12:11:40 +0200 Subject: [PATCH] Changed the global planner to use the AD map --- .../carla/agents/navigation/basic_agent.py | 117 +--- .../agents/navigation/global_route_planner.py | 499 ++++++------------ .../carla/agents/navigation/local_planner.py | 60 ++- .../tools/global_route_planner_helper.py | 208 ++++++++ PythonAPI/carla/agents/tools/map_helper.py | 203 ------- 5 files changed, 417 insertions(+), 670 deletions(-) create mode 100644 PythonAPI/carla/agents/tools/global_route_planner_helper.py delete mode 100644 PythonAPI/carla/agents/tools/map_helper.py diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 6da74334c..191e5ae9c 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -3,23 +3,20 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . -""" This module implements an agent that roams around a track following random -waypoints and avoiding other vehicles. -The agent also responds to traffic lights. """ +""" +This module implements an agent that roams around a track following random +waypoints and avoiding other vehicles. The agent also responds to traffic lights. +""" import carla -from agents.navigation.agent import Agent -from agents.navigation.local_planner import LocalPlanner, RoadOption -from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO -from agents.tools.misc import get_speed - from enum import Enum -import os -import xml.etree.ElementTree as ET -from agents.tools.map_helper import get_shortest_route -import ad_map_access as ad +from agents.navigation.agent import Agent +from agents.navigation.local_planner import LocalPlanner +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.tools.misc import get_speed + + class AgentState(Enum): """ @@ -51,55 +48,16 @@ class BasicAgent(Agent): self._base_tlight_threshold = 5.0 # meters self._base_vehicle_threshold = 5.0 # meters self._max_brake = 0.5 + self._max_steering = 0.3 self._local_planner = LocalPlanner( self._vehicle, opt_dict={ 'target_speed' : target_speed, - 'max_brake': self._max_brake + 'max_brake': self._max_brake, + 'max_steering': self._max_steering } ) - self._initialize_map() - - def _initialize_map(self): - """Initialize the AD map library and, creates the file needed to do so.""" - lat_ref = 0.0 - lon_ref = 0.0 - - opendrive_contents = self._map.to_opendrive() - xodr_name = 'BasicAgentMap.xodr' - txt_name = 'BasicAgentMap.txt' - - # Save the opendrive data into a file - with open(xodr_name, 'w') as f: - f.write(opendrive_contents) - - # Get geo reference - xml_tree = ET.parse(xodr_name) - for geo_elem in xml_tree.find('header').find('geoReference').text.split(' '): - if geo_elem.startswith('+lat_0'): - lat_ref = float(geo_elem.split('=')[-1]) - elif geo_elem.startswith('+lon_0'): - lon_ref = float(geo_elem.split('=')[-1]) - - # Save the previous info - with open(txt_name, 'w') as f: - txt_content = "[ADMap]\n" \ - "map=" + xodr_name + "\n" \ - "[ENUReference]\n" \ - "default=" + str(lat_ref) + " " + str(lon_ref) + " 0.0" - f.write(txt_content) - - # Intialize the map and remove created files - initialized = ad.map.access.init(txt_name) - if not initialized: - raise ValueError("Couldn't initialize the map") - - for fname in [txt_name, xodr_name]: - if os.path.exists(fname): - os.remove(fname) - - def emergency_stop(self): """ Send an emergency stop command to the vehicle @@ -134,12 +92,13 @@ class BasicAgent(Agent): :param start_location: starting location of the route """ if not start_location: - start_location = self._vehicle.get_location() + start_location = self._local_planner.target_waypoint.transform.location clean_queue = True else: + start_location = self._vehicle.get_location() clean_queue = False - start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + start_waypoint = self._map.get_waypoint(start_location) end_waypoint = self._map.get_waypoint(end_location) route_trace = self.trace_route(start_waypoint, end_waypoint) @@ -159,7 +118,6 @@ class BasicAgent(Agent): clean_queue=clean_queue ) - # AD Map version def trace_route(self, start_waypoint, end_waypoint): """ This method sets up a global router and returns the @@ -168,45 +126,10 @@ class BasicAgent(Agent): :param start_waypoint: initial position :param end_waypoint: final position """ - - route = get_shortest_route( - start_waypoint.transform.location, - end_waypoint.transform.location, - self._map, - sample_resolution=self._sampling_resolution, - distance=1, - world=self._vehicle.get_world() - ) - - route_with_options = [] - for waypoint in route: - route_with_options.append([waypoint, RoadOption.LANEFOLLOW]) - - return route_with_options - - # global route planner version - # def trace_route(self, start_waypoint, end_waypoint): - # """ - # This method sets up a global router and returns the optimal route - # from start_waypoint to end_waypoint - - # :param start_waypoint: initial position - # :param end_waypoint: final position - # """ - - # # Setting up global router - # if self._grp is None: - # dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._sampling_resolution) - # grp = GlobalRoutePlanner(dao) - # grp.setup() - # self._grp = grp - - # # Obtain route plan - # route = self._grp.trace_route( - # start_waypoint.transform.location, - # end_waypoint.transform.location) - - # return route + if self._grp is None: + self._grp = GlobalRoutePlanner(self._map, self._sampling_resolution, self._world) + route = self._grp.trace_route(start_waypoint, end_waypoint) + return route def run_step(self, debug=False): """ diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index 1f4696d37..be5497fc8 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/PythonAPI/carla/agents/navigation/global_route_planner.py @@ -7,381 +7,194 @@ """ This module provides GlobalRoutePlanner implementation. """ - import math +import os import numpy as np -import networkx as nx - +import xml.etree.ElementTree as ET import carla + from agents.navigation.local_planner import RoadOption +from agents.tools.global_route_planner_helper import trace_route from agents.tools.misc import vector +import ad_map_access as ad class GlobalRoutePlanner(object): """ - This class provides a very high level route plan. - Instantiate the class by passing a reference to - A GlobalRoutePlannerDAO object. + This class provides a high level route planner. """ - def __init__(self, dao): + def __init__(self, wmap, sampling_resolution, world=None): """ Constructor """ - self._dao = dao - self._topology = None - 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): - """ - Performs initial server data lookup for detailed topology - and builds graph representation of the world map. - """ - self._topology = self._dao.get_topology() - self._graph, self._id_map, self._road_id_to_edge = self._build_graph() - self._find_loose_ends() - self._lane_change_link() + self._sampling_resolution = sampling_resolution + self._world = world # TODO: remove it after debugging + self._wmap = wmap + self._initialize_map() - 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, ... }, ... } + def _initialize_map(self): + """Initialize the AD map library and, creates the file needed to do so.""" + lat_ref = 0.0 + lon_ref = 0.0 - for segment in self._topology: + opendrive_contents = self._wmap.to_opendrive() + xodr_name = 'RoutePlannerMap.xodr' + txt_name = 'RoutePlannerMap.txt' - 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 + # Save the opendrive data into a file + with open(xodr_name, 'w') as f: + f.write(opendrive_contents) - 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() - 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] = (n1, n2) + # Get geo reference + xml_tree = ET.parse(xodr_name) + for geo_elem in xml_tree.find('header').find('geoReference').text.split(' '): + if geo_elem.startswith('+lat_0'): + lat_ref = float(geo_elem.split('=')[-1]) + elif geo_elem.startswith('+lon_0'): + lon_ref = float(geo_elem.split('=')[-1]) - entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() - exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() + # Save the previous info + with open(txt_name, 'w') as f: + txt_content = "[ADMap]\n" \ + "map=" + xodr_name + "\n" \ + "[ENUReference]\n" \ + "default=" + str(lat_ref) + " " + str(lon_ref) + " 0.0" + f.write(txt_content) - # 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=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) + # Intialize the map and remove created files + initialized = ad.map.access.init(txt_name) + if not initialized: + raise ValueError("Couldn't initialize the map") - return graph, id_map, road_id_to_edge - - def _find_loose_ends(self): - """ - This method finds road segments that have an unconnected end, and - adds them to the internal graph representation - """ - count_loose_ends = 0 - hop_resolution = self._dao.get_resolution() - for segment in self._topology: - end_wp = segment['exit'] - exit_xyz = segment['exitxyz'] - road_id, section_id, lane_id = end_wp.road_id, end_wp.section_id, end_wp.lane_id - if road_id in self._road_id_to_edge and section_id in self._road_id_to_edge[road_id] and lane_id in self._road_id_to_edge[road_id][section_id]: - pass - else: - count_loose_ends += 1 - 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() - n1 = self._id_map[exit_xyz] - n2 = -1*count_loose_ends - self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) - next_wp = end_wp.next(hop_resolution) - path = [] - while next_wp is not None and next_wp and next_wp[0].road_id == road_id and next_wp[0].section_id == section_id and next_wp[0].lane_id == lane_id: - path.append(next_wp[0]) - next_wp = next_wp[0].next(hop_resolution) - if path: - n2_xyz = (path[-1].transform.location.x, - path[-1].transform.location.y, - path[-1].transform.location.z) - self._graph.add_node(n2, vertex=n2_xyz) - self._graph.add_edge( - n1, n2, - length=len(path) + 1, path=path, - entry_waypoint=end_wp, exit_waypoint=path[-1], - entry_vector=None, exit_vector=None, net_vector=None, - intersection=end_wp.is_junction, type=RoadOption.LANEFOLLOW) - - def _localize(self, location): - """ - This function finds the road segment closest to given location - location : carla.Location to be localized in the graph - return : pair node ids representing an edge in the graph - """ - waypoint = self._dao.get_waypoint(location) - edge = None - try: - edge = self._road_id_to_edge[waypoint.road_id][waypoint.section_id][waypoint.lane_id] - except KeyError: - print( - "Failed to localize! : ", - "Road id : ", waypoint.road_id, - "Section id : ", waypoint.section_id, - "Lane id : ", waypoint.lane_id, - "Location : ", waypoint.transform.location.x, - waypoint.transform.location.y) - return edge - - 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']: - if not segment['entry'].is_junction: - next_waypoint, next_road_option, next_segment = None, None, None - - if waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found: - next_waypoint = waypoint.get_right_lane() - if next_waypoint is not None and next_waypoint.lane_type == carla.LaneType.Driving and waypoint.road_id == next_waypoint.road_id: - next_road_option = RoadOption.CHANGELANERIGHT - next_segment = self._localize(next_waypoint.transform.location) - if next_segment is not None: - self._graph.add_edge( - self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, - exit_waypoint=next_waypoint, intersection=False, exit_vector=None, - path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) - right_found = True - if waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found: - next_waypoint = waypoint.get_left_lane() - if next_waypoint is not None and next_waypoint.lane_type == carla.LaneType.Driving and waypoint.road_id == next_waypoint.road_id: - next_road_option = RoadOption.CHANGELANELEFT - next_segment = self._localize(next_waypoint.transform.location) - if next_segment is not None: - self._graph.add_edge( - self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, - exit_waypoint=next_waypoint, intersection=False, exit_vector=None, - path=[], length=0, type=next_road_option, change_waypoint=next_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 = np.array(self._graph.nodes[n1]['vertex']) - l2 = np.array(self._graph.nodes[n2]['vertex']) - return np.linalg.norm(l1-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 - destination : 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._localize(origin), self._localize(destination) - - route = nx.astar_path( - self._graph, source=start[0], target=end[0], - heuristic=self._distance_heuristic, weight='length') - 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(35)): - """ - This method returns the turn decision (RoadOption) for pair of edges - around current index of route list - """ - - decision = None - previous_node = route[index-1] - current_node = route[index] - next_node = route[index+1] - next_edge = self._graph.edges[current_node, next_node] - if index > 0: - 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: - self._intersection_end_node = -1 - current_edge = self._graph.edges[previous_node, current_node] - calculate_turn = current_edge['type'] == RoadOption.LANEFOLLOW and not current_edge[ - 'intersection'] and next_edge['type'] == RoadOption.LANEFOLLOW 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['exit_vector'] - if cv is None or nv is None: - return next_edge['type'] - cross_list = [] - for neighbor in self._graph.successors(current_node): - select_edge = self._graph.edges[current_node, neighbor] - if select_edge['type'] == RoadOption.LANEFOLLOW: - 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 - elif next_cross < 0: - decision = RoadOption.LEFT - elif next_cross > 0: - decision = RoadOption.RIGHT - else: - decision = next_edge['type'] - - else: - decision = next_edge['type'] - - self._previous_decision = decision - return decision - - def abstract_route_plan(self, origin, destination): - """ - The following function generates the route plan based on - 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 are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID - CHANGELANELEFT, CHANGELANERIGHT - """ - - route = self._path_search(origin, destination) - plan = [] - - for i in range(len(route) - 1): - road_option = self._turn_decision(i, route) - plan.append(road_option) - - return plan - - def _find_closest_in_list(self, current_waypoint, waypoint_list): - min_distance = float('inf') - closest_index = -1 - for i, waypoint in enumerate(waypoint_list): - distance = waypoint.transform.location.distance( - current_waypoint.transform.location) - if distance < min_distance: - min_distance = distance - closest_index = i - - return closest_index + for fname in [txt_name, xodr_name]: + if os.path.exists(fname): + os.remove(fname) def trace_route(self, origin, destination): """ This method returns list of (carla.Waypoint, RoadOption) from origin to destination """ + route = trace_route(origin, destination, self._wmap, self._sampling_resolution) - 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() + # Add options + route_with_options = self.add_options_to_route(route) - for i in range(len(route) - 1): - road_option = self._turn_decision(i, route) - edge = self._graph.edges[route[i], route[i+1]] - path = [] + # TODO: remove it + for w in route_with_options: + wp = w[0].transform.location + carla.Location(z=0.1) + if w[1] == RoadOption.LEFT: # Yellow + color = carla.Color(255, 255, 0) + elif w[1] == RoadOption.RIGHT: # Cyan + color = carla.Color(0, 255, 255) + elif w[1] == RoadOption.CHANGELANELEFT: # Orange + color = carla.Color(255, 64, 0) + elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan + color = carla.Color(0, 64, 255) + elif w[1] == RoadOption.STRAIGHT: # Gray + color = carla.Color(128, 128, 128) + else: # LANEFOLLOW + color = carla.Color(0, 255, 0) # Green + self._world.debug.draw_point(wp, size=0.08, color=color, life_time=100) - if edge['type'] != RoadOption.LANEFOLLOW and edge['type'] != RoadOption.VOID: - 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] - if next_edge['path']: - closest_index = self._find_closest_in_list(current_waypoint, next_edge['path']) - closest_index = min(len(next_edge['path'])-1, closest_index+5) - current_waypoint = next_edge['path'][closest_index] - else: - current_waypoint = next_edge['exit_waypoint'] - route_trace.append((current_waypoint, road_option)) + return route_with_options + def add_options_to_route(self, route): + """ + This method adds the road options to a route, returning a list of [carla.Waypoint, RoadOption]. + CHANGELANELEFT and CHANGELANERIGHT are used to signalize a lane change. For the other options, + LEFT, RIGHT and STRAIGHT indicate the specific lane chosen at an intersection and outside those, + LANEFOLLOW is always used. + + This has been tested for sampling resolutions up to ~7 meters, and might fail for higher values. + + :param route (list of carla.Waypoint): list of waypoints representing the route + """ + route_with_options = [] + route_with_lane_changes = [] + + # Part 1: Add road options, excluding lane changes + if route[0].is_junction: + self._prev_at_junction = True + entry_index = 0 + else: + self._prev_at_junction = False + entry_index = None + + for i, waypoint in enumerate(route): + at_junction = waypoint.is_junction + if not at_junction and self._prev_at_junction: + # Just exited a junction, get all of its data + road_option = self._compute_options(route[entry_index], waypoint) + for j in range(entry_index, i): + route_with_options.append([route[j], road_option]) + entry_index = None + route_with_options.append([waypoint, RoadOption.LANEFOLLOW]) + elif not at_junction and not self._prev_at_junction: + # Outside a junction, always LANEFOLLOW + route_with_options.append([waypoint, RoadOption.LANEFOLLOW]) + elif not self._prev_at_junction: + # Just entered a junction, save its entrypoint and wait for the exit + entry_index = i + + self._prev_at_junction = at_junction + + # Route ended at a junction + if self._prev_at_junction: + road_option = self._compute_options(route[-1], waypoint) + for j in range(entry_index, len(route)): + route_with_options.append([route[j], road_option]) + entry_index = None + + # Part 2: Add lane changes + prev_lane_change = None + + for i in range(0, len(route_with_options) - 1): + waypoint, option = route_with_options[i] + next_waypoint, _ = route_with_options[i+1] + + # Lane changes are set to both lanes + if prev_lane_change: + route_with_lane_changes.append([waypoint, prev_lane_change]) + prev_lane_change = None + continue + + # Check the dot product between the two consecutive waypoint + direction = waypoint.transform.get_forward_vector() + np_direction = np.array([direction.x, direction.y, direction.z]) + + route_direction = next_waypoint.transform.location - waypoint.transform.location + np_route_direction = np.array([route_direction.x, route_direction.y, route_direction.z]) + + dot = np.dot(np_direction, np_route_direction) + dot /= np.linalg.norm(np_direction) * np.linalg.norm(np_route_direction) + + if 0 < dot < math.cos(math.radians(45)): + cross = np.cross(np_direction, np_route_direction) + prev_lane_change = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT + route_with_lane_changes.append([waypoint, prev_lane_change]) else: - path = path + [edge['entry_waypoint']] + edge['path'] + [edge['exit_waypoint']] - closest_index = self._find_closest_in_list(current_waypoint, path) - for waypoint in path[closest_index:]: - current_waypoint = waypoint - route_trace.append((current_waypoint, road_option)) - 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 + route_with_lane_changes.append([waypoint, option]) - return route_trace + return route_with_lane_changes + + def _compute_options(self, entry_waypoint, exit_waypoint): + """ + Computes the road option of a + """ + diff = (exit_waypoint.transform.rotation.yaw - entry_waypoint.transform.rotation.yaw) % 360 + if diff > 315.0: + option = RoadOption.STRAIGHT + elif diff > 225.0: + option = RoadOption.LEFT + elif diff > 135.0: + option = RoadOption.HALFTURN + elif diff > 45.0: + option = RoadOption.RIGHT + else: + option = RoadOption.STRAIGHT + + return option diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index 66d9a0deb..17140716d 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -11,20 +11,22 @@ import random import carla from agents.navigation.controller import VehiclePIDController -from agents.tools.misc import draw_waypoints +from agents.tools.misc import draw_waypoints, get_speed class RoadOption(Enum): """ RoadOption represents the possible topological configurations when moving from a segment of lane to other. + """ VOID = -1 LEFT = 1 RIGHT = 2 STRAIGHT = 3 - LANEFOLLOW = 4 - CHANGELANELEFT = 5 - CHANGELANERIGHT = 6 + HALFTURN = 4 + LANEFOLLOW = 5 + CHANGELANELEFT = 6 + CHANGELANERIGHT = 7 class LocalPlanner(object): @@ -58,8 +60,8 @@ class LocalPlanner(object): self._map = self._world.get_map() self._target_speed = 20.0 # Km/h - self._sampling_radius = 1.0 - self._min_distance = 8.0 + self._sampling_radius = 2.0 + self._base_min_distance = 3.0 self._dt = 1.0 / 20.0 self._max_brake = 0.3 self._max_throt = 0.75 @@ -125,10 +127,7 @@ class LocalPlanner(object): # Compute the current vehicle waypoint current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) - - # Fill the waypoint queue self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) - self._compute_next_waypoints(k=self._min_waypoint_queue_length) def set_speed(self, speed): @@ -213,7 +212,6 @@ class LocalPlanner(object): self._stop_waypoint_creation = stop_waypoint_creation - def run_step(self, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to @@ -229,24 +227,20 @@ class LocalPlanner(object): if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: self._compute_next_waypoints(k=self._min_waypoint_queue_length) - if len(self._waypoints_queue) == 0: - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - control.manual_gear_shift = False - return control - - # Get the target waypoint and move using the PID controllers - self.target_waypoint, self.target_road_option = self._waypoints_queue[0] - control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) - - # # Purge the queue of obsolete waypoints + # Purge the queue of obsolete waypoints veh_location = self._vehicle.get_location() + vehicle_speed = get_speed(self._vehicle) / 3.6 + self._min_distance = self._base_min_distance + 0.5 *vehicle_speed + num_waypoint_removed = 0 for waypoint, _ in self._waypoints_queue: - if veh_location.distance(waypoint.transform.location) < self._min_distance: + + if len(self._waypoints_queue) - num_waypoint_removed == 1: + min_distance = 1 # Don't remove the last waypoint until very close by + else: + min_distance = self._min_distance + + if veh_location.distance(waypoint.transform.location) < min_distance: num_waypoint_removed += 1 else: break @@ -255,6 +249,18 @@ class LocalPlanner(object): for _ in range(num_waypoint_removed): self._waypoints_queue.popleft() + # Get the target waypoint and move using the PID controllers. Stop if no target waypoint + if len(self._waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + else: + self.target_waypoint, self.target_road_option = self._waypoints_queue[0] + control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) + if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) @@ -303,13 +309,13 @@ def _retrieve_options(list_waypoints, current_waypoint): # the beggining of an intersection, therefore the # variation in angle is small next_next_waypoint = next_waypoint.next(3.0)[0] - link = _compute_connection(current_waypoint, next_next_waypoint) + link = compute_connection(current_waypoint, next_next_waypoint) options.append(link) return options -def _compute_connection(current_waypoint, next_waypoint, threshold=35): +def compute_connection(current_waypoint, next_waypoint, threshold=35): """ Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint (next_waypoint). diff --git a/PythonAPI/carla/agents/tools/global_route_planner_helper.py b/PythonAPI/carla/agents/tools/global_route_planner_helper.py new file mode 100644 index 000000000..3964e17db --- /dev/null +++ b/PythonAPI/carla/agents/tools/global_route_planner_helper.py @@ -0,0 +1,208 @@ +# Copyright (c) # Copyright (c) 2018-2021 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +This file has several useful functions related to the AD Map library +""" + +from __future__ import print_function + +import carla +import numpy as np +import ad_map_access as ad + +def carla_loc_to_enu(carla_location): + """Transform a CARLA location into an ENU point""" + return ad.map.point.createENUPoint(carla_location.x, -carla_location.y, carla_location.z) + +def carla_loc_to_ecef(carla_location): + """Transform a CARLA location into an ENU point""" + return ad.map.point.toECEF(carla_loc_to_enu(carla_location)) + +def enu_to_carla_loc(enu_point): + """Transform an ENU point into a CARLA location""" + return carla.Location(float(enu_point.x), float(-enu_point.y), float(enu_point.z)) + +def para_point_to_carla_waypoint(para_point, town_map, lane_type=carla.LaneType.Driving): + """Transform a para point into a CARLA waypoint""" + enu_point = ad.map.lane.getENULanePoint(para_point) + carla_point = enu_to_carla_loc(enu_point) + carla_waypoint = town_map.get_waypoint(carla_point, lane_type=lane_type) + return carla_waypoint + +def is_point_at_driving_lane(para_point, town_map): + """Checks if a parapoint is part of a CARLA driving lane""" + carla_waypoint = para_point_to_carla_waypoint(para_point, town_map, carla.LaneType.Any) + return carla_waypoint.lane_type == carla.LaneType.Driving + +def to_ad_paraPoint(location, distance=1, probability=0): + """ + Transforms a carla.Location into an ad.map.point.ParaPoint() + """ + ad_distance = ad.physics.Distance(distance) + ad_probability = ad.physics.Probability(probability) + ad_map_matching = ad.map.match.AdMapMatching() + ad_location = carla_loc_to_enu(location) + + match_results = ad_map_matching.getMapMatchedPositions(ad_location, ad_distance, ad_probability) + + if not match_results: + print("WARNING: Couldn't find a para point for CARLA location {}".format(location)) + return None + + # Filter the closest one to the given location + distance = [float(mmap.matchedPointDistance) for mmap in match_results] + return match_results[distance.index(min(distance))].lanePoint.paraPoint + +def trace_route(start_waypoint, end_waypoint, town_map, sample_resolution=1, max_distance=0.5, probability=0): + """ + Gets the shortest route between a starting and end waypoint. This transforms the given location + to AD map paraPoints, and iterates through all permutations to return the shortest route. + This is useful to ensure that the correct route is chosen when starting / ending at intersections. + + Then, the route is transformed back to a list of [carla.Waypoint, RoadOption] + + :param start_waypoint (carla.Waypoint): Starting waypoint of the route + :param end_waypoint (carla.Waypoint): Ending waypoint of the route + :param town_map (carla.Map): CARLA map instance where the route will be computed + :param sample_resolution (float): Distance between the waypoints that form the route + :param max_distance (float): Max distance between the given location and the matched AD map para points. + If this value is too large, the matching might result in waypoints on different lanes. + """ + wp_route = [] + start_location = start_waypoint.transform.location + end_location = end_waypoint.transform.location + + # Get starting point matches + start_matches = _waypoint_matches(start_waypoint, town_map, max_distance, probability) + if not start_matches: + print("WARNING: Couldn't find a paraPoint for location '{}'.".format(start_location)) + return wp_route + + # Get ending point matches + end_matches = _waypoint_matches(end_waypoint, town_map, max_distance, probability) + if not end_matches: + print("WARNING: Couldn't find a paraPoint for location '{}'.".format(end_location)) + return wp_route + + # Get the shortest route + route_segment = _filter_shortest_route(start_matches, end_matches) + if not route_segment: + print("WARNING: Couldn't find a viable route between locations " + "'{}' and '{}'.".format(start_location, end_location)) + return wp_route + + # Change the route to waypoints + wp_route = _get_route_waypoints(route_segment, sample_resolution, town_map) + return wp_route + +def _waypoint_matches(waypoint, town_map, max_distance, probability=0): + """ + Given a waypoint, maps its transform to the AD map, returning a list of possible matches. + All matches are filtered to make sure they represent driving lanes. + """ + # ECEF location of the waypoint + location = waypoint.transform.location + ecef_location= carla_loc_to_ecef(location) + + # ECEF location of a point in front of the waypoint + f_vec = waypoint.transform.get_right_vector() + front_location = location + carla.Location(x=f_vec.x, y=f_vec.y) + ecef_front_location= carla_loc_to_ecef(front_location) + + # Get the map matching and the heading hint + ecef_heading = ad.map.point.createECEFHeading(ecef_location, ecef_front_location) + ad_map_matching = ad.map.match.AdMapMatching() + ad_map_matching.addHeadingHint(ecef_heading) + + # Get the matches and filter the none driving lane ones + matches = ad_map_matching.getMapMatchedPositions( + carla_loc_to_enu(location), + ad.physics.Distance(max_distance), + ad.physics.Probability(probability) + ) + matches = [m for m in matches if is_point_at_driving_lane(m.lanePoint.paraPoint, town_map)] + + return matches + +def _filter_shortest_route(start_matches, end_matches): + """Given a set of starting and ending matches, computes all possible routes and selects the shortest one""" + route_segment = None + # Get the shortest route + min_length = float('inf') + for start_match in start_matches: + start_point = start_match.lanePoint.paraPoint + for end_match in end_matches: + # Get the route + new_route_segment = ad.map.route.planRoute( + start_point, end_match.lanePoint.paraPoint, ad.map.route.RouteCreationMode.Undefined) + if len(new_route_segment.roadSegments) == 0: + continue # The route doesn't exist, ignore it + + # Save the shortest route + length = _get_route_length(new_route_segment) + if length < min_length: + min_length = length + route_segment = new_route_segment + + return route_segment + +def _get_route_length(route): + """ + Gets the length of the route, being the sum of the road segment lengths. + Each road segment length is the mean of the length of its lane segments. + """ + route_length = 0 + for road_segment in route.roadSegments: + road_length = 0 + for lane_segment in road_segment.drivableLaneSegments: + lane_start = float(lane_segment.laneInterval.start) + lane_end = float(lane_segment.laneInterval.end) + lane_length = float(ad.map.lane.calcLength(lane_segment.laneInterval.laneId)) + + road_length += lane_length * abs(lane_end - lane_start) + + num_lanes = len(road_segment.drivableLaneSegments) + if num_lanes != 0: + route_length += road_length / num_lanes + + return route_length + +def _get_route_waypoints(route, resolution, town_map): + """ + Given a route, transforms it into a list of [carla.Waypoint, RoadOption] + + :param route (ad.map.route.FullRoute): AD map route instance created with RouteCreationMode Undefined. + Other creation modes return mode than one lane, which would need a prefiltering. + :param resolution (float): Distance between the waypoints that form the route. + :param town_map (carla.Map): CARLA map instance where the route will be computed + """ + wp_route = [] + for road_segment in route.roadSegments: + for lane_segment in road_segment.drivableLaneSegments: + lane_id = lane_segment.laneInterval.laneId + param_list = _get_lane_interval_list(lane_segment.laneInterval, resolution) + for i in range(len(param_list)): + para_point = ad.map.point.createParaPoint(lane_id, ad.physics.ParametricValue(param_list[i])) + carla_waypoint = para_point_to_carla_waypoint(para_point, town_map) + wp_route.append(carla_waypoint) + # if i == 0: + # world.debug.draw_point( + # carla_waypoint.transform.location, size=0.2, life_time=10000, color=carla.Color(255,0,0)) + # world.debug.draw_point( + # carla_waypoint.transform.location, size=0.1, life_time=10000, color=carla.Color(255,255,0)) + + return wp_route + +def _get_lane_interval_list(lane_interval, distance=1): + """ + Separates a given lane interval into smaller intervals of length equal to 'distance' + """ + start = float(lane_interval.start) + end = float(lane_interval.end) + length = float(ad.map.lane.calcLength(lane_interval.laneId)) + if start == end: + return [] + return np.arange(start, end, np.sign(end - start) * distance / length) diff --git a/PythonAPI/carla/agents/tools/map_helper.py b/PythonAPI/carla/agents/tools/map_helper.py deleted file mode 100644 index c690f4bb2..000000000 --- a/PythonAPI/carla/agents/tools/map_helper.py +++ /dev/null @@ -1,203 +0,0 @@ -# Copyright (c) # Copyright (c) 2018-2021 CVC. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" -This file has several useful functions related to the AD Map library -""" - -from __future__ import print_function - -import carla -import numpy as np -import ad_map_access as ad - -def carla_loc_to_enu(carla_location): - """Transform a CARLA location into an ENU point""" - return ad.map.point.createENUPoint(carla_location.x, -carla_location.y, carla_location.z) - -def enu_to_carla_loc(enu_point): - """Transform an ENU point into a CARLA location""" - return carla.Location(float(enu_point.x), float(-enu_point.y), float(enu_point.z)) - -def para_point_to_carla_waypoint(para_point, town_map, lane_type=carla.LaneType.Driving): - """Transform a para point into a CARLA waypoint""" - enu_point = ad.map.lane.getENULanePoint(para_point) - carla_point = enu_to_carla_loc(enu_point) - carla_waypoint = town_map.get_waypoint(carla_point, lane_type=lane_type) - return carla_waypoint - -def is_point_at_driving_lane(para_point, town_map): - """Checks if a parapoint is part of a CARLA driving lane""" - carla_waypoint = para_point_to_carla_waypoint(para_point, town_map, carla.LaneType.Any) - return carla_waypoint.lane_type == carla.LaneType.Driving - -def get_shortest_route(start_location, end_location, town_map, sample_resolution=1, distance=1, probability=0, world=None): - """ - Gets the shortest route between a starting and end location. - """ - route = [] - - ad_distance = ad.physics.Distance(distance) - ad_probability = ad.physics.Probability(probability) - ad_map_matching = ad.map.match.AdMapMatching() - route_segment = None - - start_matches = ad_map_matching.getMapMatchedPositions( - carla_loc_to_enu(start_location), ad_distance, ad_probability) - end_matches = ad_map_matching.getMapMatchedPositions( - carla_loc_to_enu(end_location), ad_distance, ad_probability) - - if not start_matches: - print("WARNING: Couldn't find a paraPoint for location '{}'.".format(start_location)) - return route - if not end_matches: - print("WARNING: Couldn't find a paraPoint for location '{}'.".format(end_location)) - return route - - # Filter the paraPoints that aren't part of driving lanes - start_matches = [match for match in start_matches if is_point_at_driving_lane(match.lanePoint.paraPoint, town_map)] - end_matches = [match for match in end_matches if is_point_at_driving_lane(match.lanePoint.paraPoint, town_map)] - - # Get the shortest route - min_length = float('inf') - for start_match in start_matches: - start_point = start_match.lanePoint.paraPoint - for end_match in end_matches: - # Get the route - new_route_segment = ad.map.route.planRoute(start_point, end_match.lanePoint.paraPoint) - if len(new_route_segment.roadSegments) == 0: - continue # The route doesn't exist, ignore it - - # Calculate route length, as a sum of the mean of the road's lanes length - length = 0 - for road_segment in new_route_segment.roadSegments: - road_length = 0 - number_lanes = 0 - for lane_segment in road_segment.drivableLaneSegments: - seg_start = float(lane_segment.laneInterval.start) - seg_end = float(lane_segment.laneInterval.end) - seg_length = float(ad.map.lane.calcLength(lane_segment.laneInterval.laneId)) - - road_length += seg_length * abs(seg_end - seg_start) - number_lanes += 1 - - if number_lanes != 0: - length += road_length / number_lanes - - # Save the shortest route - if length < min_length: - min_length = length - route_segment = new_route_segment - start_lane_id = start_point.laneId - - if not route_segment: - print("WARNING: Couldn't find a viable route between locations " - "'{}' and '{}'.".format(start_location, end_location)) - return route - - # print(route_segment) - - # Transform the AD map route representation into waypoints (If not needed to parse the altitude) - for segment in get_route_lane_list(route_segment, start_lane_id): - lane_id = segment.laneInterval.laneId - param_list = get_lane_interval_list(segment.laneInterval, sample_resolution) - for i in range(len(param_list)): - para_point = ad.map.point.createParaPoint(lane_id, ad.physics.ParametricValue(param_list[i])) - carla_waypoint = para_point_to_carla_waypoint(para_point, town_map) - route.append(carla_waypoint) - if i == 0: - world.debug.draw_point(carla_waypoint.transform.location, size=0.2, life_time=10000, color=carla.Color(255,0,0)) - world.debug.draw_point(carla_waypoint.transform.location, size=0.1, life_time=10000, color=carla.Color(255,255,0)) - - return route - -def get_lane_altitude_list(start_z, end_z, length): - """ - Gets the z values of a lane. This is a simple linear interpolation - and it won't be necessary whenever the AD map parses the altitude - """ - if length == 0: - return [] - if start_z == end_z: - return start_z*np.ones(length) - return np.arange(start_z, end_z, (end_z - start_z) / length) - -def get_route_lane_list(route, start_lane_id, prev_lane_id=None, prev_successors=[]): - """ - Given a route returns the lane segments corresponding to the route. - 'start_lane_id' is the lane id of the first point in the route. - In case this function is called more than once, use 'prev_lane_id' - and 'prev_successors' to keep computing the lanes without any information loss. - """ - segments = [] - - for road_segment in route.roadSegments: - current_segment = None - for lane_segment in road_segment.drivableLaneSegments: - successors = lane_segment.successors - predecessors = lane_segment.predecessors - lane_id = lane_segment.laneInterval.laneId - - if not prev_lane_id: - # First lane, match the lane with the starting lane id - if lane_segment.laneInterval.laneId != start_lane_id: - continue # match the lane to the starting point - - if prev_lane_id and prev_lane_id not in predecessors: - continue - - if prev_successors and lane_id not in prev_successors: - continue - - current_segment = lane_segment - break - - # Basically, the next lane won't be chosen if the current one has no successors, as it means - # that the next lane diverges from the route. Watch out with false empty successors though - - if not current_segment: - # print(len(prev_successors)) - # print("The previous lane diverged from the route, lane changing") - # TODO: This shouldn't really be random - current_segment = np.random.choice(road_segment.drivableLaneSegments) - # else: - # print(len(prev_successors)) - # print("Found a connected route lane") - - segments.append(current_segment) - prev_lane_id = current_segment.laneInterval.laneId - prev_successors = current_segment.successors - - return segments - -def to_ad_paraPoint(location, distance=1, probability=0): - """ - Transforms a carla.Location into an ad.map.point.ParaPoint() - """ - ad_distance = ad.physics.Distance(distance) - ad_probability = ad.physics.Probability(probability) - ad_map_matching = ad.map.match.AdMapMatching() - ad_location = carla_loc_to_enu(location) - - match_results = ad_map_matching.getMapMatchedPositions(ad_location, ad_distance, ad_probability) - - if not match_results: - print("WARNING: Couldn't find a para point for CARLA location {}".format(location)) - return None - - # Filter the closest one to the given location - distance = [float(mmap.matchedPointDistance) for mmap in match_results] - return match_results[distance.index(min(distance))].lanePoint.paraPoint - -def get_lane_interval_list(lane_interval, distance=1): - """ - Separates a given lane interval into smaller intervals of length equal to 'distance' - """ - start = float(lane_interval.start) - end = float(lane_interval.end) - length = float(ad.map.lane.calcLength(lane_interval.laneId)) - if start == end: - return [] - return np.arange(start, end, np.sign(end - start) * distance / length) \ No newline at end of file