Course_PerceptionROS_Unit_1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 1: Vision Basics in ROS Part 1. Follow a Red Ball"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this unit, you will start using cameras in ROS and use the **cmvision** package for blob tracking. Once you get the hang of it, then in Unit 2, you will go deeper in how this blob tracking is done and how the image can be processed."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Basically, you are going to learn how to build the example from the previous unit.**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The First Image from a Robot:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Mira Robot with Red Ball",
    "image": true,
    "name": "perception_unit1_miraball1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit1_miraball1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Roll , Pitch, and Yaw"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It's time to start working, so here you have the Mira Robot in a nice room environment with a red cricket ball.<br>\n",
    "Mira is a three degrees of freedom robot that turns its head in a **Roll-Pitch-Yaw** movement, which is very easy for camera movement. So, it's the perfect robot for this introduction to image."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Roll, Pitch and Yaw airplane example",
    "image": true,
    "name": "perception_unit0_theoryrpy",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit0_theoryrpy.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p> <a href=\"//commons.wikimedia.org/wiki/User:Auawise\" title=\"User:Auawise\">Auawise</a>\n",
    "derivative work: <a href=\"//commons.wikimedia.org/w/index.php?title=User:Jrvz&amp;action=edit&amp;redlink=1\" class=\"new\" title=\"User:Jrvz (page does not exist)\">Jrvz</a> (<a href=\"//commons.wikimedia.org/w/index.php?title=User_talk:Jrvz&amp;action=edit&amp;redlink=1\" class=\"new\" title=\"User talk:Jrvz (page does not exist)\"><span class=\"signature-talk\">talk</span></a>) - <a href=\"//commons.wikimedia.org/wiki/File:Yaw_Axis.svg\" title=\"File:Yaw Axis.svg\">Yaw_Axis.svg</a>, <a href=\"http://creativecommons.org/licenses/by-sa/3.0\" title=\"Creative Commons Attribution-Share Alike 3.0\">CC BY-SA 3.0</a>, <a href=\"https://commons.wikimedia.org/w/index.php?curid=9441238\">Link</a></p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In Mira's case, the axis are slightly different, more in the fashion of robotics than aerospace (which is inverted) :"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Roll, Pitch and Yaw in MiraRobot",
    "image": true,
    "name": "perception_unit1_rpy_mira",
    "width": "17cm"
   },
   "source": [
    " <img id=\"fig-U0.2\" src=\"img/perception_unit1_rpy_mira.png\" width=\"200\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Roll Axis Movement:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Roll Axis Moevement",
    "image": true,
    "name": "perception_unit0_miraroll",
    "width": "7cm"
   },
   "source": [
    "<table style=\"width:100%\">\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "      <img id=\"fig-U0.1\" src=\"img/perception_unit0_miraroll.gif\" width=\"200\"/>\n",
    "       <center> <figcaption><h2>Roll Axis Movement</h2></figcaption></center>\n",
    "    </figure>\n",
    "  </tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pitch Axis Movement:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Pitch Axis Movement",
    "image": true,
    "name": "perception_unit0_mirapitch",
    "width": "7cm"
   },
   "source": [
    "<table style=\"width:100%\">\n",
    "  <tr>\n",
    "    \n",
    "    <th>\n",
    "        <figure>\n",
    "      <img id=\"fig-U0.2\" src=\"img/perception_unit0_mirapitch.gif\" width=\"200\"/>\n",
    "       <center> <figcaption><h2>Pitch Axis Movement</h2></figcaption></center>\n",
    "    </figure>\n",
    "    </th>\n",
    "   \n",
    "  </tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Yaw Axis Movement:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Yaw Axis Movement",
    "image": true,
    "name": "perception_unit0_mirayaw",
    "width": "7cm"
   },
   "source": [
    "<table style=\"width:100%\">\n",
    "  <tr>\n",
    "    \n",
    "    <th>\n",
    "        <figure>\n",
    "      <img id=\"fig-U0.3\" src=\"img/perception_unit0_mirayaw.gif\" width=\"200\"/>\n",
    "       <center> <figcaption><h2>Yaw Axis Movement</h2></figcaption></center>\n",
    "    </figure>\n",
    "    </th> \n",
    "\n",
    "  </tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will also have at your disposal a script for autonomously moving the cricket ball around. So, lets move the ball first.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch blob_tracking move_ball_keyboard.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can move the ball around by just pressing keys on the keyboard. Remember:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"width:100%\">\n",
    "  \n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_i.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Move forward</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_comma.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Move backward</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_j.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Turn left and Backward</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_l.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Turn right and Forward</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_k.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Stop</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  <tr>\n",
    "    <th>\n",
    "    <figure>\n",
    "        <img src=\"img/key_q.png\"width=\"40\"></img>\n",
    "        <img src=\"img/key_z.png\"width=\"40\"></img>\n",
    "        \n",
    "    </figure>\n",
    "    </th>\n",
    "    <th>\n",
    "    <p style=\"text-align: center;\">Increase / Decrease Speed</p>\n",
    "    </th> \n",
    "  </tr>\n",
    "  \n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you are going to see what the robot Mira is seeing. For that, you are going to use a ROS graphical tool called **<i>rqt_image_view</i>** that allows you to see what the camera in the robot is publishing.\n",
    "\n",
    "To open the tool, type the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun rqt_image_view rqt_image_view"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, open the Graphical Interface by clicking on the icon:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Graphical Tool Icon",
    "image": true,
    "name": "font-awesome_desktop",
    "width": "1.3cm"
   },
   "source": [
    "<img src=\"img/font-awesome_desktop.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, you should see on the screen a window of the **<i>rqt_image_view</i>** application.\n",
    " \n",
    "On the application, select the **<i>/mira/mira/camera1/image_raw</i>** image topic and wait a few seconds until the image feed is established. You should see something similar to this."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Camera Vision",
    "image": true,
    "name": "perception_Unit1_miravision",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_Unit1_miravision.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U1-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! Now get the hang of how to move the ball around by trying to make it appear in Mira's view. Once it is in view of the Mira robot, you can close the program of WebShell #2."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U1-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Blob tracking with OpenCV and Python Part 1. Color encoding"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, what you are going to do is to create a program that can track blobs of colour in an image.<br>\n",
    "Blobs are nothing more than areas in an image with a similar color encoding, as similar as you might define it.<br>\n",
    "So, obviously, the first step is to get a color encoding that defines the object you want to be tracked.<br>\n",
    "Let's do that with the red ball, shall we?<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To obtain the color encoding, we are going to use another tool provided by ROS that allows to easily compute the RBG and YUV values of a target blob.\n",
    " \n",
    "Launch the following command in a terminal and go to the Graphical Interface Tab:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch blob_tracking start_colour_gui.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This will start a gui similar to the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Colour Gui",
    "image": true,
    "name": "perception_unit1_colourset2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit1_colourset2.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, click over the red ball and see how by successively clicking over slightly different points of the ball, a black box appears around it. Click over it until the box is just over the entire red ball:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Colour Gui all red ball detected",
    "image": true,
    "name": "perception_unit1_colourset3",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit1_colourset3.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "See the two text boxes? You have the **RGB** value of the color on average and then you have the **YUV**, which, in this case, is **(30:82, 86:111, 178:252)**. Yours could be slightly different. But, the point is that this YUV defines the color blob to be tracked. This means that this is what MiraRobot will consider to be RedBall.\n",
    "\n",
    "You can now close the WebShell #2 program."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### We are going to use those values to create a configuration file, required by the blob recognition code."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, let's create a package to launch all of the required software to track the ball.\n",
    "\n",
    "* First, create a new package named **<i>my_blob_tracking_pkg</i>**, which depends on <i>rospy</i>.\n",
    "* Inside that package, create a directory named **<i>color_files</i>** and put the **<i>colors.txt</i>** file in it. Add the values you got in the previous step, by writing the RGB in average and the YUV values like in the file below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**colors.txt**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[Colors]\n",
    "(  0, 255,  0) 0.000000 10 Green\n",
    "(  0,  0, 255) 0.000000 10 Blue\n",
    "(  255,  255, 0) 0.000000 10 Yellow\n",
    "(  255,  0, 255) 0.000000 10 Purple\n",
    "(  0,  255, 255) 0.000000 10 Teal\n",
    "(  254,  4,   4) 0.000000 10 RedBall\n",
    "\n",
    "[Thresholds]\n",
    "( 144:154, 38:48, 16:26 )\n",
    "( 24:34, 250:255, 102:112 )\n",
    "( 220:230, 0:5, 143:153 )\n",
    "( 100:110, 207:217, 229:239)\n",
    "( 173:183, 165:175, 0:5 )\n",
    "( 30:81, 86:111, 178:253 )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END colors.txt**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You see that there are two parts: **Colors** and **Thresholds**.\n",
    "\n",
    "In **Colors**, you state:<br>\n",
    "\n",
    "* The RGB value of the line around the detection, in this case, is the same color as the average RGB color detected previously, but you can put any color you want. It's just good practice to put the Average color because, that way, you can see what blob is being detected.\n",
    "* The other two numbers are not used in this version of cmvision.\n",
    "* The name of the blob representation, in this case RedBall\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In **Thresholds**, you state:<br>\n",
    "\n",
    "* Essentially, the Red, Green, and Blue value range. In the case for the RedBall, Red = [from 30 to 81], Green = [from 86 to 111], and Blue = [from 178 to 253]\n",
    "* All of them are placed in the same order as the Color List to make the correspondence.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, you can put as many colors as you wish and, afterwards, be able to track different blobs based on their names, like Green, RedBall, Teal, etc."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Blob tracking with OpenCV and Python part 2. Start blob tracking with cmvision"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The ROS package that you have used for the color setting and that you will use for the tracking is **cmvision**, http://wiki.ros.org/cmvision."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add the following launch file that starts the blob tracking code based on your **<i>colour.txt</i>** file, connect to Mira's camera, and publish the ball position and information in a ROS topic. You must add this launch file to the launch directory of your package.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**my_mira_cmvision_tc.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  \n",
    "  <arg name=\"rgb_raw_image_topic\" default=\"/mira/mira/camera1/image_raw\"/>\n",
    "  <arg name=\"color_file_path\" default=\"$(find my_blob_tracking_pkg)/color_files/colors.txt\"/>\n",
    "  \n",
    "  <!-- Location of the cmvision color file -->\n",
    "  <param name=\"cmvision/color_file\" type=\"string\" \n",
    "         value=\"$(arg color_file_path)\" />\n",
    "\n",
    "  <!-- Turn debug output on or off -->\n",
    "  <param name=\"cmvision/debug_on\" type=\"bool\" value=\"true\"/>\n",
    "\n",
    "  <!-- Turn color calibration on or off -->\n",
    "  <param name=\"cmvision/color_cal_on\" type=\"bool\" value=\"false\"/>\n",
    "\n",
    "  <!-- Enable Mean shift filtering -->\n",
    "  <param name=\"cmvision/mean_shift_on\" type=\"bool\" value=\"false\"/>\n",
    "\n",
    "  <!-- Spatial bandwidth: Bigger = smoother image -->\n",
    "  <param name=\"cmvision/spatial_radius_pix\" type=\"double\" value=\"2.0\"/>\n",
    "\n",
    "  <!-- Color bandwidth: Bigger = smoother image-->\n",
    "  <param name=\"cmvision/color_radius_pix\" type=\"double\" value=\"40.0\"/>\n",
    "\n",
    "  <node name=\"cmvision\" pkg=\"cmvision\" type=\"cmvision\" args=\"image:=$(arg rgb_raw_image_topic)\" \n",
    "        output=\"screen\" />\n",
    "</launch>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END my_mira_cmvision_tc.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you set the image topic from which the RGB data will be extracted:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<arg name=\"rgb_raw_image_topic\" default=\"/mira/mira/camera1/image_raw\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you set the path to the color file you created through the given path or, if not, the default value:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<arg name=\"color_file_path\" default=\"$(find my_blob_tracking_pkg)/color_files/colors.txt\"/>\n",
    "<!-- Location of the cmvision color file -->\n",
    "  <param name=\"cmvision/color_file\" type=\"string\" \n",
    "         value=\"$(arg color_file_path)\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, you launch the cmvision node that will make the blob tracking happen based on the image topic given:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<node name=\"cmvision\" pkg=\"cmvision\" type=\"cmvision\" args=\"image:=$(arg rgb_raw_image_topic)\" \n",
    "        output=\"screen\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The other parameters are irrelevant for this course."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch my_blob_tracking_pkg my_mira_cmvision_tc.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">**NOTE**: If you see the error message **<i>libdc1394 error: Failed to initialize libdc1394</i>**, DO NOT WORRY. It has **NO EFFECT** at all.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should, then, have blob information published in the **<i>/blobs</i>** topic. Let's check what is published in that topic:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic list | grep /blobs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That should show the topic **<i>/blobs</i>** on the screen.\n",
    "\n",
    "Now, let's check some info about the topic:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic info /blobs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It will give you the following output:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "user ~ $ rostopic info /blobs            \n",
    "Type: cmvision/Blobs                      \n",
    "Publishers:                              \n",
    " * /cmvision (http://ip-172-31-36-185:42984/)\n",
    "Subscribers: None"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's analize the message obtained through that topic:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "user ~ $ rosmsg show cmvision/Blobs                                   \n",
    "std_msgs/Header header                                    \n",
    "  uint32 seq                                    \n",
    "  time stamp                                    \n",
    "  string frame_id                                   \n",
    "uint32 image_width                                    \n",
    "uint32 image_height                                   \n",
    "uint32 blob_count                                   \n",
    "cmvision/Blob[] blobs                                   \n",
    "  string name                                   \n",
    "  uint32 red                                    \n",
    "  uint32 green                                    \n",
    "  uint32 blue                                   \n",
    "  uint32 area                                   \n",
    "  uint32 x                                    \n",
    "  uint32 y                                    \n",
    "  uint32 left                                   \n",
    "  uint32 right                                    \n",
    "  uint32 top                                    \n",
    "  uint32 bottom"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically, the **<i>Blobs</i>** message is an array of **<i>cmVision/blob</i>** messages.\n",
    "\n",
    "So, what is in a **<i>cmVision/blob</i>** message?:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "user ~ $ rosmsg show cmvision/Blob                                    \n",
    "string name                                   \n",
    "uint32 red                                    \n",
    "uint32 green                                    \n",
    "uint32 blue                                   \n",
    "uint32 area                                   \n",
    "uint32 x                                    \n",
    "uint32 y                                    \n",
    "uint32 left                                   \n",
    "uint32 right                                    \n",
    "uint32 top                                    \n",
    "uint32 bottom"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each **<i>cmVision/blob</i>** message provides the information for a single detected blob.\n",
    "\n",
    "As you can see, you will get a lot of information out of each detected blob. Anyways, the most important information is the blob **name**, the **position** in the image, and the **size**. With this, you can position the blob in a 2D, and maybe in a 3D space. You should also be able to track multiple objects in a 3D space, and know which blob it is each one, if they are sufficiently different in colour."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see the Mira camera with the tracking system already working in the Graphical Interface. If you don't have a red box around the ball, you should check that the YUV values you gave are enough, and run the color setup again, if necessary."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Blob tracking of red ball",
    "image": true,
    "name": "perception_unit1_cmvisiontrack1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit1_cmvisiontrack1.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U1-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a Python script that retrieves the needed information from the **/blob** topic to be able to:<br>\n",
    "\n",
    "* Filter all the blobs, except the RedBall blobs.\n",
    "* Retrieve its position in 2D in the image.\n",
    "* Publish a Twist message into the topic named <i>/mira/commands/velocity</i>, which will be used to move Mira's head to follow the red ball around in the 2D space."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U1-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Solution Exercise U1-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Please try to do it by yourself unless you get stuck or need some inspiration. You will learn much more if you fight for each exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "RobotIgnite",
    "image": true,
    "name": "robotignite_logo_text",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/robotignite_logo_text.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how it could be done:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from geometry_msgs.msg import Twist\n",
    "from cmvision.msg import Blobs, Blob\n",
    "\n",
    "#global\n",
    "turn = 0.0 #turning rate\n",
    "blob_position = 0 # x position for the blob\n",
    "\n",
    "# callback function checks to see if any blobs were found then\n",
    "# loop through each and get the x position.  Since the camera\n",
    "# will sometimes find many blobs in the same object we just\n",
    "# average all the x values.  You could also just take the first\n",
    "# one if you are sure you will only have one blob. \n",
    "#\n",
    "# This doesn't use multiple blobs but if are tracking several \n",
    "# objects you need to check the /data.blobs.color topic for\n",
    "# the color tag you put in your colors.txt file. \n",
    "#\n",
    "# after we have the x value we just make the robot turn to \n",
    "# keep it in the center of the image.\n",
    "\n",
    "def callback(data):\n",
    "    global turn\n",
    "    global blob_position\n",
    "\n",
    "    if(len(data.blobs)):\n",
    "\n",
    "        for obj in data.blobs:\n",
    "            if obj.name == \"RedBall\":\n",
    "                rospy.loginfo(\"Blob <\"+str(obj.name)+\"> Detected!\")\n",
    "                blob_position = obj.x\n",
    "        \n",
    "                rospy.loginfo(\"blob is at %s\"%blob_position)\n",
    "                # turn right if we set off the left cliff sensor\n",
    "                if( blob_position > 220 ):\n",
    "                    rospy.loginfo(\"TURN RIGHT\")\n",
    "                    turn = -0.1\n",
    "                # turn left if we set off the right cliff sensor\n",
    "                if( blob_position < 180 ):\n",
    "                    rospy.loginfo(\"TURN LEFT\")\n",
    "                    turn = 0.1\n",
    "        \n",
    "                if( blob_position > 180 and blob_position < 220):\n",
    "                    rospy.loginfo(\"CENTERED\")\n",
    "                    turn = 0.0\n",
    "    else: \n",
    "        turn = 0.0\n",
    "\n",
    "def run():\n",
    "    rospy.init_node(\"track_blob_color_node\", log_level=rospy.WARN)\n",
    "    global blob_position\n",
    "    # publish twist messages to /cmd_vel\n",
    "    pub = rospy.Publisher('/mira/commands/velocity', Twist, queue_size=1)\n",
    "\n",
    "    #subscribe to the robot sensor state\n",
    "    rospy.Subscriber('/blobs', Blobs, callback)\n",
    "    \n",
    "\n",
    "    global turn\n",
    "    twist = Twist()\n",
    "\n",
    "    while not rospy.is_shutdown():\n",
    "\n",
    "        # turn if we hit the line\n",
    "        if ( turn != 0.0 ):\n",
    "            str = \"Turning %s\"%turn\n",
    "            rospy.loginfo(str)\n",
    "            twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0\n",
    "            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn\n",
    "            turn = 0.0\n",
    "\n",
    "            # straight otherwise\n",
    "        else:\n",
    "            str = \"Straight %s\"%turn\n",
    "            rospy.loginfo(str)\n",
    "            twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0\n",
    "            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0\n",
    "\n",
    "            # send the message and delay\n",
    "        pub.publish(twist)\n",
    "        blob_position = 0\n",
    "        rospy.sleep(0.1)\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    try:\n",
    "        run()\n",
    "    except rospy.ROSInterruptException: pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U1-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a Python script that retrieves the needed information from the **<i>/mira/commands/velocity</i>** topic to be able to:<br>\n",
    "\n",
    "* Make Mira move its head so that the red ball is in the center of the image all the time. \n",
    "* Move the ball using the keyboard program we showed you above and really check that the head of the robot is moving to always keep the red ball in the middle of the screen. \n",
    "\n",
    "\n",
    "<span style=\"color:green;\">**NOTE: If you don't quite understand how to move Mira's head, we highly recommend you do the courses in TF, URDF, and Controllers to have a full understanding of what is going on and how Mira works**.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U1-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Solution Exercise U1-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Please try to do it by yourself unless you get stuck or need some inspiration. You will learn much more if you fight for each exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "RobotIgnite",
    "image": true,
    "name": "robotignite_logo_text",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/robotignite_logo_text.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a way in which it could be done:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import time\n",
    "import rospy\n",
    "from math import pi, sin, cos, acos\n",
    "import random\n",
    "from std_msgs.msg import Float64\n",
    "from sensor_msgs.msg import JointState\n",
    "from geometry_msgs.msg import Twist\n",
    "\"\"\"\n",
    "Topics To Write on:\n",
    "type: std_msgs/Float64\n",
    "/mira/pitch_joint_position_controller/command\n",
    "/mira/roll_joint_position_controller/command\n",
    "/mira/yaw_joint_position_controller/command\n",
    "\"\"\"\n",
    "\n",
    "class MiraBlobTracker(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        \n",
    "        rospy.loginfo(\"Mira JointMover Initialising...\")\n",
    "        self.pub_mira_roll_joint_position = rospy.Publisher('/mira/roll_joint_position_controller/command',\n",
    "                                                            Float64,\n",
    "                                                            queue_size=1)\n",
    "        self.pub_mira_pitch_joint_position = rospy.Publisher('/mira/pitch_joint_position_controller/command',\n",
    "                                                             Float64,\n",
    "                                                             queue_size=1)\n",
    "        self.pub_mira_yaw_joint_position = rospy.Publisher('/mira/yaw_joint_position_controller/command',\n",
    "                                                           Float64,\n",
    "                                                           queue_size=1)\n",
    "        joint_states_topic_name = \"/mira/joint_states\"\n",
    "        rospy.Subscriber(joint_states_topic_name, JointState, self.mira_joints_callback)\n",
    "        mira_joints_data = None\n",
    "        while mira_joints_data is None:\n",
    "            try:\n",
    "                mira_joints_data = rospy.wait_for_message(joint_states_topic_name, JointState, timeout=5)\n",
    "            except:\n",
    "                rospy.logwarn(\"Time out \" + str(joint_states_topic_name))\n",
    "                pass\n",
    "\n",
    "        self.mira_joint_dictionary = dict(zip(mira_joints_data.name, mira_joints_data.position))\n",
    "        print self.mira_joint_dictionary\n",
    "        rospy.Subscriber('/mira/commands/velocity',  Twist, self.blob_info_callback)\n",
    "        \n",
    "        \n",
    "    def blob_info_callback(self, msg):\n",
    "        rospy.loginfo(\"Blob info Detected==>\"+str(msg.angular.z))\n",
    "        turn_value = msg.angular.z\n",
    "        yaw_actual_pos = self.mira_joint_dictionary.get(\"yaw_joint\")\n",
    "        next_value = yaw_actual_pos + turn_value\n",
    "        rospy.loginfo(\"Move Head to Blob==>\"+str(next_value))\n",
    "        #self.move_mira_yaw_joint(position=next_value)\n",
    "        self.ove_mira_all_joints(roll=0.0, pitch=0.0, yaw=next_value)\n",
    "\n",
    "    def move_mira_all_joints(self, roll, pitch, yaw):\n",
    "        angle_roll = Float64()\n",
    "        angle_roll.data = roll\n",
    "        angle_pitch = Float64()\n",
    "        angle_pitch.data = pitch\n",
    "        angle_yaw = Float64()\n",
    "        angle_yaw.data = yaw\n",
    "        self.pub_mira_roll_joint_position.publish(angle_roll)\n",
    "        self.pub_mira_pitch_joint_position.publish(angle_pitch)\n",
    "        self.pub_mira_yaw_joint_position.publish(angle_yaw)\n",
    "\n",
    "    def move_mira_roll_joint(self, position):\n",
    "        \"\"\"\n",
    "        limits radians : lower=\"-0.2\" upper=\"0.2\"\n",
    "        :param position:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        angle = Float64()\n",
    "        angle.data = position\n",
    "        self.pub_mira_roll_joint_position.publish(angle)\n",
    "\n",
    "    def move_mira_pitch_joint(self, position):\n",
    "        \"\"\"\n",
    "        limits radians : lower=\"0\" upper=\"0.44\"\n",
    "        :param position:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        angle = Float64()\n",
    "        angle.data = position\n",
    "        self.pub_mira_pitch_joint_position.publish(angle)\n",
    "\n",
    "    def move_mira_yaw_joint(self, position):\n",
    "        \"\"\"\n",
    "        Limits : continuous, no limits\n",
    "        :param position:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        angle = Float64()\n",
    "        angle.data = position\n",
    "        self.pub_mira_yaw_joint_position.publish(angle)\n",
    "\n",
    "    def mira_joints_callback(self, msg):\n",
    "        \"\"\"\n",
    "        sensor_msgs/JointState\n",
    "        std_msgs/Header header\n",
    "        uint32 seq\n",
    "        time stamp\n",
    "        string frame_id\n",
    "        string[] name\n",
    "        float64[] position\n",
    "        float64[] velocity\n",
    "        float64[] effort\n",
    "\n",
    "        :param msg:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        self.mira_joint_dictionary = dict(zip(msg.name, msg.position))\n",
    "\n",
    "    def mira_check_joint_value(self, joint_name, value, error=0.1):\n",
    "        \"\"\"\n",
    "        Check the joint by name 'pitch_joint', 'roll_joint', 'yaw_joint' is near the value given\n",
    "        :param value:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        similar = self.mira_joint_dictionary.get(joint_name) >= (value - error ) and self.mira_joint_dictionary.get(joint_name) <= (value + error )\n",
    "\n",
    "        return similar\n",
    "\n",
    "    def convert_angle_to_unitary(self, angle):\n",
    "        \"\"\"\n",
    "        Removes complete revolutions from angle and converts to positive equivalent\n",
    "        if the angle is negative\n",
    "        :param angle: Has to be in radians\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        # Convert to angle between [0,360)\n",
    "        complete_rev = 2 * pi\n",
    "        mod_angle = int(angle / complete_rev)\n",
    "        clean_angle = angle - mod_angle * complete_rev\n",
    "        # Convert Negative angles to their corresponding positive values\n",
    "        if clean_angle < 0:\n",
    "            clean_angle += 2 * pi\n",
    "\n",
    "        return clean_angle\n",
    "\n",
    "    def assertAlmostEqualAngles(self, x, y,):\n",
    "        c2 = (sin(x) - sin(y)) ** 2 + (cos(x) - cos(y)) ** 2\n",
    "        angle_diff = acos((2.0 - c2) / 2.0)\n",
    "        return angle_diff\n",
    "\n",
    "    def mira_check_continuous_joint_value(self, joint_name, value, error=0.1):\n",
    "        \"\"\"\n",
    "        Check the joint by name 'pitch_joint', 'roll_joint', 'yaw_joint' is near the value given\n",
    "        We have to convert the joint values removing whole revolutions and converting negative versions\n",
    "        of the same angle\n",
    "        :param value:\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        joint_reading = self.mira_joint_dictionary.get(joint_name)\n",
    "        clean_joint_reading = self.convert_angle_to_unitary(angle=joint_reading)\n",
    "        clean_value = self.convert_angle_to_unitary(angle=value)\n",
    "\n",
    "        dif_angles = self.assertAlmostEqualAngles(clean_joint_reading, clean_value)\n",
    "        similar = dif_angles <= error\n",
    "\n",
    "        return similar\n",
    "\n",
    "    \n",
    "\n",
    "    def mira_movement_look(self, roll, pitch, yaw):\n",
    "        \"\"\"\n",
    "        Make Mira look down\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        check_rate = 5.0\n",
    "        position_roll = roll\n",
    "        position_pitch = pitch\n",
    "        position_yaw = yaw\n",
    "\n",
    "        similar_roll = False\n",
    "        similar_pitch = False\n",
    "        similar_yaw = False\n",
    "        rate = rospy.Rate(check_rate)\n",
    "        while not (similar_roll and similar_pitch and similar_yaw):\n",
    "            self.move_mira_all_joints(position_roll, position_pitch, position_yaw)\n",
    "            similar_roll = self.mira_check_continuous_joint_value(joint_name=\"roll_joint\", value=position_roll)\n",
    "            similar_pitch = self.mira_check_continuous_joint_value(joint_name=\"pitch_joint\", value=position_pitch)\n",
    "            similar_yaw = self.mira_check_continuous_joint_value(joint_name=\"yaw_joint\", value=position_yaw)\n",
    "            rate.sleep()\n",
    "\n",
    "\n",
    "    def search_for_blob_loop(self):\n",
    "        \"\"\"\n",
    "        Executed movements in a random way\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        rospy.loginfo(\"Hearing Blobs Moving Mira...\")\n",
    "        rospy.spin()\n",
    "\n",
    "\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('mira_move_head_node', anonymous=True)\n",
    "    mira_jointmover_object = MiraBlobTracker()\n",
    "    mira_jointmover_object.search_for_blob_loop()\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Did you manage? If all went well, then Mira should follow the red ball without losing track of it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise EXTRA U1-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Improve the scripts of U1-2 and U1-3, so that:<br>\n",
    "\n",
    "* Mira can track the ball in a 3D space that is not only moving the yaw axis, but also the roll and pitch. This way, the ball will be centered no matter where it goes. Remember that you can move the ball UP and DOWN by pressing the **T** and **B** keys, or stop applying upward force with the **G** key.\n",
    "* Make Mira shake its head when the ball goes too far. Use the **<i>area</i>** variable for that.\n",
    "* Create a searching pattern for Mira when it loses track of the ball."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise EXTRA U1-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise EXTRA U1-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Improve the scripts of U1-2 and U1-3, so that:<br>\n",
    "\n",
    "* Mira can track various objects at the same time\n",
    "* To spawn new objects, just spawn them through the **spawn_robot_tools** pacakge, which is installed in the system. If you don't quite understand them, please do the courses in **TF** or **URDF** to understand how this works."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise EXTRA U1-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations! You can now track anything with color. Continue to the next unit to go a little bit deeper in OpenCV in ROS and learn how to navigate following a line drawn on the ground."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#417FB1;color:white;\">**Project**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, select the project unit of this course.\n",
    "You can now do the first exercise of the Aibo Project. There, you will have to make the Aibo Robot look for its pink ball and go to its location."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#417FB1;color:white;\">**END Project**</p>"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "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.12"
  },
  "latex_metadata": {
   "chapter": "1 - Vision Basics in ROS Part 1, Follow a Red Ball",
   "chapter_title": "Unit 1: Vision Basics in ROS Part 1, Follow a Red Ball",
   "course_title": "ROS PERCEPTION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}