Mapping agent and functions for plotting variables on the map

MADRaS/__init__.py

@@ -1,7 +1,7 @@
 """Env Registration."""
-from gym.envs.registration import register
+# from gym.envs.registration import register
 
-register(
-    id='Madras-v0',
-    entry_point='MADRaS.envs:MadrasEnv',
-)
+# register(
+#     id='Madras-v0',
+#     entry_point='MADRaS.envs:MadrasEnv',
+# )

MADRaS/agents/generic/mapping_agent.py

@@ -0,0 +1,140 @@
+"""
+Runs a constant velocity traffic agent along the center of a track
+and generates a 2D map of the track. It then puts together a LUT mapping
+distance from start of the race to an (x, y) coordinate of the corresponding
+point of the road in the map.
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+from MADRaS.traffic.traffic import ConstVelTrafficAgent
+import MADRaS.utils.torcs_server_config as torcs_config
+import socket
+import subprocess
+import time
+import logging
+import logging.config
+import sys
+import pickle as pkl
+logging.basicConfig(stream=sys.stdout, level=logging.INFO)
+
+MAPDIR = "/home/anirban/Projects/MADRaS_revisited/MADRaS_tmp/MADRaS/utils/data/maps"
+
+class MappingAgentManager(object):
+    def __init__(self, track_name, visualise=False):
+        self.track_name = track_name
+        self.server_cfg = {
+            "max_cars": 1,
+            "track_names": [self.track_name],
+            "distance_to_start": 0,
+            "learning_car": ['car1-trb1']
+        }
+        self.torcs_server_config = torcs_config.TorcsConfig(
+            self.server_cfg, randomize=False)
+        self.visualise = visualise
+        self.find_free_udp_port()
+        self.torcs_server_config.generate_torcs_server_config()
+        self.start_torcs_server()
+        self.mapping_agent = MadrasMappingAgent(self.torcs_server_port, track_length=self.torcs_server_config.track_length)
+
+    def find_free_udp_port(self):
+        udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        udp.bind(('', 0))
+        _, self.torcs_server_port = udp.getsockname()
+
+    def start_torcs_server(self):
+        if self.visualise:
+            command = 'export TORCS_PORT={} && vglrun torcs -t 10000000 -nolaptime'.format(self.torcs_server_port)
+        else:
+            command = 'export TORCS_PORT={} && torcs -t 10000000 -r ~/.torcs/config/raceman/quickrace.xml -nolaptime'.format(self.torcs_server_port)
+
+        self.torcs_proc = subprocess.Popen([command], shell=True, preexec_fn=os.setsid)
+        time.sleep(1)
+
+    @property
+    def track_map(self):
+        return self.mapping_agent.map
+
+    def map(self):
+        self.mapping_agent.map_track()
+        return self.track_map
+
+    def plot_map(self):
+        if not self.track_map:
+            raise ValueError("No track_map found. Run MappingAgentManager.map() to "
+                             "generate track_map first.")
+        else:
+            coordinates = np.vstack([x[1] for x in self.track_map])
+            plt.figure()
+            plt.plot(coordinates[:, 0], coordinates[:, 1])
+            # TODO(santara): turn off axes - no point in showing axes
+            plt.savefig(os.path.join(MAPDIR, self.track_name+'.png'))
+            # if self.visualise:
+            plt.show()
+
+    def save_map(self):
+        with open(os.path.join(MAPDIR, self.track_name+'.pkl'), 'wb') as f:
+            pkl.dump(self.track_map, f)
+            f.close()
+
+
+class MadrasMappingAgent(ConstVelTrafficAgent):
+    def __init__(self, port, cfg=None, track_length=None):
+        if cfg is None:
+            cfg = {
+                "target_speed": 50,
+                "target_lane_pos": 0.0,
+                "collision_time_window": 1,
+                "pid_settings": {
+                    "accel_pid": [10.5, 0.05, 2.8],
+                    "steer_pid": [5.1, 0.001, 0.000001],
+                    "accel_scale": 1.0,
+                    "steer_scale": 0.1
+                    }
+            }
+        super(MadrasMappingAgent, self).__init__(port, cfg, "MappingAgent")
+        self.coordinates = np.zeros(2)
+        self.heading = 0  # rad. angle w.r.t. horizontal
+        self.map = [[0, self.coordinates]]
+        self.track_length = track_length
+
+    def get_coordinates(self):
+        init_pos = self.coordinates
+        init_vel = self.ob.speedX
+        accel = self.action[1]
+        delta_t = 1/50.
+        delta_s = init_vel * delta_t + 0.5 * accel * delta_t ** 2
+        delta_pos = [delta_s * np.cos(self.heading), delta_s * np.sin(self.heading)]
+        final_pos = init_pos + delta_pos
+        return final_pos
+
+    def map_track(self):
+        self.wait_for_observation()
+        num_steps = 0
+        while True:
+            self.action = self.get_action()
+            delta_heading = np.deg2rad(self.action[0] * 2.21)  # Found the number 2.21 through trial and error
+            self.heading += delta_heading
+            self.coordinates = self.get_coordinates()
+            self.map.append([self.ob.distFromStart, self.coordinates])
+            try:
+                self.ob, _, done, _ = self.env.step(0, self.client, self.action)
+
+            except Exception as e:
+                logging.debug("Exception {} caught by {} traffic agent at port {}".format(
+                                str(e), self.name, self.port))
+                self.wait_for_observation()
+            num_steps += 1
+            if done or (self.ob.distRaced > self.track_length):
+                break
+
+            self.detect_and_prevent_imminent_crash_out_of_track()
+            self.PID_controller.update(self.ob)
+
+
+if __name__=="__main__":
+    mapping_manager = MappingAgentManager('spring', False)
+    map = mapping_manager.map()
+    mapping_manager.plot_map()
+    mapping_manager.save_map()

MADRaS/agents/rllib/eval_rllib_agent.py

@@ -1,27 +1,191 @@
 """
-TODO(santara): write a python script to use rollout.py that does not depend on the Gym
-registry of Madras-v0 
-
 Steps for use:
 
-1. Register Madras in OpenAI gym by adding the following snippet to
-   /home/anirban/anaconda3/lib/python3.7/site-packages/gym/__init__.py or equivalent
-```
-import gym
-gym.envs.register(
-     id='Madras-v0',
-     entry_point='envs:MadrasEnv',
-)
-```
-
-2. Set checkpoint path
-3. run: python eval_rllib_agent.py
+1. Set checkpoint path
+2. run: python eval_rllib_agent.py
 """
 
 
 import os
+from MADRaS.utils.evaluate_trajectories import TrajEvaluator
+
+SIM_OPTIONS_TEMPLATE = """
+# TODO(santara) Each car model should have its own PID parameters, assign track length and track width correctly for each track
+torcs_server_port: 60934
+server_config:
+  max_cars: 1
+  track_names:
+    - {}
+  distance_to_start: 25
+  torcs_server_config_dir: /home/anirban/.torcs/config/raceman/  # This must be the value for TORCS with rendering on
+  scr_server_config_dir: /home/anirban/usr/local/share/games/torcs/drivers/scr_server/
+  traffic_car: p406           # get full list of cars here: /home/anirban/usr/local/share/games/torcs/cars/
+  learning_car:               # get full list of cars here: /home/anirban/usr/local/share/games/torcs/cars/
+    - {}
+
+
+randomize_env: False
+add_noise_to_actions: False
+action_noise_std: 0.1  # Only in effect when add_noise_to_actions is True
+noisy_observations: False  # Adds sensor noise. See Section 7.7 of the scr_server paper: https://arxiv.org/abs/1304.1672
+
+vision: False  # whether to use vision input
+throttle: True
+gear_change: False
+client_max_steps: -1  # to be interpreted as np.inf
+visualise: False  # whether to render TORCS window
+no_of_visualisations: 1  # To visualize multiple training instances (under MPI) in parallel set it to more than 1
+track_len: 6355.65  # in metres. All track lengths can be found here: http://www.berniw.org/trb/tracks/tracklist.php
+max_steps: 20000 #15000 #20000  # max episode length
+track_width: 12.0
+target_speed: 27.78 # 13.89  # metres per sec
+state_dim: 60
+early_stop: True
+normalize_actions: True  # all actions in [-1, 1]
+
+# PID params
+pid_assist: True
+pid_settings:
+  accel_pid:
+    - 10.5  # a_x
+    - 0.05  # a_y
+    - 2.8   # a_z
+  steer_pid:
+    - 5.1
+    - 0.001
+    - 0.000001
+  accel_scale: 1.0
+  steer_scale: 0.1
+  pid_latency: 5
+
+# Observation mode
+observations:
+  mode: SingleAgentSimpleLapObs
+  normalize: False  # gym_torcs normalizes by default
+  obs_min:
+    angle: -3.142
+    track: 0.0
+    trackPos: -1.0
+  obs_max:
+    angle: 3.142
+    track: 200.0
+    trackPos: 1.0
+
+# Reward function
+rewards:
+  ProgressReward2:
+    scale: 1.0
+  AvgSpeedReward:
+    scale: 1.0
+  CollisionPenalty:
+    scale: 10.0
+  TurnBackwardPenalty:
+    scale: 10.0
+  AngAcclPenalty:
+    scale: 5.0
+    max_ang_accl: 2.0
+
+# Done function
+dones:
+  - RaceOver
+  - TimeOut
+  - Collision
+  - TurnBackward
+  - OutOfTrack
+
+"""
+
+
+
+CARS = [
+     "car1-stock1",
+     "car1-stock2",
+     "155-DTM",
+     "car3-trb1",
+     "kc-2000gt",
+     "buggy",
+     "baja-bug"
+]
+
+TRACKS = [
+    "aalborg",
+    "alpine-1",
+    "alpine-2",
+    "brondehach",
+    "g-track-1",
+    "g-track-2",
+    "g-track-3",
+    "corkscrew",
+    "eroad",
+    "e-track-2",
+    "e-track-3",
+    "e-track-4",
+    "e-track-6",
+    "forza",
+    "ole-road-1",
+    "ruudskogen",
+    "street-1",
+    "wheel-1",
+    "wheel-2",
+    "spring"
+]
+
+CHECKPOINT = "<path to checkpoint>"
+
+
+def log_results_single_agent_no_traffic():
+
+    RESULTS_TEMPLATE = """
+    ==================================================
+        Car: {}\n
+        Track: {}\n
+        Average distance covered: {}\n
+        Average speed: {}\n
+        Successful race completion rate: {}\n
+        Num trajectories evaluated: {}\n
+    ==================================================
+    """
+    OUTFILE = open("<path to evaluation directory>/results.txt", 'a')
+    OUTFILE.write("Evaluating {} on {} in {}\n\n".format(CHECKPOINT, CARS, TRACKS))
+    SIM_OPTIONS_PATH = "<path to MADRaS root directory>/MADRaS/envs/data/madras_config.yml"
+
+    for track in TRACKS:
+        for car in CARS:
+              print("\n\nCAR: {}\n\n".format(car))
+              with open(SIM_OPTIONS_PATH, "w") as f:
+                  f.write(SIM_OPTIONS_TEMPLATE.format(track, car))
+              data_path = "<path to evaluation directory>/eval_trajs_{}_{}.pkl".format(car, track)
+              os.system("rllib rollout {} --env Madras-v0 --steps 1000000 --run PPO --no-render --out {}".format(CHECKPOINT, data_path))
+              os.system("pkill torcs")
+              evaluator = TrajEvaluator(track_name=track, data_path=data_path)
+              OUTFILE.write(RESULTS_TEMPLATE.format(car, track, evaluator.avg_frac_track_covered, evaluator.avg_speed,
+                                                    evaluator.race_complete, evaluator.num_trajs))
+
+    OUTFILE.close()
+
+
+def log_results_single_agent_in_traffic():
+
+    RESULTS_TEMPLATE = """
+    ==================================================
+        Successful race completion rate: {}\n
+        Num trajectories evaluated: {}\n
+    ==================================================
+    """
+
+    OUTFILE = open("<path to evaluation directory>/results.txt", 'a')
+    OUTFILE.write("Evaluating {} on 2 traffic agents\n\n".format(CHECKPOINT))
+    data_path = "<path to evaluation directory>/eval_trajs.pkl"
+    os.system("rllib rollout {} --env Madras-v0 --steps 7000 --run PPO --no-render --out {}".format(CHECKPOINT, data_path))
+    os.system("pkill torcs")
+    os.system("pkill torcs")
+    evaluator = TrajEvaluator(data_path=data_path)
+    OUTFILE.write(RESULTS_TEMPLATE.format(evaluator.rank_one_fraction, evaluator.num_trajs))
+    print(RESULTS_TEMPLATE.format(evaluator.rank_one_fraction, evaluator.num_trajs))
+
+    OUTFILE.close()
 
 
-CHECKPOINT = "/home/anirban/ray_results/PPO_madras_env_2019-12-10_19-23-07ytuoid_5/checkpoint_71/checkpoint-71"
-os.system("rllib rollout {} --env Madras-v0 --steps 1000000 --run PPO --no-render".format(CHECKPOINT))
-os.system("pkill torcs")
+if __name__=='__main__':
+    log_results_single_agent_in_traffic()
+#     log_results_single_agent_no_traffic()

MADRaS/envs/data/sim_options.yml → MADRaS/envs/data/madras_config.yml

@@ -1,6 +1,7 @@
 torcs_server_port: 60934
 server_config:
   max_cars: 10
+  min_traffic_cars: 1
   track_names:
     - forza
     - aalborg