From 3360d27486e5b173cbd7cd3707586e173ba20460 Mon Sep 17 00:00:00 2001
From: glopezdiest <58212725+glopezdiest@users.noreply.github.com>
Date: Tue, 27 Apr 2021 15:02:18 +0200
Subject: [PATCH] Improved agent vehicle detection (#3973)

* Improved agent vehicle detection

* Minor CHANGELOG error

Co-authored-by: Jacopo Bartiromo <32928804+jackbart94@users.noreply.github.com>
---
 CHANGELOG.md                               |  3 ++-
 PythonAPI/carla/agents/navigation/agent.py | 27 ++++++++++++++++++----
 PythonAPI/carla/agents/tools/misc.py       |  1 -
 3 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index a19876e5f..bca434c7f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,5 @@
 ## Latest
-	
+
   * Fixed bug causing the RoadOptions at the BehaviorAgent to not work as intended
   * Upgrading to Unreal Engine 4.26
   * Added Lincoln 2020 vehicle dimensions for CarSim integration
@@ -19,6 +19,7 @@
   * Fix bug in lidar channel point count
   * Fix imu: some weird cases were given nan values
   * When setting a global plan at the LocalPlanner, it is now optional to stop the automatic fill of the waypoint buffer
+  * Improved agent's vehicle detection to also take into account the actor bounding boxes
   * API extensions:
   - Added `set_wheel_steer_direction()` function to change the bone angle of each wheel of a vehicle
   - Added `get_wheel_steer_angle()` function to get the steer angle of a vehicle whee
diff --git a/PythonAPI/carla/agents/navigation/agent.py b/PythonAPI/carla/agents/navigation/agent.py
index f816db492..f6fcfa3b2 100644
--- a/PythonAPI/carla/agents/navigation/agent.py
+++ b/PythonAPI/carla/agents/navigation/agent.py
@@ -188,7 +188,6 @@ class Agent(object):
 
     def _is_vehicle_hazard(self, vehicle_list):
         """
-
         :param vehicle_list: list of potential obstacle to check
         :return: a tuple given by (bool_flag, vehicle), where
                  - bool_flag is True if there is a vehicle ahead blocking us
@@ -196,22 +195,40 @@ class Agent(object):
                  - vehicle is the blocker object itself
         """
 
-        ego_vehicle_location = self._vehicle.get_location()
-        ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
+        ego_vehicle_transform = self._vehicle.get_transform()
+        ego_vehicle_forward_vector = ego_vehicle_transform.get_forward_vector()
+        ego_vehicle_extent = self._vehicle.bounding_box.extent.x
+        ego_vehicle_waypoint = self._map.get_waypoint(self._vehicle.get_location())
 
+        # Get the transform of the front of the ego
+        ego_vehicle_front_transform = ego_vehicle_transform
+        ego_vehicle_front_transform.location += carla.Location(
+            x=ego_vehicle_extent * ego_vehicle_forward_vector.x,
+            y=ego_vehicle_extent * ego_vehicle_forward_vector.y,
+        )
         for target_vehicle in vehicle_list:
             # do not account for the ego vehicle
             if target_vehicle.id == self._vehicle.id:
                 continue
 
             # if the object is not in our lane it's not an obstacle
+            target_vehicle_transform = target_vehicle.get_transform()
+            target_vehicle_forward_vector = target_vehicle_transform.get_forward_vector()
+            target_vehicle_extent = target_vehicle.bounding_box.extent.x
             target_vehicle_waypoint = self._map.get_waypoint(target_vehicle.get_location())
             if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
                     target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
                 continue
 
-            if is_within_distance_ahead(target_vehicle.get_transform(),
-                                        self._vehicle.get_transform(),
+            # Get the transofrm of the back of the vehicle
+            target_vehicle_rear_transform = target_vehicle_transform
+            target_vehicle_rear_transform.location -= carla.Location(
+                x=target_vehicle_extent * target_vehicle_forward_vector.x,
+                y=target_vehicle_extent * target_vehicle_forward_vector.y,
+            )
+
+            if is_within_distance_ahead(target_vehicle_rear_transform,
+                                        ego_vehicle_front_transform,
                                         self._proximity_vehicle_threshold):
                 return (True, target_vehicle)
 
diff --git a/PythonAPI/carla/agents/tools/misc.py b/PythonAPI/carla/agents/tools/misc.py
index f9c3d84be..34016fff0 100644
--- a/PythonAPI/carla/agents/tools/misc.py
+++ b/PythonAPI/carla/agents/tools/misc.py
@@ -45,7 +45,6 @@ def is_within_distance_ahead(target_transform, current_transform, max_distance):
 
     :param target_transform: location of the target object
     :param current_transform: location of the reference object
-    :param orientation: orientation of the reference object
     :param max_distance: maximum allowed distance
     :return: True if target object is within max_distance ahead of the reference object
     """