Merge pull request #1198 from carla-simulator/pravinblaze-agent

Pravinblaze agent

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

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):
     """
@@ -31,46 +38,85 @@ class BasicAgent(Agent):
         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],
+        end_waypoint = self._map.get_waypoint(
-                                                             location[1],
+            carla.Location(location[0], location[1], location[2]))
-                                                             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 = []
+            #   Generate waypoints to next junction
-        while not solution:
+            wp_choice = current_waypoint.next(self._hop_resolution)
-            for _ in range(len(active_list)):
+            while len(wp_choice) == 1:
-                trajectory = active_list.pop()
+                current_waypoint = wp_choice[0]
-                if len(trajectory) > 1000:
+                solution.append((current_waypoint, RoadOption.LANEFOLLOW))
-                    continue
+                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
+            #   Select appropriate path at the junction
-                current_waypoint, _ = trajectory[-1]
+            if len(wp_choice) > 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
 
-                    # keep adding nodes
+                # Current heading vector
-                    trajectory.append((next_waypoints[0], next_option))
+                current_transform = current_waypoint.transform
-                    current_waypoint, _ = trajectory[-1]
+                current_location = current_transform.location
-                    next_waypoints = current_waypoint.next(5.0)
+                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:
+                direction = 0
-                    # multiple choices
+                if action == RoadOption.LEFT:
-                    for waypoint in next_waypoints:
+                    direction = 1
-                        next_option = compute_connection(current_waypoint, waypoint)
+                elif action == RoadOption.RIGHT:
-                        active_list.append(trajectory + [(waypoint, next_option)])
+                    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
 

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
+        agents.navigation.local_planner.RoadOption elements
-        Possible values (for now) are START, GO_STRAIGHT, LEFT, RIGHT, STOP
+        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]

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)

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]