Merge pull request #46 from jerryz123/pedestrians

Added pedestrians

.travis.yml

@@ -15,5 +15,6 @@ script:
   - python tests/test_supervisor.py #or py.test for Python version 3.5 and below
   - python tests/test_q_lidar.py #or py.test for Python version 3.5 and below
   - python tests/test_traj_class.py #or py.test for Python version 3.5 and below
+  - python tests/test_pedestrians.py #or py.test for Python version 3.5 and below
   - cd docs && make html
 

configs/default_config.json

@@ -13,11 +13,12 @@
         "state_space":"Q-LIDAR",
         "use_traffic_lights":true,
         "use_pedestrians":false,
-        "number_of_pedestrians":0,
+        "number_of_pedestrians":4,
         "agent_mappings":{
             "Car":"PlanningPursuitAgent",
             "TrafficLight":"TrafficLightAgent",
-            "CrosswalkLight":"CrosswalkLightAgent"
+            "CrosswalkLight":"CrosswalkLightAgent",
+            "Pedestrian":"PedestrianAgent"
         }
     },
 

docs/source/agents.rst

@@ -34,3 +34,10 @@ Used for a human supervisor to operate the vehicle. Currently, the use of keyboa
 
 .. autoclass:: gym_urbandriving.agents.tele_op.keyboard_agent.KeyboardAgent
    :members: eval_policy
+
+Pedestrian
+^^^^^^^^^^
+FLUIDS also packages a basic pedestrian controller for controling the movement of background pedestrians. Pedestrians display simple behavior, walking forwards unless blocked by a crosswalk signal.
+
+.. autoclass:: gym_urbandriving.agents.supervisor.pedestrian_supervisor.PedestrianAgent
+   :members: eval_policy

gym_urbandriving/agents/__init__.py

@@ -14,3 +14,4 @@
 
 from gym_urbandriving.agents.tele_op.keyboard_agent import KeyboardAgent
 from gym_urbandriving.agents.tele_op.null_agent import NullAgent
+from gym_urbandriving.agents.supervisor.pedestrian_supervisor import PedestrianAgent

gym_urbandriving/agents/background/crosswalk_light_agent.py

@@ -20,7 +20,7 @@ def __init__(self, agentnum=0, w_d=40, r_d=200):
         self.agentnum = agentnum
 
     def eval_policy(self, state):
-        obj = state.dynamic_objects[self.agentnum]
+        obj = state.dynamic_objects['crosswalk_lights'][str(self.agentnum)]
         assert(type(obj) == CrosswalkLight)
         if obj.time_in_color < self.color_times[obj.color]:
             return None

gym_urbandriving/agents/supervisor/pedestrian_supervisor.py

@@ -0,0 +1,29 @@
+class PedestrianAgent(object):
+    """
+    Supervisor Agent for controlling pedestrians
+    
+    Attributes
+    ----------
+    agent_num : int
+        Index of this agent in the world
+    """
+    def __init__(self, agent_num=0):
+        self.agent_num = agent_num
+    def eval_policy(self, state):
+        """
+        Returns action based on state of world
+        
+        Parameters
+        ----------
+        state : PositionState
+            State of the world
+
+        Returns
+        -------
+        Turning angle, acceleration pair
+        """
+        if (state.collides_any(self.agent_num, "pedestrians")):
+            ped = state.dynamic_objects['pedestrians'][str(self.agent_num)]
+            vel = -ped.vel
+            return 0, vel
+        return 0, 1

gym_urbandriving/assets/crosswalk_light.py

@@ -1,5 +1,6 @@
 from gym_urbandriving.assets.primitives.rectangle import Rectangle
 from gym_urbandriving.assets.primitives.shape import Shape
+import numpy as np
 
 class CrosswalkLight(Rectangle):
     """
@@ -16,10 +17,12 @@ class CrosswalkLight(Rectangle):
     """
     colors = {"white" : "cross_white.png",
               "red" : "cross_red.png"}
-    def __init__(self, x, y, angle, initial_color="red", time_in_color = 0):
+    def __init__(self, x, y, angle=0, init_color="red", angle_deg=None, time_in_color=0):
+        if (angle_deg is not None):
+            angle = np.deg2rad(angle_deg)
         Rectangle.__init__(self, x, y, 15, 15, angle)
         self.time_in_color = time_in_color
-        self.color = initial_color
+        self.color = init_color
 
     def step(self, action):
         self.time_in_color += 1

gym_urbandriving/envs/urbandriving_env.py

@@ -95,7 +95,9 @@ def _step(self, action, background_simplified=False,supervisor=False):
         background_car_actions = []
         controlled_car_actions = []
         background_traffic_actions = []
-
+        pedestrian_actions = []
+        crosswalk_light_actions = []
+
         ### GET ALL ACTIONS ####
 
         for agent in self.current_state.bg_agents['background_cars']:
@@ -104,9 +106,12 @@ def _step(self, action, background_simplified=False,supervisor=False):
         for agent in self.current_state.bg_agents['traffic_lights']:
             background_traffic_actions.append(agent.eval_policy(self.current_state))
 
-
         for i,agent in enumerate(self.current_state.bg_agents['controlled_cars']):
             controlled_car_actions.append(agent.eval_policy(action[i],self.current_state,simplified=background_simplified))
+        for i,agent in enumerate(self.current_state.bg_agents['pedestrians']):
+            pedestrian_actions.append(agent.eval_policy(self.current_state))
+        for i,agent in enumerate(self.current_state.bg_agents['crosswalk_lights']):
+            crosswalk_light_actions.append(agent.eval_policy(self.current_state))
 
 
 
@@ -117,10 +122,14 @@ def _step(self, action, background_simplified=False,supervisor=False):
         for index, dobj in self.current_state.dynamic_objects['traffic_lights'].items():
             dobj.step(background_traffic_actions[int(index)])
 
-
         for i, dobj in self.current_state.dynamic_objects['controlled_cars'].items():
             dobj.step(controlled_car_actions[int(i)].get_value())
 
+        for i, dobj in self.current_state.dynamic_objects['pedestrians'].items():
+            dobj.step(pedestrian_actions[int(i)])
+        for i, dobj in self.current_state.dynamic_objects['crosswalk_lights'].items():
+            dobj.step(crosswalk_light_actions[int(i)])
+
         self.current_state.time += 1
         dynamic_coll, static_coll, controlled_car_collisions = self.current_state.get_collisions()
         state = self.current_state

gym_urbandriving/state/configs/four_way_intersection.json

@@ -34,6 +34,16 @@
         { "x": 560, "y": 600, "init_color":"red"  , "angle_deg":-90 },
         { "x": 440, "y": 400, "init_color":"red"  , "angle_deg":90 }
     ],
+    "crosswalk_lights": [
+        { "x": 375, "y": 390, "init_color":"red"  , "angle_deg":-90 , "time_in_color":80},
+        { "x": 390, "y": 375, "init_color":"white", "angle_deg":0 },
+        { "x": 610, "y": 625, "init_color":"white", "angle_deg":-180 },
+        { "x": 625, "y": 610, "init_color":"red"  , "angle_deg":90  , "time_in_color":80},
+        { "x": 375, "y": 610, "init_color":"red"  , "angle_deg":90  , "time_in_color":80},
+        { "x": 390, "y": 625, "init_color":"white", "angle_deg":0 },
+        { "x": 625, "y": 390, "init_color":"red"  , "angle_deg":-90 , "time_in_color":80},
+        { "x": 610, "y": 375, "init_color":"white", "angle_deg":-180 }
+    ],
     "goal_states": [
         { "x":550, "y":100, "vel":2, "angle_deg":90 },
         { "x":450, "y":900, "vel":2, "angle_deg":-90 },

gym_urbandriving/state/state.py

@@ -1,7 +1,7 @@
 import gym
 from copy import deepcopy
 import numpy as np
-from gym_urbandriving.assets import TrafficLight, Terrain, Street, Lane, Sidewalk, Car, CrosswalkLight
+from gym_urbandriving.assets import TrafficLight, Terrain, Street, Lane, Sidewalk, Car, CrosswalkLight, Pedestrian
 from gym_urbandriving.agents import *
 import json
 import os
@@ -76,11 +76,30 @@ def randomize(self):
                     self.dynamic_objects['background_cars'][str(car_index)].destination = self.assign_goal_states(start)
                     break
 
+        self.dynamic_objects['traffic_lights'] = {}
         if self.agent_config['use_traffic_lights']:
-            self.dynamic_objects['traffic_lights'] = {}
+
             for i, traffic_light in enumerate(self.state_config['traffic_lights']):
                 self.dynamic_objects['traffic_lights'][str(i)] = TrafficLight(**traffic_light)
+        self.dynamic_objects['crosswalk_lights'] = {}
+        self.dynamic_objects['pedestrians'] = {}        
+        if self.agent_config['use_pedestrians']:
+
+            for i, crosswalk_light in enumerate(self.state_config['crosswalk_lights']):
+                self.dynamic_objects['crosswalk_lights'][str(i)] = CrosswalkLight(**crosswalk_light)
+
+            start_sidewalks = [s for s in self.static_objects if type(s) == Sidewalk]
+
+            for ped_index in range(self.agent_config['number_of_pedestrians']):
+                while True:
+                    start = np.random.random_integers(0, len(start_sidewalks) - 1)
+                    sidewalk = start_sidewalks[start]
+                    ped = sidewalk.generate_man()
+                    if not self.is_in_collision(ped):
+                        self.dynamic_objects['pedestrians'][str(ped_index)] = ped
+                        break
         #TODO Add pedestrians
+
         self.create_agents()
 
     def assign_goal_states(self, start_lane):
@@ -101,10 +120,12 @@ def create_agents(self):
         for k, v in six.iteritems(self.agent_config['agent_mappings']):
             agent_mappings[{"Car":Car,
                             "TrafficLight":TrafficLight,
-                            "CrosswalkLight":CrosswalkLight}[k]] = {"PlanningPursuitAgent":PlanningPursuitAgent,
-                                                                    "TrafficLightAgent":TrafficLightAgent,
-                                                                    "CrosswalkLightAgent":CrosswalkLightAgent, 
-                                                                    "NullAgent": NullAgent}[v]
+                            "CrosswalkLight":CrosswalkLight,
+                            "Pedestrian":Pedestrian}[k]] = {"PlanningPursuitAgent":PlanningPursuitAgent,
+                                                            "TrafficLightAgent":TrafficLightAgent,
+                                                            "CrosswalkLightAgent":CrosswalkLightAgent,
+                                                            "NullAgent": NullAgent,
+                                                            "PedestrianAgent":PedestrianAgent}[v]
 
 
         self.bg_agents = {}
@@ -124,7 +145,7 @@ def create_agents(self):
             self.bg_agents['controlled_cars'].append(agent)
 
     def is_in_collision(self,car):
-        
+
         for obj in self.static_objects:
           if car.collides(obj):
               return True
@@ -182,17 +203,21 @@ def collides_any(self, agentnum,type_of_agent = 'background_cars'):
         bool
             True if this object is colliding
         """
+
         dynamic_collisions, static_collisions, _ = self.get_collisions()
         for coll in dynamic_collisions:
-            if (agentnum in coll) and (type_of_agent in coll):
+            id1, id2, t1, t2 = coll
+            if (agentnum, type_of_agent) in [(id1, t1), (id2, t2)]:
                 return True
         for coll in static_collisions:
-            if (agentnum in coll) and (type_of_agent in coll):
+            id1, id2, t1, t2 = coll
+            if (agentnum, type_of_agent) is (id1, t1):
                 return True
         return False
 
     def collides_any_dynamic(self, agentnum,type_of_agent = 'background_cars'):
         dynamic_collisions, static_collisions, _ = self.get_collisions()
+
         for coll in dynamic_collisions:
             if (agentnum in coll) and (type_of_agent in coll):
                 return True

tests/test_pedestrians.py

@@ -0,0 +1,41 @@
+import json
+import gym
+import gym_urbandriving as uds
+from gym_urbandriving import *
+from gym_urbandriving.agents import *
+from gym_urbandriving.assets import *
+from gym_urbandriving.planning import Trajectory
+import numpy as np
+import IPython
+
+with open('configs/default_config.json') as json_data_file:
+    data = json.load(json_data_file)
+
+
+data['agents']['action_space'] = "velocity"
+data['recorded_data']['state_space'] = 'raw'
+data['use_pedestrians'] = True
+data['number_of_pedestrians'] = 4
+sup = VelocitySupervisor(agent_num = 0)
+
+
+env = uds.UrbanDrivingEnv(data)
+state = env.current_state
+
+for i in range(3):
+    action = sup.eval_policy(state)
+    obs,reward,done,info_dict = env.step([action])
+    state = obs[0]
+
+
+
+sup = SteeringSupervisor(agent_num = 0)
+data['agents']['action_space'] = "steering"
+env = uds.UrbanDrivingEnv(data)
+state = env.current_state
+
+for i in range(3): 
+    action = sup.eval_policy(state)
+    obs,reward,done,info_dict = env.step([action])
+    state = obs[0]
+