Course_Drones_Unit2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/drone_course_image2.png\" width=\"700\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"700\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Programming Drones"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 2: Drone Exploration"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: <b>2 hours</b><br><br>\n",
    "In this unit, you are going to see some very interesting tools that will allow you to explore unknown environments with your drone, using a camera-based SLAM."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this unit, you are going to see how you can fly your drone in unknown, GPS-denied environments, which is one of the main problems that drones are facing nowadays.\n",
    "\n",
    "The main things we will need to do here are to be able to localize the robot within an unknown environment, and to be able to move it by it sending goals; and all of this just by using its own sensors. To do this, we'll need to solve the Simultaenous Localization and Mapping issue, which is a well-known problem in robotics, and was already treated in the Navigation Course. Now, though, we are going to use a different approach to solve this problem, which is called Parallel Tracking and Mapping (PTAM). This approach is an implementation of Monocular SLAM, and was developed by Georg Kein and David Murray. You can get more information on this subject by clicking the following link:\n",
    "http://www.robots.ox.ac.uk/~gk/PTAM/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Luckily for us, a ROS package that implements this PTAM solution into ROS already exists. The package is called <a href=\"\" target=\"_blank\">tum_ardrone</a>, and was developed by the amazing guys at TUM Computer Vision Group. You can check out some information about them through the following links:\n",
    "\n",
    "* Website: https://vision.in.tum.de/\n",
    "* Repository: https://github.com/tum-vision"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The approach we're going to see in this unit basically consists of three major components:\n",
    "\n",
    "1) **Monocular SLAM**: For monocular SLAM, our solution is based on Parallel Tracking and Mapping (PTAM). After map initialization, we rotate the visual map such that the xy-plane corresponds to the horizontal plane according to the accelerometer data, and scale it such that the average keypoint depth is 1. Throughout tracking, the scale of the map λ ∈ R is estimated using a novel method described in Section IV-A. Furthermore, we use the pose estimates from the EKF to identify and reject falsely-tracked frames.\n",
    "\n",
    "2) **Extended Kalman Filter**: In order to fuse all available data, we employ an extended Kalman filter (EKF). We derived and calibrated a full motion model of the quadrocopter’s flight dynamics and reaction to control commands, which we will describe in more detail in Section IV-B. This EKF is also used to compensate for the different time delays in the system, arising from wireless LAN communication and computationally complex visual tracking.\n",
    "\n",
    "3) **PID Control**: Based on the position and velocity estimates from the EKF at t +∆tcontrol, we apply PID control to steer the quadrocopter towards the desired goal location in a global coordinate system. According to the stated estimate, we rotate the generated control commands to the robot-centric coordinate system and send them to the quadrocopter. For each of the four degrees-of-freedom, we employ a separate PID controller for which we experimentally determined suitable controller gains."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Working with the tum_ardrone package"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **tum_ardrone** ROS package provides autonomous flight with PTAM-based visual navigation. It is made for the Parrot AR.Drone, but later, we are going to see how you can adapt this package to also work with other drones."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's stop the talking, and let's have a look at the tool!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, execute the following command in order to start the GUI for the tum_ardrone package"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch tum_ardrone tum_ardrone.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you can open the Graphic Tools by hitting this icon in the IDE section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/font-awesome_desktop.png\"width=\"25\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And a new tab will open, showing something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_ardrone_gui.png\"width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you can visualize three different windows."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "* **tum_ardrone GUI**: The main window where you can interact with the different applications of the package."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **PTAM Drone Camera Feed**: A video stream of what the drone is seeing. You will also use this screen for sending \"goals\" to the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **PTAM Drone Map View**: A map based on the different data provided by the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But let's have a more detailed look at them by parts."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### GUI"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **tum_ardrone** GUI offers a simple QT GUI to control the **drone_autopilot** node and the **drone_stateestimation** node, and to fly the drone manually via keyboard or joystick (not available in this case). Let's have a quick look at some of the sections of this GUI tool."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, you can see some information regarding the communication status between the nodes and the drone in the top-right corner of the window."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_node_status.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If everything is fine, you should have a value higher than 100Hz in the _Drone Navdata_ parameter, and a value of 33Hz in the _Pose Estimates_ parameter. This means that the communication between the drone and the nodes is correct."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Right under this section, you can visualize the status of the **Autopilot node**. This node will allow you to pilot the drone by sending waypoints via the camera feed screen, but you will see this later. At this moment, you haven't started the autopilot node yet, so you should see all the values at zero."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_autopilot.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, you have the status of the StateEstimation node. This node is in charge of estimating the drone's position based on sent navdata (you will see more information on this navdata later), sent control commands, and PTAM."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_state.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, on the bottom-right corner of the window, you have the Control Source section. This will allow you to switch between different control modes of the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_control_source.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, on the left side of the window, you have three more sections. In the top-left corner, you have the Commands sections, where you will be able to send commands to the node in order to do something."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_commands.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Right below this, you can find some useful buttons; for instance, for automatically landing or taking off, or sending commands introduced in the above section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_buttons.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, on the bottom-left corner of the window, you have the messages section. Here, you will see all the logs generated by the application, so you will know what's going on."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_messages.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So, once all the main parts of the GUI window have been introduced, let's try something! For instance, let's try the Keyboard mode. To use this mode, you'll have to select the Keyboard option in the Control Source section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_control_keyboard.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you will be able to control the drone with the following keys:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **q,a**: fly up & down.\n",
    "* **i,j,k,l**: fly horizontally.\n",
    "* **u,o**: rotate yaw.\n",
    "* **F1**: toggle emergency\n",
    "* **s**: takeoff\n",
    "* **d**: land"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**NOTE**: Bear in mind that in order to send these commands through the keyboard, you need to have the focus window focused, not the simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Nothing really new here. It's more or less what you saw in the previous chapter. The **Autopilot** mode, on the other hand, is much more interesting. Let's have a look at it! To activate this mode, follow the next steps:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1.- First, type the command \"autoInit 500 800\" in the Commands Section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_autoinit.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2.- Next, click \"Clear and Send\" (you might need to click Reset first, if you have been doing other stuff before this)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_send.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see some messages like this one appearing in the Messages field."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_autopilot_logs.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3.- Now, the Autopilot mode is activated. You can click on the video window to interactively set targets (relative to the current position). In the following section, you are going to see how this works."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### PTAM Drone Camera Feed"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Setting the drone_autopilot node"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have the video window divided by two axis, **x** and **y**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_video.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Clicking on the video window will generate waypoints, which are sent to the drone_autopilot node. When using the mouse, you have two possible options: left-click (with the left button) and right-click (with the right-button). Depending on which click you perform, the waypoints you send will be handled one way or another. It works like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **left-click**: fly (x,y,0)m relative to the current position, image-center is (0,0), borders are 2m, respectively."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's see some examples of how this works. For instance, if you left-click on the following point of the window:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_video2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are telling the robot to fly 1m on its x-axis (which, in this case, is forward), and 0m on its y-axis (which, in this case, is sideways). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/camera_move3.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On the other hand, if you left-click on the following point of the window:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_video3.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are telling the robot to fly 2m on its x-axis (which, in this case, is sideways), and 0m on its y-axis (which, in this case, is forward)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/camera_move4.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **right-click**: fly (0,0,y)m and rotate yaw by x degrees, image-center is (0,0), borders are 2m and 90 degrees, respectively."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's see some examples of how this works. For instance, if you right-click on the following point of the window:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_video4.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are telling the robot to rotate 90 degrees to the right side, while staying at the same height. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/camera_move1.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If, for instance, you right-click on the following point of the window:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_video2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are now telling the robot to go up 1 meter, while not rotating at all."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/camera_move2.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Does all of this make sense? Well, you'll be able to practice a little bit with the following exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.1</p>\n",
    "<br>\n",
    "a) Launch the **tum_ardrone** nodes as shown in the explanation.\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch tum_ardrone tum_ardrone.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Have the drone take off and switch to the Autopilot mode."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Move the drone around using the camera-based Navigation, as described in the above explanations."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is quite interesting, right? But in fact, we are not really using anything related to PTAM SLAM. For that, we will need to use the **drone_stateestimation** node. So... let's see how to start it!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Setting the drone_stateestimation node"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**IMPORTANT**: To work properly, this node requires messages to be sent on both **/ardrone/navdata** (>100Hz) and **/ardrone/image_raw** (>10Hz), i.e. a connected drone with a running ardrone_autonomy node, or a .bag replay of at least those two channels. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we previously said, this node will estimate the drone's position based on sent navdata, sent control commands, and PTAM. In order to have it running, first of all, you'll need to have the drone flying. So, if you don't, the first thing you'll need to do is make the drone take off."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once the drone is flying, you'll need to focus on the camera window. You will see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_state1.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Notice on the bottom of the window some yellow letters that read: **Point the camera at planar scene and press spacebar to start tracking for initial map**. So, that's what you'll need to do! Just make sure that the drone is looking at the scene, and press the spacebar button on your keyboard."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Automatically, after you press the spacebar, you'll see some points appear in the image. Something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_state2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, on the bottom of the window, the text has changed. It reads: **Translate the camera slowly sideways, and press spacebar to perform stereo init**. So, now you'll have to move the drone sideways just a little bit. This way, the camera will also move sideways a little bit. Then, immediately after this, you'll have to press the spacebar again."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Automatically, after you press the spacebar, you'll see that the points now appear much clearer in the image. Something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_state3.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**NOTE**: To properly estimate PTAM's scale, it is best to fly up and down a little bit (for instance, 1m up and 1m down) immediately after initialization."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You have set up the **drone_stateestimation** node. So, you are now tracking the position of your drone in relation to the objects that are captured by the camera."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you can see a gif of the whole process."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/drone_stateestimation.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also have a look at the **PTAM Map** window now. You'll see that the points captured by your camera are placed on the map as well."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_map.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, you can now see how the points that represent the house are placed on the map with an estimated distance in relation to the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.2</p>\n",
    "<br>\n",
    "a) Launch the **tum_ardrone** nodes as shown in the explanation.\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch tum_ardrone tum_ardrone.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Make the drone take off and switch to the Autopilot mode."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Set up the PTAM system by reproducing the steps shown above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Move the drone around and see how the map gets built."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Recording and reproducing a flight"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A very interesting and useful thing that you can do with drones is record a flight of the drone in order to reproduce it later. And this can be easily achieved by using bag files."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically, during the flight of the drone, there are three topics that we need to monitor if we want to reproduce this flight later. They are the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **/cmd_vel**: The data related to the movements and speeds of the drone.\n",
    "\n",
    "\n",
    "* **/ardrone_takeoff**: The data related to the takeoff of the drone.\n",
    "\n",
    "\n",
    "* **/ardrone/land**: The data related to the landing of the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**NOTE**: Bear in mind that these topics are specific to the AR Drone. If you want to reproduce these steps for another drone, you will have to check the equivalent topics that are used for that drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to record a bag file with the data from all these topics, you will have to execute the following command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosbag record -O recordedFlight.bag /ardrone/takeoff /ardrone/land /cmd_vel"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To stop recording, just press Ctrl+C in the console where you have launched the previous command."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, to reproduce this bag file, you have to execute the following command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosbag play recordedFlight.bag"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also do something similar in order to apply the PTAM system you've been using in this unit. But in that case, you'll need to also record two extra topics."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **/ardrone/image_raw**: The data related to the drone's camera.\n",
    "\n",
    "\n",
    "* **/ardrone/navdata**: Navigation-related data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the data from these three topics, you will be able to reproduce the whole PTAM process. This may be useful for doing research on the PTAM subject. You can have a look at the following link if you are intereseted in this subject: https://github.com/tum-vision/tum_ardrone"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.3</p>\n",
    "<br>\n",
    "a) Launch the **tum_ardrone** nodes as shown in the explanation.\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "Execute in WebShell #1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch tum_ardrone tum_ardrone.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Start recording a flight as explained above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Set up the Autopilot mode, have the drone take off, and fly it around."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**NOTE**: Bear in mind that for this course, the space available on the hard disk is limited, so the flight you record must be short."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) Take the drone back to the initial position, and play the recorded flight."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Reproducing predefined movements."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are many functionalities and other things related to the tum_ardrone package that, due to a lack of time, we cannot go over in this course. But in this last section, we are going to have a quick look at one of them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You previously had a look at the different parts that appear in the GUI of the application. But we overlooked a very interesting tool. Within this GUI, there is a section where you can load different predefined flights that are performed using PTAM and estimate the drone's position each time. Let's see how to use it!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, you'll need to set up the PTAM system, as shown in previous sections."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/drone_stateestimation.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, in the GUI, you can open the **Load File** menu and select one of the predefined files."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_movement_select.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For instance, let's select the **NikolausHorizontal.txt** file. You'll end up with something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_commands_nikolaus.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you just have to click on the **Send** button, and the drone will start executing the predefined movement."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/nikolaus_hor.gif\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, in the PTAM map window, you will visualize the whole trajectory executed by the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_map_nikolaus.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.4</p>\n",
    "<br>\n",
    "a) Launch the **tum_ardrone** nodes as shown in the explanation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Set up the PTAM system."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Try to execute another trajectory. For instance, the **square1x1.txt**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 2.5</p>\n",
    "<br>\n",
    "a) Have a look at how the different trajectories are created."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Create your own trajectory file, and execute it with the drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 2.5</p>"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "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": 1
}