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:
+            raise ValueError("fail_safe should be string or list of strings.\nSetting fail_safe to None\n")
+            failsafe_list = None
+
+        failsafe_map = {
+            'instantaneous': self.get_safe_action_instantaneous,
+            'safe_velocity': self.get_safe_velocity_action,
+            'feasible_accel': self.get_feasible_action,
+            'obey_speed_limit': self.get_obey_speed_limit_action
+        }
+        self.failsafes = []
+        if failsafe_list:
+            for check in failsafe_list:
+                try:
+                    self.failsafes.append(failsafe_map.get(check))
+                except ValueError:
+                    print(f'{check} is not a valid failsafe')
+                    raise
+        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:
@@ -100,23 +133,33 @@ def get_action(self, env):
         if env.k.vehicle.get_edge(self.veh_id)[0] == ":":
             return None
 
-        accel = self.get_accel(env)
+        accel = self.get_accel(env, noise=False, failsafe=False)
 
         # if no acceleration is specified, let sumo take over for the current
         # time step
         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/core/experiment.py

@@ -1,11 +1,14 @@
 """Contains an experiment class for running simulations."""
 from flow.core.util import emission_to_csv
 from flow.utils.registry import make_create_env
-import datetime
+from flow.utils.data_pipeline import write_dict_to_csv, get_extra_info
+from collections import defaultdict
+from datetime import datetime
 import logging
 import time
 import os
 import numpy as np
+import uuid
 
 
 class Experiment:
@@ -81,7 +84,7 @@ def __init__(self, flow_params, custom_callables=None):
         self.env = create_env()
 
         logging.info(" Starting experiment {} at {}".format(
-            self.env.network.name, str(datetime.datetime.utcnow())))
+            self.env.network.name, str(datetime.utcnow())))
 
         logging.info("Initializing environment.")
 
@@ -137,10 +140,20 @@ def rl_actions(*_):
         t = time.time()
         times = []
 
+        # data pipeline
+        extra_info = defaultdict(list)
+        source_id = 'flow_{}'.format(uuid.uuid4().hex)
+
+        if convert_to_csv and self.env.simulator == "traci":
+            dir_path = self.env.sim_params.emission_path
+            trajectory_table_path = os.path.join(dir_path, '{}.csv'.format(source_id))
+
         for i in range(num_runs):
             ret = 0
             vel = []
             custom_vals = {key: [] for key in self.custom_callables.keys()}
+            run_id = "run_{}".format(i)
+            self.env.pipeline_params = (extra_info, source_id, run_id)
             state = self.env.reset()
             for j in range(num_steps):
                 t0 = time.time()
@@ -153,6 +166,14 @@ def rl_actions(*_):
                 vel.append(np.mean(self.env.k.vehicle.get_speed(veh_ids)))
                 ret += reward
 
+                # collect additional information for the data pipeline
+                get_extra_info(self.env.k.vehicle, extra_info, veh_ids, source_id, run_id)
+
+                # write to disk every 100 steps
+                if convert_to_csv and self.env.simulator == "traci" and j % 100 == 0:
+                    write_dict_to_csv(trajectory_table_path, extra_info, not j and not i)
+                    extra_info.clear()
+
                 # Compute the results for the custom callables.
                 for (key, lambda_func) in self.custom_callables.items():
                     custom_vals[key].append(lambda_func(self.env))
@@ -184,7 +205,6 @@ def rl_actions(*_):
             time.sleep(0.1)
 
             # collect the location of the emission file
-            dir_path = self.env.sim_params.emission_path
             emission_filename = \
                 "{0}-emission.xml".format(self.env.network.name)
             emission_path = os.path.join(dir_path, emission_filename)
@@ -194,5 +214,6 @@ def rl_actions(*_):
 
             # Delete the .xml version of the emission file.
             os.remove(emission_path)
+            write_dict_to_csv(trajectory_table_path, extra_info)
 
         return info_dict

flow/core/kernel/vehicle/aimsun.py

@@ -65,6 +65,7 @@ def __init__(self,
         # number of vehicles to exit the network for every time-step
         self._num_arrived = []
         self._arrived_ids = []
+        self._arrived_rl_ids = []
 
         # contains conversion from Flow-ID to Aimsun-ID
         self._id_aimsun2flow = {}
@@ -174,11 +175,17 @@ def update(self, reset):
         added_vehicles = self.kernel_api.get_entered_ids()
         exited_vehicles = self.kernel_api.get_exited_ids()
 
+        # keep track of arrived rl vehicles
+        arrived_rl_ids = []
+
         # add the new vehicles if they should be tracked
         for aimsun_id in added_vehicles:
             veh_type = self.kernel_api.get_vehicle_type_name(aimsun_id)
             if veh_type in self.tracked_vehicle_types:
                 self._add_departed(aimsun_id)
+            if aimsun_id in self.get_rl_ids():
+                arrived_rl_ids.append(aimsun_id)
+        self._arrived_rl_ids.append(arrived_rl_ids)
 
         # remove the exited vehicles if they were tracked
         if not reset:
@@ -639,6 +646,16 @@ def get_arrived_ids(self):
         """See parent class."""
         raise NotImplementedError
 
+    def get_arrived_rl_ids(self, k=1):
+        """See parent class."""
+        if len(self._arrived_rl_ids) > 0:
+            arrived = []
+            for arr in self._arrived_rl_ids[-k:]:
+                arrived.extend(arr)
+            return arrived
+        else:
+            return 0
+
     def get_departed_ids(self):
         """See parent class."""
         raise NotImplementedError