Behavior-Planner-Exercise.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Behavior Planner for Navigating Traffic\n",
    "Base code from the Udacity - Self-Driving Car Nanodegree.\n",
    "\n",
    "Navigate through highway traffic where the car needs to be in a specific lane at some distance down the road."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import random\n",
    "import time\n",
    "from IPython.display import clear_output"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Road\n",
    "\n",
    "Road is in charge of populating and updating the position of all the cars on the road. This includes our car that is navigating traffic refered to as 'ego'."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class Road(object):\n",
    "    update_width = 70\n",
    "    \n",
    "    # Simple marker\n",
    "#     ego_rep = \" *** \"\n",
    "\n",
    "    # State based marker\n",
    "    ego_rep = {'KL':   \"  V  \", \n",
    "               'PLCL': \"  V: \", \n",
    "               'PLCR': \" :V  \", \n",
    "               'LCL':  \" V>> \", \n",
    "               'LCR':  \" <<V \"}\n",
    "    \n",
    "    ego_key = -1\n",
    "    def __init__(self, speed_limit, traffic_density, lane_speeds):\n",
    "        self.num_lanes = len(lane_speeds)\n",
    "        self.lane_speeds = lane_speeds\n",
    "        self.speed_limit = speed_limit\n",
    "        self.density = traffic_density\n",
    "        self.camera_center = self.update_width / 2\n",
    "        self.vehicles = {}\n",
    "        self.vehicles_added = 0\n",
    "        \n",
    "    def get_ego(self):\n",
    "        return self.vehicles[self.ego_key]\n",
    "    \n",
    "    def populate_traffic(self):\n",
    "        start_s = int(max(self.camera_center - (self.update_width / 2), 0))\n",
    "        for l in range(self.num_lanes):\n",
    "            lane_speed = self.lane_speeds[l]\n",
    "            vehicle_just_added = False\n",
    "            for s in range(start_s, start_s + self.update_width):\n",
    "                if vehicle_just_added:\n",
    "                    vehicle_just_added = False\n",
    "                    continue\n",
    "                if random.random() < self.density:\n",
    "                    vehicle = Vehicle(l, s, lane_speed, 0)\n",
    "                    vehicle.state = \"CS\"\n",
    "                    self.vehicles_added += 1\n",
    "                    self.vehicles[self.vehicles_added] = vehicle\n",
    "                    vehicle_just_added = True\n",
    "\n",
    "    def advance(self):\n",
    "        predictions = {}\n",
    "        for v_id, v in self.vehicles.items():\n",
    "            preds = v.generate_predictions()\n",
    "            predictions[v_id] = preds\n",
    "        for v_id, v in self.vehicles.items():\n",
    "            if v_id == self.ego_key:\n",
    "                v.update_state(predictions)\n",
    "                v.realize_state(predictions)\n",
    "            v.increment()\n",
    "\n",
    "    def add_ego(self, lane_num, s, config_data):\n",
    "        for v_id, v in self.vehicles.items():\n",
    "            if v.lane == lane_num and v.s == s:\n",
    "                del self.vehicles[v_id]\n",
    "        ego = Vehicle(lane_num, s, self.lane_speeds[lane_num], 0)\n",
    "        ego.configure(config_data)\n",
    "        ego.state = \"KL\"\n",
    "        self.vehicles[self.ego_key] = ego\n",
    "\n",
    "    def cull(self):\n",
    "        ego = self.vehicles[self.ego_key]\n",
    "        center_s = ego.s\n",
    "        claimed = set([(v.lane, v.s) for v in self.vehicles.values()])\n",
    "        for v_id, v in self.vehicles.items():\n",
    "            if v.s > (center_s + self.update_width / 2) or v.s < (center_s - self.update_width / 2):\n",
    "                try:\n",
    "                    claimed.remove((v.lane,v.s))\n",
    "                except:\n",
    "                    continue\n",
    "                del self.vehicles[v_id]\n",
    "\n",
    "                placed = False\n",
    "                while not placed:\n",
    "                    lane_num = random.choice(range(self.num_lanes))\n",
    "                    ds = random.choice(range(self.update_width/2-15,self.update_width/2 -1 ))\n",
    "                    if lane_num > self.num_lanes / 2:\n",
    "                        ds *= -1\n",
    "                    s = center_s + ds\n",
    "                    if (lane_num, s) not in claimed:\n",
    "                        placed = True\n",
    "                        speed = self.lane_speeds[lane_num]\n",
    "                        vehicle = Vehicle(lane_num, s, speed, 0)\n",
    "                        self.vehicles_added += 1\n",
    "                        self.vehicles[self.vehicles_added] = vehicle\n",
    "                        print ('adding vehicle {} at lane {} with s={}'.format(self.vehicles_added, lane_num, s))\n",
    "\n",
    "    def __repr__(self):\n",
    "        s = self.vehicles.get(self.ego_key).s\n",
    "        self.camera_center = max(s, self.update_width / 2)\n",
    "        s_min = max(self.camera_center - self.update_width /2, 0)\n",
    "        s_max = s_min + self.update_width\n",
    "        road = [[\"     \" if i % 3 == 0 else \"     \"for ln in range(self.num_lanes)] for i in range(self.update_width)]\n",
    "        for v_id, v in self.vehicles.items():\n",
    "            if s_min <= v.s < s_max:\n",
    "                if v_id == self.ego_key:\n",
    "                    # Simple ' *** ' marker\n",
    "#                     marker = self.ego_rep\n",
    "\n",
    "                    # State based marker\n",
    "                    marker = self.ego_rep[self.vehicles[self.ego_key].state]\n",
    "                else:\n",
    "                    marker = \" %03d \" % v_id\n",
    "                road[int(v.s - s_min)][v.lane] = marker\n",
    "        s = \"\"\n",
    "        i = s_min\n",
    "        for l in road:\n",
    "            if i % 20 == 0:\n",
    "                s += \"%03d - \" % i\n",
    "            else:\n",
    "                s += \"      \"\n",
    "            i += 1\n",
    "            s += \"|\" + \"|\".join(l) + \"|\"\n",
    "            s += \"\\n\"\n",
    "        return s"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Vehicle\n",
    "\n",
    "Vehicles are responsible for their own behavior such as slowing down to not run into a slower car ahead.\n",
    "\n",
    "This is also where our behavior planning happens in update_state()."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class Vehicle(object):\n",
    "    L = 1\n",
    "    preferred_buffer = 6 # impacts \"keep lane\" behavior.\n",
    "\n",
    "    def __init__(self, lane, s, v, a):\n",
    "        self.lane = lane\n",
    "        self.s = s \n",
    "        self.v = v\n",
    "        self.a = a\n",
    "        self.state = \"CS\"\n",
    "        self.max_acceleration = None\n",
    "\n",
    "    # TODO - Implement this method.\n",
    "    def update_state(self, predictions):\n",
    "        \"\"\"\n",
    "        Updates the \"state\" of the vehicle by assigning one of the\n",
    "        following values to 'self.state':\n",
    "\n",
    "        \"KL\" - Keep Lane\n",
    "        - The vehicle will attempt to drive its target speed, unless there is \n",
    "        traffic in front of it, in which case it will slow down.\n",
    "\n",
    "        \"LCL\" or \"LCR\" - Lane Change Left / Right\n",
    "        - The vehicle will IMMEDIATELY change lanes and then follow longitudinal\n",
    "        behavior for the \"KL\" state in the new lane.\n",
    "\n",
    "        \"PLCL\" or \"PLCR\" - Prepare for Lane Change Left / Right\n",
    "        - The vehicle will find the nearest vehicle in the adjacent lane which is\n",
    "        BEHIND itself and will adjust speed to try to get behind that vehicle.\n",
    "\n",
    "        INPUTS\n",
    "        - predictions \n",
    "        A dictionary. The keys are ids of other vehicles and the values are arrays\n",
    "        where each entry corresponds to the vehicle's predicted location at the \n",
    "        corresponding timestep. The FIRST element in the array gives the vehicle's\n",
    "        current position. Example (showing a car with id 3 moving at 2 m/s):\n",
    "\n",
    "        {\n",
    "        3 : [\n",
    "        {\"s\" : 4, \"lane\": 0},\n",
    "        {\"s\" : 6, \"lane\": 0},\n",
    "        {\"s\" : 8, \"lane\": 0},\n",
    "        {\"s\" : 10, \"lane\": 0},\n",
    "        ]\n",
    "        }\n",
    "\n",
    "        \"\"\"\n",
    "        \n",
    "        \n",
    "        self.state = 'KL' # this is an example of how you change state.\n",
    "\n",
    "        \n",
    "    def configure(self, road_data):\n",
    "        \"\"\"\n",
    "        Called by simulator before simulation begins. Sets various\n",
    "        parameters which will impact the ego vehicle. \n",
    "        \"\"\"\n",
    "        self.target_speed = road_data['speed_limit']\n",
    "        self.lanes_available = road_data[\"num_lanes\"]\n",
    "        self.max_acceleration = road_data['max_acceleration']\n",
    "        goal = road_data['goal']\n",
    "        self.goal_lane = goal[1]\n",
    "        self.goal_s = goal[0]\n",
    "\n",
    "    def __repr__(self):\n",
    "        s = \"s:    {}\\n\".format(self.s)\n",
    "        s +=\"lane: {}\\n\".format(self.lane)\n",
    "        s +=\"v:    {}\\n\".format(self.v)\n",
    "        s +=\"a:    {}\\n\".format(self.a)\n",
    "        return s\n",
    "\n",
    "    def increment(self, dt=1):\n",
    "        self.s += self.v * dt\n",
    "        self.v += self.a * dt\n",
    "\n",
    "\n",
    "    def state_at(self, t):\n",
    "        \"\"\"\n",
    "        Predicts state of vehicle in t seconds (assuming constant acceleration)\n",
    "        \"\"\"\n",
    "        s = self.s + self.v * t + self.a * t * t / 2\n",
    "        v = self.v + self.a * t\n",
    "        return self.lane, s, v, self.a\n",
    "\n",
    "    def collides_with(self, other, at_time=0):\n",
    "        \"\"\"\n",
    "        Simple collision detection.\n",
    "        \"\"\"\n",
    "        l,   s,   v,   a = self.state_at(at_time)\n",
    "        l_o, s_o, v_o, a_o = other.state_at(at_time)\n",
    "        return l == l_o and abs(s-s_o) <= L \n",
    "\n",
    "    def will_collide_with(self, other, timesteps):\n",
    "        for t in range(timesteps+1):\n",
    "            if self.collides_with(other, t):\n",
    "                return True, t\n",
    "        return False, None\n",
    "\n",
    "    def realize_state(self, predictions):\n",
    "        \"\"\"\n",
    "        Given a state, realize it by adjusting acceleration and lane.\n",
    "        Note - lane changes happen instantaneously.\n",
    "        \"\"\"\n",
    "        state = self.state\n",
    "        if   state == \"CS\"  : self.realize_constant_speed()\n",
    "        elif state == \"KL\"  : self.realize_keep_lane(predictions)\n",
    "        elif state == \"LCL\" : self.realize_lane_change(predictions, \"L\")\n",
    "        elif state == \"LCR\" : self.realize_lane_change(predictions, \"R\")\n",
    "        elif state == \"PLCL\": self.realize_prep_lane_change(predictions, \"L\")\n",
    "        elif state == \"PLCR\": self.realize_prep_lane_change(predictions, \"R\")\n",
    "\n",
    "\n",
    "    def realize_constant_speed(self):\n",
    "        self.a = 0\n",
    "\n",
    "    def _max_accel_for_lane(self, predictions, lane, s):\n",
    "        delta_v_til_target = self.target_speed - self.v\n",
    "        max_acc = min(self.max_acceleration, delta_v_til_target)\n",
    "        in_front = [v for (v_id, v) in predictions.items() if v[0]['lane'] == lane and v[0]['s'] > s ]\n",
    "        if len(in_front) > 0:\n",
    "            leading = min(in_front, key=lambda v: v[0]['s'] - s)\n",
    "            next_pos = leading[1]['s']\n",
    "            my_next = s + self.v\n",
    "            separation_next = next_pos - my_next\n",
    "            available_room = separation_next - self.preferred_buffer\n",
    "            max_acc = min(max_acc, available_room)\n",
    "        return max_acc\n",
    "\n",
    "    def realize_keep_lane(self, predictions):\n",
    "        self.a = self._max_accel_for_lane(predictions, self.lane, self.s)\n",
    "\n",
    "    def realize_lane_change(self, predictions, direction):\n",
    "        delta = -1\n",
    "        if direction == \"L\": \n",
    "            delta = 1\n",
    "        self.lane += delta\n",
    "        self.a = self._max_accel_for_lane(predictions, self.lane, self.s)\n",
    "\n",
    "    def realize_prep_lane_change(self, predictions, direction):\n",
    "        delta = -1\n",
    "        if direction == \"L\": \n",
    "            delta = 1\n",
    "        lane = self.lane + delta\n",
    "        ids_and_vehicles = [(v_id, v) for (v_id, v) in predictions.items() if v[0]['lane'] == lane and v[0]['s'] <= self.s]\n",
    "        if len(ids_and_vehicles) > 0:\n",
    "            vehicles = [v[1] for v in ids_and_vehicles]\n",
    "\n",
    "            nearest_behind = max(ids_and_vehicles, key=lambda v: v[1][0]['s'])\n",
    "\n",
    "            print (\"nearest behind : {}\".format(nearest_behind))\n",
    "            nearest_behind = nearest_behind[1]\n",
    "            target_vel = nearest_behind[1]['s'] - nearest_behind[0]['s']\n",
    "            delta_v = self.v - target_vel\n",
    "            delta_s = self.s - nearest_behind[0]['s']\n",
    "            if delta_v != 0:\n",
    "                print (\"delta_v {}\".format(delta_v))\n",
    "                print (\"delta_s {}\".format(delta_s))\n",
    "                time = -2 * delta_s / delta_v\n",
    "                if time == 0:\n",
    "                    a = self.a\n",
    "                else:\n",
    "                    a = delta_v / time\n",
    "                print (\"raw a is {}\".format(a))\n",
    "                if a > self.max_acceleration: \n",
    "                    a = self.max_acceleration\n",
    "                if a < -self.max_acceleration: \n",
    "                    a = -self.max_acceleration\n",
    "                self.a = a\n",
    "                print (\"time : {}\".format(time))\n",
    "                print (\"a: {}\".format(self.a))\n",
    "            else :\n",
    "                min_acc = max(-self.max_acceleration, -delta_s)\n",
    "                self.a = min_acc\n",
    "\n",
    "    def generate_predictions(self, horizon=10):\n",
    "        predictions = []\n",
    "        for i in range(horizon):\n",
    "            lane, s, v, a = self.state_at(i)\n",
    "            predictions.append({'s':s, 'lane': lane})\n",
    "        return predictions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Run Simulation\n",
    "\n",
    "Here we set some hyperparameters and run the simulation.  The simulation outputs an animation of our target car navigating through traffic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     | 007 |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     | 008 |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "300 - |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     | 009 |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     | 010 |     |\n",
      "      |     |     |  V  |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     | 011 |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "320 - |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     | 012 |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "      |     |     |     |     |\n",
      "\n",
      "You missed the goal. You are in lane 2 instead of 3.\n"
     ]
    }
   ],
   "source": [
    "# impacts default behavior for most states\n",
    "SPEED_LIMIT = 60 \n",
    "\n",
    "# all traffic in lane (besides ego) follow these speeds\n",
    "LANE_SPEEDS = [6,7,8,9]\n",
    "# Random lane speeds\n",
    "# LANE_SPEEDS = np.random.randint(1, 10, 4)\n",
    "\n",
    "# Number of available \"cells\" which should have traffic\n",
    "TRAFFIC_DENSITY = 0.15\n",
    "# Random traffic density\n",
    "# TRAFFIC_DENSITY = np.random.uniform(0.1, 0.5)\n",
    "\n",
    "# At each timestep, ego can set acceleration to value between \n",
    "# -MAX_ACCEL and MAX_ACCEL\n",
    "MAX_ACCEL = 2\n",
    "\n",
    "# s value and lane number of goal.\n",
    "GOAL = (300, 3)\n",
    "\n",
    "# Apply a random goal distance and goal lane\n",
    "# GOAL = (np.random.randint(100,500), np.random.randint(0,len(LANE_SPEEDS)-1))\n",
    "\n",
    "# These affect the visualization\n",
    "FRAMES_PER_SECOND = 5\n",
    "AMOUNT_OF_ROAD_VISIBLE = 40\n",
    "\n",
    "\n",
    "road = Road(SPEED_LIMIT, TRAFFIC_DENSITY, LANE_SPEEDS)\n",
    "road.update_width = AMOUNT_OF_ROAD_VISIBLE\n",
    "road.populate_traffic()\n",
    "ego_config = config = {\n",
    "    'speed_limit' : SPEED_LIMIT,\n",
    "    'num_lanes' : len(LANE_SPEEDS),\n",
    "    'goal' : GOAL,\n",
    "    'max_acceleration': MAX_ACCEL\n",
    "}\n",
    "road.add_ego(2, 0, ego_config)\n",
    "timestep = 0\n",
    "while road.get_ego().s <= GOAL[0]:\n",
    "    timestep += 1\n",
    "    if timestep > 150: \n",
    "        print(\"Taking too long to reach goal. Go faster!\")\n",
    "        break\n",
    "    road.advance()\n",
    "    clear_output(wait=True)\n",
    "    print(road)\n",
    "    time.sleep(float(1.0) / FRAMES_PER_SECOND)\n",
    "ego = road.get_ego()\n",
    "if ego.lane == GOAL[1]:\n",
    "    print(\"You got to the goal in {} seconds!\".format(timestep))\n",
    "else:\n",
    "    print(\"You missed the goal. You are in lane {} instead of {}.\".format(ego.lane, GOAL[1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}