Only take targets driving roughly in the same direction as IDM target

src/baselines/rule_policies.py

@@ -82,6 +82,7 @@ class IDMRulePolicy(BaseAlgorithm):
         self._env = env
         self.t_future = t_future
         self.half_angle = half_angle
+        self.max_heading_diff = 120
 
         # Default IDM parameters
         assert target_speed>0, 'negative target speed'
@@ -192,7 +193,11 @@ class IDMRulePolicy(BaseAlgorithm):
         dl = (dxys*np.hstack((-np.sin(psi), np.cos(psi)))).sum(-1) # (nv, )
         alpha = to_circle(np.arctan2(dl, df))
 
-        val_idx = np.arange(nv)[(np.abs(alpha) < self.half_angle*np.pi/180) & (np.arange(nv) != agent)]
+        heading_diff = to_circle(psi - psi[agent]).flatten()
+
+        val_idx = np.arange(nv)[
+            (np.abs(alpha) < self.half_angle*np.pi/180) & (np.arange(nv) != agent) & (np.abs(heading_diff) < self.max_heading_diff*np.pi/180)
+            ]
 
         if len(val_idx)==0:
             i = None