Course_Drones_Unit3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/hector_scene.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 3: Adapting the PTAM framework for another drone"
   ]
  },
  {
   "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 how you can adapt the camera-based SLAM system you saw in the previous unit, which was created for the Parrot AR Drone, to another drone."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous unit, you learned how to work with the **tum_ardrone** package in order to perform camera-based Navigation. But that package is meant to only work with the Parrot AR Drone, which is kind of limited. So, in this unit, what I want to explain to you is how you can adapt this package to work with other nodes. You are not going to do a 100% adaptation, but you are going to learn the basic stuff you need to know in order to do it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, as may have already noticed, the simulation and the drone you are going to use in this unit are different. For this unit, you are going to use the **hector_quadrotor** drone. It is a very basic drone model simulation, with all the basic sensors we need, like a camera, an imu, etc. You can check the simulation repository in the following link: https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/hector_quadrotor.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that I have introduced you to the new drone, let's begin with the unit! The adaptation process will be divided in four main parts, which are the following:\n",
    "\n",
    "* Download the code\n",
    "\n",
    "* Modify the tum_ardrone package\n",
    "\n",
    "* Publish Navdata\n",
    "\n",
    "* Additional Steps"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So... let's stop the talking and begin with the real work!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Downloading the code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous unit, you were working with a preinstalled and preconfigured version of the **tum_ardrone** package. But this version is not accessible to students. So, if we want to modify this package in order to adapt it to other drones, the first thing we'll need to do is download it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, go to the **src** folder in your workspace."
   ]
  },
  {
   "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": [
    "roscd;\n",
    "cd ..;\n",
    "cd src;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can download the package using the followng command:"
   ]
  },
  {
   "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": [
    "git clone https://github.com/tum-vision/tum_ardrone.git"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, after downloading the package, make sure that the branch you downloaded is the **indigo-devel**. You can check that by going to the downloaded package and executing the following command:"
   ]
  },
  {
   "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": [
    "git checkout indigo-devel"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_indigo_devel.png\"width=\"800\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will also need to download the **ardrone_autonomy** package. You will need this package in order to be able to publish the rewired navdata used by the **tum_ardrone** package. You will get deeper into this topic later. To download this package, you can execute the following command:"
   ]
  },
  {
   "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": [
    "roscd;\n",
    "cd ..;\n",
    "cd src;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "git clone https://github.com/AutonomyLab/ardrone_autonomy.git"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the end, you should have both packages in your workspace, like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/tum_download_pkgs.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Modifying the tum_ardrone package"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, now it's time to start the modifications. There are many topics that can be used by this package, but for this adaptation, we are going to focus only on the main ones. These topics are the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* A topic for where to send velocity commands to the drone. In this simulation (and usually always), this topic is named **cmd_vel**.\n",
    "\n",
    "\n",
    "* A topic for where to read data from the camera. In this simulation, this topic is named **/front_cam/camera/image**.\n",
    "\n",
    "\n",
    "* Two topics for taking off and landing the drone. In this simulation, these topics are named **/takeoff** and **/land**.\n",
    "\n",
    "\n",
    "* A topic for where all the navigation data will be published. This topic is known as the **navdata** topic, and unfortunately, this is something that is only provided in the Parrot AR Drone simulation. So, for this simulation, we'll need to make a workaround in order to have this topic. But don't worry about this now, we'll see it later."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, basically, the work we are going to do with the **tum_ardrone** package will be to remap all the default topics for the AR Drone simulation to the equivalent topics in this simulation. Easy, right?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this task, you'll need to focus on three files:\n",
    "\n",
    "* **RosThread.cpp** (which is located in the UINode folder)\n",
    "\n",
    "\n",
    "* **ControlNode.cpp** (which is located in the autopilot folder )\n",
    "\n",
    "\n",
    "* **EstimationNode.cpp** (which is located in the stateestimation folder)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### RosThread.cpp"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within the **RosThread::run()** function of this file, you'll need to find the following lines and modify them as shown below:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "vel_pub\t   = nh_.advertise<geometry_msgs::Twist>(nh_.resolveName(\"cmd_vel\"),1);\n",
    "vel_sub\t   = nh_.subscribe(nh_.resolveName(\"cmd_vel\"),50, &RosThread::velCb, this);\n",
    "\n",
    "navdata_sub\t   = nh_.subscribe(nh_.resolveName(\"hector/navdata\"),50, &RosThread::navdataCb, this);\n",
    "\n",
    "takeoff_pub\t   = nh_.advertise<std_msgs::Empty>(nh_.resolveName(\"/takeoff\"),1);\n",
    "land_pub\t   = nh_.advertise<std_msgs::Empty>(nh_.resolveName(\"/land\"),1);\n",
    "\n",
    "takeoff_sub\t   = nh_.subscribe(nh_.resolveName(\"/takeoff\"),1, &RosThread::takeoffCb, this);\n",
    "land_sub\t   = nh_.subscribe(nh_.resolveName(\"/land\"),1, &RosThread::landCb, this);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### ControlNode.cpp"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within the **ControlNode::ControlNode()** function of this file, you'll need to find the following lines and modify them as shown below:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "control_channel = nh_.resolveName(\"cmd_vel\");\n",
    "takeoff_channel = nh_.resolveName(\"/takeoff\");\n",
    "land_channel = nh_.resolveName(\"/land\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### EstimationNode.cpp"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within the **EstimationNode::EstimationNode()** function of this file, you'll need to find the following lines and modify them as shown below:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "navdata_channel = nh_.resolveName(\"hector/navdata\");\n",
    "control_channel = nh_.resolveName(\"cmd_vel\");\n",
    "video_channel = nh_.resolveName(\"/front_cam/camera/image\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! You have already done the necessary work in order to adapt the **tum_ardrone** package for this simulation. Of course, there are more topics that you could remap. But, depending on the simulation you use, they will probably not be available, so you would have to publish those topics yourself (like we will do with the **navdata** topic). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, at this point, you are ready to compile the package in order to create all the libraries and executables, and to check that you haven't broken anything in the process. Also, you will compile the **ardrone_autnomy** package, which will be required for the next section. In order to compile your workspace, you can execute the following commands:"
   ]
  },
  {
   "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": [
    "roscd;\n",
    "cd ..;\n",
    "catkin_make;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**NOTE**: Don't worry if some warnings appear during the compilation. This is normal. Just make sure that the compilation finishes and it is 100% completed."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Publish Navdata"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great job! You are almost done! This one is the last step to finishing this adaptation, although it is also the most difficult. In this section, you are going to see how to create a node that publishes all the required **navdata** into a topic, which will be used by the **tum_ardrone** package."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But, first of all, let's have a look at what we need to publish into this topic. The type of message that we are going to publish into this topic is the following: **ardrone_autonomy/Navdata**. You can have a look at the structure of the message by executing the following command:"
   ]
  },
  {
   "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": [
    "rosmsg show ardrone_autonomy/Navdata"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will get a structure like this one:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "std_msgs/Header header\n",
    "  uint32 seq\n",
    "  time stamp\n",
    "  string frame_id\n",
    "float32 batteryPercent\n",
    "uint32 state\n",
    "int32 magX\n",
    "int32 magY\n",
    "int32 magZ\n",
    "int32 pressure\n",
    "int32 temp\n",
    "float32 wind_speed\n",
    "float32 wind_angle\n",
    "float32 wind_comp_angle\n",
    "float32 rotX\n",
    "float32 rotY\n",
    "float32 rotZ\n",
    "int32 altd\n",
    "float32 vx\n",
    "float32 vy\n",
    "float32 vz\n",
    "float32 ax\n",
    "float32 ay\n",
    "float32 az\n",
    "uint8 motor1\n",
    "uint8 motor2\n",
    "uint8 motor3\n",
    "uint8 motor4\n",
    "uint32 tags_count\n",
    "uint32[] tags_type\n",
    "uint32[] tags_xc\n",
    "uint32[] tags_yc\n",
    "uint32[] tags_width\n",
    "uint32[] tags_height\n",
    "float32[] tags_orientation\n",
    "float32[] tags_distance\n",
    "float32 tm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Wait! Don't panic! It's not as huge as it seems, trust me. Most of the variables that are contained in this message can be overlooked, at least when working with a simulation. We are just going to focus on providing some of them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's go! The first thing you are going to do is create a new 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": [
    "roscd;\n",
    "cd ..;\n",
    "cd src;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_create_pkg publish_navdata"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Within this package, create two new folders, named **src** and **launch**. Inside the src folder, create a file named **publish_navdata_code.py**; and inside the launch file, create a file named **publish_navdata.launch**. In the end, you should have a structure like this one."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/navdata_pkg.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! Now, let's copy the following code to the **publish_navdata_code.py** file. Later, I'll analyze the code and what each part of it does."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**publish_navdata_code.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from ardrone_autonomy.msg import Navdata\n",
    "from sensor_msgs.msg import Imu, Range\n",
    "from geometry_msgs.msg import PoseStamped, Twist\n",
    "\n",
    "rotX = 0\n",
    "rotY = 0\n",
    "rotZ = 0\n",
    "height = 0\n",
    "vx = 0\n",
    "vy = 0\n",
    "vz = 0\n",
    "ax = 0\n",
    "ay = 0\n",
    "az = 1\n",
    "M_PI = 3.14159265358979323846264338327950288\n",
    "\n",
    "def imu_callback(msg):\n",
    "    global rotX\n",
    "    global rotY\n",
    "    global rotZ\n",
    "    rotX = msg.orientation.x\n",
    "    rotY = msg.orientation.y\n",
    "    rotZ = msg.orientation.z\n",
    "    \n",
    "def pose_callback(msg):\n",
    "    global height\n",
    "    height = msg.pose.position.z\n",
    "    \n",
    "def vel_callback(msg):\n",
    "    global vx\n",
    "    global vy\n",
    "    global vz\n",
    "    vx = msg.linear.x\n",
    "    vy = msg.linear.y\n",
    "    vz = msg.linear.z\n",
    "\n",
    "\n",
    "rospy.init_node('publish_navdata')\n",
    "\n",
    "rate = rospy.Rate(2)\n",
    "\n",
    "imu_sub = rospy.Subscriber('/raw_imu', Imu, imu_callback)\n",
    "pose_sub = rospy.Subscriber('/ground_truth_to_tf/pose', PoseStamped, pose_callback)\n",
    "vel_sub = rospy.Subscriber('/cmd_vel', Twist, vel_callback)\n",
    "\n",
    "navdata_pub = rospy.Publisher('/hector/navdata', Navdata, queue_size=1)\n",
    "navdata = Navdata()\n",
    "\n",
    "while not rospy.is_shutdown():\n",
    "\n",
    "    navdata.batteryPercent = 100\n",
    "    navdata.rotX = rotX / M_PI * 180\n",
    "    navdata.rotY = rotY / M_PI * 180\n",
    "    navdata.rotZ = rotZ / M_PI * 180\n",
    "    navdata.altd = height * 1000\n",
    "    navdata.vx = vx\n",
    "    navdata.vy = vy\n",
    "    navdata.vz = vz\n",
    "    navdata.ax = ax\n",
    "    navdata.ay = ay\n",
    "    navdata.az = az\n",
    "    navdata.tm = rospy.get_time() * 1000000\n",
    "    \n",
    "    navdata.header.stamp = rospy.Time.now()\n",
    "    navdata.header.frame_id = \"base_link\"\n",
    "    navdata.state = 2\n",
    "    navdata.magX = 0\n",
    "    navdata.magY = 0\n",
    "    navdata.magZ = 0\n",
    "    navdata.pressure = 0\n",
    "    navdata.temp = 0\n",
    "    navdata.wind_speed = 0.0\n",
    "    navdata.wind_angle = 0.0\n",
    "    navdata.wind_comp_angle = 0.0\n",
    "    navdata.tags_count = 0\n",
    "\n",
    "    navdata_pub.publish(navdata)\n",
    "    rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's analyze this code more deeply now."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import rospy\n",
    "from ardrone_autonomy.msg import Navdata\n",
    "from sensor_msgs.msg import Imu, Range\n",
    "from geometry_msgs.msg import PoseStamped, Twist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are just importing some necessary modules and messages that you will later use for the Publishers and Subscribers of the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rotX = 0\n",
    "rotY = 0\n",
    "rotZ = 0\n",
    "height = 0\n",
    "vx = 0\n",
    "vy = 0\n",
    "vz = 0\n",
    "ax = 0\n",
    "ay = 0\n",
    "az = 1\n",
    "M_PI = 3.14159265358979323846264338327950288"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you are defining some global variables to make them easier to use through the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def imu_callback(msg):\n",
    "    global rotX\n",
    "    global rotY\n",
    "    global rotZ\n",
    "    rotX = msg.orientation.x\n",
    "    rotY = msg.orientation.y\n",
    "    rotZ = msg.orientation.z\n",
    "    \n",
    "def pose_callback(msg):\n",
    "    global height\n",
    "    height = msg.pose.position.z\n",
    "    \n",
    "def vel_callback(msg):\n",
    "    global vx\n",
    "    global vy\n",
    "    global vz\n",
    "    vx = msg.linear.x\n",
    "    vy = msg.linear.y\n",
    "    vz = msg.linear.z\n",
    "    \n",
    "imu_sub = rospy.Subscriber('/raw_imu', Imu, imu_callback)\n",
    "pose_sub = rospy.Subscriber('/ground_truth_to_tf/pose', PoseStamped, pose_callback)\n",
    "vel_sub = rospy.Subscriber('/cmd_vel', Twist, vel_callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These are the Subscriber definitions and callbacks. Each time a message is published in one of these topics (**/raw_imu**, **/ground_truth_to_tf/pose**, and **/cmd_vel**), the related callbacks are called. Inside the callbacks, all you are doing is putting the values we are interested in into global variables. \n",
    "\n",
    "* From the **/imu_raw** topic, which contains the data from the Imu sensor, we get the values of the orientation of the drone.\n",
    "\n",
    "\n",
    "* From the **/ground_truth_to_tf/pose** topic, which contains the data from the pose of the drone, we get the values of the z-axis, which is the altitude of the drone.\n",
    "\n",
    "\n",
    "* From the **/cmd_vel** topic, which contains the data from the velocities sent to the drone, we get the values of all the linear components of these velocities."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "navdata_pub = rospy.Publisher('/hector/navdata', Navdata, queue_size=1)\n",
    "navdata = Navdata()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, we are just defining a Publisher, indicating in what topic we are going to publish all this navdat data. In this case, the topic where we will publish this data will be named **/hector_navdata**. Also, we created an instance of the **ardrone_autonomy/Navdata** message."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "while not rospy.is_shutdown():\n",
    "\n",
    "    navdata.batteryPercent = 100\n",
    "    navdata.rotX = rotX / M_PI * 180\n",
    "    navdata.rotY = rotY / M_PI * 180\n",
    "    navdata.rotZ = rotZ / M_PI * 180\n",
    "    navdata.altd = height * 1000\n",
    "    navdata.vx = vx\n",
    "    navdata.vy = vy\n",
    "    navdata.vz = vz\n",
    "    navdata.ax = ax\n",
    "    navdata.ay = ay\n",
    "    navdata.az = az\n",
    "    navdata.tm = rospy.get_time() * 1000000\n",
    "    \n",
    "    navdata.header.stamp = rospy.Time.now()\n",
    "    navdata.header.frame_id = \"base_link\"\n",
    "    navdata.state = 2\n",
    "    navdata.magX = 0\n",
    "    navdata.magY = 0\n",
    "    navdata.magZ = 0\n",
    "    navdata.pressure = 0\n",
    "    navdata.temp = 0\n",
    "    navdata.wind_speed = 0.0\n",
    "    navdata.wind_angle = 0.0\n",
    "    navdata.wind_comp_angle = 0.0\n",
    "    navdata.tags_count = 0\n",
    "\n",
    "    navdata_pub.publish(navdata)\n",
    "    rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we just create a loop in order to keep publishing this data constantly, updating the values with the current ones we read from the sensors (using the Subscribers you saw above). Inside the whole loop, we fill all the variables that the **Navdata** message contains. As you can see, there are many values that we are leaving as 0. You can try to fill these values if you want, but that is something I will leave to you."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, all you have to do now is fill in the launch file to start this node. The launch file could be something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**publish_navdata.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <node pkg=\"publish_navdata\" type=\"publish_navdata_code.py\" name=\"publish_navdata\" output=\"screen\">\n",
    "        \n",
    "    </node>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Awesome!! So, you are now done! You can launch the code and check that everything works fine."
   ]
  },
  {
   "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 publish_navdata publish_navdata.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, if you have a look at the topics in the simulation, you should see that a new topic has appeared, named **/hector_navdata**."
   ]
  },
  {
   "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": [
    "rostopic list"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will see the new topic in the list."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/hector_navdata.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also have a look at what is being published into the topic."
   ]
  },
  {
   "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": [
    "rostopic echo /hector/navdata -n1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/hector_navdata_echo.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Awesome!! Now everything is ready and set up in order to use the **tum_ardrone** package with the hector_quadrotor drone. So, to conclude this chapter, just launch the tum_ardrone package and check that everything goes fine. You should be able to visualize  something like this in the Graphic Tools window."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/PTAM_launched.png\" width=\"800\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 3.1</p>\n",
    "<br>\n",
    "To successfully complete the chapter, set up the PTAM nodes as you learned to do in the previous chapter, and test that everything works fine."
   ]
  },
  {
   "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
}