Course_PerceptionROS_Unit_2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 2: Vision Basics in ROS Part 2. Follow a line"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this unit, you will learn how to start using the most basic and also the most powerful tool for perception in ROS: **OpenCV**.\n",
    "OpneCV is the most extensive and complete library for image recognition. With it, you will be able to work with images like never before: applying filters, postprocessing, and working with images in any way you want."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# How to use OpenCV in ROS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you might have guessed, OpenCV is not a ROS library, but it's been integrated nicely into it with <a href=\"http://wiki.ros.org/cv_bridge\">OpenCV_bridge</a>.\n",
    "**This package allows the ROS imaging topics to use the OpenCV image variable format**.\n",
    "\n",
    "For example, OpenCV images come in BGR image format, while regular ROS images are in the more standard RGB encoding. OpenCV_bridge provides a nice feature to convert between them. Also, there are many other functions to transfer images to OpenCV variables transparently.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To learn how to use OpenCV, you will use the RGB camera of this Turtlebot robot:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "TurtleBot ready to follow lines",
    "image": true,
    "name": "perception_unit2_linefollower1",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/perception_unit2_linefollower1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that this turtlebot is in a very strange environment. On the floor, there is a yellow path painted and some stars of different colours."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Map of lines to be followed",
    "image": true,
    "name": "perception_unit2_map",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit2_map.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you will have to do is make the robot move in this environment, following the yellow line. For that, we will divide the work into the following phases:\n",
    "\n",
    "* Get images from the ROS topic and convert them into OpenCV format\n",
    "* Process the images using OpenCV libraries to obtain the data we want for the task\n",
    "* Move the robot along the yellow line, based on the data obtained\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1· Get Images from a ROS topic and show them with OpenCV"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before doing anything, create a new package called **my_following_line_package**, with dependency in <i>rospy</i>. Also, create inside this package 2 new folder: one named **launch** and the other one **scripts**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step will be to get the images from a ROS topic, convert them into the OpenCV format, and visualize them in the Graphical Interface window.\n",
    "\n",
    "Here you have an example of how to do it:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\" id=\"line-basics\">**line_follower_basics.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import roslib\n",
    "import sys\n",
    "import rospy\n",
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import Image\n",
    "\n",
    "\n",
    "class LineFollower(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)\n",
    "\n",
    "    def camera_callback(self,data):\n",
    "        \n",
    "        try:\n",
    "            # We select bgr8 because its the OpneCV encoding by default\n",
    "            cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "        except CvBridgeError as e:\n",
    "            print(e)\n",
    "\n",
    "        cv2.imshow(\"Image window\", cv_image)\n",
    "        cv2.waitKey(1)\n",
    "\n",
    "\n",
    "def main():\n",
    "    line_follower_object = LineFollower()\n",
    "    rospy.init_node('line_following_node', anonymous=True)\n",
    "    try:\n",
    "        rospy.spin()\n",
    "    except KeyboardInterrupt:\n",
    "        print(\"Shutting down\")\n",
    "    cv2.destroyAllWindows()\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END line_follower_basics.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here there are several elements to comment on:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These imports are the basics necessary to work with images in ROS.<br>\n",
    "You have OpenCV2 library (cv2). Why the 2? Because there is already a version 3.<br>\n",
    "You have the numpy library, which makes the matrix and other operations easy to work with, and\n",
    "CV_Bridge, which allows ROS to work with OpenCV easily."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The subscriber to the image topic. This topic publishes information of the type **sensor_msgs/Image<b/>. Execute the following command to see what all the different variables are inside this message type:"
   ]
  },
  {
   "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": [
    "rosmsg show  sensor_msgs/Image"
   ]
  },
  {
   "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",
    "uint32 height                                          \n",
    "uint32 width                                           \n",
    "string encoding                                          \n",
    "uint8 is_bigendian                                           \n",
    "uint32 step                                          \n",
    "uint8[] data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can extract data from certain variables by doing as follows:"
   ]
  },
  {
   "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": [
    "rostopic echo -n1 /camera/rgb/image_raw/height"
   ]
  },
  {
   "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": [
    "rostopic echo -n1 /camera/rgb/image_raw/width "
   ]
  },
  {
   "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": [
    "rostopic echo -n1 /camera/rgb/image_raw/encoding"
   ]
  },
  {
   "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": [
    "rostopic echo -n1 /camera/rgb/image_raw/data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It should give you something like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "user ~ $ rostopic echo -n1 /camera/rgb/image_raw/height                                                                                                      \n",
    "480                                   \n",
    "---                                                                                                                                                          \n",
    "user ~ $ rostopic echo -n1 /camera/rgb/image_raw/width                                                                                                       \n",
    "640                                                                                                                                                          \n",
    "---                                                                                                                                                          \n",
    "user ~ $ rostopic echo -n1 /camera/rgb/image_raw/encoding                                                                                                    \n",
    "rgb8                                                                                                                                                         \n",
    "---  \n",
    "user ~ $ rostopic echo -n1 /camera/rgb/image_raw/data \n",
    "[129, 104, 104, 129, 104,\n",
    "...\n",
    "129, 104, 104, 129, 104]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The most important information here is:<br>\n",
    "\n",
    "* **height and width**: These are the dimensions in camera pixels. In this case, it's **480 by 640**.\n",
    "* **encoding**: How these pixels are encoded. This means what each value in the data array will mean. In this case, it's **rgb8**. This means that the values in data will be a color value represented as red/green/blue in 8-bit integers.\n",
    "* **data**: The Image data.\n",
    "\n",
    "If you want the full documentation of this class, please refer to: http://docs.ros.org/api/sensor_msgs/html/msg/Image.html"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Thanks to the cv_bridge package, we can easily convert the image data contained in an ImageSensorData into a format that OpenCV understands. By converting it into OpenCV, we can use all the power of that library to process the images of the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "try:\n",
    "    # We select bgr8 because its the OpenCV encoding by default\n",
    "    cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "except CvBridgeError as e:\n",
    "    print(e)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Retrieve the image from a ROS topic and store it in an OpenCV variable. The var <i>data</i> is the one that contains a ROS message with the image captured by the camera."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cv2.imshow(\"Image window\", cv_image)\n",
    "cv2.waitKey(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This will open a gui, where you can see the contents of the variable **cv_image**. This is essential to see the effects of the different filters and cropping of the image afterwards."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This command will close all the image windows when the program is terminated."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When executing this program, you will have to see the image by clicking on the **Graphical Interface** icon:<br>"
   ]
  },
  {
   "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\"><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This program will give you an image similar to this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Basic Image View",
    "image": true,
    "name": "perception_unit3_filter1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/line_follower_basics_result.png\" width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Create a new package named **my_following_line_package**. Inside this package, create an **scripts** folder, and place in there the <a href=\"#line-basics\">line_follower_basics.py</a> file.\n",
    "\n",
    "b) Launch the code using the command below, and test that it actually works."
   ]
  },
  {
   "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": [
    "rosrun my_following_line_package line_follower_basics.py"
   ]
  },
  {
   "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": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2· Apply Filters To the Image"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The raw image is useless unless you filter it to only see the color you want to track, and you crop the parts of the image you are not interested in. This is to make your program faster."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also need to extract some data from the images in a way that we can move the robot to follow the line."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### First step: Get Image Info and Crop the image"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before you start using the images for detecting things, you must take into account two things:\n",
    "\n",
    "* One of the most basic pieces of data that you need to work with images are the **dimensions**. Is it 800x600? 1200x1024? 60x60?<br>\n",
    "This is crucial to positioning the elements detected in the image.\n",
    "* And second is **cropping** the image. It's very important to work as soon as possible with the minimum size of the image required for the task. This makes the detecting system mush faster."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# We get image dimensions and crop the parts of the image we dont need\n",
    "# Bear in mind that because its image matrix first value is start and second value is down limit.\n",
    "# Select the limits so that they get the line not too close, not too far, and the minimum portion possible\n",
    "# To make the process faster.\n",
    "height, width, channels = cv_image.shape\n",
    "descentre = 160\n",
    "rows_to_watch = 20\n",
    "crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So why this values and not other values? Well, it depends on the task. In this case, you are interested in lines that aren't too far away from the robot, nor too near. If you concentrate on lines too far away, it won't follow the lines, but it will just go across the map. On the other hand, concentrating on lines that are too close won't give the robot time to adapt to changes in the line.\n",
    "\n",
    "It's also vital to optimize the region of the image as a result of cropping. If it's too big, too much data will be processed, making your program too slow. On the other hand, it has to have enough image to work with. At the end, you will have to adapt it to each situation.\n",
    "\n",
    "Do **Exercise U2.3** to test the effects of different values."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Second step: Convert from BGR to HSV"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that OpenCV works with BGR instead of RGB, for historical reasons (some cameras, in the days when OpenCV was created, worked with BGR).<br>\n",
    "Well, it seems that it is not very easy to work with either RGB or BGR, to differentiate colors. That's why HSV is used. The idea behind HSV is to remove the component of color saturation. This way, it's easier to recognise the same color in different light conditions, which is a serious issue in image recognition.\n",
    "More information: https://en.wikipedia.org/wiki/HSL_and_HSV"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "HSV cone",
    "image": true,
    "name": "HSV_color_solid_cone_chroma_gray",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/HSV_color_solid_cone_chroma_gray.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p>By <a href=\"//commons.wikimedia.org/wiki/File:Hcl-hcv_models.svg\" title=\"File:Hcl-hcv models.svg\">Hcl-hcv_models.svg</a>: <a href=\"//commons.wikimedia.org/wiki/User:Jacobolus\" title=\"User:Jacobolus\">Jacob Rus</a>\n",
    "<a href=\"//commons.wikimedia.org/wiki/File:HSV_color_solid_cone.png\" title=\"File:HSV color solid cone.png\">HSV_color_solid_cone.png</a>: <a href=\"//commons.wikimedia.org/wiki/User:SharkD\" title=\"User:SharkD\">SharkD</a>\n",
    "derivative work: <span style=\"border:1px solid #f57900;padding:1px;\"><a href=\"//commons.wikimedia.org/wiki/User:SharkD\" title=\"User:SharkD\"><span style=\"color:#8f5902;padding-left:1px;\">SharkD</span></a> <a href=\"//commons.wikimedia.org/wiki/User_talk:SharkD\" title=\"User talk:SharkD\"><span style=\"color:#fff;background:#fcaf3e;\">&nbsp;Talk&nbsp;</span></a></span> - <a href=\"//commons.wikimedia.org/wiki/File:Hcl-hcv_models.svg\" title=\"File:Hcl-hcv models.svg\">Hcl-hcv_models.svg</a>\n",
    "<a href=\"//commons.wikimedia.org/wiki/File:HSV_color_solid_cone.png\" title=\"File:HSV color solid cone.png\">HSV_color_solid_cone.png</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=9802544\">Link</a>**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Convert from RGB to HSV\n",
    "hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)\n",
    "\n",
    "# Define the Yellow Colour in HSV\n",
    "#RGB\n",
    "#[[[222,255,0]]]\n",
    "#BGR\n",
    "#[[[0,255,222]]]\n",
    "\"\"\"\n",
    "To know which color to track in HSV, Put in BGR. Use ColorZilla to get the color registered by the camera\n",
    ">>> yellow = np.uint8([[[B,G,R ]]])\n",
    ">>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)\n",
    ">>> print( hsv_yellow )\n",
    "[[[ 34 255 255]]\n",
    "\"\"\"\n",
    "lower_yellow = np.array([20,100,100])\n",
    "upper_yellow = np.array([50,255,255])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in this piece of code, you are converting the cropped_image (crop_img) into HSV.<br>\n",
    "Then, you select which color in HSV you want to track, selecting from the base of the color cone, a single point. Because HSV values are quite difficult to generate, it's better that you use a color picker tool like  <a href=\"https://chrome.google.com/webstore/detail/colorzilla/bhlhnicpbhignbdhedgjhgdocnmhomnp\" title=\"ColorZilla\">ColorZilla</a> to pick the RGB coding of the color to track. In this case, it's the yellow line in the simulation.<br>\n",
    "\n",
    "Once you have it, use the example code given in a Python terminal or create a tiny program that uses numpy as np and cv2 to convert it to HSV.<br>\n",
    "In the example given, the color of the line is HSV = [[[ 34 255 255]]."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, you have to select an upper and lower bound to define the region of the cone base that you will consider as Yellow. The bigger the region is, the more gradients of your picked colour will be accepted. This will depend on how your robot detects the colour variations in the image and how critical is mixing similar colours."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "HSV filter",
    "image": true,
    "name": "perception_unit3_filter2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_filter2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Third step: Apply the mask"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you need to generate a version of the cropped image in which you only see two colors: black and white. The white will be all the colors you consider yellow and the rest will be black. It's a binary image.\n",
    "\n",
    "Why do you need to do this? It basically has two functions:<br>\n",
    "\n",
    "* In doing this, you **don't have continuous detection**. It is the color or it's NOT, there is no in-between. This is vital for the centroid calculation that will be done after, because it only works on the principal of YES or NO.\n",
    "* Second, it will allow the **generation of the result image** afterwards, in which you extract everything on the image except the color line, seeing only what you are interested in seeing."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Threshold the HSV image to get only yellow colors\n",
    "mask = cv2.inRange(hsv, lower_yellow, upper_yellow)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Bitwise-AND mask and original image\n",
    "res = cv2.bitwise_and(crop_img,crop_img, mask= mask)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You, then, merge the cropped, colored image in HSV with the binary mask image, to color only the detections, leaving the rest in black."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Binary Mask Filter",
    "image": true,
    "name": "perception_unit3_filter3",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_filter3.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Fourth step: Get The Centroids, draw a circle where the centroid is and show all the images"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Centroids**, in essence, represent points in space where mass concentrates; the center of mass. Centroid and centers of mass are the same thing as far as this course is concerned. And they are calculated using Integrals.\n",
    "\n",
    "This is extrapolated into images. But, instead of having mass, we have color. The place where there is more of the color that you are looking for is where the centroid will be. It's the center of mass of the blobs seen in an image.\n",
    "\n",
    "That's why you applied the mask to make the image binary. This way, you can easily calculate where the center of mass is located. This is because it's a discrete function, not a continuous one. This means that it allows us to integrate in a discrete fashion, and not need a function describing the fluctuations in quantity of color throughout the region."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These centroids are vital in blob tracking because they give you a precise point in space where the blob is. You will use this to follow the blob and, therefore, follow the line."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is needed to calculate the centroids of the color blobs. You use the image moments for this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Calculate centroid of the blob of binary image using ImageMoments\n",
    "m = cv2.moments(mask, False)\n",
    "try:\n",
    "    cx, cy = m['m10']/m['m00'], m['m01']/m['m00']\n",
    "except ZeroDivisionError:\n",
    "    cy, cx = height/2, width/2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see here, you will obtain the coordinates of the cropped image where detection of the centroid of the positive yellow color occur. If nothing is detected, it will be positioned in the center of the image."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">Keep in mind that you have to assign the correct **Cy, Cx values**. Don't get the height and width mixed up, or you will have problems in the following exercises.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you want a more detailed explanation of an OpenCV exercise and all that can be obtained with contour features, you can have a look at the following link: http://docs.opencv.org/trunk/dd/d49/tutorial_py_contour_features.html\n",
    "\n",
    "If you are interested in all the mathematical justifications, check this other link: https://en.wikipedia.org/wiki/Image_moment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Draw the centroid in the resultut image\n",
    "# cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]]) \n",
    "cv2.circle(res,(int(cx), int(cy)), 10,(0,0,255),-1)\n",
    "\n",
    "cv2.imshow(\"Original\", cv_image)\n",
    "cv2.imshow(\"HSV\", hsv)\n",
    "cv2.imshow(\"MASK\", mask)\n",
    "cv2.imshow(\"RES\", res)\n",
    "\n",
    "cv2.waitKey(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "OpenCV allows you to draw a lot of things over the images, not only geometric shapes. But in this case, a circle will suffice."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cv2.circle(res,(centre_cicle_x, centre_cicle_y), LineWidth,(BGRColour of line),TypeOfLine)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are using this feature to draw a circle in the location of the calculated centroid."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Added centroid",
    "image": true,
    "name": "perception_unit3_filter4",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_filter4.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "More Info: http://docs.opencv.org/2.4/modules/core/doc/drawing_functions.html"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And then, finally, you show all of the image variables with their titles:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "All image filters and original image",
    "image": true,
    "name": "perception_unit2_linefollowfilters",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit2_linefollowfilters.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3· Move the TurtleBot based on the position of the Centroid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "error_x = cx - width / 2;\n",
    "twist_object = Twist();\n",
    "twist_object.linear.x = 0.2;\n",
    "twist_object.angular.z = -error_x / 100;\n",
    "rospy.loginfo(\"ANGULAR VALUE SENT===>\"+str(twist_object.angular.z))\n",
    "# Make it start turning\n",
    "self.movekobuki_object.move_robot(twist_object)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This control is passed on a Proportional control. This means that it will oscillate a lot and probably have an error. But, it is the simplest way of moving the robot and get the job done.<br>\n",
    "It always gives a constant linear movement and the angular Z velocity depends on the difference between the centroid center in X and the center of the image."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move the robot, you can use this module:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\" id=\"move-robot\">**move_robot.py**</p>"
   ]
  },
  {
   "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",
    "\n",
    "\n",
    "class MoveKobuki(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)\n",
    "        self.last_cmdvel_command = Twist()\n",
    "        self._cmdvel_pub_rate = rospy.Rate(10)\n",
    "        self.shutdown_detected = False\n",
    "\n",
    "    def move_robot(self, twist_object):\n",
    "        self.cmd_vel_pub.publish(twist_object)\n",
    "                                    \n",
    "    def clean_class(self):\n",
    "        # Stop Robot\n",
    "        twist_object = Twist()\n",
    "        twist_object.angular.z = 0.0\n",
    "        self.move_robot(twist_object)\n",
    "        self.shutdown_detected = True\n",
    "\n",
    "def main():\n",
    "    rospy.init_node('move_robot_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    movekobuki_object = MoveKobuki()\n",
    "    twist_object = Twist()\n",
    "    # Make it start turning\n",
    "    twist_object.angular.z = 0.5\n",
    "    \n",
    "    \n",
    "    rate = rospy.Rate(5)\n",
    "    \n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # works better than the rospy.is_shut_down()\n",
    "        movekobuki_object.clean_class()\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        movekobuki_object.move_robot(twist_object)\n",
    "        rate.sleep()\n",
    "\n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END move_robot.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how all of this code would be put together:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\" id=\"follow-line-hsv\">**follow_line_step_hsv.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import Image\n",
    "from move_robot import MoveKobuki\n",
    "\n",
    "class LineFollower(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)\n",
    "        self.movekobuki_object = MoveKobuki()\n",
    "\n",
    "    def camera_callback(self,data):\n",
    "        \n",
    "        try:\n",
    "            # We select bgr8 because its the OpneCV encoding by default\n",
    "            cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "        except CvBridgeError as e:\n",
    "            print(e)\n",
    "            \n",
    "        # We get image dimensions and crop the parts of the image we don't need\n",
    "        # Bear in mind that because the first value of the image matrix is start and second value is down limit.\n",
    "        # Select the limits so that it gets the line not too close and not too far, and the minimum portion possible\n",
    "        # To make process faster.\n",
    "        height, width, channels = cv_image.shape\n",
    "        descentre = 160\n",
    "        rows_to_watch = 20\n",
    "        crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]\n",
    "        \n",
    "        # Convert from RGB to HSV\n",
    "        hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)\n",
    "        \n",
    "        # Define the Yellow Colour in HSV\n",
    "        #RGB\n",
    "        #[[[222,255,0]]]\n",
    "        #BGR\n",
    "        #[[[0,255,222]]]\n",
    "        \"\"\"\n",
    "        To know which color to track in HSV, Put in BGR. Use ColorZilla to get the color registered by the camera\n",
    "        >>> yellow = np.uint8([[[B,G,R ]]])\n",
    "        >>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)\n",
    "        >>> print( hsv_yellow )\n",
    "        [[[ 34 255 255]]\n",
    "        \"\"\"\n",
    "        lower_yellow = np.array([20,100,100])\n",
    "        upper_yellow = np.array([50,255,255])\n",
    "\n",
    "        # Threshold the HSV image to get only yellow colors\n",
    "        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)\n",
    "        \n",
    "        # Calculate centroid of the blob of binary image using ImageMoments\n",
    "        m = cv2.moments(mask, False)\n",
    "        try:\n",
    "            cx, cy = m['m10']/m['m00'], m['m01']/m['m00']\n",
    "        except ZeroDivisionError:\n",
    "            cy, cx = height/2, width/2\n",
    "        \n",
    "        \n",
    "        # Bitwise-AND mask and original image\n",
    "        res = cv2.bitwise_and(crop_img,crop_img, mask= mask)\n",
    "        \n",
    "        # Draw the centroid in the resultut image\n",
    "        # cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]]) \n",
    "        cv2.circle(res,(int(cx), int(cy)), 10,(0,0,255),-1)\n",
    "\n",
    "        cv2.imshow(\"Original\", cv_image)\n",
    "        cv2.imshow(\"HSV\", hsv)\n",
    "        cv2.imshow(\"MASK\", mask)\n",
    "        cv2.imshow(\"RES\", res)\n",
    "        \n",
    "        cv2.waitKey(1)\n",
    "        \n",
    "        \n",
    "        error_x = cx - width / 2;\n",
    "        twist_object = Twist();\n",
    "        twist_object.linear.x = 0.2;\n",
    "        twist_object.angular.z = -error_x / 100;\n",
    "        rospy.loginfo(\"ANGULAR VALUE SENT===>\"+str(twist_object.angular.z))\n",
    "        # Make it start turning\n",
    "        self.movekobuki_object.move_robot(twist_object)\n",
    "        \n",
    "    def clean_up(self):\n",
    "        self.movekobuki_object.clean_class()\n",
    "        cv2.destroyAllWindows()\n",
    "        \n",
    "        \n",
    "\n",
    "def main():\n",
    "    rospy.init_node('line_following_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    line_follower_object = LineFollower()\n",
    "   \n",
    "    rate = rospy.Rate(5)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # works better than the rospy.is_shut_down()\n",
    "        line_follower_object.clean_up()\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        rate.sleep()\n",
    "\n",
    "    \n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END follow_line_step_hsv.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add to the **scripts** directory of your package the 2 Python scripts provided above: <a href=\"#move-robot\">move_robot.py</a> and <a href=\"#follow-line-hsv\">follow_line_step_hsv.py</a>. Execute it and see how it performs."
   ]
  },
  {
   "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": [
    "rosrun my_following_line_package follow_line_step_hsv.py"
   ]
  },
  {
   "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": [
    "Try some improvements:\n",
    "\n",
    "* Lower the speed of the robot to see if it works better. Change the linear and the angular speeds.\n",
    "* Change the behavior of the robot, maybe create a recovery behavior for when it loses the line."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try changing the color to be tracked. Try to track the three different star colors. Use the color picker for getting the exact RGB color for:\n",
    "\n",
    "* RedStar\n",
    "* GreenStar\n",
    "* BlueStar\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how it should look with the blue star:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Colour Blue tracking",
    "image": true,
    "name": "perception_unit2_bluestar",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit2_bluestar.png\" width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, try changing the values of the upper and lower bounds to the following options and see the results:\n",
    "\n",
    "* LOOSE color detection: lower_yellow = np.array([0,50,50]), upper_yellow = np.array([255,255,255])\n",
    "* STRICT color detection: lower_yellow = np.array([33,254,254]), upper_yellow = np.array([36,255,255])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of what you should see when changing the lower and upper:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "LOOSE color detection:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "perception_unit2_colourbounds_open2",
    "width": "10cm"
   },
   "source": [
    "<img id=\"fig-A.1\" src=\"img/perception_unit2_colourbounds_open2.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "STRICT color detection:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "perception_unit2_colourbounds_closed2",
    "width": "10cm"
   },
   "source": [
    "<img id=\"fig-A.2\" src=\"img/perception_unit2_colourbounds_closed2.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "See that in the loose version, all the colors that are green and yellow are detected. While in the strict, you can see that even in the yellow line, there is a part that is cut off because it's slightly different to the camera sensor."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try executing the code with different values in the **descentre** and the **rows_to_watch** parameters and see how the follow line program performs.\n",
    "\n",
    "Try the following values:<br>\n",
    "\n",
    "* descentre = 0, rows_to_watch = 20. In this case, you control the center of the image and a very small piece.\n",
    "* descentre = 0, rows_to_watch = 200. In this case, you are controlling nearly all of the lower part of the image.\n",
    "* descentre = 200, rows_to_watch = 20. In this case, you are controlling only a small fraction of the lower part of the image.\n",
    "\n",
    "Test these values and see which one is better."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see images like these:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Images 1:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "perception_unit2_mask0_20ab",
    "width": "15cm"
   },
   "source": [
    "<img id=\"fig-A.1\" src=\"img/perception_unit2_mask0_20ab.png\" width=\"1000\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Images 2:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "perception_unit2_mask0_200_ab",
    "width": "15cm"
   },
   "source": [
    "<img id=\"fig-A.1\" src=\"img/perception_unit2_mask0_200_ab.png\" width=\"1000\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Images 3:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "perception_unit2_mask200_20_ab",
    "width": "15cm"
   },
   "source": [
    "<img id=\"fig-A.1\" src=\"img/perception_unit2_mask200_20_ab.png\" width=\"1000\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From this first experience, you might have seen that there are two main problems:\n",
    "\n",
    "* When the robot follows a single line, it works, but when various paths are available it just goes crazy until only one is in sight. This is because you do not have the information for multiple blob detection to then apply a policy to select one direction.\n",
    "* When the centroid is detected in the far end of the image, the robot goes wild and turns too much, oscillating. This is due to the proportional control used. This could be fixed using a complete PID control.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will address these two issues now."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Additional Step: Follow Multiple Centroids"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, this is the same code as before ( <a href=\"#follow-line-hsv\">follow_line_step_hsv.py</a> ) except for the fact that it tracks multiple blobs, letting you choose the path to follow."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "contours, _, ___ = cv2.findContours(mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_TC89_L1)\n",
    "rospy.loginfo(\"Number of centroids==>\"+str(len(contours)))\n",
    "centres = []\n",
    "for i in range(len(contours)):\n",
    "    moments = cv2.moments(contours[i])\n",
    "    try:\n",
    "        centres.append((int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])))\n",
    "        cv2.circle(res, centres[-1], 10, (0, 255, 0), -1)\n",
    "    except ZeroDivisionError:\n",
    "        pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This code uses the **findContours()** functions to extract all of the contours, and then calculate the moments for each one.<br>\n",
    "If the **m00** is null, it considers the centers useless and doesn't paint them.<br>\n",
    "It, then, paints a green circle in each contour center. This gives the following result:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Multiple centroids",
    "image": true,
    "name": "perception_unit2_multiple2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit2_multiple2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have the full code for reference:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**follow_line_step_multiple.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import Image\n",
    "from move_robot import MoveKobuki\n",
    "\n",
    "class LineFollower(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)\n",
    "        self.movekobuki_object = MoveKobuki()\n",
    "\n",
    "    def camera_callback(self,data):\n",
    "        \n",
    "        try:\n",
    "            # We select bgr8 because its the OpneCV encoding by default\n",
    "            cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "        except CvBridgeError as e:\n",
    "            print(e)\n",
    "            \n",
    "        # We get image dimensions and crop the parts of the image we don't need\n",
    "        # Bear in mind that because the first value of the image matrix is start and second value is down limit.\n",
    "        # Select the limits so that it gets the line not too close and not too far, and the minimum portion possible\n",
    "        # To make the process faster.\n",
    "        height, width, channels = cv_image.shape\n",
    "        descentre = 160\n",
    "        rows_to_watch = 20\n",
    "        crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]\n",
    "        \n",
    "        # Convert from RGB to HSV\n",
    "        hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)\n",
    "        \n",
    "        # Define the Yellow Colour in HSV\n",
    "        #[[[ 30 196 235]]]\n",
    "        \"\"\"\n",
    "        To know which color to track in HSV, Put in BGR. Use ColorZilla to get the color registered by the camera\n",
    "        >>> yellow = np.uint8([[[B,G,R ]]])\n",
    "        >>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)\n",
    "        >>> print( hsv_yellow )\n",
    "        [[[ 60 255 255]]]\n",
    "        \"\"\"\n",
    "        lower_yellow = np.array([20,100,100])\n",
    "        upper_yellow = np.array([50,255,255])\n",
    "\n",
    "        # Threshold the HSV image to get only yellow colors\n",
    "        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)\n",
    "        \n",
    "        # Calculate centroid of the blob of binary image using ImageMoments\n",
    "        m = cv2.moments(mask, False)\n",
    "        try:\n",
    "            cx, cy = m['m10']/m['m00'], m['m01']/m['m00']\n",
    "        except ZeroDivisionError:\n",
    "            cy, cx = height/2, width/2\n",
    "        \n",
    "        # Bitwise-AND mask and original image\n",
    "        res = cv2.bitwise_and(crop_img,crop_img, mask= mask)\n",
    "        \n",
    "        contours, _, ___ = cv2.findContours(mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_TC89_L1)\n",
    "        rospy.loginfo(\"Number of centroids==>\"+str(len(contours)))\n",
    "        centres = []\n",
    "        for i in range(len(contours)):\n",
    "            moments = cv2.moments(contours[i])\n",
    "            try:\n",
    "                centres.append((int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])))\n",
    "                cv2.circle(res, centres[-1], 10, (0, 255, 0), -1)\n",
    "            except ZeroDivisionError:\n",
    "                pass\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(str(centres))\n",
    "        \n",
    "        \n",
    "        \n",
    "        # Draw the centroid in the resulting image\n",
    "        # cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]]) \n",
    "        cv2.circle(res,(int(cx), int(cy)), 10,(0,0,255),-1)\n",
    "        \n",
    "        \n",
    "\n",
    "        cv2.imshow(\"Original\", cv_image)\n",
    "        #cv2.imshow(\"HSV\", hsv)\n",
    "        #cv2.imshow(\"MASK\", mask)\n",
    "        cv2.imshow(\"RES\", res)\n",
    "        \n",
    "        cv2.waitKey(1)\n",
    "        \n",
    "        \n",
    "        error_x = cx - width / 2;\n",
    "        twist_object = Twist();\n",
    "        twist_object.linear.x = 0.2;\n",
    "        twist_object.angular.z = -error_x / 100;\n",
    "        rospy.loginfo(\"ANGULAR VALUE SENT===>\"+str(twist_object.angular.z))\n",
    "        # Make it start turning\n",
    "        self.movekobuki_object.move_robot(twist_object)\n",
    "        \n",
    "        \n",
    "    def clean_up(self):\n",
    "        self.movekobuki_object.clean_class()\n",
    "        cv2.destroyAllWindows()\n",
    "        \n",
    "        \n",
    "\n",
    "def main():\n",
    "    rospy.init_node('line_following_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    line_follower_object = LineFollower()\n",
    "\n",
    "    rate = rospy.Rate(5)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # works better than the rospy.is_shut_down()\n",
    "        line_follower_object.clean_up()\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        rate.sleep()\n",
    "\n",
    "    \n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END follow_line_step_multiple.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a new Python script with this multiple contours system and try the following:\n",
    "\n",
    "* When faced with a dilemma, make the robot select the right center to follow so that it stays in the circular loop path.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Solution for U2-5**</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": [
    "This is a way that it could be done:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import Image\n",
    "from move_robot import MoveKobuki\n",
    "\n",
    "class LineFollower(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)\n",
    "        self.movekobuki_object = MoveKobuki()\n",
    "\n",
    "    def camera_callback(self,data):\n",
    "        \n",
    "        try:\n",
    "            # We select bgr8 because its the OpneCV encoding by default\n",
    "            cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "        except CvBridgeError as e:\n",
    "            print(e)\n",
    "            \n",
    "        # We get image dimensions and crop the parts of the image we dont need\n",
    "        # Bear in mind that because the first value of the image matrix is start and second value is down limit.\n",
    "        # Select the limits so that it gets the line not too close and not too far, and the minimum portion possible\n",
    "        # To make the process faster.\n",
    "        height, width, channels = cv_image.shape\n",
    "        descentre = 160\n",
    "        rows_to_watch = 100\n",
    "        crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]\n",
    "        \n",
    "        # Convert from RGB to HSV\n",
    "        hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)\n",
    "        \n",
    "        \n",
    "        lower_yellow = np.array([20,100,100])\n",
    "        upper_yellow = np.array([50,255,255])\n",
    "\n",
    "        # Threshold the HSV image to get only yellow colors\n",
    "        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)\n",
    "        \n",
    "        # Bitwise-AND mask and original image\n",
    "        res = cv2.bitwise_and(crop_img,crop_img, mask= mask)\n",
    "        \n",
    "        contours, _, ___ = cv2.findContours(mask, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_TC89_L1)\n",
    "        rospy.loginfo(\"Number of centroids==>\"+str(len(contours)))\n",
    "        centres = []\n",
    "        for i in range(len(contours)):\n",
    "            moments = cv2.moments(contours[i])\n",
    "            try:\n",
    "                centres.append((int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])))\n",
    "                cv2.circle(res, centres[-1], 10, (0, 255, 0), -1)\n",
    "            except ZeroDivisionError:\n",
    "                pass\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(str(centres))\n",
    "        #Select the right centroid\n",
    "        # [(542, 39), (136, 46)], (x, y)\n",
    "        most_right_centroid_index = 0\n",
    "        index = 0\n",
    "        max_x_value = 0\n",
    "        for candidate in centres:\n",
    "            # Retrieve the cx value\n",
    "            cx = candidate[0]\n",
    "            # Get the Cx more to the right\n",
    "            if cx >= max_x_value:\n",
    "                max_x_value = cx\n",
    "                most_right_centroid_index = index\n",
    "            index += 1\n",
    "        \n",
    "        \n",
    "        try:\n",
    "            cx = centres[most_right_centroid_index][0]\n",
    "            cy = centres[most_right_centroid_index][1]\n",
    "            rospy.logwarn(\"Winner ==\"+str(cx)+\",\"+str(cy)+\"\")\n",
    "        except:\n",
    "            cy, cx = height/2, width/2\n",
    "        \n",
    "        # Draw the centroid in the resulting image\n",
    "        # cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]]) \n",
    "        cv2.circle(res,(int(cx), int(cy)), 5,(0,0,255),-1)\n",
    "        \n",
    "        \n",
    "\n",
    "        cv2.imshow(\"Original\", cv_image)\n",
    "        #cv2.imshow(\"HSV\", hsv)\n",
    "        #cv2.imshow(\"MASK\", mask)\n",
    "        cv2.imshow(\"RES\", res)\n",
    "        \n",
    "        cv2.waitKey(1)\n",
    "        \n",
    "        \n",
    "        error_x = cx - width / 2;\n",
    "        twist_object = Twist();\n",
    "        twist_object.linear.x = 0.2;\n",
    "        twist_object.angular.z = -error_x / 100;\n",
    "        rospy.loginfo(\"ANGULAR VALUE SENT===>\"+str(twist_object.angular.z))\n",
    "        # Make it start turning\n",
    "        self.movekobuki_object.move_robot(twist_object)\n",
    "        \n",
    "        \n",
    "    def clean_up(self):\n",
    "        self.movekobuki_object.clean_class()\n",
    "        cv2.destroyAllWindows()\n",
    "        \n",
    "        \n",
    "\n",
    "def main():\n",
    "    rospy.init_node('line_following_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    line_follower_object = LineFollower()\n",
    "\n",
    "    rate = rospy.Rate(5)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # works better than the rospy.is_shut_down()\n",
    "        line_follower_object.clean_up()\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        rate.sleep()\n",
    "\n",
    "    \n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, it's the exact same code, but it only uses multiple centroids and it selects the one that has the higher value of X, and therefore, always going to the right. You can also see that a red circle is drawn in the selected centroid, to be able to see it visually."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## PID controller with perception"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Controlling a robot means moving the robot based on the information provided by sensor data. Every time you work with perception for robots, you will have this coupling. So, you better start learning a little bit of control, so that you can make the most of your robot's perception.\n",
    "\n",
    "One way of controlling the movement of the robot a little better and making it smoother is to apply a PID controller to the control values. Luckily for us, there is a <a href=\"http://wiki.ros.org/pid\">PID ROS package</a> that makes using PIDs much easier."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, you have to get used to this new tool, so here is an example for testing how the PID package works."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**pid_test.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This first one launches all the needed elements for PID ROS to work and also your own PID test script **pid_control.py**.\n",
    "\n",
    "It launches the following:\n",
    "\n",
    "* **pkg=\"pid\" type=\"controller\"**. This starts the PID control system with the values given, PID values, and also some upper and lower limits, where the signal value can't exceed. It also sets the windup limit and a range in the loop frequency. This loop frequency depends on how precise you want the control to be and depends on how fast the system changes.\n",
    "* **pid_control.py**: The one that publishes the values desired, and the state of the system. It makes the real system or simulated system work.\n",
    "* **rqt_plot**: Will plot the values of the PID, so the **<i>/setpoint</i>** , the **<i>/state</i>**, and the **<i>/control_effort</i>** topics in the Graphical Tools space.\n",
    "* **rqt_reconfigure**: This will allow you to change the PID values in execution time to see the effects inmediately in the Graphical Interface window."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "<launch>\n",
    "  <node name=\"follow_line_pid\" pkg=\"pid\" type=\"controller\" >\n",
    "      <param name=\"Kp\" value=\"1.0\" />\n",
    "      <param name=\"Ki\" value=\"0.0\" />\n",
    "      <param name=\"Kd\" value=\"0.1\" />\n",
    "      <param name=\"upper_limit\" value=\"2.0\" />\n",
    "      <param name=\"lower_limit\" value=\"-2.0\" />\n",
    "      <param name=\"windup_limit\" value=\"2.0\" />\n",
    "      <param name=\"max_loop_frequency\" value=\"10.0\" />\n",
    "      <param name=\"min_loop_frequency\" value=\"5.0\" />\n",
    "    </node>\n",
    "    \n",
    "    <node name=\"pid_node\" pkg=\"my_following_line_package\" type=\"pid_control.py\" output=\"screen\"/>\n",
    "    \n",
    "    <node name=\"rqt_plot\" pkg=\"rqt_plot\" type=\"rqt_plot\"                                                                                              \n",
    "    args=\"/control_effort/data /state/data /setpoint/data\" />                                                                                          \n",
    "    <node name=\"rqt_reconfigure\" pkg=\"rqt_reconfigure\" type=\"rqt_reconfigure\" />\n",
    "    \n",
    "    \n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END pid_test.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**pid_control.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This Python script works in this way:\n",
    "\n",
    "* You set a desired value to reach, publishing in the **/setpoint** topic. In this case, it is \"0.0.\"\n",
    "* You also publish the \"real\" state of your system in the **/state** topic. In this case, it's a sine wave that is not affected by the control in reality. But, this will make the PID system work, and try and move the sine wave to the desired value 0.0.\n",
    "* You also subscribe to the **/control_effort**, to see what values the controller is issuing to try and make the state value move to the setpoint (0.0 in this case). It should be more or less an inverted sine wave to the state value."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from std_msgs.msg import Float64\n",
    "from random import randint\n",
    "from math import sin\n",
    "\n",
    "class PID(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self._setpoint_pub = rospy.Publisher(\"/setpoint\",Float64,queue_size=1)\n",
    "        self._state_pub = rospy.Publisher(\"/state\",Float64,queue_size=1)\n",
    "        self._control_effort_sub = rospy.Subscriber('/control_effort', Float64, self.control_effort_callback)\n",
    "        self._control_effort_value = Float64()\n",
    "\n",
    "    def control_effort_callback(self,data):\n",
    "        self._control_effort_value.data = data.data\n",
    "        \n",
    "        \n",
    "    def setpoint_update(self, value):\n",
    "        value_object = Float64()\n",
    "        value_object.data = value\n",
    "        self._setpoint_pub.publish(value_object)\n",
    "    \n",
    "    def state_update(self, value):\n",
    "        value_object = Float64()\n",
    "        value_object.data = value\n",
    "        self._state_pub.publish(value_object)\n",
    "    \n",
    "    def get_control_effort(self):\n",
    "        return self._control_effort_value.data\n",
    "        \n",
    "        \n",
    "\n",
    "def sinus_test():\n",
    "    rospy.init_node('sinus_pid_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    pid_object = PID()\n",
    "    \n",
    "    \n",
    "    rate = rospy.Rate(10.0)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    setPoint_value = 0.0\n",
    "    pid_object.setpoint_update(value=setPoint_value)\n",
    "    \n",
    "    i = 0\n",
    "    \n",
    "    \n",
    "    while not ctrl_c:\n",
    "        state_value = sin(i)\n",
    "        pid_object.state_update(value=state_value)\n",
    "        effort_value = pid_object.get_control_effort()\n",
    "        #print (\"state_value ==>\"+str(state_value))\n",
    "        #print (\"effort_value ==>\"+str(effort_value))\n",
    "        rate.sleep()\n",
    "        i += 0.1\n",
    "\n",
    "def step_test():\n",
    "    rospy.init_node('step_pid_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    pid_object = PID()\n",
    "    \n",
    "    \n",
    "    rate = rospy.Rate(10.0)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    setPoint_value = 0.0\n",
    "    pid_object.setpoint_update(value=setPoint_value)\n",
    "    \n",
    "    i = 0\n",
    "    state_value = 1.0\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        \n",
    "        pid_object.state_update(value=state_value)\n",
    "        effort_value = pid_object.get_control_effort()\n",
    "        #print (\"state_value ==>\"+str(state_value))\n",
    "        #print (\"effort_value ==>\"+str(effort_value))\n",
    "        rate.sleep()\n",
    "        i += 1\n",
    "        if i > 30:\n",
    "            state_value *= -1\n",
    "            i = 0\n",
    "    \n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    step_test()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see something similar to this in the Graphical Tools Tab:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "PID control Gui",
    "image": true,
    "name": "perception_unit3_pid",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_pid.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you are seeing is the **step_test**, where a step value of 1 and -1 is alternated. The red line is the **<i>/set_point</i>**. It is the value you set. And the blue line is the **effort**, the signal issued by the PID to counteract that signal. Because this is a dummy test, and there is no real physical system, the effort has no effect whatsoever. This is why the effort stays at an inverse value of the signal. In a real system, it would affect the state and, therefore, the effort would diminish until the \"set_point\" and the \"state\" had the same value.<br>\n",
    "Also, note that because you have set an upper_limit of 2 and a lower_limit of -2, the effort values can't exceed those values. This can be seen when the Kd is increased to 1.0."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you don't know what is going on here, please have a look at this practical <a href=\"https://www.flitetest.com/articles/p-i-and-sometimes-d-gains-in-a-nutshell\">guide to PID</a>:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**pid_control.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-6**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* Execute the code given and see the response, depending on the PID values given.\n",
    "* See how when increasing the **Kd**, the values go faster, but have higher peak values.\n",
    "* See how setting the **upper_limits** and **lower_limits** also affects the behavior of the effort value.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-6**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U2-7**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's apply this knowledge to the robot system.<br>\n",
    "\n",
    "\n",
    "* Create a launch file for your new follow_line Python script that uses the PID.\n",
    "* This launch will not only launch your follow_line script, but also the PID package for the control.\n",
    "* If you want, you can also plot the values with rqt_plot and start the reconfigure for tweeking the PID values.\n",
    "\n",
    "\n",
    "Objective:<br>\n",
    "\n",
    "* Make the robot follow the lines smoothly, with no big oscillations, but with a fast response. It has to be precise in its movements."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U2-7**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Solution Exercise U2.7**</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": [
    "This is a way that it could be done:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The launch file that starts the PID system is **pid_movement.launch**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**pid_movement.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "<launch>\n",
    "    <!-- Limits are based on the camera width of 640 -->\n",
    "  <node name=\"follow_line_pid\" pkg=\"pid\" type=\"controller\" >\n",
    "      <param name=\"Kp\" value=\"0.8\" />\n",
    "      <param name=\"Ki\" value=\"0.0\" />\n",
    "      <param name=\"Kd\" value=\"0.05\" />\n",
    "      <param name=\"upper_limit\" value=\"640\" />\n",
    "      <param name=\"lower_limit\" value=\"-640\" />\n",
    "      <param name=\"windup_limit\" value=\"640\" />\n",
    "      <param name=\"max_loop_frequency\" value=\"10.0\" />\n",
    "      <param name=\"min_loop_frequency\" value=\"10.0\" />\n",
    "    </node>\n",
    "    \n",
    "\n",
    "    <node name=\"rqt_plot\" pkg=\"rqt_plot\" type=\"rqt_plot\"                                                                                              \n",
    "    args=\"/control_effort/data /state/data /setpoint/data\" />                                                                                         \n",
    "\n",
    "    <node name=\"rqt_reconfigure\" pkg=\"rqt_reconfigure\" type=\"rqt_reconfigure\" />\n",
    "    \n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And this is the main launch:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**follow_line_with_pid.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "<launch>\n",
    "    <include file=\"$(find my_following_line_package)/launch/pid_movement.launch\"/>\n",
    "    <node name=\"line_following_node\" pkg=\"my_following_line_package\" type=\"follow_line_step_pid.py\" output=\"screen\"/>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And this is a way of moving the robot with PID. In this case, no multiple centroids are found:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**follow_line_step_pid.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "import cv2\n",
    "import numpy as np\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "from geometry_msgs.msg import Twist\n",
    "from sensor_msgs.msg import Image\n",
    "from move_robot import MoveKobuki\n",
    "from pid_control import PID\n",
    "\n",
    "class LineFollower(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        rospy.logwarn(\"Init line Follower\")\n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/rgb/image_raw\",Image,self.camera_callback)\n",
    "        self.movekobuki_object = MoveKobuki()\n",
    "        self.pid_object = PID()\n",
    "        \n",
    "\n",
    "    def camera_callback(self,data):\n",
    "        \n",
    "        try:\n",
    "            # We select bgr8 because its the OpneCV encoding by default\n",
    "            cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")\n",
    "        except CvBridgeError as e:\n",
    "            print(e)\n",
    "            \n",
    "        # We get image dimensions and crop the parts of the image we don't need\n",
    "        # Bear in mind that because the first value of the image matrix is start and second value is down limit.\n",
    "        # Select the limits so that it gets the line not too close and not too far, and the minimum portion possible\n",
    "        # To make the process faster.\n",
    "        height, width, channels = cv_image.shape\n",
    "        descentre = 160\n",
    "        rows_to_watch = 20\n",
    "        crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]\n",
    "        \n",
    "        # Convert from RGB to HSV\n",
    "        hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)\n",
    "        \n",
    "        # Define the Yellow Color in HSV\n",
    "        #[[[ 30 196 235]]]\n",
    "        \"\"\"\n",
    "        To know which color to track in HSV, Put in BGR. Use ColorZilla to get the color registered by the camera\n",
    "        >>> yellow = np.uint8([[[B,G,R ]]])\n",
    "        >>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)\n",
    "        >>> print( hsv_yellow )\n",
    "        [[[ 60 255 255]]]\n",
    "        \"\"\"\n",
    "        lower_yellow = np.array([20,100,100])\n",
    "        upper_yellow = np.array([50,255,255])\n",
    "\n",
    "        # Threshold the HSV image to get only yellow colors\n",
    "        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)\n",
    "        \n",
    "        # Calculate centroid of the blob of binary image using ImageMoments\n",
    "        m = cv2.moments(mask, False)\n",
    "        try:\n",
    "            cx, cy = m['m10']/m['m00'], m['m01']/m['m00']\n",
    "        except ZeroDivisionError:\n",
    "            cy, cx = height/2, width/2\n",
    "        \n",
    "        \n",
    "        # Bitwise-AND mask and original image\n",
    "        res = cv2.bitwise_and(crop_img,crop_img, mask= mask)\n",
    "        \n",
    "        # Draw the centroid in the resulting image\n",
    "        cv2.circle(res,(int(cx), int(cy)), 10,(0,0,255),-1)\n",
    "\n",
    "        cv2.imshow(\"RES\", res)\n",
    "        \n",
    "        cv2.waitKey(1)\n",
    "        \n",
    "        # Move the Robot, center it in the middle of the width 640 => 320:\n",
    "        setPoint_value = width/2\n",
    "        self.pid_object.setpoint_update(value=setPoint_value)\n",
    "        \n",
    "        \n",
    "        twist_object = Twist();\n",
    "        twist_object.linear.x = 0.2;\n",
    "        \n",
    "        # Make it start turning\n",
    "        \n",
    "        self.pid_object.state_update(value=cx)\n",
    "        effort_value = self.pid_object.get_control_effort()\n",
    "        # We divide the effort to map it to the normal values for angular speed in the turtlebot\n",
    "        rospy.logwarn(\"Set Value==\"+str(setPoint_value))\n",
    "        rospy.logwarn(\"State Value==\"+str(cx))\n",
    "        rospy.logwarn(\"Effort Value==\"+str(effort_value))\n",
    "        angular_effort_value = effort_value / 200.0\n",
    "        \n",
    "        twist_object.angular.z = angular_effort_value;\n",
    "        rospy.logwarn(\"TWist ==\"+str(twist_object.angular.z))\n",
    "        self.movekobuki_object.move_robot(twist_object)\n",
    "        \n",
    "    def clean_up(self):\n",
    "        self.movekobuki_object.clean_class()\n",
    "        cv2.destroyAllWindows()\n",
    "        \n",
    "        \n",
    "\n",
    "def main():\n",
    "    rospy.init_node('line_following_node', anonymous=True)\n",
    "    \n",
    "    \n",
    "    line_follower_object = LineFollower()\n",
    "    \n",
    "    rate = rospy.Rate(5)\n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # works better than the rospy.is_shut_down()\n",
    "        line_follower_object.clean_up()\n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        ctrl_c = True\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        rate.sleep()\n",
    "\n",
    "    \n",
    "    \n",
    "if __name__ == '__main__':\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the only main difference from what you have learned:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Move the Robot:\n",
    "setPoint_value = width/2\n",
    "self.pid_object.setpoint_update(value=setPoint_value)\n",
    "\n",
    "error_x = cx - width / 2;\n",
    "twist_object = Twist();\n",
    "twist_object.linear.x = 0.2;\n",
    "\n",
    "# Make it start turning\n",
    "\n",
    "self.pid_object.state_update(value=cx)\n",
    "effort_value = self.pid_object.get_control_effort()\n",
    "# We divide the effort to map it to the normal values for angular speed in the turtlebot\n",
    "angular_effort_value = effort_value / 100.0\n",
    "\n",
    "twist_object.angular.z = angular_effort_value;\n",
    "self.movekobuki_object.move_robot(twist_object)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In essence, you are setting the **setPoint**, which is always to try to have the center of the line in the middle of the image.\n",
    "\n",
    "Then the state, which in this case is the centroid of the blob, is published.<br>\n",
    "And finally, the effort given by the control PID running is retrieved, converted to a sensible value, and then the Twist is published."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise EXTRA U2-8**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a definitive script that can follow the correct path (loop path) with the PID control and the multiple centroids' information."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END EXTRA Exercise U2-8**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise EXTRA U2-9**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a new script that, when published on a topic called \"objective\", will do the following:<br>\n",
    "\n",
    "* If yellow, it will follow the loop path.\n",
    "* If red, it will follow the loop path until it reaches the red star path.\n",
    "* If green, freen, will follow the loop path until it reaches the green star path.\n",
    "* If blue, it will follow the loop path until it reaches the blue star path.\n",
    "* If none, it will stop.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END EXTRA Exercise U2-9**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations! You now have a good basic knowledge of OpenCV in ROS. Now, it's just practice. In the next unit, you will learn how to do face detection in ROS."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#417FB1;color:white;\">**Project**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now do the second exercise of the Aibo Project. There, you will have to make the Aibo Robot follow a white line on the floor until it reaches a green star, which represents the wireless charging zone."
   ]
  },
  {
   "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": "2 - Vision Basics in ROS Part 2, Follow a line",
   "chapter_title": "Unit 2: Vision Basics in ROS Part 2, Follow a line",
   "course_title": "ROS PERCEPTION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}