Course_SummitXLInstructionManual_Unit_3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 3: Detect and localise person"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit3_intro.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotnik_logo.png\" width=\"300\"/>"
   ]
  },
  {
   "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 a PTZ RGB normal camera**: This system doesnt give you directly the position of the person, but detects the persons through Video with OpenCV.\n",
    "* As extra, in this unit you will have to learn how to move the PTZ camera around."
   ]
  },
  {
   "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=\"/hokuyo_base/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 paremeters for finer tuning."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have it, you should be able to launch it and start detecting people right away. The detections will be published in the topic **/leg_tracker_measurements**. 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/summit_xl_instruction_manual_unit3_legmarker0.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit3_legmarker1.png\" width=\"800\"></img>"
   ]
  },
  {
   "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 = \"summit_xl\"\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."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you basically retrieve the position of the leg detection and publish the marker using the mesh **standingv2.dae** as marker."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "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": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit3_legmarker4.png\" width=\"800\"></img>"
   ]
  },
  {
   "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 PTZ RGB camera to detect persons. For this you will have to first know how to move the camera."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Move the PTZ Camera"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move the camera you have to publish in the topic **/axis_camera/ptz_command**, a message of type **robotnik_msgs/ptz**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# PAN value                                                                                                                                                  \n",
    "float32 pan                                                                                                                                                  \n",
    "# Tilt value                                                                                                                                                 \n",
    "float32 tilt                                                                                                                                                 \n",
    "# Zoom value                                                                                                                                                 \n",
    "float32 zoom                                                                                                                                                 \n",
    "# Flag for relative  movements                                                                                                                               \n",
    "bool relative"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "See that because its a PTZ Camera, you can move the Pan, Tilt and also make Zoom. You also can state of the movements are relative or not."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how you could move the camera. Bare in mind that the movements are based on speed. This menas that you are sending how fast tin radians per second will the pan, or tilt move. This has  to be sent continuously, otherside the movement will stop."
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "move_ptz_cam.py"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "import time\n",
    "import rospy\n",
    "from math import pi, sin, cos, acos\n",
    "import random\n",
    "from robotnik_msgs.msg import ptz\n",
    "from sensor_msgs.msg import JointState\n",
    "from geometry_msgs.msg import Twist\n",
    "\"\"\"\n",
    "Topics To Write on:\n",
    "type: std_msgs/Float64\n",
    "/joint_pan_position_controller/command                                                                                                                       \n",
    "/joint_tilt_position_controller/command\n",
    "\n",
    "\n",
    "rostopic info /axis_camera/ptz_command                                                                                                      \n",
    "Type: robotnik_msgs/ptz\n",
    "# PAN value                                                                                                                                                  \n",
    "float32 pan                                                                                                                                                  \n",
    "# Tilt value                                                                                                                                                 \n",
    "float32 tilt                                                                                                                                                 \n",
    "# Zoom value                                                                                                                                                 \n",
    "float32 zoom                                                                                                                                                 \n",
    "# Flag for relative  movements                                                                                                                               \n",
    "bool relative\n",
    "\"\"\"\n",
    "\n",
    "class PTZCamMove(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        \n",
    "        rospy.loginfo(\"PTZ Cam JointMover Initialising...\")\n",
    "        self.pub_joint_pan_position = rospy.Publisher('/axis_camera/ptz_command',\n",
    "                                                            ptz,\n",
    "                                                            queue_size=1)\n",
    "        \n",
    "\n",
    "    def init_ptz_pos(self):\n",
    "        rospy.loginfo(\"Init Pos Start...\")\n",
    "        ptz_object = ptz()\n",
    "        ptz_object.pan = pan\n",
    "        ptz_object.tilt = 0.5\n",
    "        ptz_object.zoom = zoom\n",
    "        \n",
    "        rate = rospy.Rate(5)\n",
    "        for i in range(10):\n",
    "            self.move_all_joints(ptz_object.pan, ptz_object.tilt, ptz_object.zoom, ptz_object.relative)\n",
    "            rate.sleep()\n",
    "\n",
    "    def move_all_joints(self, pan, tilt, zoom, relative=True):\n",
    "        ptz_object = ptz()\n",
    "        ptz_object.pan = pan\n",
    "        ptz_object.tilt = tilt\n",
    "        ptz_object.zoom = zoom\n",
    "        ptz_object.relative = relative\n",
    "        \n",
    "        self.pub_joint_pan_position.publish(ptz_object)\n",
    "    \n",
    "    \n",
    "    def pan_camera(self):\n",
    "        \n",
    "        rate = rospy.Rate(10)\n",
    "        pan_speed = 1.0\n",
    "        tilt_speed = 0.0\n",
    "        zoom = 0.0\n",
    "        \n",
    "        rospy.loginfo(\"Pan left...\")\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Pan right...\")\n",
    "        pan_speed *= -1\n",
    "        for i in range(80):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Pan center...\")\n",
    "        pan_speed *= -1\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "    def tilt_camera(self):\n",
    "        \n",
    "        rate = rospy.Rate(10)\n",
    "        pan_speed = 0.0\n",
    "        tilt_speed = 1.0\n",
    "        zoom = 0.0\n",
    "        \n",
    "        rospy.loginfo(\"Tilt left...\")\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Tilt right...\")\n",
    "        tilt_speed *= -1\n",
    "        for i in range(80):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Tilt center...\")\n",
    "        tilt_speed *= -1\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "    def zoom_camera(self):\n",
    "        \n",
    "        rate = rospy.Rate(10)\n",
    "        pan_speed = 0.0\n",
    "        tilt_speed = 0.0\n",
    "        zoom = 100.0\n",
    "        \n",
    "        rospy.loginfo(\"Zoom in...\")\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Zoom out...\")\n",
    "        zoom *= -1\n",
    "        for i in range(80):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "            \n",
    "        \n",
    "        rospy.loginfo(\"Zoom in...\")\n",
    "        zoom *= -1\n",
    "        for i in range(40):\n",
    "            self.move_all_joints(pan_speed, tilt_speed, zoom)\n",
    "            rate.sleep()\n",
    "\n",
    "    \n",
    "    def start_loop(self):\n",
    "        \"\"\"\n",
    "        Executed movements in a random way\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        rospy.loginfo(\"Start PTZ Moving Test...\")\n",
    "        \n",
    "        rospy.spin()\n",
    "\n",
    "\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('ptz_cam_move_node', anonymous=True, log_level=rospy.INFO)\n",
    "    jointmover_object = PTZCamMove()\n",
    "    jointmover_object.pan_camera()\n",
    "    jointmover_object.tilt_camera()\n",
    "    jointmover_object.zoom_camera()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U3-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a new movement system that checks on the joint_states of the pan and tilt, so that you can send a position , and it will reach that position sending speed commands."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U3-2</p>"
   ]
  },
  {
   "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": [
    "**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",
    "from move_ptz_cam import PTZCamMove\n",
    "\n",
    "class DetectPersonCam(object):\n",
    "\n",
    "    def __init__(self):\n",
    "    \n",
    "        self.bridge_object = CvBridge()\n",
    "        self.image_sub = rospy.Subscriber(\"/camera/image_raw\",Image,self.camera_callback)\n",
    "        self.pedestrian_detector = PedestrianDetector()\n",
    "        self.ptz_cam_mover = PTZCamMove()\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",
    "    def move_camera(self,roll,pitch):\n",
    "        self.ptz_cam_mover.move_all_joints(roll, pitch)\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/image_raw\",Image,self.camera_callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you susbrcibe to the topic where the Camera images are published."
   ]
  },
  {
   "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 fucntion sends the data to the pedestrian detector."
   ]
  },
  {
   "cell_type": "raw",
   "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(\"Pdestrian CAM\", image)\n",
    "    \tcv2.waitKey(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When executing this code you should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit3_pedestriandetector.png\" width=\"800\"></img>"
   ]
  },
  {
   "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 the Summit XL. 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 find 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": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "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
}