From 9c128aa123b798163d390dfeb344ccb0b9767b39 Mon Sep 17 00:00:00 2001
From: Tucker Alban <montgomery.alban@huawei.com>
Date: Wed, 22 Mar 2023 09:45:17 -0400
Subject: [PATCH] Add impatience for unblocking traffic. (#1913)

---
 smarts/core/local_traffic_provider.py | 25 +++++++++++++++++++++----
 1 file changed, 21 insertions(+), 4 deletions(-)

diff --git a/smarts/core/local_traffic_provider.py b/smarts/core/local_traffic_provider.py
index 098cccd300..e28b2679cf 100644
--- a/smarts/core/local_traffic_provider.py
+++ b/smarts/core/local_traffic_provider.py
@@ -54,6 +54,8 @@
 from .utils.math import min_angles_difference_signed, radians_to_vec, vec_to_radians
 from .vehicle import VEHICLE_CONFIGS, VehicleState
 
+MAX_IMPATIENCE = 3.0
+
 
 def _safe_division(n: float, d: float, default=math.inf):
     """This method uses a short circuit form where `and` converts right side to true|false (as 1|0) in which cases are:
@@ -522,6 +524,8 @@ def __init__(self, flow: Dict[str, Any], owner: LocalTrafficProvider):
         self._wait_to_restart = float(self._vtype.get("jmWaitToRestart", 0.0))
         self._stopped_at_feat = dict()
         self._waiting_at_feat = defaultdict(float)
+        self._time_to_impatience = 3
+        self._current_impatience = 0
 
         self._max_angular_velocity = 26  # arbitrary, based on limited testing
         self._prev_angular_err = None
@@ -531,6 +535,10 @@ def __init__(self, flow: Dict[str, Any], owner: LocalTrafficProvider):
 
         owner._actors_created += 1
 
+    @property
+    def _impatience(self):
+        return min(1, max(0, self._current_impatience))
+
     @classmethod
     def from_flow(cls, flow: Dict[str, Any], owner: LocalTrafficProvider):
         """Factory to construct a _TrafficActor object from a flow dictionary."""
@@ -1751,7 +1759,8 @@ def _compute_acceleration(self, dt: float) -> float:
             ),
             1e-13,
         )
-        min_time_cush = float(self._vtype.get("tau", 1.0))
+        tau = float(self._vtype.get("tau", 1.0))
+        min_time_cush = tau - tau * self._impatience
         if (
             not self._near_dest(min_time_cush * speed_denom)
             and time_cush < min_time_cush
@@ -1764,7 +1773,7 @@ def _compute_acceleration(self, dt: float) -> float:
             return 0
 
         space_cush = max(min(self._target_lane_win.gap, self._lane_win.gap), 1e-13)
-        if space_cush < self._min_space_cush:
+        if space_cush < self._min_space_cush - self._min_space_cush * self._impatience:
             if self.speed > 0:
                 severity = 4 * _safe_division(
                     (self._min_space_cush - space_cush), self._min_space_cush
@@ -1776,11 +1785,11 @@ def _compute_acceleration(self, dt: float) -> float:
 
         # magic numbers / weights here were just what looked reasonable in limited testing
         P = 0.0060 * (self._target_speed - my_speed)
-        I = -0.0150 / space_cush + -0.0333 / time_cush
+        I = (-0.0150 / space_cush + -0.0333 / time_cush) * (1 - self._impatience)
         D = -0.0010 * my_acc
         PID = (P + I + D) / dt
 
-        PID -= 0.02 * self._imperfection * random.random()
+        PID += 0.02 * self._imperfection * (random.random() - 0.5)
 
         PID = np.clip(PID, -1.0, 1.0)
 
@@ -1792,6 +1801,14 @@ def _compute_acceleration(self, dt: float) -> float:
     def compute_next_state(self, dt: float):
         """Pre-computes the next state for this traffic actor."""
         self._compute_lane_speeds()
+        if math.isclose(self.speed, 0, abs_tol=1.5):
+            self._current_impatience = min(
+                MAX_IMPATIENCE, self._current_impatience + dt / self._time_to_impatience
+            )
+        else:
+            self._current_impatience = max(
+                0, self._current_impatience - dt / self._time_to_impatience
+            )
 
         self._pick_lane(dt)
         self._check_speed(dt)