From 3f587a8988741eeb9081c1f6d3d71b5e0a7ebb48 Mon Sep 17 00:00:00 2001
From: Daniel <github.blurry@9ox.net>
Date: Tue, 5 Mar 2024 16:36:11 +0100
Subject: [PATCH] Dynamic Traffic Light distance

BasicAgent and the BehaviourAgent do differ here, this allows both options for both agents.
---
 PythonAPI/carla/agents/conf/agent_settings_backend.py  |  9 +++++++++
 PythonAPI/carla/agents/navigation/basic_agent.py       |  8 +++++++-
 PythonAPI/carla/agents/navigation/behavior_agent.py    | 10 ++++++++--
 .../carla/agents/navigation/constant_velocity_agent.py |  7 ++++++-
 4 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py
index 2c1aa65221..90119eab6f 100644
--- a/PythonAPI/carla/agents/conf/agent_settings_backend.py
+++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py
@@ -420,7 +420,16 @@ class BasicAgentObstacleSettings(AgentConfig):
     USAGE: max_tlight_distance  = base_tlight_threshold  + detection_speed_ratio * vehicle_speed
     """
     
+    dynamic_threshold_by_speed : bool = True
+    """
+    Whether to add a dynamic threshold based on the vehicle speed to the base threshold.
+    
+    Usage: base_threshold + dynamic_threshold_by_speed * vehicle_speed
+    """
+    
     detection_angles : BasicAgentObstacleDetectionAngles = field(default_factory=BasicAgentObstacleDetectionAngles)
+    """Defines detection angles used when checking for obstacles."""
+    
 
 @dataclass
 class BehaviorAgentObstacleSettings(BasicAgentObstacleSettings):
diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py
index 39361bb307..7be0a25cdd 100644
--- a/PythonAPI/carla/agents/navigation/basic_agent.py
+++ b/PythonAPI/carla/agents/navigation/basic_agent.py
@@ -177,7 +177,13 @@ def run_step(self):
             hazard_detected = True
 
         # Check if the vehicle is affected by a red traffic light
-        max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed
+        if self.config.obstacles.dynamic_threshold_by_speed:
+            # Basic agent setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed
+        else:
+            # Behavior setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold
+        
         affected_by_tlight, _ = self._affected_by_traffic_light(self._lights_list, max_tlight_distance)
         if affected_by_tlight:
             hazard_detected = True
diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py
index cfc1816734..5153b94259 100644
--- a/PythonAPI/carla/agents/navigation/behavior_agent.py
+++ b/PythonAPI/carla/agents/navigation/behavior_agent.py
@@ -95,7 +95,13 @@ def traffic_light_manager(self):
         """
         actor_list = self._world.get_actors()
         lights_list = actor_list.filter("*traffic_light*")
-        affected, _ = self._affected_by_traffic_light(lights_list)
+        if self.config.obstacles.dynamic_threshold_by_speed:
+            # Basic agent setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed
+        else:
+            # Behavior setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold
+        affected, _ = self._affected_by_traffic_light(lights_list, max_distance=max_tlight_distance)
 
         return affected
 
@@ -301,7 +307,7 @@ def run_step(self, debug=False):
         elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]):
             target_speed = min([
                 self.config.speed.max_speed,
-                self.config.live_info.current_speed_limit - 5])
+                self.config.live_info.current_speed_limit - self.config.speed.intersection_speed_decrease])
             self._local_planner.set_speed(target_speed)
             control = self._local_planner.run_step(debug=debug)
 
diff --git a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py
index 2b47a8a995..843f9bba85 100644
--- a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py
+++ b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py
@@ -95,7 +95,12 @@ def run_step(self):
             hazard_detected = True
 
         # Check if the vehicle is affected by a red traffic light
-        max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.distance.distance_ratio * vehicle_speed
+        if self.config.obstacles.dynamic_threshold_by_speed:
+            # Basic agent setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed
+        else:
+            # Behavior setting:
+            max_tlight_distance = self.config.obstacles.base_tlight_threshold
         affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance)
         if affected_by_tlight:
             hazard_speed = 0