Course_PerceptionROS_Unit_3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 3: Flat Surface and Object Recognition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One of the most useful perception skills is being able to recognize objects. This allows you to create robots that can, for instance, grasp objects or understand the world around them a little bit better.<br>\n",
    "There are two main skills to master here:<br>\n",
    "\n",
    "* **Recognize flat surfaces**: This skill allows it to detect places where objects normally are, like tables and shelves.It's the first step to searching for objects.\n",
    "\n",
    "\n",
    "* **Recognize objects**: Once you know where to look, you have to be able to recognise different object in the scene and localise where they are from your robot's location.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will work on this in a world with a table and a coke can on top of it. The robot you will use is the Fetch robot, which you can move around by using the topic **/base_controller/command**, which uses Twist commands. You can also use other systems for moving the torso up and down and the head to look around. But in this object recognition Unit, only the movement of the base is really necessary."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Fetch robot in object recognition world",
    "image": true,
    "name": "perception_unit3_demo",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_demo.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Table Top Detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step in recognising objects is knowing where these objects are. For this, you are going to use a port of the <a href=\"http://wg-perception.github.io/object_recognition_core/\">tabletop_object_detector</a> package to be able to detect flat surfaces and represent that detections in RViz."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, as usual, 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\n",
    "catkin_create_pkg my_object_recognition_pkg rospy object_recognition_core"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will have to add dependencies on each new package you need inside.<br>\n",
    "So you are going to use this launch file:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**init_table_top.launch**</p>"
   ]
  },
  {
   "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_fetch.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": [
    "<p style=\"background:green;color:white;\">**END init_table_top.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, as you can see, you are launching a binary called **detection** with a configuration file as the argument. This file, called **detection.tabletop_fetch.ros.ork,** is where all the input sensors and values for the table detection are set. It's a YAML file with a different extension, .ork. So the first thing is to create a <i>conf</i> directory inside your package. Then, create this file:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**detection.tabletop_fetch.ros.ork**</p>"
   ]
  },
  {
   "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: '/head_camera_rgb_optical_frame'\n",
    "    rgb_image_topic: '/head_camera/rgb/image_raw'\n",
    "    rgb_camera_info: '/head_camera/rgb/camera_info'\n",
    "    depth_image_topic: '/head_camera/depth_registered/image_raw'\n",
    "    depth_camera_info: '/head_camera/depth_registered/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": [
    "<p style=\"background:green;color:white;\">**detection.tabletop_fetch.ros.ork**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Of all the parameters you have here, the only ones that are relevant most of the time are the following ones:\n",
    "\n",
    "rgb_frame_id: '/head_camera_rgb_optical_frame'<br>\n",
    "rgb_image_topic: '/head_camera/rgb/image_raw'<br>\n",
    "rgb_camera_info: '/head_camera/rgb/camera_info'<br>\n",
    "depth_image_topic: '/head_camera/depth_registered/image_raw'<br>\n",
    "depth_camera_info: '/head_camera/depth_registered/camera_info'<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This sets the correct image topics as inputs so that the recognition can be made. Once you have it, 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": [
    "The output you should get:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[ INFO] [1500548343.897178230]: Initialized ROS. node_name: /tabletop_server_node                                                                                \n",
    "[ INFO] [1500548344.396954355, 244.844000000]: System already initialized. node_name: /tabletop_server_node                                                      \n",
    "[ INFO] [1500548344.407887012, 244.844000000]: Subscribed to topic:/head_camera/rgb/image_raw [queue_size: 1][tcp_nodelay: 0]                                    \n",
    "[ INFO] [1500548344.413947574, 244.845000000]: Subscribed to topic:/head_camera/depth_registered/image_raw [queue_size: 1][tcp_nodelay: 0]                       \n",
    "[ INFO] [1500548344.420596160, 244.845000000]: Subscribed to topic:/head_camera/rgb/camera_info [queue_size: 1][tcp_nodelay: 0]                                  \n",
    "[ INFO] [1500548344.428558453, 244.846000000]: Subscribed to topic:/head_camera/depth_registered/camera_info [queue_size: 1][tcp_nodelay: 0]                     \n",
    "126 :: 0.5 , 1 , 0.995004 , /base_link , /head_mount_kinect_rgb_optical_frame                                                                                    \n",
    "[ INFO] [1500548345.434429706, 244.987000000]: publishing to topic:/tabletop/clusters                                                                            \n",
    "[ INFO] [1500548345.436181607, 244.987000000]: publishing to topic:/table_array  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And now, open the RVIZ and add all of the elements you want to see (like the Camera element or PointCloud2 elements). To visualize the Table detection, you will have to add **OrkTable** element. You select the topic where the table data is published, in this case, **/table_array**. You can then check certain options, like \"bounding_box,\" to have a bounding box around the detection, or the \"top\" option to see what is being considered as the top of the surface."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Tabletop 1:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "TableTop detection in RVIZ",
    "image": true,
    "name": "perception_unit3_tabletop1",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_tabletop1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Tabletop 2:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "TableTop detection in RVIZ CloseUp",
    "image": true,
    "name": "perception_unit3_tabletop2",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_tabletop2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "OrkTable configuration:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "TableTop RVIZ element details",
    "image": true,
    "name": "perception_unit3_tabletop3",
    "width": "7cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_tabletop3.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's have a look at all of the data extracted from the **/table_array** topic:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "user ~ $ rostopic info /table_array                                   \n",
    "Type: object_recognition_msgs/TableArray\n",
    "Publishers:\n",
    " * /tabletop_server_node (http://ip-172-31-35-50:55275/)\n",
    "Subscribers:                                    \n",
    " * /rviz_1500548389692878208 (http://ip-172-31-35-50:47121/)\n",
    "user ~ $ rosmsg show object_recognition_msgs/TableArray                                   \n",
    "std_msgs/Header header                                    \n",
    "  uint32 seq                                    \n",
    "  time stamp                                    \n",
    "  string frame_id                                   \n",
    "object_recognition_msgs/Table[] tables                                    \n",
    "  std_msgs/Header header                                    \n",
    "    uint32 seq                                    \n",
    "    time stamp                                    \n",
    "    string frame_id                                   \n",
    "  geometry_msgs/Pose pose                                   \n",
    "    geometry_msgs/Point position                                    \n",
    "      float64 x                                   \n",
    "      float64 y                                   \n",
    "      float64 z                                   \n",
    "    geometry_msgs/Quaternion orientation                                    \n",
    "      float64 x                                   \n",
    "      float64 y                                   \n",
    "      float64 z                                   \n",
    "      float64 w                                   \n",
    "  geometry_msgs/Point[] convex_hull                                   \n",
    "    float64 x                                   \n",
    "    float64 y                                   \n",
    "    float64 z"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, you can pinpoint the location of any surface detected, or even filter the floor, because you know the height of each surface."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "More info:<br>\n",
    "http://wg-perception.github.io/ork_tutorials/\n",
    "http://wg-perception.github.io/ork_tutorials/tutorial02/tutorial.html<br>\n",
    "http://wiki.ros.org/tabletop_object_detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U3-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a script that makes the Fetch robot approach the table based on the data from the /table_array. This will allow you to look for a table and reach it after searching for objects."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**Help Exercise U3-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have the code to make the Fetch robot move and look around. Use it as you see fit:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import numpy\n",
    "import copy\n",
    "import actionlib\n",
    "import rospy\n",
    "import time\n",
    "from math import sin, cos\n",
    "from moveit_python import (MoveGroupInterface,\n",
    "                           PlanningSceneInterface,\n",
    "                           PickPlaceInterface)\n",
    "from moveit_python.geometry import rotate_pose_msg_by_euler_angles\n",
    "\n",
    "from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal\n",
    "from control_msgs.msg import PointHeadAction, PointHeadGoal\n",
    "from geometry_msgs.msg import Twist, Pose\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal\n",
    "from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes\n",
    "from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint\n",
    "from nav_msgs.msg import Odometry\n",
    "\n",
    "# Move base using navigation stack\n",
    "class MoveBasePublisher(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self._move_base_publisher = rospy.Publisher('/base_controller/command', Twist, queue_size=1)\n",
    "        self.odom_subs = rospy.Subscriber(\"/odom\", Odometry, self.odom_callback)\n",
    "        self.actual_pose = Pose()\n",
    "    \n",
    "    def odom_callback(self,msg):\n",
    "        self.actual_pose = msg.pose.pose\n",
    "\n",
    "    def move_to(self, twist_object):\n",
    "        self._move_base_publisher.publish(twist_object)\n",
    "    \n",
    "    def stop(self):\n",
    "        twist_object = Twist()\n",
    "        self.move_to(twist_object)\n",
    "        \n",
    "    def move_forwards_backwards(self,speed_ms,position_x, allowed_error=0.1):\n",
    "        \"\"\"\n",
    "        distance_metres: positive is go forwards, negative go backwards\n",
    "        \"\"\"\n",
    "        rate = rospy.Rate(5)\n",
    "        move_twist = Twist()\n",
    "        \n",
    "        start_x = self.actual_pose.position.x\n",
    "        direction_speed = numpy.sign(position_x - start_x)\n",
    "        move_twist.linear.x = direction_speed*speed_ms\n",
    "        \n",
    "        \n",
    "        in_place = False\n",
    "        while not in_place:\n",
    "            self.move_to(move_twist)\n",
    "            x_actual = self.actual_pose.position.x\n",
    "            print x_actual\n",
    "\n",
    "            if  abs(position_x - x_actual) <= allowed_error:\n",
    "                print \"Reached position\"\n",
    "                break\n",
    "\n",
    "            rate.sleep()\n",
    "        self.stop()\n",
    "\n",
    "\n",
    "# Move base using navigation stack\n",
    "class MoveBaseClient(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.client = actionlib.SimpleActionClient(\"move_base\", MoveBaseAction)\n",
    "        rospy.loginfo(\"Waiting for move_base...\")\n",
    "        self.client.wait_for_server()\n",
    "\n",
    "    def goto(self, x, y, theta, frame=\"map\"):\n",
    "        move_goal = MoveBaseGoal()\n",
    "        move_goal.target_pose.pose.position.x = x\n",
    "        move_goal.target_pose.pose.position.y = y\n",
    "        move_goal.target_pose.pose.orientation.z = sin(theta/2.0)\n",
    "        move_goal.target_pose.pose.orientation.w = cos(theta/2.0)\n",
    "        move_goal.target_pose.header.frame_id = frame\n",
    "        move_goal.target_pose.header.stamp = rospy.Time.now()\n",
    "\n",
    "        # TODO wait for things to work\n",
    "        self.client.send_goal(move_goal)\n",
    "        self.client.wait_for_result()\n",
    "\n",
    "# Send a trajectory to controller\n",
    "class FollowTrajectoryClient(object):\n",
    "\n",
    "    def __init__(self, name, joint_names):\n",
    "        self.client = actionlib.SimpleActionClient(\"%s/follow_joint_trajectory\" % name,\n",
    "                                                   FollowJointTrajectoryAction)\n",
    "        rospy.loginfo(\"Waiting for %s...\" % name)\n",
    "        self.client.wait_for_server()\n",
    "        self.joint_names = joint_names\n",
    "\n",
    "    def move_to(self, positions, duration=5.0):\n",
    "        if len(self.joint_names) != len(positions):\n",
    "            print(\"Invalid trajectory position\")\n",
    "            return False\n",
    "        trajectory = JointTrajectory()\n",
    "        trajectory.joint_names = self.joint_names\n",
    "        trajectory.points.append(JointTrajectoryPoint())\n",
    "        trajectory.points[0].positions = positions\n",
    "        trajectory.points[0].velocities = [0.0 for _ in positions]\n",
    "        trajectory.points[0].accelerations = [0.0 for _ in positions]\n",
    "        trajectory.points[0].time_from_start = rospy.Duration(duration)\n",
    "        follow_goal = FollowJointTrajectoryGoal()\n",
    "        follow_goal.trajectory = trajectory\n",
    "\n",
    "        self.client.send_goal(follow_goal)\n",
    "        self.client.wait_for_result()\n",
    "\n",
    "# Point the head using controller\n",
    "class PointHeadClient(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.client = actionlib.SimpleActionClient(\"head_controller/point_head\", PointHeadAction)\n",
    "        rospy.loginfo(\"Waiting for head_controller...\")\n",
    "        self.client.wait_for_server()\n",
    "\n",
    "    def look_at(self, x, y, z, frame, duration=1.0):\n",
    "        goal = PointHeadGoal()\n",
    "        goal.target.header.stamp = rospy.Time.now()\n",
    "        goal.target.header.frame_id = frame\n",
    "        goal.target.point.x = x\n",
    "        goal.target.point.y = y\n",
    "        goal.target.point.z = z\n",
    "        goal.min_duration = rospy.Duration(duration)\n",
    "        self.client.send_goal(goal)\n",
    "        self.client.wait_for_result()\n",
    "\n",
    "# Tools for grasping\n",
    "class GraspingClient(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.scene = PlanningSceneInterface(\"base_link\")\n",
    "        self.pickplace = PickPlaceInterface(\"arm\", \"gripper\", verbose=True)\n",
    "        self.move_group = MoveGroupInterface(\"arm\", \"base_link\")\n",
    "\n",
    "   \n",
    "    def getGraspableCube(self):\n",
    "        \n",
    "        # nothing detected\n",
    "        return None, None\n",
    "\n",
    "    def getSupportSurface(self, name):\n",
    "        for surface in self.support_surfaces:\n",
    "            if surface.name == name:\n",
    "                return surface\n",
    "        return None\n",
    "\n",
    "    def getPlaceLocation(self):\n",
    "        pass\n",
    "\n",
    "    def pick(self, block, grasps):\n",
    "        success, pick_result = self.pickplace.pick_with_retry(block.name,\n",
    "                                                              grasps,\n",
    "                                                              support_name=block.support_surface,\n",
    "                                                              scene=self.scene)\n",
    "        self.pick_result = pick_result\n",
    "        return success\n",
    "\n",
    "    def place(self, block, pose_stamped):\n",
    "        places = list()\n",
    "        l = PlaceLocation()\n",
    "        l.place_pose.pose = pose_stamped.pose\n",
    "        l.place_pose.header.frame_id = pose_stamped.header.frame_id\n",
    "\n",
    "        # copy the posture, approach and retreat from the grasp used\n",
    "        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture\n",
    "        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach\n",
    "        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat\n",
    "        places.append(copy.deepcopy(l))\n",
    "        # create another several places, rotate each by 90 degrees in yaw direction\n",
    "        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)\n",
    "        places.append(copy.deepcopy(l))\n",
    "        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)\n",
    "        places.append(copy.deepcopy(l))\n",
    "        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)\n",
    "        places.append(copy.deepcopy(l))\n",
    "\n",
    "        success, place_result = self.pickplace.place_with_retry(block.name,\n",
    "                                                                places,\n",
    "                                                                scene=self.scene)\n",
    "        return success\n",
    "\n",
    "    def tuck(self):\n",
    "        joints = [\"shoulder_pan_joint\", \"shoulder_lift_joint\", \"upperarm_roll_joint\",\n",
    "                  \"elbow_flex_joint\", \"forearm_roll_joint\", \"wrist_flex_joint\", \"wrist_roll_joint\"]\n",
    "        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]\n",
    "        while not rospy.is_shutdown():\n",
    "            result = self.move_group.moveToJointPosition(joints, pose, 0.02)\n",
    "            if result.error_code.val == MoveItErrorCodes.SUCCESS:\n",
    "                return\n",
    "\n",
    "\n",
    "def demo():\n",
    "    # Create a node\n",
    "    rospy.init_node(\"face_recognition_demo_node\")\n",
    "\n",
    "    # Make sure sim time is working\n",
    "    while not rospy.Time.now():\n",
    "        pass\n",
    "\n",
    "    \n",
    "    move_base_pub = MoveBasePublisher()\n",
    "    \n",
    "    rospy.loginfo(\"Start Sequence complete\")\n",
    "    \n",
    "    rospy.loginfo(\"Move to position...\")\n",
    "    move_base_pub.move_forwards_backwards(speed_ms=0.3,position_x=1.0, allowed_error=0.1)\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    demo()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U3-1**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2D and 3D Object Finder"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is a basic approach to detecting objects, although you can already differentiate between objects and localize them. For this, you will use **find-object-2d** package.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to get closer to the table to see the objects as closely as possible, for better detection. You can use your recently created script to move the Fetch robot closer to the table."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2D Object Finder"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the launch file you would have to make in order to start the basic system:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**start_find_object_2d.launch**</p>"
   ]
  },
  {
   "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=\"/head_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": [
    "<p style=\"background:green;color:white;\">**END start_find_object_2d.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You only have to set the RGB camera image source and the system is ready to go. In this case, it's **/head_camera/rgb/image_raw**. Once you launch it, you should go to The Graphical Tools and 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/perception_unit3_objectrec2dinit.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, it's time to teach the Fetch robot to recognize a coke can. To do this, select the **Edit->AddObjectFromScene**. You can also add previously taken images directly. But, bear in mind that there are some peculiarities. The images appear in this objectrecogniser mirrored, if you compare them with the images from the cameras directly. So be careful with that."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Cam View , see that the image is mirrored in the object recognition Gui",
    "image": true,
    "name": "perception_unit3_mirrorimage1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_mirrorimage1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So in the Adding Object menu, you just have to follow the steps to select a region of the image that you consider to be the object. Once done, you should be detecting the object. This system compares the image 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": [
    "Can Picture:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Add Object",
    "image": true,
    "name": "perception_unit3_addobject1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_addobject1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Can detected:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Add Object result",
    "image": true,
    "name": "perception_unit3_addobject2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_addobject2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Fantastic! Now you can add as many objects as you want, or even turn the object around and take images from different points of view. This will make it detect the object all the time, but keep in mind that without the proper filtering, the system will consider them to be different objects.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, the last step is to save all of the objects added. There are 2 main ways:<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",
    "You can also save the object images directly by right clicking on them, but <font style=\"color:red\">BEWARE</font> that if you do so, these images can't be used for later detection because they aren't mirrored, therefore, the recognition won't work. You can see an example of it here. It's the same image, but one is the manually saved version that is not mirrored and isn't detected, while the other one is correctly saved."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Wrong Save example",
    "image": true,
    "name": "perception_unit3_wrongsaveexample",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_wrongsaveexample.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, once you have your session or your images stored, you need to be able to always start an object recognition session with all of that stored data. To do so, you have the following options:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For saved sessions in my_object_recognition_pkg, in the directory saved_pictures2d:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <arg name=\"camera_rgb_topic\" default=\"/head_camera/rgb/image_raw\" />\n",
    "\t<!-- Nodes -->\n",
    "\t<node name=\"find_object_2d\" pkg=\"find_object_2d\" type=\"find_object_2d\" output=\"screen\">\n",
    "\t\t<remap from=\"image\" to=\"$(arg camera_rgb_topic)\"/>\n",
    "\t\t<param name=\"gui\" value=\"true\" type=\"bool\"/>\n",
    "\t\t<param name=\"session_path\" value=\"$(find my_object_recognition_pkg)/saved_pictures2d/coke_session.bin\" type=\"str\"/>\n",
    "\t\t<param name=\"settings_path\" value=\"~/.ros/find_object_2d.ini\" type=\"str\"/>\n",
    "\t</node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For saved images in my_object_recognition_pkg, in the directory saved_pictures2d:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <arg name=\"camera_rgb_topic\" default=\"/head_camera/rgb/image_raw\" />\n",
    "\t<!-- Nodes -->\n",
    "\t<node name=\"find_object_2d\" pkg=\"find_object_2d\" type=\"find_object_2d\" output=\"screen\">\n",
    "\t\t<remap from=\"image\" to=\"$(arg camera_rgb_topic)\"/>\n",
    "\t\t<param name=\"gui\" value=\"true\" type=\"bool\"/>\n",
    "\t\t<param name=\"objects_path\" value=\"$(find my_object_recognition_pkg)/saved_pictures2d\" type=\"str\"/>\n",
    "\t\t<param name=\"settings_path\" value=\"~/.ros/find_object_2d.ini\" type=\"str\"/>\n",
    "\t</node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "More Info:<br>\n",
    "http://wiki.ros.org/find_object_2d<br>\n",
    "http://introlab.github.io/find-object/<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U3-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a launch file that starts the 2D object recognition through a saved session, where you have saved the coke can object.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U3-2**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Move and Spawn objects "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You have to test your object_recognition system with the same object in different positions, or even in movement.<br>\n",
    "You also need to test it with various objects in the scene to be sure that it doesn't mistake a human head with a coke.<br>\n",
    "Here you are going to learn how to move objects around in a Gazebo scene, and spawn new ones also."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To make an object move in a scene, there are 2 steps:<br>\n",
    "\n",
    "* **Make the object movable in the Gazebo**: For that, you will use the move_model.launch from our spawn_robot_tools_pkg.This makes a topic called \"name_object/cmd_vel\" available on which you can then publish and move the model around.\n",
    "* **Publish in the correct topic:** To move the object through the keyboard\n",
    "\n",
    "If you want to know exactly how this works, <i>we highly recommend you do the RobotIgniteAcademy course in TF and the URDF robot creation</i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**make_cokecan_movable.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "\n",
    "<launch> \n",
    "    <arg name=\"robot_name\" default=\"coke_can\" />\n",
    "\n",
    "    <include file=\"$(find spawn_robot_tools_pkg)/launch/move_model.launch\">\n",
    "        <arg name=\"robot_name\" value=\"$(arg robot_name)\" />\n",
    "    </include>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END make_cokecan_movable.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**move_coke_keyboard.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n",
    "\n",
    "<launch> \n",
    "    <arg name=\"robot_name\" default=\"coke_can\" />\n",
    "    <node name=\"$(arg robot_name)_twist_keyboard\" pkg=\"spawn_robot_tools_pkg\" type=\"model_twist_keyboard.py\" args=\"$(arg robot_name)\" output=\"screen\"/>\n",
    "</launch> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**move_coke_keyboard.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move other objects in the scene, just change the name of the model.<br>\n",
    "To know the name of that the model has in the Gazebo, you can ask the gazebo service:"
   ]
  },
  {
   "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": [
    "rosservice call /gazebo/get_world_properties"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So now, how do you spawn new objects in the scene?<br>\n",
    "There are many ways to do this. Here, you will learn one.<br>\n",
    "To learn alternative ways, **<i>we highly recommend that you do the RobotIgniteAcademy course in TF and the URDF robot creation</i>**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This method needs you to have the sdf files of the models on your package and the whole model installed in the .gazebo/models path. Because you are using RobotIgniteAcademy, you already have the needed models installed there. For simplicity, we have granted access to three of them: beer, coke can, and hammer. You really only need the sdf files of each model. The rest will be retrieved from the gazebo path, not your package. You can copy them to your package by:"
   ]
  },
  {
   "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 object_recogn_tc<br>\n",
    "cp models -r /home/user/catkin_ws/src/my_object_recognition_pkg/models"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have them, just execute the following launch file to spawn the model where you wish, probably in the table location.<br>\n",
    "For reference, the coke_can is spawned in the center of the table at XYZ = (-2.0,0.0,0.8).<br>\n",
    "Bear in mind that if there is already an existing model with the same name, you won't be able to spawn it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**spawn_coke.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "    <arg name=\"sdf_robot_file\" value=\"$(find my_object_recognition_pkg)/models/coke_can/model.sdf\"/>\n",
    "    <arg name=\"robot_name\" value=\"coke_can\" />\n",
    "    <arg name=\"x\" default=\"-2.0\" />\n",
    "    <arg name=\"y\" default=\"0.2\" />\n",
    "    <arg name=\"z\" default=\"0.8\" />\n",
    "    <arg name=\"yaw\" default=\"0.0\" />\n",
    "\n",
    "    <include file=\"$(find spawn_robot_tools_pkg)/launch/spawn_sdf.launch\">\n",
    "        <arg name=\"sdf_robot_file\" value=\"$(arg sdf_robot_file)\"/>\n",
    "        <arg name=\"robot_name\" default=\"$(arg robot_name)\" />\n",
    "        <arg name=\"x\" value=\"$(arg x)\" />\n",
    "        <arg name=\"y\" value=\"$(arg y)\" />\n",
    "        <arg name=\"z\" value=\"$(arg z)\" />\n",
    "        <arg name=\"yaw\" value=\"$(arg yaw)\" />\n",
    "    </include>\n",
    "    \n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END spawn_coke.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, in conclusion, to spawn an object, for example the coke_can, and make it movable at the same time, you will have to execute this launch file:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**spawn_ready_coke.launch**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <!-- Spawn CokeCan Model -->\n",
    "    <include file=\"$(find my_object_recognition_pkg)/launch/spawn_coke.launch\" />\n",
    "    <!-- Make CokeCan Movable -->\n",
    "    <include file=\"$(find my_object_recognition_pkg)/launch/make_cokecan_movable.launch\" />\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END spawn_ready_coke.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And afterwards, in another terminal, execute the **move_coke_keyboard.launch**."
   ]
  },
  {
   "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 spawn_ready_coke.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch my_object_recognition_pkg move_coke_keyboard.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And to delete the object, just RIGHTCLICK on it and delete it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U3-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Do the following:<br>\n",
    "\n",
    "* Delete the existing coke model in the scene.\n",
    "* Spawn a new coke_can on the table.\n",
    "* Move the coke can around and see how robust the detection system is with only one picture.\n",
    "* Then, take as many pictures as you need to never lose track of the object. The best method is to pivot the coke can in place until it stops detecting. Then, add it as a new object, and repeat the process until it detects it from 360 degrees.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U3-3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should, at the end, have a detection scheme similar to this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "360 degrees object recognition",
    "image": true,
    "name": "perception_Unit3_coke360",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/perception_Unit3_coke360.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U3-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, do do the same as in Exercise U3-3, but with a beer can and a hammer.<br>\n",
    "For this, you will have to spawn and move them first.<br>\n",
    "Remmember to save the session at the end so that you don't lose all of your object images."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U3-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3D Object Detection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, the only thing missing is having the position of the objects in the 3D space, to be able to grasp them. To know how to grasp an object, please do the <i>Manipulation course in RobotIgniteAcademy</i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The only real difference with the 2D detection will be the sensors involved and the fact that the ObjectPoseStamped will be transformed into TFs."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:red;color:white;\">**Warning**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You have to create another round of session photos in the 3D system because, otherwise, the detections won't work as well as they should. Especially for the TF transformations."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**start_find_object_3d_session.launch**</p>"
   ]
  },
  {
   "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_pictures2d/coke_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=\"/head_camera/rgb/image_raw\"/>\n",
    "\t\t<remap from=\"depth_registered/image_raw\" to=\"/head_camera/depth_registered/image_raw\"/>\n",
    "\t\t<remap from=\"depth_registered/camera_info\" to=\"/head_camera/depth_registered/camera_info\"/>\n",
    "\t</node>\n",
    "\t\n",
    "\t<!-- Example of tf synchronisation with the objectsStamped message -->\n",
    "\t<node name=\"tf_example\" pkg=\"find_object_2d\" type=\"tf_example\" output=\"screen\">\n",
    "\t\t<param name=\"map_frame_id\" value=\"/map\" type=\"string\"/>\n",
    "\t\t<param name=\"object_prefix\" value=\"object\" type=\"str\"/>\n",
    "\t</node>\n",
    "\t\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END start_find_object_3d_session.launch**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Launching this, you should then get the TF of the object detected 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": [
    "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": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "...\n",
    "Object 7 detected, Qt corners at (158.168777,220.963058) (293.200804,220.873430) (158.105995,441.616646) (291.453685,443.002172) \n",
    "..."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**Exercise U3-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Do the following:<br>\n",
    "\n",
    "* Launch the 3D object detector in a session with only one image per object. This way, you won't get a clutter of TF frames.\n",
    "* Move the objects around through a Python script. This way, you will be able to see how the TFs change and how accurate they are in relation to the real position of the objects."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END Exercise U3-4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "RVIZ TF of recognised object",
    "image": true,
    "name": "perception_unit3_object_recognitionTF1",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_object_recognitionTF1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And with multiple object images per object, something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "RVIZ TF of 360 degrees recognition, see multiple TF published",
    "image": true,
    "name": "perception_unit3_multiple_objects_recognitiontf",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/perception_unit3_multiple_objects_recognitiontf.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The TFs appearing can be lowered by decreasing the time you consider a TF obsolete. Because most of these TFs are from previous detections that stay there for a while until they are old enough to be considered irrelevant."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**EXTRA Exercise U3-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Do the following:<br>\n",
    "\n",
    "* Spawn two objects on the table and make them move on the table.\n",
    "* Detect one of the objects.\n",
    "* Track that object and make the Fetch robot track it with the head to keep the object in the center of its view.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">**END EXTRA Exercise U3-5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations! You now know how to recognise objects and localise them in space. Go on to the next unit to learn about Human Face Detection."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#417FB1;color:white;\">**Project**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now do the third exercise of the Aibo Project. There you will have to make the Aibo robot recognise its nice bone and go to it."
   ]
  },
  {
   "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": "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
}