merge custom output and failsafes to master

flow/controllers/base_controller.py

@@ -34,8 +34,12 @@ class BaseController(metaclass=ABCMeta):
         specified to in this model are as desired.
     delay : int
         delay in applying the action (time)
-    fail_safe : str
-        Should be either "instantaneous" or "safe_velocity"
+    fail_safe : list of str or str
+        List of failsafes which can be "instantaneous", "safe_velocity",
+        "feasible_accel", or "obey_speed_limit". The order of applying the
+        falsafes will be based on the order in the list.
+    display_warnings : bool
+        Flag for toggling on/off printing failsafe warnings to screen.
     noise : double
         variance of the gaussian from which to sample a noisy acceleration
     """
@@ -45,6 +49,7 @@ def __init__(self,
                  car_following_params,
                  delay=0,
                  fail_safe=None,
+                 display_warnings=True,
                  noise=0):
         """Instantiate the base class for acceleration behavior."""
         self.veh_id = veh_id
@@ -56,7 +61,29 @@ def __init__(self,
         self.delay = delay
 
         # longitudinal failsafe used by the vehicle
-        self.fail_safe = fail_safe
+        if isinstance(fail_safe, str):
+            failsafe_list = [fail_safe]
+        elif isinstance(fail_safe, list) or fail_safe is None:
+            failsafe_list = fail_safe
+        else:
+            failsafe_list = None
+            raise ValueError("fail_safe should be string or list of strings. Setting fail_safe to None\n")
+
+        failsafe_map = {
+            'instantaneous': self.get_safe_action_instantaneous,
+            'safe_velocity': self.get_safe_velocity_action,
+            'feasible_accel': lambda _, accel: self.get_feasible_action(accel),
+            'obey_speed_limit': self.get_obey_speed_limit_action
+        }
+        self.failsafes = []
+        if failsafe_list:
+            for check in failsafe_list:
+                if check in failsafe_map:
+                    self.failsafes.append(failsafe_map.get(check))
+                else:
+                    raise ValueError('Skipping {}, as it is not a valid failsafe.'.format(check))
+
+        self.display_warnings = display_warnings
 
         self.max_accel = car_following_params.controller_params['accel']
         # max deaccel should always be a positive
@@ -77,8 +104,8 @@ def get_action(self, env):
         time step.
 
         This method also augments the controller with the desired level of
-        stochastic noise, and utlizes the "instantaneous" or "safe_velocity"
-        failsafes if requested.
+        stochastic noise, and utlizes the "instantaneous", "safe_velocity",
+        "feasible_accel", and/or "obey_speed_limit" failsafes if requested.
 
         Parameters
         ----------
@@ -90,6 +117,12 @@ def get_action(self, env):
         float
             the modified form of the acceleration
         """
+        # clear the current stored accels of this vehicle to None
+        env.k.vehicle.update_accel(self.veh_id, None, noise=False, failsafe=False)
+        env.k.vehicle.update_accel(self.veh_id, None, noise=False, failsafe=True)
+        env.k.vehicle.update_accel(self.veh_id, None, noise=True, failsafe=False)
+        env.k.vehicle.update_accel(self.veh_id, None, noise=True, failsafe=True)
+
         # this is to avoid abrupt decelerations when a vehicle has just entered
         # a network and it's data is still not subscribed
         if len(env.k.vehicle.get_edge(self.veh_id)) == 0:
@@ -107,16 +140,26 @@ def get_action(self, env):
         if accel is None:
             return None
 
+        # store the acceleration without noise to each vehicle
+        # run fail safe if requested
+        env.k.vehicle.update_accel(self.veh_id, accel, noise=False, failsafe=False)
+        accel_no_noise_with_failsafe = accel
+
+        for failsafe in self.failsafes:
+            accel_no_noise_with_failsafe = failsafe(env, accel_no_noise_with_failsafe)
+
+        env.k.vehicle.update_accel(self.veh_id, accel_no_noise_with_failsafe, noise=False, failsafe=True)
+
         # add noise to the accelerations, if requested
         if self.accel_noise > 0:
-            accel += np.random.normal(0, self.accel_noise)
+            accel += np.sqrt(env.sim_step) * np.random.normal(0, self.accel_noise)
+        env.k.vehicle.update_accel(self.veh_id, accel, noise=True, failsafe=False)
 
-        # run the failsafes, if requested
-        if self.fail_safe == 'instantaneous':
-            accel = self.get_safe_action_instantaneous(env, accel)
-        elif self.fail_safe == 'safe_velocity':
-            accel = self.get_safe_velocity_action(env, accel)
+        # run the fail-safes, if requested
+        for failsafe in self.failsafes:
+            accel = failsafe(env, accel)
 
+        env.k.vehicle.update_accel(self.veh_id, accel, noise=True, failsafe=True)
         return accel
 
     def get_safe_action_instantaneous(self, env, action):
@@ -162,6 +205,13 @@ def get_safe_action_instantaneous(self, env, action):
                 # if the vehicle will crash into the vehicle ahead of it in the
                 # next time step (assuming the vehicle ahead of it is not
                 # moving), then stop immediately
+                if self.display_warnings:
+                    print(
+                        "=====================================\n"
+                        "Vehicle {} is about to crash. Instantaneous acceleration "
+                        "clipping applied.\n"
+                        "=====================================".format(self.veh_id))
+
                 return -this_vel / sim_step
             else:
                 # if the vehicle is not in danger of crashing, continue with
@@ -223,8 +273,8 @@ def safe_velocity(self, env):
         Returns
         -------
         float
-            maximum safe velocity given a maximum deceleration and delay in
-            performing the breaking action
+            maximum safe velocity given a maximum deceleration, delay in
+            performing the breaking action, and speed limit
         """
         lead_id = env.k.vehicle.get_leader(self.veh_id)
         lead_vel = env.k.vehicle.get_speed(lead_id)
@@ -235,4 +285,97 @@ def safe_velocity(self, env):
 
         v_safe = 2 * h / env.sim_step + dv - this_vel * (2 * self.delay)
 
+        # check for speed limit  FIXME: this is not called
+        # this_edge = env.k.vehicle.get_edge(self.veh_id)
+        # edge_speed_limit = env.k.network.speed_limit(this_edge)
+
+        if this_vel > v_safe:
+            if self.display_warnings:
+                print(
+                    "=====================================\n"
+                    "Speed of vehicle {} is greater than safe speed. Safe velocity "
+                    "clipping applied.\n"
+                    "=====================================".format(self.veh_id))
+
         return v_safe
+
+    def get_obey_speed_limit_action(self, env, action):
+        """Perform the "obey_speed_limit" failsafe action.
+
+        Checks if the computed acceleration would put us above edge speed limit.
+        If it would, output the acceleration that would put at the speed limit
+        velocity.
+
+        Parameters
+        ----------
+        env : flow.envs.Env
+            current environment, which contains information of the state of the
+            network at the current time step
+        action : float
+            requested acceleration action
+
+        Returns
+        -------
+        float
+            the requested action clipped by the speed limit
+        """
+        # check for speed limit
+        this_edge = env.k.vehicle.get_edge(self.veh_id)
+        edge_speed_limit = env.k.network.speed_limit(this_edge)
+
+        this_vel = env.k.vehicle.get_speed(self.veh_id)
+        sim_step = env.sim_step
+
+        if this_vel + action * sim_step > edge_speed_limit:
+            if edge_speed_limit > 0:
+                if self.display_warnings:
+                    print(
+                        "=====================================\n"
+                        "Speed of vehicle {} is greater than speed limit. Obey "
+                        "speed limit clipping applied.\n"
+                        "=====================================".format(self.veh_id))
+                return (edge_speed_limit - this_vel) / sim_step
+            else:
+                return -this_vel / sim_step
+        else:
+            return action
+
+    def get_feasible_action(self, action):
+        """Perform the "feasible_accel" failsafe action.
+
+        Checks if the computed acceleration would put us above maximum
+        acceleration or deceleration. If it would, output the acceleration
+        equal to maximum acceleration or deceleration.
+
+        Parameters
+        ----------
+        action : float
+            requested acceleration action
+
+        Returns
+        -------
+        float
+            the requested action clipped by the feasible acceleration or
+            deceleration.
+        """
+        if action > self.max_accel:
+            action = self.max_accel
+
+            if self.display_warnings:
+                print(
+                    "=====================================\n"
+                    "Acceleration of vehicle {} is greater than the max "
+                    "acceleration. Feasible acceleration clipping applied.\n"
+                    "=====================================".format(self.veh_id))
+
+        if action < -self.max_deaccel:
+            action = -self.max_deaccel
+
+            if self.display_warnings:
+                print(
+                    "=====================================\n"
+                    "Deceleration of vehicle {} is greater than the max "
+                    "deceleration. Feasible acceleration clipping applied.\n"
+                    "=====================================".format(self.veh_id))
+
+        return action

flow/controllers/car_following_models.py

@@ -56,15 +56,18 @@ def __init__(self,
                  v_des=8,
                  time_delay=0.0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate a CFM controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
 
         self.veh_id = veh_id
         self.k_d = k_d
@@ -132,15 +135,18 @@ def __init__(self,
                  v_des=8,
                  time_delay=0.0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate a Bilateral car-following model controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
 
         self.veh_id = veh_id
         self.k_d = k_d
@@ -212,15 +218,18 @@ def __init__(self,
                  a=0,
                  time_delay=0.0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate a Linear Adaptive Cruise controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
 
         self.veh_id = veh_id
         self.k_1 = k_1
@@ -289,15 +298,18 @@ def __init__(self,
                  v_max=30,
                  time_delay=0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate an Optimal Vehicle Model controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
         self.veh_id = veh_id
         self.v_max = v_max
         self.alpha = alpha
@@ -364,15 +376,18 @@ def __init__(self,
                  h_st=5,
                  time_delay=0.0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate a Linear OVM controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
         self.veh_id = veh_id
         # 4.8*1.85 for case I, 3.8*1.85 for case II, per Nakayama
         self.v_max = v_max
@@ -445,6 +460,7 @@ def __init__(self,
                  time_delay=0.0,
                  noise=0,
                  fail_safe=None,
+                 display_warnings=True,
                  car_following_params=None):
         """Instantiate an IDM controller."""
         BaseController.__init__(
@@ -453,7 +469,9 @@ def __init__(self,
             car_following_params,
             delay=time_delay,
             fail_safe=fail_safe,
-            noise=noise)
+            noise=noise,
+            display_warnings=display_warnings,
+        )
         self.v0 = v0
         self.T = T
         self.a = a
@@ -546,16 +564,18 @@ def __init__(self,
                  tau=1,
                  delay=0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate a Gipps' controller."""
         BaseController.__init__(
             self,
             veh_id,
             car_following_params,
             delay=delay,
             fail_safe=fail_safe,
-            noise=noise
-            )
+            noise=noise,
+            display_warnings=display_warnings,
+        )
 
         self.v_desired = v0
         self.acc = acc
@@ -627,7 +647,8 @@ def __init__(self,
                  want_max_accel=False,
                  time_delay=0,
                  noise=0,
-                 fail_safe=None):
+                 fail_safe=None,
+                 display_warnings=True):
         """Instantiate an Bando controller."""
         BaseController.__init__(
             self,
@@ -636,6 +657,7 @@ def __init__(self,
             delay=time_delay,
             fail_safe=fail_safe,
             noise=noise,
+            display_warnings=display_warnings,
         )
         self.veh_id = veh_id
         self.v_max = v_max