{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Control"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setting up Colab"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "colab_nb = 'google.colab' in str(get_ipython())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  from google.colab import drive\n",
    "  drive.mount('/content/drive')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  %cd /content/drive/My Drive/aad/code/tests/control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  !pip install pyclothoids"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise\n",
    "Please go to `code/exercises/control/pure_pursuit.py` and work on the \"TODO\" items!\n",
    "This notebook will run your pure pursuit and PID implementation in a simple simulation. If your implementation works fine for this simple simulation, you have successfully finished the exercise :). Optionally, you can also test your implementation in Carla. For details regarding the Carla simulation, check the book."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First you can set `run_student_code = False` and see the sample solution at work. After that set `run_student_code = True` and see your implementation at work."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "run_student_code = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys\n",
    "sys.path.append('../../')\n",
    "if run_student_code:\n",
    "    from exercises.control import pure_pursuit\n",
    "else:\n",
    "    from solutions.control import pure_pursuit"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "from vehicle import Vehicle\n",
    "from track import Track\n",
    "from simulation import Simulation, show_img"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next cell lets you choose a visualization option. The remaining cells run the simulation. If you choose a new visualization option, you need to rerun those cells. Explanation of the visualization options:\n",
    "* **None** No visualization, but you will see how much the vehicle deviated from the lane center with `sim.plot_error()`. Hence, this is good enough to test and tune your controller\n",
    "* **offline** A gif with a visualization will be created. The simulation cell will take longer to execute.\n",
    "* **online** You see the visualization while the simulation is executed. You will get problems with the visualization if you have print statements in your `pure_pursuit.py`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from ipywidgets import RadioButtons\n",
    "print(\"Choose visualization option\")\n",
    "viz = RadioButtons(options=['None', 'offline', 'online'],\n",
    "                   value = 'None', # default value   \n",
    "                   description='', disabled=False)\n",
    "display(viz)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that there is a \"TODO\" item in the following cell. You need to tune the parameters of the PID controller here. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# set up simulation\n",
    "wheel_base = 2.65\n",
    "# TODO: Tune your PID here\n",
    "Kp, Ki, Kd = 3,0,0\n",
    "pp = pure_pursuit.PurePursuit(wheel_base=wheel_base, waypoint_shift=0)\n",
    "pid = pure_pursuit.PIDController(Kp, Ki, Kd, 0)\n",
    "controller = pure_pursuit.PurePursuitPlusPID(pure_pursuit=pp, pid=pid)\n",
    "vehicle = Vehicle(wheel_base=wheel_base)\n",
    "sim = Simulation(vehicle, Track(), controller)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# run simulation\n",
    "from IPython.display import clear_output\n",
    "img_list = []\n",
    "for i in range(1,1000):\n",
    "    try:\n",
    "        sim.step()\n",
    "        # visualization\n",
    "        if viz.value!=\"None\":\n",
    "          img = sim.cv_plot()\n",
    "          if i%2==0:\n",
    "            img_list.append(img)\n",
    "            if viz.value==\"online\":\n",
    "              show_img(img)\n",
    "          if viz.value==\"online\":\n",
    "            clear_output(wait=True)\n",
    "        # check for simulation end\n",
    "        if len(sim.waypoints) < 10:\n",
    "            break\n",
    "\n",
    "    except KeyboardInterrupt:\n",
    "        break\n",
    "   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The distance from the vehicle's reference point to the lane center line is called the cross track error. It should be as close to zero as possible. Let's see how it evolved in the simulation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sim.plot_error()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, let us have a look at the velocity over time. The desired velocity is marked with a dashed line."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "sim.plot_velocity()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The simulation video will be stored as a gif, and this gif will be displayed here. If visualization was set to \"None\", you will just see a black square."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import imageio\n",
    "if viz.value==\"None\":\n",
    "  img_list = [np.uint8(np.zeros((100,100,3)))]\n",
    "imageio.mimsave('control.gif', img_list, fps=20)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import Image\n",
    "Image(open('control.gif','rb').read())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {},
 "nbformat": 4,
 "nbformat_minor": 0
}