Waypoint buffer cleans on route reset (#3264)

CHANGELOG.md

@@ -53,8 +53,10 @@
   * Replace deprectated `platform.dist()` with recommended `distro.linux_distribution()`
   * Improved the performance on capture sensors.
   * Fixed minor typo in the introduction section of documentation.
+  * Fixed a bug at the local planner when changing the route, causing it to maintain the first part of the previous one. This was only relevant when using very large buffer sizes.
   * Added automatic calculation of vehicle's BB
 
+
 ## CARLA 0.9.9
 
   * Introduced hybrid mode for Traffic Manager

PythonAPI/carla/agents/navigation/behavior_agent.py

@@ -118,7 +118,7 @@ class BehaviorAgent(Agent):
 
         route_trace = self._trace_route(self.start_waypoint, self.end_waypoint)
 
-        self._local_planner.set_global_plan(route_trace)
+        self._local_planner.set_global_plan(route_trace, clean)
 
     def reroute(self, spawn_points):
         """

PythonAPI/carla/agents/navigation/local_planner.py

@@ -191,10 +191,29 @@ class LocalPlanner(object):
             self._waypoints_queue.append((next_waypoint, road_option))
 
     def set_global_plan(self, current_plan):
+        """
+        Resets the waypoint queue and buffer to match the new plan. Also
+        sets the global_plan flag to avoid creating more waypoints
+
+        :param current_plan: list of (carla.Waypoint, RoadOption)
+        :return:
+        """
+
+        # Reset the queue
         self._waypoints_queue.clear()
         for elem in current_plan:
             self._waypoints_queue.append(elem)
         self._target_road_option = RoadOption.LANEFOLLOW
+
+        # and the buffer
+        self._waypoint_buffer.clear()
+        for _ in range(self._buffer_size):
+            if self._waypoints_queue:
+                self._waypoint_buffer.append(
+                    self._waypoints_queue.popleft())
+            else:
+                break
+
         self._global_plan = True
 
     def run_step(self, debug=False):
@@ -203,7 +222,7 @@ class LocalPlanner(object):
         follow the waypoints trajectory.
 
         :param debug: boolean flag to activate waypoints debugging
-        :return:
+        :return: control to be applied
         """
 
         # not enough waypoints in the horizon? => add more!
@@ -222,7 +241,7 @@ class LocalPlanner(object):
 
         #   Buffering the waypoints
         if not self._waypoint_buffer:
-            for i in range(self._buffer_size):
+            for _ in range(self._buffer_size):
                 if self._waypoints_queue:
                     self._waypoint_buffer.append(
                         self._waypoints_queue.popleft())
@@ -253,6 +272,11 @@ class LocalPlanner(object):
         return control
 
     def done(self):
+        """
+        Returns whether or not the planner has finished
+
+        :return: boolean
+        """
         return len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0
 
 def _retrieve_options(list_waypoints, current_waypoint):

PythonAPI/carla/agents/navigation/local_planner_behavior.py

@@ -135,7 +135,7 @@ class LocalPlanner(object):
 
         self._target_speed = speed
 
-    def set_global_plan(self, current_plan):
+    def set_global_plan(self, current_plan, clean=False):
         """
         Sets new global plan.
 
@@ -143,6 +143,16 @@ class LocalPlanner(object):
         """
         for elem in current_plan:
             self.waypoints_queue.append(elem)
+
+        if clean:
+            self._waypoint_buffer.clear()
+            for _ in range(self._buffer_size):
+                if self.waypoints_queue:
+                    self._waypoint_buffer.append(
+                        self.waypoints_queue.popleft())
+                else:
+                    break
+
         self._global_plan = True
 
     def get_incoming_waypoint_and_direction(self, steps=3):