Course_ShadowHandInstructionManual_Unit5.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Mastering with ROS: Shadow Hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 5: Perception and Object Recognition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time of completion: <b>1h</b><br><br>\n",
    "This Unit will show you how to use Perception and Object Recognition to get the position of graspable objects."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous Chapter, you learned how to perform grasping tasks using the **Smart Grasping System**. But you need to know that this system gets the position of the objects to grasp directly from the simulation data. So, as you may imagine, this is not accurate with the real environments, where there isn't any simulation data. In real scenarios (not simulated), robots can only get data from the environment they are in from their sensors. For instance, a Kinect camera."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in this Unit, we are going to see how we can get information about the position of the objects we want to grasp not from the simulation data, but from the sensors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One of the most usefull perception skills is being able to recognise objects. This allows you to create robots that can grasp objects and understand the world around them a little bit better.\n",
    "<br><br>\n",
    "There are two main skills to master here:<br>\n",
    "\n",
    "* **Recognise flat surfaces**: This allows the robot to detect places where objects usually are. For instance, tables or shelves. It's the first step to take when searching for objects.\n",
    "\n",
    "* **Recognise objects**: Once you know where to look, you have to be able to recognise different object in the scene and localise where they are placed in the environment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this Unit, we have made a couple of modifications in the simulation, in order to make easier the task of recognising objects in the environment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* The ball to grasp has now a black and white texture, which makes it easier to detect by the camera.\n",
    "* The camera is closer to the talbe, in order to be able to get a better view of the scenario."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch robot in object recognition world",
    "image": true,
    "name": "perception_unit3_demo",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/ball_with_texture.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/closer_camera.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So... with the proper introductions made, let's start working!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Table Top Detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step to take in order to be able to recognise objects, is knowing where these objects are more likely to be. For this, we are going to use a part of the <a href=\"http://wg-perception.github.io/object_recognition_core/\">tabletop_object_detector</a> package. With this package, we will be able to detect flat surfaces and represent it in RVIZ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to see how you can use this package, just follow the next exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 5.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) The first step is to create your own object recognition package:"
   ]
  },
  {
   "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": [
    "roscd;cd ../src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_create_pkg my_object_recognition_pkg rospy object_recognition_core"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Inside this package, create a **launch** folder containing a launch file named **init_table_top.launch**. Copy the following code inside this file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    \n",
    "    <arg name=\"tabletop_ork_file\" value=\"$(find my_object_recognition_pkg)/conf/detection.tabletop_shadow.ros.ork\"/>\n",
    "    \n",
    "    <node pkg=\"object_recognition_core\"\n",
    "    type=\"detection\"\n",
    "    name=\"tabletop_server_node\"\n",
    "    args=\"-c $(arg tabletop_ork_file)\"\n",
    "    output=\"screen\">\n",
    "    </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, as you can see, you are launching a binary file called **detection** with a configuration file as the argument. This configuration file, called **detection.tabletop_fetch.ros.ork**, is where all the input sensors and values for the table detection will be set. It's basically like a YAML file, but with a different extension (**.ork**)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) So, the next step will be to create a directory named **conf** inside your package. Then, create a file named **detection.tabletop_shadow.ros.ork**. Inside this file, copy the following code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "source1:\n",
    "  type: RosKinect\n",
    "  module: 'object_recognition_ros.io'\n",
    "  parameters:\n",
    "    rgb_frame_id: '/camera_depth_optical_frame'\n",
    "    rgb_image_topic: '/camera/rgb/image_raw'\n",
    "    rgb_camera_info: '/camera/rgb/camera_info'\n",
    "    depth_image_topic: '/camera/depth/image_raw'\n",
    "    depth_camera_info: '/camera/depth/camera_info'\n",
    "    #\n",
    "    #crop_enabled: True\n",
    "    #x_min: -0.4\n",
    "    #x_max: 0.4\n",
    "    #y_min: -1.0\n",
    "    #y_max: 0.2\n",
    "    #z_min: 0.3\n",
    "    #z_max: 1.5\n",
    "\n",
    "sink1:\n",
    "  type: TablePublisher\n",
    "  module: 'object_recognition_tabletop'\n",
    "  inputs: [source1]\n",
    "\n",
    "pipeline1:\n",
    "  type: TabletopTableDetector\n",
    "  module: 'object_recognition_tabletop'\n",
    "  inputs: [source1]\n",
    "  outputs: [sink1]\n",
    "  parameters:\n",
    "    table_detector:\n",
    "      min_table_size: 4000\n",
    "      plane_threshold: 0.01\n",
    "    #clusterer:\n",
    "    #  table_z_filter_max: 0.35\n",
    "    #  table_z_filter_min: 0.025\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Of all the parameters you see here, the only ones that are really relevant most of the times are this ones:\n",
    "<br><br>\n",
    "**rgb_frame_id**: '/camera_depth_optical_frame'<br>\n",
    "**rgb_image_topic**: '/camera/rgb/image_raw'<br>\n",
    "**rgb_camera_info**: '/camera/rgb/camera_info'<br>\n",
    "**depth_image_topic**: '/camera/depth/image_raw'<br>\n",
    "**depth_camera_info**: '/head_camera/depth/camera_info'<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This will set the correct image topics as inputs so that the recognition can be made."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) With everything set up, just execute the launch file."
   ]
  },
  {
   "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_object_recognition_pkg init_table_top.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) Finally, open RVIZ and add all the elements you want to visualize (like the Camera or the PointCloud2 elements). In order to visualise the Tabletop detection, you will have to add the **OrkTable** element. Then, you have to set the topic where the table data is being published, in this case, **/table_array**. You can then check certain options, like **bounding_box**, in order to have a bounding box around the tabletop detection, or the **top** option in order to visualize what is being considered as the top of the surface."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the end, you should have something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "TableTop RVIZ element details",
    "image": true,
    "name": "perception_unit3_tabletop3",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/tabletop.png\" width=\"700\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 5.1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Let's get some pictures!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next step will be to take some pictures of the object we want to grasp, in order to detect some key points that define the object. With this key points, we will be able to detect the object later, by comparing the pictures taken with the object being detected by the camera."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this purpose we will use the **find_object_2d** package. So, in order to see how to do this, just follow the next exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 5.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Inside your object recognition package, create a new launch file called **start_find_object_2d.launch**. Copy the following code inside it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "\n",
    "<launch> \n",
    "    <arg name=\"camera_rgb_topic\" default=\"/camera/rgb/image_raw\" />\n",
    "    <node name=\"find_object_2d_node\" pkg=\"find_object_2d\" type=\"find_object_2d\" output=\"screen\">\n",
    "        <remap from=\"image\" to=\"$(arg camera_rgb_topic)\"/>\n",
    "    </node>\n",
    "    \n",
    "</launch> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, you just need to set the RGB camera image source and the system will be ready to go. In this case, it's **/camera/rgb/image_raw**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Launch this file and go to The Graphical Tools tab. You should see something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "CokeCan in ObjectRecognition Gui",
    "image": true,
    "name": "perception_unit3_objectrec2dinit",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/photos1.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After a few seconds, you will be able to see the scene."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/photos2.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, it's time to get some pics of the object we want to grasp! In order to do this, select the **Edit -> Add object from scene** option."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/photos3.png\" width=\"400\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also add previously taken images directly, but bear in mind that there are some peculiarities. The images appear in this object recogniser mirrored, if you compare them with the images from the cameras. So you should be careful with that."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "d) In the **Add Object** screen, you just have to follow the steps in order to select the section of the image that you consider to be the object."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Click on the **Take picture** button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Add Object",
    "image": true,
    "name": "perception_unit3_addobject1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/photos4.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Select the desired section of the image. In this case, it's the ball. Try to make the selection a little bit bigger than the ball itself."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Add Object result",
    "image": true,
    "name": "perception_unit3_addobject2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/photos5.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, click the **End** button."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/photos6.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! Once done, you should be detecting the object in the table. This system compares the images received by the camera with the saved ones, and looks for matches. If it matches in enough points, it considers it the desired object."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/photos7.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "e) So, the last step will be to save all of the objects added. There are 2 main ways of doing this:<br>\n",
    "\n",
    "* Saving the objects as images: **File -> Save Objects**. This will save all of the images taken in a folder\n",
    "* Saving the whole session: **File -> Save Session**. This will save a binary with all of the images and settings. This is the most compact way of doing it, although you won't have access to the images of the objects. It depends on your needs\n",
    "\n",
    "For now, let's just save the whole session. Inside your package, create a new folder named **saved_pictures**, and save the session inside this folder. You can name it **ball_session**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 5.2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, once you have your session stored, you need to be able to always start an object recognition session with all of that stored data. In order to do so, just follow the next exercise!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise 5.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Create a new launch file inside your package named **start_find_object_3d_session.launch**, and copy the following content into it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\t\t\n",
    "\t<node name=\"find_object_3d\" pkg=\"find_object_2d\" type=\"find_object_2d\" output=\"screen\">\n",
    "\t\t<param name=\"gui\" value=\"true\" type=\"bool\"/>\n",
    "\t\t<param name=\"settings_path\" value=\"~/.ros/find_object_2d.ini\" type=\"str\"/>\n",
    "\t\t<param name=\"subscribe_depth\" value=\"true\" type=\"bool\"/>\n",
    "\t\t<param name=\"session_path\" value=\"$(find my_object_recognition_pkg)/saved_pictures/ball_session.bin\" type=\"str\"/>\n",
    "\t\t<param name=\"objects_path\" value=\"\" type=\"str\"/>\n",
    "\t\t<param name=\"object_prefix\" value=\"object\" type=\"str\"/>\n",
    "\t\t\n",
    "\t\t<remap from=\"rgb/image_rect_color\" to=\"/camera/rgb/image_raw\"/>\n",
    "\t\t<remap from=\"depth_registered/image_raw\" to=\"/camera/depth/image_raw\"/>\n",
    "\t\t<remap from=\"depth_registered/camera_info\" to=\"/camera/depth/camera_info\"/>\n",
    "\t</node>\n",
    "\t\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Launch the file. You should then be able to get the TF of the detected object published. If you have multiple images of the same object, you will get multiple frames of objects. It's up to you to filter them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/object_detected1.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/object_detected2.png\" width=\"500\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) You can also see the object detected by executing the following command in another terminal while the prior launch is working:"
   ]
  },
  {
   "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": [
    "rosrun find_object_2d print_objects_detected"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<img src=\"img/object_dected_cmd.png\" width=\"1000\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">End of Exercise 5.3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Awesome! So now we are able to get the TF of the object we want to grasp. But... how can we get the position of that object? Which, in fact, is the important data we really want to know if we want to grasp it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well, as you have seen in the previous exercise, we now have the TF from the detected object to the **camera_depth_optical_frame** being published. And, obviously, we also have the TF from this camera frame to the **world** frame, which represents the center of the environment. So... with this TF data being published, we can already know the position of that object related to the world frame!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in order to get the position of the object, you just need to check the value of its TF regarding the world frame. You can check that by using the following command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun tf tf_echo world <object_frame>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, if the frame of your object is named, like in this notebook, **object_8**, the command would be:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun tf tf_echo world object_8"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After a few seconds, you will get something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/object_tf.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Awesome! So now, you are able to get the position of the object to grasp by using Object Recognition! Now, you can go to the Next Unit (Project) and use this data to actually Grasp the object."
   ]
  }
 ],
 "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": "3 - Flat Surface and Object Recognition",
   "chapter_title": "Unit 3: Flat Surface and Object Recognition",
   "course_title": "ROS PERCEPTION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}