diff --git a/CHANGELOG.md b/CHANGELOG.md index 7315b0c2e..33cadadde 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,8 @@ ## Latest changes * Fixed global route planner to handle round about turns and made the code consistent with local planner + * Basic agent integrated with global router + * Fixed local planner to avoid premature route pruning at path overlaps * Upgraded to Unreal Engine 4.21 * Upgraded Boost to 1.69.0 * Added point transformation functionality for LibCarla and PythonAPI diff --git a/PythonAPI/agents/navigation/basic_agent.py b/PythonAPI/agents/navigation/basic_agent.py index a0234f55f..818295868 100644 --- a/PythonAPI/agents/navigation/basic_agent.py +++ b/PythonAPI/agents/navigation/basic_agent.py @@ -10,10 +10,17 @@ waypoints and avoiding other vehicles. The agent also responds to traffic lights. """ +import math + +import numpy as np + import carla from agents.navigation.agent import * from agents.navigation.local_planner import LocalPlanner from agents.navigation.local_planner import compute_connection, RoadOption +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO +from agents.tools.misc import vector class BasicAgent(Agent): """ @@ -28,49 +35,88 @@ class BasicAgent(Agent): """ super(BasicAgent, self).__init__(vehicle) - self._proximity_threshold = 10.0 # meters + self._proximity_threshold = 10.0 # meters self._state = AgentState.NAVIGATING self._local_planner = LocalPlanner(self._vehicle, opt_dict={'target_speed' : target_speed}) + self._hop_resolution = 2.0 # setting up global router self._current_plan = None def set_destination(self, location): start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - end_waypoint = self._map.get_waypoint(carla.Location(location[0], - location[1], - location[2])) + end_waypoint = self._map.get_waypoint( + carla.Location(location[0], location[1], location[2])) + solution = [] + + # Setting up global router + dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map()) + grp = GlobalRoutePlanner(dao) + grp.setup() + + # Obtain route plan + x1 = start_waypoint.transform.location.x + y1 = start_waypoint.transform.location.y + x2 = end_waypoint.transform.location.x + y2 = end_waypoint.transform.location.y + route = grp.plan_route((x1, y1), (x2, y2)) current_waypoint = start_waypoint - active_list = [ [(current_waypoint, RoadOption.LANEFOLLOW)] ] + route.append(RoadOption.VOID) + for action in route: - solution = [] - while not solution: - for _ in range(len(active_list)): - trajectory = active_list.pop() - if len(trajectory) > 1000: - continue + # Generate waypoints to next junction + wp_choice = current_waypoint.next(self._hop_resolution) + while len(wp_choice) == 1: + current_waypoint = wp_choice[0] + solution.append((current_waypoint, RoadOption.LANEFOLLOW)) + wp_choice = current_waypoint.next(self._hop_resolution) + # Stop at destination + if current_waypoint.transform.location.distance( + end_waypoint.transform.location) < self._hop_resolution: break + if action == RoadOption.VOID: break - # expand this trajectory - current_waypoint, _ = trajectory[-1] - next_waypoints = current_waypoint.next(5.0) - while len(next_waypoints) == 1: - next_option = compute_connection(current_waypoint, next_waypoints[0]) - current_distance = next_waypoints[0].transform.location.distance(end_waypoint.transform.location) - if current_distance < 5.0: - solution = trajectory + [(end_waypoint, RoadOption.LANEFOLLOW)] - break + # Select appropriate path at the junction + if len(wp_choice) > 1: - # keep adding nodes - trajectory.append((next_waypoints[0], next_option)) - current_waypoint, _ = trajectory[-1] - next_waypoints = current_waypoint.next(5.0) + # Current heading vector + current_transform = current_waypoint.transform + current_location = current_transform.location + projected_location = current_location + \ + carla.Location( + x=math.cos(math.radians(current_transform.rotation.yaw)), + y=math.sin(math.radians(current_transform.rotation.yaw))) + v_current = vector(current_location, projected_location) - if not solution: - # multiple choices - for waypoint in next_waypoints: - next_option = compute_connection(current_waypoint, waypoint) - active_list.append(trajectory + [(waypoint, next_option)]) + direction = 0 + if action == RoadOption.LEFT: + direction = 1 + elif action == RoadOption.RIGHT: + direction = -1 + elif action == RoadOption.STRAIGHT: + direction = 0 + select_criteria = float('inf') + + # Choose correct path + for wp_select in wp_choice: + v_select = vector( + current_location, wp_select.transform.location) + cross = float('inf') + if direction == 0: + cross = abs(np.cross(v_current, v_select)[-1]) + else: + cross = direction*np.cross(v_current, v_select)[-1] + if cross < select_criteria: + select_criteria = cross + current_waypoint = wp_select + + # Generate all waypoints within the junction + # along selected path + solution.append((current_waypoint, action)) + current_waypoint = current_waypoint.next(self._hop_resolution)[0] + while current_waypoint.is_intersection: + solution.append((current_waypoint, action)) + current_waypoint = current_waypoint.next(self._hop_resolution)[0] assert solution diff --git a/PythonAPI/agents/navigation/global_route_planner.py b/PythonAPI/agents/navigation/global_route_planner.py index add4d48c6..94c1c0c10 100644 --- a/PythonAPI/agents/navigation/global_route_planner.py +++ b/PythonAPI/agents/navigation/global_route_planner.py @@ -46,13 +46,12 @@ class GlobalRoutePlanner(object): 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 - return : list of turn by turn navigation decisions as - NavEnum elements - Possible values (for now) are START, GO_STRAIGHT, LEFT, RIGHT, STOP + agents.navigation.local_planner.RoadOption elements + Possible values (for now) are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID """ - threshold = math.radians(5.0) + threshold = math.radians(4.0) route = self.path_search(origin, destination) plan = [] @@ -99,10 +98,8 @@ class GlobalRoutePlanner(object): """ 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 """ @@ -123,7 +120,6 @@ class GlobalRoutePlanner(object): """ 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 """ @@ -156,7 +152,6 @@ class GlobalRoutePlanner(object): 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 """ @@ -194,10 +189,8 @@ class GlobalRoutePlanner(object): 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 @@ -207,10 +200,8 @@ class GlobalRoutePlanner(object): 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 """ @@ -226,10 +217,8 @@ class GlobalRoutePlanner(object): 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] diff --git a/PythonAPI/agents/navigation/local_planner.py b/PythonAPI/agents/navigation/local_planner.py index 977ee2a58..ff2460f71 100644 --- a/PythonAPI/agents/navigation/local_planner.py +++ b/PythonAPI/agents/navigation/local_planner.py @@ -14,7 +14,6 @@ import random import carla from agents.navigation.controller import VehiclePIDController -from agents.navigation.global_route_planner import NavEnum from agents.tools.misc import distance_vehicle, draw_waypoints class RoadOption(Enum): @@ -72,9 +71,11 @@ class LocalPlanner(object): self._vehicle_controller = None self._global_plan = None # queue with tuples of (waypoint, RoadOption) - self._waypoints_queue = deque(maxlen=200) + self._waypoints_queue = deque(maxlen=600) + self._buffer_size = 5 + self._waypoint_buffer = deque(maxlen=self._buffer_size) - # + # initializing controller self.init_controller(opt_dict) def __del__(self): @@ -202,10 +203,19 @@ class LocalPlanner(object): return control + # Buffering the waypoints + if not self._waypoint_buffer: + for i in range(self._buffer_size): + if self._waypoints_queue: + self._waypoint_buffer.append( + self._waypoints_queue.popleft()) + else: + break + # current vehicle waypoint self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) # target waypoint - self._target_waypoint, self._target_road_option = self._waypoints_queue[0] + self._target_waypoint, self._target_road_option = self._waypoint_buffer[0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint) @@ -213,13 +223,13 @@ class LocalPlanner(object): vehicle_transform = self._vehicle.get_transform() max_index = -1 - for i, (waypoint, _) in enumerate(self._waypoints_queue): + for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle( waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): - self._waypoints_queue.popleft() + self._waypoint_buffer.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0) diff --git a/PythonAPI/agents/tools/misc.py b/PythonAPI/agents/tools/misc.py index a65322a32..b0ab1eb50 100644 --- a/PythonAPI/agents/tools/misc.py +++ b/PythonAPI/agents/tools/misc.py @@ -88,3 +88,15 @@ def distance_vehicle(waypoint, vehicle_transform): dy = waypoint.transform.location.y - loc.y return math.sqrt(dx * dx + dy * dy) + +def vector(location_1, location_2): + """ + Returns the unit vector from location_1 to location_2 + location_1, location_2 : carla.Location objects + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + + return [x/norm, y/norm, z/norm]