Improved vehicle detection at junction

PythonAPI/carla/agents/navigation/basic_agent.py

@@ -11,6 +11,7 @@ It can also make use of the global route planner to follow a specifed route
 
 import carla
 from enum import Enum
+from shapely.geometry import Polygon
 
 from agents.navigation.local_planner import LocalPlanner
 from agents.navigation.global_route_planner import GlobalRoutePlanner
@@ -281,21 +282,65 @@ class BasicAgent(object):
         for target_vehicle in vehicle_list:
             target_transform = target_vehicle.get_transform()
             target_wpt = self._map.get_waypoint(target_transform.location)
-            if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id:
-                next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0]
-                if not next_wpt:
-                    continue
-                if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id:
-                    continue
 
-            target_forward_vector = target_transform.get_forward_vector()
-            target_extent = target_vehicle.bounding_box.extent.x
-            target_rear_transform = target_transform
-            target_rear_transform.location -= carla.Location(
-                x=target_extent * target_forward_vector.x,
-                y=target_extent * target_forward_vector.y,
-            )
+            # Simplified version for outside junctions
+            if not ego_wpt.is_junction or not target_wpt.is_junction:
+
+                if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id:
+                    next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0]
+                    if not next_wpt:
+                        continue
+                    if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id:
+                        continue
+
+                target_forward_vector = target_transform.get_forward_vector()
+                target_extent = target_vehicle.bounding_box.extent.x
+                target_rear_transform = target_transform
+                target_rear_transform.location -= carla.Location(
+                    x=target_extent * target_forward_vector.x,
+                    y=target_extent * target_forward_vector.y,
+                )
+
+                if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]):
+                    return (True, target_vehicle)
+
+            # Waypoints aren't reliable, check the proximity of the vehicle to the route
+            else:
+                ego_location = ego_transform.location
+                lateral_extent = self._vehicle.bounding_box.extent.y
+
+                # Get geodesic boudnary box
+                route_bb = []
+                for wp, _ in self._local_planner.get_plan():
+                    if ego_location.distance(wp.transform.location) > max_distance:
+                        break
+
+                    right_vector = wp.transform.get_right_vector()
+                    right_point = wp.transform.location + carla.Location(
+                        lateral_extent * right_vector.x, lateral_extent * right_vector.y)
+                    route_bb.append([right_point.x, right_point.y, right_point.z])
+                    left_point = wp.transform.location + carla.Location(
+                        - lateral_extent * right_vector.x, - lateral_extent * right_vector.y)
+                    route_bb.append([left_point.x, left_point.y, left_point.z])
+
+                ego_polygon = Polygon(route_bb)
+
+                # Compare the two polygons
+                for target_vehicle in vehicle_list:
+                    target_extent = target_vehicle.bounding_box.extent.x
+                    if target_vehicle.id == self._vehicle.id:
+                        continue
+                    if ego_location.distance(target_vehicle.get_location()) > max_distance:
+                        continue
+
+                    target_bb = target_vehicle.bounding_box
+                    target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform())
+                    target_list = [[v.x, v.y, v.z] for v in target_vertices]
+                    target_polygon = Polygon(target_list)
+
+                    if ego_polygon.intersects(target_polygon):
+                        return (True, target_vehicle)
+
+                return (False, None)
 
-            if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]):
-                return (True, target_vehicle)
         return (False, None)

PythonAPI/carla/agents/navigation/local_planner.py

@@ -181,12 +181,12 @@ class LocalPlanner(object):
     def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True):
         """
         Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs
-        If 'clean_queue`, erases the previous plan, and if not, it is added to the old one
-        The 'stop_waypoint_creation' flag avoids creating more random waypoints
+        The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one
+        The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints
 
         :param current_plan: list of (carla.Waypoint, RoadOption)
         :param stop_waypoint_creation: bool
-        :param ceal_queue: bool
+        :param clean_queue: bool
         :return:
         """
         if clean_queue:
@@ -275,6 +275,10 @@ class LocalPlanner(object):
             except IndexError as i:
                 return None, RoadOption.VOID
 
+    def get_plan(self):
+        """Returns the current plan of the local planner"""
+        return self._waypoints_queue
+
     def done(self):
         """
         Returns whether or not the planner has finished

PythonAPI/carla/requirements.txt

@@ -2,3 +2,4 @@ networkx
 numpy; python_version < '3.0'
 numpy==1.18.4; python_version >= '3.0'
 distro
+Shapely==1.6.4.post2