Merge pull request #584 from flow-project/ACC_controller

added Linear Adaptive Controller

flow/controllers/__init__.py

@@ -14,7 +14,7 @@
 from flow.controllers.base_controller import BaseController
 from flow.controllers.car_following_models import CFMController, \
     BCMController, OVMController, LinearOVM, IDMController, \
-    SimCarFollowingController
+    SimCarFollowingController, LACController
 from flow.controllers.velocity_controllers import FollowerStopper, \
     PISaturation
 
@@ -34,5 +34,5 @@
     "CFMController", "BCMController", "OVMController", "LinearOVM",
     "IDMController", "SimCarFollowingController", "FollowerStopper",
     "PISaturation", "StaticLaneChanger", "SimLaneChangeController",
-    "ContinuousRouter", "GridRouter", "BayBridgeRouter"
+    "ContinuousRouter", "GridRouter", "BayBridgeRouter", "LACController"
 ]

flow/controllers/car_following_models.py

@@ -168,6 +168,75 @@ def get_accel(self, env):
             self.k_c * (self.v_des - this_vel)
 
 
+class LACController(BaseController):
+    """Linear Adaptive Cruise Control.
+
+    Attributes
+    ----------
+    veh_id : str
+        Vehicle ID for SUMO identification
+    car_following_params : flow.core.params.SumoCarFollowingParams
+        see parent class
+    k_1 : float
+        design parameter (default: 0.8)
+    k_2 : float
+        design parameter (default: 0.9)
+    h : float
+        desired time gap  (default: 1.0)
+    tau : float
+        lag time between control input u and real acceleration a (default:0.1)
+    time_delay : float
+        time delay (default: 0.5)
+    noise : float
+        std dev of normal perturbation to the acceleration (default: 0)
+    fail_safe : str
+        type of flow-imposed failsafe the vehicle should posses, defaults
+        to no failsafe (None)
+    """
+
+    def __init__(self,
+                 veh_id,
+                 car_following_params,
+                 k_1=0.3,
+                 k_2=0.4,
+                 h=1,
+                 tau=0.1,
+                 a=0,
+                 time_delay=0.0,
+                 noise=0,
+                 fail_safe=None):
+        """Instantiate a Linear Adaptive Cruise controller."""
+        BaseController.__init__(
+            self,
+            veh_id,
+            car_following_params,
+            delay=time_delay,
+            fail_safe=fail_safe,
+            noise=noise)
+
+        self.veh_id = veh_id
+        self.k_1 = k_1
+        self.k_2 = k_2
+        self.h = h
+        self.tau = tau
+        self.a = a
+
+    def get_accel(self, env):
+        """See parent class."""
+        lead_id = env.k.vehicle.get_leader(self.veh_id)
+        lead_vel = env.k.vehicle.get_speed(lead_id)
+        this_vel = env.k.vehicle.get_speed(self.veh_id)
+        headway = env.k.vehicle.get_headway(self.veh_id)
+        L = env.k.vehicle.get_length(self.veh_id)
+        ex = headway - L - self.h * this_vel
+        ev = lead_vel - this_vel
+        u = self.k_1*ex + self.k_2*ev
+        a_dot = -(self.a/self.tau) + (u/self.tau)
+        self.a = a_dot*env.sim_step + self.a
+
+        return self.a
+
+
 class OVMController(BaseController):
     """Optimal Vehicle Model controller.
 

flow/controllers/velocity_controllers.py

@@ -86,14 +86,14 @@ def get_accel(self, env):
             dx_1 = self.dx_1_0 + 1 / (2 * self.d_1) * dv_minus**2
             dx_2 = self.dx_2_0 + 1 / (2 * self.d_2) * dv_minus**2
             dx_3 = self.dx_3_0 + 1 / (2 * self.d_3) * dv_minus**2
-
+            v = min(max(lead_vel, 0), self.v_des)
             # compute the desired velocity
             if dx <= dx_1:
                 v_cmd = 0
             elif dx <= dx_2:
-                v_cmd = this_vel * (dx - dx_1) / (dx_2 - dx_1)
+                v_cmd = v * (dx - dx_1) / (dx_2 - dx_1)
             elif dx <= dx_3:
-                v_cmd = this_vel + (self.v_des - this_vel) * (dx - dx_2) \
+                v_cmd = v + (self.v_des - this_vel) * (dx - dx_2) \
                         / (dx_3 - dx_2)
             else:
                 v_cmd = self.v_des

tests/fast_tests/test_controllers.py

@@ -7,7 +7,7 @@
 
 from flow.controllers.routing_controllers import ContinuousRouter
 from flow.controllers.car_following_models import IDMController, \
-    OVMController, BCMController, LinearOVM, CFMController
+    OVMController, BCMController, LinearOVM, CFMController, LACController
 from flow.controllers import FollowerStopper, PISaturation
 from tests.setup_scripts import ring_road_exp_setup
 import os
@@ -502,7 +502,7 @@ def test_get_action(self):
             for veh_id in ids
         ]
 
-        expected_accel = [-16.666667, 0, 0., -5., 5.]
+        expected_accel = [0, 0, 0, -5, 5]
 
         np.testing.assert_array_almost_equal(requested_accel, expected_accel)
 
@@ -582,5 +582,58 @@ def test_get_action(self):
         np.testing.assert_array_almost_equal(requested_accel, expected_accel)
 
 
+class TestLACController(unittest.TestCase):
+    """
+    Tests that the LAC Controller returning mathematically accurate values.
+    """
+
+    def setUp(self):
+        # add a few vehicles to the network using the requested model
+        # also make sure that the input params are what is expected
+        contr_params = {
+            "k_1": 0.3,
+            "k_2": 0.4,
+            "h": 1,
+            "tau": 0.1,
+            "noise": 0
+        }
+
+        vehicles = VehicleParams()
+        vehicles.add(
+            veh_id="test",
+            acceleration_controller=(LACController, contr_params),
+            routing_controller=(ContinuousRouter, {}),
+            car_following_params=SumoCarFollowingParams(
+                accel=15, decel=5),
+            num_vehicles=5)
+
+        # create the environment and scenario classes for a ring road
+        self.env, scenario = ring_road_exp_setup(vehicles=vehicles)
+
+    def tearDown(self):
+        # terminate the traci instance
+        self.env.terminate()
+
+        # free data used by the class
+        self.env = None
+
+    def test_get_action(self):
+        self.env.reset()
+        ids = self.env.k.vehicle.get_ids()
+
+        test_headways = [5, 10, 15, 20, 25]
+        for i, veh_id in enumerate(ids):
+            self.env.k.vehicle.set_headway(veh_id, test_headways[i])
+
+        requested_accel = [
+            self.env.k.vehicle.get_acc_controller(veh_id).get_action(self.env)
+            for veh_id in ids
+        ]
+
+        expected_accel = [0., 1.5, 3., 4.5, 6.]
+
+        np.testing.assert_array_almost_equal(requested_accel, expected_accel)
+
+
 if __name__ == '__main__':
     unittest.main()