From 23a6091109e9fd4ee401a3f2d33d83cef887a415 Mon Sep 17 00:00:00 2001
From: Guillermo <glopez@cvc.uab.es>
Date: Fri, 2 Jul 2021 13:30:29 +0200
Subject: [PATCH] Another lane change option iteration

---
 .../carla/agents/navigation/basic_agent.py    | 31 ++++++++++---------
 .../agents/navigation/global_route_planner.py | 31 ++++++++++---------
 2 files changed, 32 insertions(+), 30 deletions(-)

diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py
index 7f95f0bb8..e8b3a0cef 100644
--- a/PythonAPI/carla/agents/navigation/basic_agent.py
+++ b/PythonAPI/carla/agents/navigation/basic_agent.py
@@ -116,21 +116,22 @@ class BasicAgent(object):
         route_trace = self.trace_route(start_waypoint, end_waypoint)
         self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue)
 
-        for w in route_trace:
-            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)
+        # for i, w in enumerate(route_trace):
+        #     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)
+        #     self._world.debug.draw_string(wp, str(i), color=color, life_time=100)
 
     def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True):
         """
diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py
index c467213fa..e30dfd00c 100644
--- a/PythonAPI/carla/agents/navigation/global_route_planner.py
+++ b/PythonAPI/carla/agents/navigation/global_route_planner.py
@@ -59,6 +59,8 @@ class GlobalRoutePlanner(object):
         LANEFOLLOW is always used.
 
         This has been tested for sampling resolutions up to ~7 meters, and might fail for higher values.
+        As lane changes are marked in pairs, it will fail for even number of consecutive lane changes
+        (as they have an odd amount of lane change waypoints)
 
         :param route (list of carla.Waypoint): list of waypoints representing the route
         """
@@ -98,7 +100,8 @@ class GlobalRoutePlanner(object):
                 route_with_options.append([route[j], road_option])
             entry_index = None
 
-        # Part 2: Add lane changes
+        # Part 2: Add lane changes. Use the route direction and not the waypoint as waypoints might not
+        # correspond to the correct lane
         lane_change_type = None
 
         for i in range(0, len(route_with_options)):
@@ -112,6 +115,14 @@ class GlobalRoutePlanner(object):
                 prev_direction = waypoint.transform.get_forward_vector()
                 np_prev_direction = np.array([prev_direction.x, prev_direction.y, prev_direction.z])
 
+            # Lane changes are marked at both lanes
+            if lane_change_type:
+                route_with_lane_changes.append([waypoint, lane_change_type])
+                prev_direction = waypoint.transform.get_forward_vector()
+                np_prev_direction = np.array([prev_direction.x, prev_direction.y, prev_direction.z])
+                lane_change_type = None
+                continue
+
             # Check the dot product between the two consecutive waypoint
             route_direction = route_with_options[i+1][0].transform.location - waypoint.transform.location
             np_route_direction = np.array([route_direction.x, route_direction.y, route_direction.z])
@@ -123,21 +134,11 @@ class GlobalRoutePlanner(object):
                 dot = 1
 
             if dot < math.cos(math.radians(45)):
-                if lane_change_type:
-                    # Last lane change waypoint
-                    new_option = lane_change_type
-                    lane_change_type = None
-                else:
-                    # First lane change waypoint
-                    cross = np.cross(np_prev_direction, np_route_direction)
-                    lane_change_type = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT
-                    new_option = lane_change_type
-
+                cross = np.cross(np_prev_direction, np_route_direction)
+                lane_change_type = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT
+                new_option = lane_change_type
             else:
-                if lane_change_type:
-                    new_option = lane_change_type
-                else:
-                    new_option = option
+                new_option = option
 
             route_with_lane_changes.append([waypoint, new_option])
             np_prev_direction = np_route_direction