Merge pull request #8 from jerryz123/traffic_cam

Traffic light support

examples/collect_data.py

@@ -10,7 +10,9 @@
 import pygame
 from copy import deepcopy
 from random import random
+
 from gym_urbandriving.agents import AccelAgent, KeyboardAgent, NullAgent
+from gym_urbandriving import Car
 
 
 def early_stop_actions(actions):
@@ -49,7 +51,7 @@ def run_and_collect():
 
     env = uds.UrbanDrivingEnv(init_state=init_state,
                               visualizer=vis,
-                              bgagent=AccelAgent,
+                              agent_mappings={Car:NullAgent},
                               max_time=100,
                               randomize=True,
                               nthreads=4)

examples/test.py

@@ -3,37 +3,61 @@
 import cProfile
 import time
 
-from gym_urbandriving.agents import KeyboardAgent, AccelAgent, NullAgent
+from gym_urbandriving.agents import KeyboardAgent, AccelAgent, NullAgent, TrafficLightAgent
+from gym_urbandriving.assets import Car, TrafficLight
 
 import numpy as np
 
+"""
+ Test File, to demonstrate general functionality of environment
+"""
+
+
 def f():
+    # Instantiate a PyGame Visualizer of size 800x800
     vis = uds.PyGameVisualizer((800, 800))
-    init_state = uds.state.SimpleIntersectionState(ncars=4, nped=2)
+
+    # Create a simple-intersection state, with 4 cars, no pedestrians, and traffic lights
+    init_state = uds.state.SimpleIntersectionState(ncars=1, nped=0, traffic_lights=True)
+
+    # Create the world environment initialized to the starting state
+    # Specify the max time the environment will run to 500
+    # Randomize the environment when env._reset() is called
+    # Specify what types of agents will control cars and traffic lights
+    # Use ray for multiagent parallelism
     env = uds.UrbanDrivingEnv(init_state=init_state,
                               visualizer=vis,
-                              max_time=250,
+                              max_time=500,
                               randomize=True,
-                              bgagent=NullAgent,
+                              agent_mappings={Car:NullAgent,
+                                              TrafficLight:TrafficLightAgent},
                               use_ray=True
     )
+
     env._render()
     state = init_state
+
+    # Car 0 will be controlled by our KeyboardAgent
     agent = KeyboardAgent()
     action = None
+
+    # Simulation loop
     while(True):
+        # Determine an action based on the current state.
+        # For KeyboardAgent, this just gets keypresses
         action = agent.eval_policy(state)
         start_time = time.time()
+
+        # Simulate the state
         state, reward, done, info_dict = env._step(action)
         env._render()
-        print(state.min_dist_to_coll(0))
         done = False
+        # If we crash, sleep for a moment, then reset
         if done:
             print("done")
             time.sleep(1)
-            print(info_dict["dynamic_collisions"])
             env._reset()
             state = env.current_state
 
-
+# Collect profiling data
 cProfile.run('f()', 'temp/stats')

examples/test_model.py

@@ -6,6 +6,7 @@
 import pickle
 
 from gym_urbandriving.agents import ModelAgent
+from gym_urbandriving.assets import Car
 
 def test_model():
     """
@@ -27,7 +28,7 @@ def test_model():
 
     env = uds.UrbanDrivingEnv(init_state=init_state,
                               visualizer=vis,
-                              bgagent=ModelAgent,
+                              agent_mappings={Car:ModelAgent},
                               max_time=250,
                               randomize=True,
                               nthreads=4)

examples/tree_search_train.py

@@ -8,10 +8,14 @@
 import numpy as np
 import pygame
 from copy import deepcopy
+from random import random
+
+from gym_urbandriving.assets import Car
+from gym_urbandriving.agents import AccelAgent, KeyboardAgent, NullAgent
 
 def run():
     """
-    Main function to be run to test simple_path_agent with hard coded path. 
+    Main function to be run to test simple_path_agent with hard coded path.
 
     Examples
     --------
@@ -23,20 +27,19 @@ def run():
     init_state = uds.state.SimpleIntersectionState(ncars=2, nped=0)
 
 
-    env = uds.UrbanDrivingEnv(init_state=init_state,
+    env = uds.UrbanDrivingEnv(init_state=None,
                               visualizer=vis,
-                              bgagent=AccelAgent,
-                              max_time=250,
-                              randomize=True,
-                              use_ray=False)
+                              agent_mappings={Car:NullAgent},
+                              max_time=-1,
+                              randomize=False,
+    )
 
     env._reset()
     state = env.current_state
     agent = TreeSearchAgent()
     action = None
 
     while(True):
-        print 
         action = agent.eval_policy(deepcopy(state))
         start_time = time.time()
         state, reward, done, info_dict = env._step(action)

gym_urbandriving/agents/__init__.py

@@ -2,6 +2,7 @@
 from gym_urbandriving.agents.accel_agent import AccelAgent
 from gym_urbandriving.agents.null_agent import NullAgent
 from gym_urbandriving.agents.model_agent import ModelAgent
+from gym_urbandriving.agents.traffic_light_agent import TrafficLightAgent
 from gym_urbandriving.agents.simple_path_agent import SimplePathAgent
 from gym_urbandriving.agents.tree_search_agent import TreeSearchAgent
 

gym_urbandriving/agents/accel_agent.py

@@ -49,18 +49,25 @@ def eval_policy(self, state, nsteps=8):
         best_distance = 0
         for action in self.actions:
             self.planning_env._reset()
+            pos = state.dynamic_objects[self.agent_num].get_pos()
+            dist_to_coll = state.min_dist_to_coll(self.agent_num)
             next_state, r, done, info_dict = self.planning_env._step(action,
                                                                 self.agent_num)
-            for i in range(nsteps):
-                next_state, r, done, info_dict = self.planning_env._step(action,
-                                                                    self.agent_num)
-                time = self.planning_env.time
-                if (done and next_state.collides_any(self.agent_num)):
-                    break
-
-            if time == nsteps + 1:
-                return action
-            if time > best_time:
+            delta_x = next_state.dynamic_objects[self.agent_num].get_pos()
+            delta_x = next_state.min_dist_to_coll(self.agent_num) + next_state.dynamic_objects[self.agent_num].vel
+            # for i in range(nsteps):
+            #     next_state, r, done, info_dict = self.planning_env._step(action,
+            #                                                         self.agent_num)
+            #     time = self.planning_env.time
+            #     if (done and next_state.collides_any(self.agent_num)):
+            #         break
+
+            # if time == nsteps + 1:
+            #     return action
+            # if time > best_time:
+            #     best_action = action
+            #     best_time = time
+            if delta_x > best_distance:
                 best_action = action
-                best_time = time
+                best_distance = delta_x
         return best_action

gym_urbandriving/agents/traffic_light_agent.py

@@ -0,0 +1,31 @@
+from gym_urbandriving.assets import TrafficLight
+
+class TrafficLightAgent:
+    """
+    Agent for controlling traffic one traffic light. 
+    
+    Attributes:
+    -----------
+    g_d : Duration of green light, in ticks
+    r_d : Duration of red light, in ticks
+    y_d : Duration of yellow light, in ticks
+    
+    g_d + y_d == r_d
+    """
+    transitions = {"green":"yellow",
+                   "yellow":"red",
+                   "red":"green"}
+    def __init__(self, agentnum=0, g_d=100, y_d=20, r_d=120):
+        assert(g_d + y_d == r_d)
+        self.color_times = {"green":g_d,
+                            "yellow":y_d,
+                            "red":r_d}
+        self.agentnum = agentnum
+
+    def eval_policy(self, state):
+        obj = state.dynamic_objects[self.agentnum]
+        assert(type(obj) == TrafficLight)
+        if obj.time_in_color < self.color_times[obj.color]:
+            return None
+        return self.transitions[obj.color]
+

gym_urbandriving/assets/__init__.py

@@ -4,8 +4,8 @@
 from gym_urbandriving.assets.lane import Lane
 from gym_urbandriving.assets.sidewalk import Sidewalk
 from gym_urbandriving.assets.pedestrian import Pedestrian
-from gym_urbandriving.assets.kinematic_car import KinematicCar
 from gym_urbandriving.assets.car import Car
+from gym_urbandriving.assets.traffic_light import TrafficLight
 
 
 

gym_urbandriving/assets/car.py

@@ -5,6 +5,7 @@
 from gym_urbandriving.assets.terrain import Terrain
 from gym_urbandriving.assets.sidewalk import Sidewalk
 from gym_urbandriving.assets.pedestrian import Pedestrian
+from scipy.integrate import odeint
 
 from gym import spaces
 
@@ -35,14 +36,21 @@ class Car(Rectangle):
         Width of car
 
     """
-    def __init__(self, x, y, xdim=60, ydim=30, angle=0.0, vel=0.0,
-                 max_vel=7.5, mass=100.0):
+    def __init__(self, x, y, xdim=80, ydim=40, angle=0.0, vel=0.0,
+                 max_vel=5, mass=100.0, dynamics_model="point"):
         Rectangle.__init__(self, x, y, xdim, ydim, angle, mass=mass, sprite="grey_car.png")
         self.vel = vel
         self.max_vel = max_vel
+        self.dynamics_model = dynamics_model
+        self.l_f = self.l_r = self.ydim / 2.0
 
+    def step(self, action):
+        if self.dynamics_model == "kinematic":
+            self.kinematic_model_step(action)
+        else:
+            self.point_model_step(action)
 
-    def step(self, action, info_dict=None):
+    def point_model_step(self, action, info_dict=None):
         """
         Updates the car for one timestep based on point model
         
@@ -58,12 +66,12 @@ def step(self, action, info_dict=None):
             action_steer, action_acc = 0.0, 0.0
         else:
             action_steer, action_acc = action
+
         self.angle += action_steer
         self.angle %= 360.0
         self.angle = self.angle
         acc = action_acc
-        acc = max(min(self.acc, self.max_vel - self.vel), -self.max_vel)
-
+        acc = max(min(acc, self.max_vel - self.vel), -self.max_vel)
         t = 1
         dist = self.vel * t + 0.5 * acc * (t ** 2)
         dx = dist * np.cos(np.radians(self.angle))
@@ -72,6 +80,65 @@ def step(self, action, info_dict=None):
         self.y += dy
         self.vel += acc
         self.vel = max(min(self.vel, self.max_vel), -self.max_vel)
+
+    def kinematic_model_step(self, action):
+        """
+        Updates the car for one timestep.
+
+        Args:
+            action: 1x2 array, steering / acceleration action.
+            info_dict: dict, contains information about the environment.
+        """
+        self.shapely_obj = None
+        if action is None:
+            action = [0, 0]
+        # Unpack actions, convert angles to radians
+        delta_f, a = action
+        delta_f, rad_angle = np.radians(10*delta_f), np.radians(self.angle)
+
+        # Clamp acceleration if above maximum velocity
+        if a > self.max_vel - self.vel:
+            a = self.max_vel - self.vel
+        elif a < -self.max_vel - self.vel:
+            a = - self.max_vel - self.vel
+        # Differential equations
+        ode_state = [self.x, self.y, self.vel, rad_angle]
+        aux_state = (a, delta_f)
+        t = np.arange(0.0, 1.0, 0.1)
+        delta_ode_state = odeint(self.integrator, ode_state, t, args=aux_state)
+        x, y, vel, angle = delta_ode_state[-1]
+
+        # Update car
+        self.x, self.y, self.vel, self.angle = x, y, vel, np.rad2deg(angle)
+        self.angle %= 360.0
+
+    def integrator(self, state, t, acc, delta_f):
+        """
+        Calculates numerical values of differential
+        equation variables for dynamics.
+        SciPy ODE integrator calls this function.
+
+        Args:
+            state: 1x6 array, contains x, y, dx_body, dy_body, rad_angle, rad_dangle
+                of car.
+            t: float, timestep.
+            mu: float, friction coefficient.
+            delta_f: float, steering angle.
+            a_f: float, acceleration.
+
+        Returns:
+            output: list, contains dx, dy, ddx_body, ddy_body, dangle, ddangle.
+        """
+        x, y, vel, rad_angle = state
+
+        # Differential equations
+        beta = np.arctan((self.l_r / (self.l_f + self.l_r)) * np.tan(delta_f))
+        dx = vel * np.cos(rad_angle + beta)
+        dy = vel * -np.sin(rad_angle + beta)
+        dangle = (vel / self.l_r) * np.sin(beta)
+        dvel = acc
+        output = [dx, dy, dvel, dangle]
+        return output
 
     def get_state(self):
         """
@@ -86,9 +153,8 @@ def can_collide(self, other):
         """
         Specifies whether this object can collide with another object
         """
-        from gym_urbandriving.assets.kinematic_car import KinematicCar
         from gym_urbandriving.assets.lane import Lane
-        if type(other) in {Terrain, Sidewalk, Car, KinematicCar, Pedestrian}:
+        if type(other) in {Terrain, Sidewalk, Car, Pedestrian}:
             return True
 
         if type(other) is Lane: