{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# AR Obstacle Avoidance\n",
    "In this document, you will learn how to write obstacle avoidance code for your Intel Aero RTF drone, utilizing tools we have already seen in the course of the program. You can find our presentation on this here: https://docs.google.com/presentation/d/1TayWiAUiq3rBzKxJigS7_iZ-nUdeq2IVuoayHSu3zeA/edit?usp=sharing\n",
    "\n",
    "Key tools:\n",
    "\n",
    " - `ar_track_alvar`\n",
    " - Closed loop velocity control\n",
    " - Open loop velocity control\n",
    " - `rqt_plot` to plot error signals\n",
    "\n",
    "## Challenge\n",
    " - Start >1 m away from an AR tag and fly to the default height of 0.75 m\n",
    " - Fly forward until the AR tag is <= 1 m away from the drone\n",
    " - Based on AR tag height, fly over/under the AR tag 'obstacle' and return to the default height on the other side\n",
    " - Resume going forward (but pilot-in-command needs to take over before the drone hits the net).\n",
    " \n",
    "### Checkpoints\n",
    "#### Height Control\n",
    "Demonstrate the drone automatically controlling its height.\n",
    "\n",
    " * Graph the commanded z velocity with `rqt_plot` and show a TA before you fly\n",
    " * Hover the drone low over the ground and show it rises/lowers to 0.75 m\n",
    " \n",
    "#### Over/Under logic\n",
    "Show the drone an AR tag and based on the AR tag's height the drone will decide whether to go over or under the AR tag\n",
    "\n",
    " * Make sure you are holding the drone over the ground so it knows its height off the ground\n",
    " * This checkpoint doesn't require you to fly\n",
    " \n",
    "#### State transitions\n",
    "Do after completing Over/Under logic checkpoint. Demonstrate that the drone changes its height setpoint after detecting the AR tag to avoid the obstacle and, after a period of time, reverts to the default height.\n",
    "\n",
    " * This does not require you to actually fly the drone, so you can complete this before you finish Height Control"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Launching Obstacle Avoidance\n",
    "You will be using a launch file to execute obstacle avoidance code called `obstacle_avoid.launch`. This launch file will be provided to you in `aero_control` upstream for you to use with your obstacle avoidance code. \n",
    "\n",
    "`obstacle_avoid.launch` does a few things:\n",
    "\n",
    "- Starts `obstacle_avoid.py`, your avoidance code. Don't forget to make it executable with `chmod +x`.\n",
    "- Starts MAVROS\n",
    "- Starts the Intel RealSense Camera \n",
    "- Starts `ar_track_alvar`\n",
    "\n",
    "You will also need `aero-optical-flow` and `aero-teraranger.service` running outside of the launch file to be able to fly autonomously.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Code overview\n",
    "We recommend you start by copying code from your open loop controller, as we are not providing skeleton code. Here is an overview of what your controller needs to do:\n",
    "\n",
    " 1. Cruise forward until you are close enough to an AR tag, using P control to control your height\n",
    "    * Send horizontal velocities and a height setpoint to the streaming thread\n",
    "    * In the streaming thread, calculate vertical velocities using current height and height setpoint using a proportional controller.\n",
    " 2. Calculate the AR tag's height using the drone's pose and the pose of the AR tag relative to the drone. Look at the AR tag localization lab for ideas.\n",
    " 3. Decide based on the height of the drone whether to go up or down.\n",
    "    * Change the height setpoint\n",
    "    * Have a check in the streaming thread that prevents the drone from moving forward if it isn't close enough to its target altitude yet - this prevents the drone from plowing forward into an obstacle while changing altitude\n",
    " 4. After a certain length of time, return to the default height\n",
    "    * Use `rospy.sleep(dur)`, NOT `time.sleep(dur)`\n",
    "    * Reset the height setpoint to the default height\n",
    "\n",
    "### Side topic: Finite-state Machine\n",
    "To do obstacle avoidance, we are creating a simple [finite-state machine](https://en.wikipedia.org/wiki/Finite-state_machine). This means that there are a limited number of tasks, or \"states\" that the drone could be occupying, such as `normal flight` or `going under`. At each time step, you will need to update this state with new information from AR tag detections, pose information, and time from when you last saw an AR tag.\n",
    "\n",
    "### Mistakes to avoid\n",
    " * Do not forget to read the directions\n",
    " * Do not avoid the obstacle by hard-coding to go up, over, and across\n",
    " * Do not hard-code any constants (ex. threshold height, PID constants)\n",
    " * Check how far away the AR tag you saw was\n",
    "    * Don’t begin your obstacle avoidance routine until the obstacle is close enough\n",
    " * Alvar occasionally sees tags with a position of (nan nan nan).\n",
    "    * Comparing nan to anything returns false!\n",
    " * Do not use `time.sleep()` - use `rospy.sleep()`\n",
    "    * This plays better with ROS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Debugging\n",
    "By now, you have gained experience with `rqt_image_view` and `rostopic echo`. THere is one last tool you need to know: `rqt_plot`. This tool will allow us to graph numerical ros topics, such as velocity commands, which can be essential for tuning PID loops.\n",
    "\n",
    "### Accessing parts of ROS topics\n",
    "Whenever you want to view the commanded velocity, you run `rostopic echo /mavros/setpoint_velocity/cmd_vel_unstamped`. This prints out the whole ROS message. However, you may want to print only parts of the message, such as the angular component of the message. Do this by extending the name of the ROS topic: ex. `rostopic echo /mavros/setpoint_velocity/cmd_vel_unstamped/angular`\n",
    "\n",
    "### `rqt_plot`\n",
    "Like `rqt_image_view`, this is a graphical tool (and can be very slow). Accordingly, you must have run `ssh` with the `-X` or `-Y` flags to be able to use it. You can either add the correct ROS topics in the GUI (not recommended) or add them via the command line: ex. `rqt_plot /topic_name`. Note that this only plots vectors and scalars: if you try to do `rqt_plot /mavros/setpoint_velocity/cmd_vel_unstamped` it will plot nothing. You need to tell `rqt_plot` to plot only a vector or a scalar component of the message (such as the `angular` or `linear` components of `/mavros/setpoint_velocity/cmd_vel_unstamped`.\n",
    "\n",
    "You can pass multiple topics too: simply add them one after another: `rqt_plot /mavros/setpoint_velocity/cmd_vel_unstamped /some_fake_topic`\n",
    "\n",
    "Be careful: topics with `/` at the end are NOT the same as topics without the `/`: `/mavros/setpoint_velocity/cmd_vel_unstamped` is not the same as `/mavros/setpoint_velocity/cmd_vel_unstamped/`"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.15+"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}