Course_T3InstructionManual_Unit3_old.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 3: Detect and localise a person"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotis_logo.png\" width=\"400\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/waffle_person.png\" width=\"500\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"300\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this unit you are goint to learn how to detect a person using two different systems:<br>\n",
    "* **Detect through leg detection with laser sensor**: This system detect persons based on laser readings of leg patterns. This allows you to have even the position of the person, not only detect if there is a person or not.\n",
    "* **Detect person through an RGB camera from the stereo-cam**: This system doesnt give you directly the position of the person, but detects the persons through Video with OpenCV.\n",
    "* As extra, you are going to learn how to **Get Point cloud** from stereocamera: Use the stereocamera to extract pointcloud data from the scene."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Person leg Detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is a classical way to detect people. Its fast and relatively robust. You will use a ROS package called **leg_detector**, for more information http://wiki.ros.org/leg_detector.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to generate a launch file that sets all the parameters:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_legdetector.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch> \n",
    "    <arg name=\"scan\" default=\"/scan\" />\n",
    "    <arg name=\"machine\" default=\"localhost\" />\n",
    "    <arg name=\"user\" default=\"\" />\n",
    "    <!-- Leg Detector -->\n",
    "    <node pkg=\"leg_detector\" type=\"leg_detector\" name=\"leg_detector\" args=\"scan:=$(arg scan) $(find leg_detector)/config/trained_leg_detector.yaml\" respawn=\"true\" output=\"screen\">\n",
    "        <param name=\"fixed_frame\" type=\"string\" value=\"map\" />\n",
    "    </node>\n",
    "    \n",
    "</launch> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see here the only thing that you have to set really is the scan topic, which is the topic for the laser. The rest of parameters are set in a yaml file trained_leg_detector.yaml, which you can leave as it is unless you want to optimise or make set all the peremeters for finer tuning."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<param name=\"fixed_frame\" type=\"string\" value=\"map\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note this parameter **fixed_frame**. It uses the **map** frame as refference. This means that you **HAVE** to publish the **map** frame one way or another. So before anything else, you have to launch your **navigation** system. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* You launch the **start_navigation_with_map.launch**. You can load it with the **mymap_empty.yaml** map. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <arg name=\"model\" default=\"waffle\" doc=\"model type [burger, waffle]\"/>\n",
    "\n",
    "  <!-- Turtlebot3 -->\n",
    "  <include file=\"$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch\" />\n",
    "\n",
    "  <!-- Map server -->\n",
    "  <arg name=\"map_file\" default=\"$(find t3_course)/maps/mymap_empty.yaml\"/>\n",
    "  <!-- <arg name=\"map_file\" default=\"/home/user/catkin_ws/src/my_map.yaml\"/> -->\n",
    "  <node name=\"map_server\" pkg=\"map_server\" type=\"map_server\" args=\"$(arg map_file)\">\n",
    "  </node>\n",
    "\n",
    "  <!-- AMCL -->\n",
    "  <include file=\"$(find turtlebot3_navigation)/launch/amcl.launch.xml\"/>\n",
    "\n",
    "  <!-- move_base -->\n",
    "  <arg name=\"cmd_vel_topic\" default=\"/cmd_vel\" />\n",
    "  <arg name=\"odom_topic\" default=\"odom\" />\n",
    "  <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "    <param name=\"base_local_planner\" value=\"dwa_local_planner/DWAPlannerROS\" />\n",
    "\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/local_costmap_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/global_costmap_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/move_base_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find turtlebot3_navigation)/param/dwa_local_planner_params.yaml\" command=\"load\" />\n",
    "\n",
    "    <remap from=\"cmd_vel\" to=\"$(arg cmd_vel_topic)\"/>\n",
    "    <remap from=\"odom\" to=\"$(arg odom_topic)\"/>\n",
    "  </node>\n",
    "  \n",
    "  <!-- Start RVIZ for Localization -->\n",
    "  <include file=\"$(find my_jackal_tools)/launch/view_robot.launch\">\n",
    "    <arg name=\"config\" value=\"person\" />\n",
    "  </include>\n",
    "  \n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have it, you should be able to launch **start_legdetector.launch** and start detecting people right away. The detections will be published in the topic **/leg_tracker_measurements**. Here you have output example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "---                                                                                                                                                                       \n",
    "header:                                                                                                                                                                   \n",
    "  seq: 668                                                                                                                                                                \n",
    "  stamp:                                                                                                                                                                  \n",
    "    secs: 40                                                                                                                                                              \n",
    "    nsecs: 788000000                                                                                                                                                      \n",
    "  frame_id: ''                                                                                                                                                            \n",
    "people:                                                                                                                                                                   \n",
    "  -                                                                                                                                                                       \n",
    "    header:                                                                                                                                                               \n",
    "      seq: 0                                                                                                                                                              \n",
    "      stamp:                                                                                                                                                              \n",
    "        secs: 40                                                                                                                                                          \n",
    "        nsecs: 765000000                                                                                                                                                  \n",
    "      frame_id: map                                                                                                                                                       \n",
    "    name: leg_detector                                                                                                                                                    \n",
    "    object_id: legtrack0                                                                                                                                                  \n",
    "    pos:                                                                                                                                                                  \n",
    "      x: 2.95611178591                                                                                                                                                    \n",
    "      y: -0.127522822998                                                                                                                                                  \n",
    "      z: 0.334633714587                                                                                                                                                   \n",
    "    reliability: 0.923895539057                                                                                                                                           \n",
    "    covariance: [0.10543790524172501, 0.0, 0.0, 0.0, 0.10543790524172501, 0.0, 0.0, 0.0, 10000.0]                                                                         \n",
    "    initialization: 0                                                                                                                                                     \n",
    "  -                                                                                                                                                                       \n",
    "    header:                                                                                                                                                               \n",
    "      seq: 0                                                                                                                                                              \n",
    "      stamp:                                                                                                                                                              \n",
    "        secs: 40                                                                                                                                                          \n",
    "        nsecs: 765000000                                                                                                                                                  \n",
    "      frame_id: map                                                                                                                                                       \n",
    "    name: leg_detector                                                                                                                                                    \n",
    "    object_id: legtrack1                                                                                                                                                  \n",
    "    pos:                                                                                                                                                                  \n",
    "      x: 2.96855903689                                                                                                                                                    \n",
    "      y: 0.133168364643                                                                                                                                                   \n",
    "      z: 0.334657142144                                                                                                                                                   \n",
    "    reliability: 0.939497802961                                                                                                                                           \n",
    "    covariance: [0.10196496436200578, 0.0, 0.0, 0.0, 0.10196496436200578, 0.0, 0.0, 0.0, 10000.0]                                                                         \n",
    "    initialization: 0                                                                                                                                                     \n",
    "cooccurrence: []"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see its posible that its making multiple detections, thats why post processing as improvement."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This topic is of type **PositionMeasurementArray**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "### PositionMeasurementArray\n",
    "\n",
    "std_msgs/Header header                                                                                                         \n",
    "  uint32 seq                                                                                                                   \n",
    "  time stamp                                                                                                                   \n",
    "  string frame_id                                                                                                              \n",
    "people_msgs/PositionMeasurement[] people                                                                                       \n",
    "  std_msgs/Header header                                                                                                       \n",
    "    uint32 seq                                                                                                                 \n",
    "    time stamp                                                                                                                 \n",
    "    string frame_id                                                                                                            \n",
    "  string name                                                                                                                  \n",
    "  string object_id                                                                                                             \n",
    "  geometry_msgs/Point pos                                                                                                      \n",
    "    float64 x                                                                                                                  \n",
    "    float64 y                                                                                                                  \n",
    "    float64 z                                                                                                                  \n",
    "  float64 reliability                                                                                                          \n",
    "  float64[9] covariance                                                                                                        \n",
    "  byte initialization                                                                                                          \n",
    "float32[] cooccurrence"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have Position information and also reliability of the detections."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But lets go a step further. You need to visualise those detections in space. For that by default the leg_detector already has a topic that publishes RVIZ markers called **/visualization_marker**. This is depicts the person detection through a sphere based on the reliability changing its colour and appearance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit3_markerreal2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Open RVIZ and add a **Marker** element, and select the topic **/visualization_marker**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit3_markerbasic.png\" width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can aslo generate a custom marker that will be much more descriptive when visualised. For that you will have to create a python script that reads the leg_detections from **/leg_tracker_measurements**, and publish it in a new marker_topic with its own custom marker, in this case will be a mesh of the person detected."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**leg_detector_marker_publisher.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import rospy\n",
    "from people_msgs.msg import PositionMeasurementArray\n",
    "\n",
    "\n",
    "#!/usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from visualization_msgs.msg import Marker\n",
    "from geometry_msgs.msg import Point\n",
    "\n",
    "\n",
    "class MarkerBasics(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.marker_objectlisher = rospy.Publisher('/marker_person_detected_leg', Marker, queue_size=1)\n",
    "        self.rate = rospy.Rate(1)\n",
    "        self.init_marker(index=0,z_val=0)\n",
    "    \n",
    "    def init_marker(self,index=0, z_val=0):\n",
    "        self.marker_object = Marker()\n",
    "        self.marker_object.header.frame_id = \"/map\"\n",
    "        self.marker_object.header.stamp    = rospy.get_rostime()\n",
    "        self.marker_object.ns = \"waffle\"\n",
    "        self.marker_object.id = index\n",
    "        self.marker_object.type = Marker.MESH_RESOURCE\n",
    "        self.marker_object.action = Marker.ADD\n",
    "        \n",
    "        my_point = Point()\n",
    "        self.marker_object.pose.position = my_point\n",
    "        \n",
    "        self.marker_object.pose.orientation.x = 0\n",
    "        self.marker_object.pose.orientation.y = 0\n",
    "        self.marker_object.pose.orientation.z = 1.0\n",
    "        self.marker_object.pose.orientation.w = 1.0\n",
    "        self.marker_object.scale.x = 1.0\n",
    "        self.marker_object.scale.y = 1.0\n",
    "        self.marker_object.scale.z = 1.0\n",
    "    \n",
    "        self.marker_object.color.r = 0.0\n",
    "        self.marker_object.color.g = 0.0\n",
    "        self.marker_object.color.b = 0.0\n",
    "        # This has to be otherwise it will be transparent\n",
    "        self.marker_object.color.a = 0.0\n",
    "        self.marker_object.mesh_resource = \"package://person_sim/models/person_standing/meshes/standingv2.dae\"\n",
    "        self.marker_object.mesh_use_embedded_materials = True\n",
    "        # If we want it for ever, 0, otherwise seconds before desapearing\n",
    "        self.marker_object.lifetime = rospy.Duration(0)\n",
    "\n",
    "    def update_position(self,position, reliability):\n",
    "        \n",
    "        self.marker_object.pose.position = position\n",
    "        \n",
    "        self.marker_objectlisher.publish(self.marker_object)\n",
    "\n",
    "\"\"\"\n",
    "### PositionMeasurementArray\n",
    "\n",
    "std_msgs/Header header                                                                                                         \n",
    "  uint32 seq                                                                                                                   \n",
    "  time stamp                                                                                                                   \n",
    "  string frame_id                                                                                                              \n",
    "people_msgs/PositionMeasurement[] people                                                                                       \n",
    "  std_msgs/Header header                                                                                                       \n",
    "    uint32 seq                                                                                                                 \n",
    "    time stamp                                                                                                                 \n",
    "    string frame_id                                                                                                            \n",
    "  string name                                                                                                                  \n",
    "  string object_id                                                                                                             \n",
    "  geometry_msgs/Point pos                                                                                                      \n",
    "    float64 x                                                                                                                  \n",
    "    float64 y                                                                                                                  \n",
    "    float64 z                                                                                                                  \n",
    "  float64 reliability                                                                                                          \n",
    "  float64[9] covariance                                                                                                        \n",
    "  byte initialization                                                                                                          \n",
    "float32[] cooccurrence\n",
    "\"\"\"\n",
    "class LegDetector(object):\n",
    "    def __init__(self):\n",
    "        rospy.Subscriber(\"/leg_tracker_measurements\", PositionMeasurementArray, self.leg_detect_callback)\n",
    "\n",
    "        self.markerbasics_object = MarkerBasics()\n",
    "\n",
    "    def leg_detect_callback(self,data):\n",
    "        for people_object in data.people:\n",
    "            self.markerbasics_object.update_position(people_object.pos, people_object.reliability)\n",
    "    \n",
    "    def start_loop(self):\n",
    "        # spin() simply keeps python from exiting until this node is stopped\n",
    "        rospy.spin()\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('leg_detections_listener_node', anonymous=True)\n",
    "    legdetector_object = LegDetector()\n",
    "    legdetector_object.start_loop()\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you need more information about how this is done, we highly reccomend you to do the RVIZ Markers course in RobotIgniteAcademy.<br>\n",
    "Here you basically retrieve the position of the leg detection and publish the marker using the mesh **standingv2.dae** as marker.<br>\n",
    "Bare in mind that because the leg detector doesnt give you orientation information the marker will have always the same orientation unless you detect the orientation of the person by other menas, such as movement direction."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you execute the python script **leg_detector_marker_publisher.py**, You should get something similar to this by launching instead of your rviz config the one given by RobotIgniteSystem installed packages:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<!-- Start RVIZ for Localization -->\n",
    "    <include file=\"$(find jackal_tools)/launch/view_robot.launch\">\n",
    "        <arg name=\"config\" value=\"person\" />\n",
    "    </include>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit3_rvizmarkerview.png\" width=\"600\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "As you can see, the elements of the **global** map and **localcostmap** are activated.<br>\n",
    "Its also selected a diferent camera view, the XYOrbit, that allows you to see better the Person 3D model."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U3-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Based on the leg detection data, get the direction in which the person is moving and change the orientation of the custom marker to better match the real person."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U3-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pedestrian Detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you are going to use the Stereo RGB camera to detect persons."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Detect pedestrians with OpenCV script"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are going to learn how to acces the camera data in ROS, and convert it to OpenCV format. Then you will process the image data to detect pedestrians and draw a bounding box around them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this you will create two new scripts:<br>\n",
    "* **the main detect person**, shich will retrieve the image data from the Summit XL robot ROS topics and send it to a pedestrian detector script. \n",
    "\n",
    "* **The pedestrian detector script**: This script will do the tough work of detecting and drawing the bounding box around the person detected."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are **two** main topics for the images, one for each of the two cameras:\n",
    "* /front/left/image_raw\n",
    "* /front/right/image_raw"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**detect_person_camera.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\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 pedestrian_detector import PedestrianDetector\n",
    "\n",
    "class DetectPersonCam(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.pedestrian_detector = PedestrianDetector()\n",
    "        self.init_time = rospy.get_time()\n",
    "        self.now_pitch = 0\n",
    "        self.now_roll = 0\n",
    "        self.process_this_frame = True\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",
    "        # To process only half of the images for faster performance\n",
    "        if self.process_this_frame:\n",
    "            self.pedestrian_detector.detect_pedestrian(cv_image)\n",
    "        \n",
    "        self.process_this_frame = not self.process_this_frame\n",
    "    \n",
    "    \n",
    "\n",
    "\n",
    "def main():\n",
    "    rospy.init_node('detect_person_camera_node', anonymous=True)\n",
    "    \n",
    "    detect_personcam_object = DetectPersonCam()\n",
    "    \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": [
    "Comment on the following:"
   ]
  },
  {
   "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": [
    "Here you susbrcibe to the topic where the Camera images are published for the left camera. We could use the **right** camera, just pick one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding=\"bgr8\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you get that image and tranform it into OpenCV BGR8bit format. If you want to know more details, please reffer to the ROS perception Course in RobotIgniteAcademy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "if self.process_this_frame:\n",
    "    ...\n",
    "self.process_this_frame = not self.process_this_frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This condition makes the program procss only half of the images, making it much faster."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.pedestrian_detector.detect_pedestrian(cv_image)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function sends the data to the pedestrian detector."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**pedestrian_detector.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\"\"\"\n",
    "USAGE\n",
    "python detect.py --images images\n",
    "\"\"\"\n",
    "# import the necessary packages\n",
    "from __future__ import print_function\n",
    "from imutils.object_detection import non_max_suppression\n",
    "from imutils import paths\n",
    "import numpy as np\n",
    "import argparse\n",
    "import imutils\n",
    "import cv2\n",
    "\n",
    "\n",
    "class PedestrianDetector(object):\n",
    "    def __init__(self):\n",
    "        # initialize the HOG descriptor/person detector\n",
    "        self.hog = cv2.HOGDescriptor()\n",
    "        self.hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())\n",
    "\n",
    "    def detect_pedestrian(self, image):\n",
    "\n",
    "    \timage = imutils.resize(image, width=min(400, image.shape[1]))\n",
    "    \torig = image.copy()\n",
    "    \n",
    "    \t# detect people in the image\n",
    "    \t(rects, weights) = self.hog.detectMultiScale(image, winStride=(4, 4),\n",
    "    \t\tpadding=(8, 8), scale=1.05)\n",
    "    \n",
    "    \t# draw the original bounding boxes\n",
    "    \t\"\"\"\n",
    "    \tfor (x, y, w, h) in rects:\n",
    "    \t\tcv2.rectangle(orig, (x, y), (x + w, y + h), (0, 0, 255), 2)\n",
    "        \"\"\"\n",
    "    \t# apply non-maxima suppression to the bounding boxes using a\n",
    "    \t# fairly large overlap threshold to try to maintain overlapping\n",
    "    \t# boxes that are still people\n",
    "    \trects = np.array([[x, y, x + w, y + h] for (x, y, w, h) in rects])\n",
    "    \tpick = non_max_suppression(rects, probs=None, overlapThresh=0.65)\n",
    "    \n",
    "    \t# draw the final bounding boxes\n",
    "    \tfor (xA, yA, xB, yB) in pick:\n",
    "    \t\tcv2.rectangle(image, (xA, yA), (xB, yB), (0, 255, 0), 2)\n",
    "    \n",
    "    \t# show some information on the number of bounding boxes\n",
    "    \tfilename = \"LiveCam\"\n",
    "    \tprint(\"[INFO] {}: {} original boxes, {} after suppression\".format(\n",
    "    \t\tfilename, len(rects), len(pick)))\n",
    "    \n",
    "    \t# show the output images\n",
    "    \t#cv2.imshow(\"Before NMS\", orig)\n",
    "    \tcv2.imshow(\"Pedestrian CAM\", image)\n",
    "    \tcv2.waitKey(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "When executing this code you should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit3_persondetectgui2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit3_persondetectgui3.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U3-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a method that based on the data of the pedestrian detection bounding boxes, publishes the positon relative to Jackal robot. For that you will have to make calibration tests to see the size of the bounding box based on the distance, and TF publications. If you need more information on how to do that, in the course ** ROS TF** and **ROS Perception** you will it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U3-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations, you have finished the theory of this course, now to the micro project!"
   ]
  }
 ],
 "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"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}