debugging the global router for basic_agent

PythonAPI/agents/navigation/basic_agent.py

@@ -43,7 +43,7 @@ class BasicAgent(object):
         self._map = self._vehicle.get_world().get_map()
         self._proximity_threshold = 10.0  # meters
         self._state = AgentState.NAVIGATING
-        self._local_planner = LocalPlanner(self._vehicle)
+        self._local_planner = LocalPlanner(self._vehicle, plan=None)
 
         # setting up global router
         self._current_plan = None

PythonAPI/agents/navigation/local_planner.py

@@ -13,6 +13,7 @@ from collections import deque
 import random
 
 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):
@@ -39,7 +40,7 @@ class LocalPlanner(object):
     # total distance)
     MIN_DISTANCE_PERCENTAGE = 0.9
 
-    def __init__(self, vehicle, opt_dict={}):
+    def __init__(self, vehicle, opt_dict={}, plan=None):
         """
         :param vehicle: actor to apply to local planner logic onto
         :param opt_dict: dictionary of arguments with the following semantics:
@@ -73,13 +74,13 @@ class LocalPlanner(object):
         self._waypoints_queue = deque(maxlen=200)
 
         #
-        self.init_controller(opt_dict)
+        self.init_controller(opt_dict, plan)
 
     def __del__(self):
         self._vehicle.destroy()
         print("Destroying ego-vehicle!")
 
-    def init_controller(self, opt_dict):
+    def init_controller(self, opt_dict, plan):
         """
         Controller initialization.
 
@@ -131,7 +132,10 @@ class LocalPlanner(object):
         self._target_road_option = RoadOption.LANEFOLLOW
 
         # fill waypoint trajectory queue
-        self._compute_next_waypoints_global_plan(k=200)
+        if plan is None:
+            self._compute_next_waypoints(k=200)
+        else:
+            self._compute_next_waypoints_global_plan(k=200)
 
     def set_speed(self, speed):
         """
@@ -181,13 +185,16 @@ class LocalPlanner(object):
         :return:
         """
         # check we do not overflow the queue
+        print(self._global_plan)
+
         available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue)
         k = min(available_entries, k)
 
-        for _ in range(k):
+        for i in range(k):
             last_waypoint = self._waypoints_queue[-1][0]
             next_waypoints = list(last_waypoint.next(self._sampling_radius))
 
+            print(i)
             if len(next_waypoints) == 1:
                 # only one option available ==> lanefollowing
                 next_waypoint = next_waypoints[0]
@@ -197,13 +204,16 @@ class LocalPlanner(object):
                 road_options_list = retrieve_options(next_waypoints, last_waypoint)
                 planned_option = self._global_plan.pop(0)
 
+                print("--- PLanned option = {}".format(planned_option))
+                print("+++ List options = {}".format(road_options_list))
+
                 if planned_option == NavEnum.FOLLOW_LANE:
                     planned_option = RoadOption.LANEFOLLOW
-                if planned_option == NavEnum.LEFT:
-                    planned_option = RedOption.LEFT
-                if planned_option == NavEnum.RIGHT:
-                    planned_option = ReadOption.RIGHT
-                if planned_option == NavEnum.GO_STRAIGHT:
+                elif planned_option == NavEnum.LEFT:
+                    planned_option = RoadOption.LEFT
+                elif planned_option == NavEnum.RIGHT:
+                    planned_option = RoadOption.RIGHT
+                elif planned_option == NavEnum.GO_STRAIGHT:
                     planned_option = RoadOption.STRAIGHT
                 else:
                     planned_option = RoadOption.VOID
@@ -214,12 +224,21 @@ class LocalPlanner(object):
                     print('Option not found!!')
                     import pdb; pdb.set_trace()
 
+                road_option = planned_option
+
             self._waypoints_queue.append((next_waypoint, road_option))
 
     def set_global_plan(self, current_plan):
+        self._waypoints_queue.clear()
         self._global_plan = current_plan
         self._global_plan.pop(0)
 
+        # compute initial waypoints
+        self._waypoints_queue.append((self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW))
+        self._target_road_option = RoadOption.LANEFOLLOW
+        self._compute_next_waypoints_global_plan(k=100)
+
+
     def run_step(self, debug=True):
         """
         Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
@@ -231,7 +250,10 @@ class LocalPlanner(object):
 
         # not enough waypoints in the horizon? => add more!
         if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
-            self._compute_next_waypoints_global_plan(k=100)
+            if not self._global_plan:
+                self._compute_next_waypoints(k=100)
+            else:
+                self._compute_next_waypoints_global_plan(k=100)
 
         # current vehicle waypoint
         self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())

PythonAPI/automatic_control.py

@@ -75,7 +75,9 @@ except IndexError:
 
 import carla
 from carla import ColorConverter as cc
-from agents.navigation.roaming_agent import *
+#from agents.navigation.roaming_agent import *
+from agents.navigation.basic_agent import *
+
 
 
 # ==============================================================================
@@ -614,7 +616,8 @@ def game_loop(args):
         world = World(client.get_world(), hud)
         controller = KeyboardControl(world, False)
 
-        agent = RoamingAgent(world.vehicle)
+        agent = BasicAgent(world.vehicle)
+        agent.set_destination((0.8, 48.4))
 
         clock = pygame.time.Clock()
         while True: