Course Project-Es.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS en una Sola Semana"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Proyecto del Curso"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Tiempo estimado para completarlo: 11 horas"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Gana la carrera de Spheros!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "En este proyecto, tendrás que hacer que un <a href=\"http://www.sphero.com/sphero\">robot Sphero</a> se mueva a través de un labertinto más rápido que los demás estudiantes. El más rápido ganará un premio.<br>\n",
    "\n",
    "Para este objetivo, deberás aplicar todas las cosas que vas aprendiendo durante el curso. Es realmente importante que lo completes porque todas las estructuras que crees para este proyecto se te preguntarán en <span style=\"color: red\">el exámen</span>.<br>\n",
    "\n",
    "Haz que el robot salga del laberinto tan rápido como puedas y ganarás el premio. Lo ideal sería que el Sphero salga de forma limpia, pero puede pasar que choques con los límites del laberinto. Puedes usar la detección de colisiones para obtener datos que te ayuden en tu estrategia para salir.\n",
    "\n",
    "Básicamente, en este proyecto tendrás que:<br>\n",
    "<ol>\n",
    "  <li>Aplicar toda la teoría dada en el curso</li>\n",
    "  <li>Decidir una estrategia</li>\n",
    "  <li>Implementar esta estrategia en el entorno de simulación</li>\n",
    "  <li>Hacer tantos tests como sean necesarios en el entorno de simulación hasta que funcione</li>\n",
    "</ol>\n",
    "\n",
    "<center>\n",
    "<h3> Así es como se lleva a cabo el desarrollo robótico en la vida real.</h3>\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<figure>\n",
    "  <img id=\"fig-p.1\" src=\"img/sphero_real_sim.jpg\"/>\n",
    "   <center> <figcaption>Fig-p.1 - Sphero simulado y real</figcaption></center>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Una nota final</b>: Debido a que el código que desarrolles debería funcionar en un robot real si fuese necesario, no puedes mover el Sphero en un bucle cerrado. Esto es porque en la realidad el circuito podría ser distinto, el robot podría no ser tan sensitivo, podría haber errores en las lecturas, etc. Así que deberías crear un programa que pueda lidiar con todas esas circunstancias."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Qué proporciona el Sphero para programarlo?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Así que la pregunta principal es, qué puedes hacer con el Sphero desde el punto de vista de programación ROS? Qué sensores y actuadores proporciona el Sphero que te permitirán salir del laberinto?<br>\n",
    "\n",
    "Buena pregunta!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sensores"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "  <li><b>Sensor IMU</b>: El Sphero tiene una <a href=\"https://en.wikipedia.org/wiki/Inertial_measurement_unit\">Unidad de Medimiento Inercial</a> (IMU) que proporciona información sobre la aceleración y la orientación. Los valores del sensor se proporcionan a través del tópico <span style=\"color: green\"><i>/sphero/imu/data</i></span></li>\n",
    "  <li><b>Odometría</b>: La odometría robot puede ser leída a través del tópico <span style=\"color: green\"><i>/odom</i></span></li> \n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Actuadores"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "  <li><b>Velocidad</b>: Puedes enviar comandos de velocidad para mover el robot al tópico <span style=\"color: green\"><i>/cmd_vel</i></span></li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ahora que conoces los tópicos relevantes del robot, debes averiguar los tipos de mensajes y cómo usarlos con el fin de que el robot lleve a cabo la estrategia que quieras."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Cómo testear en el entorno de simulación?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "  <li>Crea tus paquetes y tu código en el entorno de simulación tal y como has hecho durante el curso.</li>\n",
    "  <li>Usa las consolas para obtener información acerca del estado del robot simulado.</li>\n",
    "  <li>Usa el IDE para crear tus programas y ejecútalos a través de las consolas, observando los resultados en la ventana de simulación. Puedes usar otras consolas para observar llamadas a tópicos, servicios o actions.</li>\n",
    "  <li>Todo lo que crees en esta unidad será automáticamente guardado en tu espacio. Puedes volver a esta unidad en cualquier momento y continuar con tu trabajo desde el punto en que lo dejaste.</li>\n",
    "  <li><span style=\"color: green\">Cada vez que necesites resetear la posición del robot presiona el botón reset en la ventana de simulación.</span>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Ideas para empezar a trabajar"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Aquí hay una lista de cosas por las que puedes empezar. No tienes por qué seguirlas. Se proporcionan sólo por si no sabes por dónde empezar."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ol>\n",
    "  <li>Empieza observando algunos de los mensajes que están publicando los tópicos de los sensores. Trata de hacerte una idea sobre qué es la información que proporcionan. Mueve el robot en la simulación y observa cómo esos mensajes cambian de valores. Es muy importante que entiendas cómo los cambios en el robot producen cambios en los tópicos.</li>\n",
    "  <li>Trata de mover el robot enviando mensajes al tópico <i>/cmd_vel</i> (ya sea a través de consola o de programas Python).</li>\n",
    "  <li>Observa cómo los mensajes de los tópicos cambian cuando el robot choca con un obstáculo.</li>\n",
    "  <li>Es la odometría fiable? Puedes mover el robot las cantidades exactas incluso cuando choca con algo?</li>\n",
    "  <li>Usa las herramientas de debugging para tratar de encontrar qué es lo que no funciona y por qué (por ejemplo, la herramienta <i>rviz</i> es muy útil para este fin).</li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pasos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estos son los pasos que deberías seguir durante el proyecto. Estos pasos te asegurarán que has practicado y creado todas las estructuras pedidas en el examen final de este curso. Si sigues todos los pasos mencionados aquí, encontrarás el examen muy fácil.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ol>\n",
    "<li><a href=\"#step1\">Paso 1: Escribe y Lee en Tópicos</a></li>\n",
    "<li><a href=\"#step2\">Paso 2: Usa tópicos a través de Servicios</a></li>\n",
    "<li><a href=\"#step3\">Paso 3: Usa tópicos a través de Actions</a></li>\n",
    "<li><a href=\"#step4\">Paso 4: Crea un programa principal que lo gestione todo</a></li>\n",
    "<li><a href=\"#stepExtra\">Paso EXTRA: Cómo usar módulos Python de diferentes paquetes</a></li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step1\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Paso 1: Leer y Escribir en Tópicos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Necesitarás:<br>\n",
    "<ol>\n",
    "    <li>Crea un paquete llamado <span style=\"color: green\"><i>my_sphero_topics</i></span> que contenga todos los paquetes relacionados con tópicos</li>\n",
    "    <li>Crea un <span style=\"color: green\">publisher</span> que te permita mover el Sphero.</li>\n",
    "    <li>Crea <span style=\"color: green\">dos subscribers</span> que obtengan los datos que necesitas de Odometría y del IMU.</li>\n",
    "</ol>\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">1. Crea el paquete my_sphero_topics, con <i>rospy</i> como dependencia.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">2. Crea el Publisher para mover el Sphero.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para mover el Sphero necesitas publicar en el tópico <span style=\"color: green\">/cmd_vel</span>.<br>\n",
    "Es importante que siempre encapsules tus subscribers y publishers dentro de clases. Esto te permite guardar valores y gestionar las publicaciones/callbacks fácilmente.<br>\n",
    "\n",
    "Primero debes comprobar si hay un tópico como /cmd_vel corriendo. No siempre será así de simple. En robots reales necesitarás acceder al código para ver cual es el nombre del tópico que mueve el robot o incluso usar rostopic info /nombre_del_tópico para saber cual podría ser.\n",
    "\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rostopic list  <br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br>\n",
    "\n",
    "Como habrás visto, hay un tópico /cmd_vel.<br>\n",
    "Ahora debes averiguar el tipo de mensajes que usa el tópico /cmd_vel.<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rostopic info /cmd_vel  <br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br>\n",
    "\n",
    "Comprueba que funciona bien publicando diferentes valores:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rostopic pub /cmd_vel <span style=\"color:white\">tipo_de_mensaje_de_cmd_vel</span> [TAB][TAB]<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br>\n",
    "\n",
    "\n",
    "Cuando tengas toda la información, estás listo para crear la clase Python.<br>\n",
    "Crea un fichero python en el directorio src del paquete que acabas de crear \"my_sphero_topics\".<br>\n",
    "Este fichero sólo tiene que tener la clase, pero también un método de comprobar que la clase funciona.\n",
    "\n",
    "Aquí tienes un ejemplo de como podrías hacerlo:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from geometry_msgs.msg import Twist\n",
    "\n",
    "\n",
    "class CmdVelPub(object):\n",
    "    def __init__(self):\n",
    "        self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)\n",
    "        self._twist_object = Twist()\n",
    "        self.linearspeed = 0.2\n",
    "        self.angularspeed = 0.5\n",
    "        \n",
    "    def move_robot(self, direction):\n",
    "        if direction == \"forwards\":\n",
    "            self._twist_object.linear.x = self.linearspeed\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        elif direction == \"right\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = self.angularspeed\n",
    "        elif direction == \"left\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = -self.angularspeed\n",
    "        elif direction == \"backwards\":\n",
    "            self._twist_object.linear.x = -self.linearspeed\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        elif direction == \"stop\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        else:\n",
    "            pass\n",
    "        \n",
    "        self._cmd_vel_pub.publish(self._twist_object)\n",
    "\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('cmd_vel__publisher_node')\n",
    "    cmd_publisher_object = CmdVelPub()\n",
    "    \n",
    "    rate = rospy.Rate(1)\n",
    "    \n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # funciona mejor que rospy.is_shut_down()\n",
    "        global ctrl_c\n",
    "        global twist_object\n",
    "        global pub\n",
    "        \n",
    "        rospy.loginfo(\"shutdown time!\")\n",
    "        \n",
    "        ctrl_c = True\n",
    "        cmd_publisher_object.move_robot(direction=\"stop\")\n",
    "    \n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        cmd_publisher_object.move_robot(direction=\"forwards\")\n",
    "        rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">3. Crea Dos Subrcibers que extraen los datos que necesitas de Odometría y del IMU.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para obtener los datos de Odometría y del IMU, necesitas leer los tópicos adecuados. Trata de encontrarlos por ti mismoescribiendo:<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rostopic list  <br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br>\n",
    "\n",
    "Los has encontrado? Qué tipo de mensajes usan? Son los mismos que listados a continuación?<br>\n",
    "<span style=\"color: blue\">/sphero/imu/data, tipo = sensor_msgs/Imu</span><br>\n",
    "<span style=\"color: blue\">/odom, tipo = nav_msgs/Odometry</span><br>\n",
    "\n",
    "Están publicando? Cómo son los datos?<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rostopic echo <span style=\"color: white\">nombre_del_topico</span><br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br>\n",
    "\n",
    "Cuando tengas la información, estás listo para crear las clases python para cada uno.<br>\n",
    "Crea dos ficheros python distintos en el directorio src del paquete que acabas de crear \"my_sphero_topics\".<br>\n",
    "Los ficheros deben tener no sólo la clase, pero algun método para testear los objetos de la clase.\n",
    "\n",
    "<span style=\"color: red\">Recuerda que debes mover el Sphero para ver algunos cambios tanto en /odom como en /sphero/imu/data. Usa el programa creado previamente para moverlo</span>\n",
    "\n",
    "Aquí tienes un ejemplo de cómo podrías hacer esto:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from nav_msgs.msg import Odometry\n",
    "\n",
    "class OdomTopicReader(object):\n",
    "    def __init__(self, topic_name = '/odom'):\n",
    "        self._topic_name = topic_name\n",
    "        self._sub = rospy.Subscriber(self._topic_name, Odometry, self.topic_callback)\n",
    "        self._odomdata = Odometry()\n",
    "    \n",
    "    def topic_callback(self, msg):\n",
    "        self._odomdata = msg\n",
    "        rospy.logdebug(self._odomdata)\n",
    "    \n",
    "    def get_odomdata(self):\n",
    "        \"\"\"\n",
    "        Retorna los últimos datos de odom\n",
    "\n",
    "        std_msgs/Header header                                                                                                                 \n",
    "          uint32 seq                                                                                                                           \n",
    "          time stamp                                                                                                                           \n",
    "          string frame_id                                                                                                                      \n",
    "        string child_frame_id                                                                                                                  \n",
    "        geometry_msgs/PoseWithCovariance pose                                                                                                  \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",
    "          float64[36] covariance                                                                                                               \n",
    "        geometry_msgs/TwistWithCovariance twist                                                                                                \n",
    "          geometry_msgs/Twist twist                                                                                                            \n",
    "            geometry_msgs/Vector3 linear                                                                                                       \n",
    "              float64 x                                                                                                                        \n",
    "              float64 y                                                                                                                        \n",
    "              float64 z                                                                                                                        \n",
    "            geometry_msgs/Vector3 angular                                                                                                      \n",
    "              float64 x                                                                                                                        \n",
    "              float64 y                                                                                                                        \n",
    "              float64 z                                                                                                                        \n",
    "          float64[36] covariance                                                                                                               \n",
    "        \n",
    "        \"\"\"\n",
    "        return self._odomdata\n",
    "    \n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('odom_topic_subscriber', log_level=rospy.INFO)\n",
    "    odom_reader_object = OdomTopicReader()\n",
    "    rospy.loginfo(odom_reader_object.get_odomdata())\n",
    "    rate = rospy.Rate(0.5)\n",
    "    \n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # funciona mejor que rospy.is_shut_down()\n",
    "        global ctrl_c\n",
    "        print \"shutdown time!\"\n",
    "        ctrl_c = True\n",
    "\n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        data = odom_reader_object.get_odomdata()\n",
    "        rospy.loginfo(data)\n",
    "        rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "El tópico /sphero/imu/data se hará exactamente de la misma manera. Así que pruébalo tu mismo."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step2\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Paso 2: Usa tópicos a través de Servicios"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ahora necesitas ir un paso más allá. En lugar de tener propiamente un subscriber/publisher, necesitas crear un servicio que lea de los tópicos.<br>\n",
    "Tienes que hacer lo siguiente:<br>\n",
    "<ol>\n",
    "    <li>Crea un servicio que cuando lo llames te diga si el robot se ha chocado o no, mediante los datos del imu. También tiene que dar alguna información extra cómo en que dirección moverse ahora que ha chocado.</li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">1. Crea un servicio que cuando lo llames te diga si el robot se ha chocado o no. También te dice en qué dirección moverte si se ha chocado.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lo primero para crear tu propio servicio, es determinar qué datos necesitas:\n",
    "<ol>\n",
    "<li>qué datos de entrada necesitas ( <span style=\"color:blue\">request</span> )</li>\n",
    "<li>qué tipo de datos quieres que el servicio devuelva ( <span style=\"color:blue\">response</span> )</li>\n",
    "</ol>\n",
    "\n",
    "Ahora has de buscar un mensaje de servicio ya hecho en el sistema. Puedes encontrarlos en los paquetes <span style=\"color:blue\">std_srvs</span> o <span style=\"color:blue\">rospy_tutorials</span>. También puedes encontrar otros mensajes de servicios creados en paquetes no estándares. Ten en cuenta que usando paquetes qué pueden no estar instalados en el sistema ROS o de paquetes externos no es lo recomendado. En este caso, es mejor generar tus propios mensajes de servicios y usar los tipos básicos de mensajes de <i>std_msgs</i>.<br>\n",
    "Siempre es preferible usar mensajes ya hechos, simplemente porque es más rápdido y no tienes que lidiar con la compilación."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "En este caso necesitas un mensaje de servicio que tenga la siguiente estructura:<br>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "# request, Empty porque no se necesitan datos\n",
    "---\n",
    "#response\n",
    "bool movement_successfull\n",
    "string extra_data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "En este caso dispònes de un mensaje de servicio que tiene esta estructura exacta.<br>\n",
    "Está en el paquete <span style=\"color: blue\">std_srv</span> y se llama <span style=\"color: blue\">Trigger.srv</span>. Esto no es una coincidencia, porque esta estructura de servicio es muy útil porque normalmente pides a un servicio que te dé información, sin proveer ningún input."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Así que sólo tienes que crear un servicio que use el mensaje Trigger.srv, que lee del tópico imu y te dice si has chocado o no. También te dirá basandose en los datos del imu, en qué dirección moverte ahora.<br>\n",
    "Este es un ejemplo de cómo podrías hacerlo:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">1.1 Modifica el subscriber del tópico /sphero/imu/data</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Primero debes modificar el subscriber de /sphero/imu/data para que sea capaz de decirte en qué dirección ha sido el choque. Aquí tienes una manera de cómo podrías hacerlo:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: red; color: white\"> Cuando necesites usar un objeto de otro fichero python, tiene que estar en el mismo paquete. Usar módulos python de otros paquetes no es tan fácil como parece.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from sensor_msgs.msg import Imu\n",
    "\n",
    "\n",
    "class ImuTopicReader(object):\n",
    "    def __init__(self, topic_name = '/sphero/imu/data'):\n",
    "        self._topic_name = topic_name\n",
    "        self._sub = rospy.Subscriber(self._topic_name, Imu, self.topic_callback)\n",
    "        self._imudata = Imu()\n",
    "        self._threshhold = 7.00\n",
    "    def topic_callback(self, msg):\n",
    "        self._imudata = msg\n",
    "        rospy.logdebug(self._imudata)\n",
    "    \n",
    "    def get_imudata(self):\n",
    "        \"\"\"\n",
    "        Retorna los últimos datos del imu\n",
    "\n",
    "        std_msgs/Header header                                                                                                          \n",
    "          uint32 seq                                                                                                                    \n",
    "          time stamp                                                                                                                    \n",
    "          string frame_id                                                                                                               \n",
    "        geometry_msgs/Quaternion orientation                                                                                            \n",
    "          float64 x                                                                                                                     \n",
    "          float64 y                                                                                                                     \n",
    "          float64 z                                                                                                                     \n",
    "          float64 w                                                                                                                     \n",
    "        float64[9] orientation_covariance                                                                                               \n",
    "        geometry_msgs/Vector3 angular_velocity                                                                                          \n",
    "          float64 x                                                                                                                     \n",
    "          float64 y                                                                                                                     \n",
    "          float64 z                                                                                                                     \n",
    "        float64[9] angular_velocity_covariance                                                                                          \n",
    "        geometry_msgs/Vector3 linear_acceleration                                                                                       \n",
    "          float64 x                                                                                                                     \n",
    "          float64 y                                                                                                                     \n",
    "          float64 z                                                                                                                     \n",
    "        float64[9] linear_acceleration_covariance                                                                                                              \n",
    "        \n",
    "        \"\"\"\n",
    "        return self._imudata\n",
    "    \n",
    "    def four_sector_detection(self):\n",
    "        \"\"\"\n",
    "        Detecta en qué dirección hay un obstáculo que ha provocado que el robot choque\n",
    "        Basándose en los datos del imu\n",
    "        Ejes:\n",
    "         ^y\n",
    "         |\n",
    "        zO-->x\n",
    "        \n",
    "        \"\"\"\n",
    "        x_accel = self._imudata.linear_acceleration.x\n",
    "        y_accel = self._imudata.linear_acceleration.y\n",
    "        z_accel = self._imudata.linear_acceleration.z\n",
    "        \n",
    "        \n",
    "        axis_list = [x_accel, y_accel, z_accel]\n",
    "        \n",
    "        max_axis_index = axis_list.index(max(axis_list))\n",
    "        positive = axis_list[max_axis_index] >= 0\n",
    "        significative_value = axis_list[max_axis_index] > self._threshhold\n",
    "        \n",
    "        \n",
    "        if significative_value:\n",
    "            if max_axis_index == 0:\n",
    "                # El ganador está en el eje x, por lo que es un choque lateral izquierda/derecha\n",
    "                rospy.logwarn(\"[X=\"+str(x_accel))\n",
    "                rospy.loginfo(\"Y=\"+str(y_accel)+\", Z=\"+str(z_accel)+\"]\")\n",
    "                if positive:\n",
    "                    message = \"right\"\n",
    "                else:\n",
    "                    message = \"left\"\n",
    "            \n",
    "            elif max_axis_index == 1:\n",
    "                # El ganador está en el eje Y, por lo que es un choque frontal/traser\n",
    "                rospy.logwarn(\"[Y=\"+str(y_accel))\n",
    "                rospy.loginfo(\"X=\"+str(x_accel)+\", Z=\"+str(z_accel)+\"]\")\n",
    "                if positive:\n",
    "                    message = \"front\"\n",
    "                else:\n",
    "                    message = \"back\"\n",
    "            elif max_axis_index == 2:\n",
    "                # El eje Z es el ganador, por lo que ha sido un choque que lo ha hecho saltar\n",
    "                rospy.logwarn(\"[Z=\"+str(z_accel))\n",
    "                rospy.loginfo(\"X=\"+str(x_accel)+\", Y=\"+str(y_accel)+\"]\")\n",
    "                \n",
    "                if positive:\n",
    "                    message = \"up\"\n",
    "                else:\n",
    "                    message = \"down\"\n",
    "            else:\n",
    "                message = \"unknown_direction\"\n",
    "        else:\n",
    "            rospy.loginfo(\"X=\"+str(x_accel)+\"Y=\"+str(y_accel)+\", Z=\"+str(z_accel)+\"]\")\n",
    "            message = \"nothing\"\n",
    "        \n",
    "        return self.convert_to_dict(message)\n",
    "        \n",
    "    def convert_to_dict(self, message):\n",
    "        \"\"\"\n",
    "        Convierte el mensaje dado a un diccionario diciendo en qué dirección ha detectado algo\n",
    "        \"\"\"\n",
    "        detect_dict = {}\n",
    "        # We consider that when there is a big Z axis component there has been a very bif front crash\n",
    "        detection_dict = {\"front\":(message==\"front\" or message==\"up\" or message==\"down\"),\n",
    "                          \"left\":message==\"left\",\n",
    "                          \"right\":message==\"right\",\n",
    "                          \"back\":message==\"back\"}\n",
    "        return detection_dict\n",
    "        \n",
    "    \n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('imu_topic_subscriber', log_level=rospy.INFO)\n",
    "    imu_reader_object = ImuTopicReader()\n",
    "    rospy.loginfo(imu_reader_object.get_imudata())\n",
    "    rate = rospy.Rate(0.5)\n",
    "    \n",
    "    ctrl_c = False\n",
    "    def shutdownhook():\n",
    "        # funciona mejor que rospy.is_shut_down()\n",
    "        global ctrl_c\n",
    "        print \"shutdown time!\"\n",
    "        ctrl_c = True\n",
    "\n",
    "    rospy.on_shutdown(shutdownhook)\n",
    "    \n",
    "    while not ctrl_c:\n",
    "        data = imu_reader_object.get_imudata()\n",
    "        rospy.loginfo(data)\n",
    "        rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Una manera de evaluar el límite que consideras choque, o cómo se deben colocar los ejes es ejecutando el código, y entonces controlando el Sphero cn:<br>\n",
    "<span style=\"color:blue\">roslaunch turtlebot_teleop keyboard_teleop.launch</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">1.2 Crea el Servicio y el Client que te dice si ha habido un choque y en qué dirección moverse</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Por qué necesitas crear también un Client? Bueno, no necesitas esto ara que el core de tu programa funcione, pero es altamente recomendable porque te permite testear el Server. Así que una manera de hacerlo podría ser la siguiente. Ten en cuenta que no hay sólo una manera de hacer las cosas, y este es sólo un ejemplo básico:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from std_srvs.srv import Trigger, TriggerResponse\n",
    "from imu_topic_susbcriber import ImuTopicReader\n",
    "import time\n",
    "\n",
    "\n",
    "class CrashDirectionService(object):\n",
    "    def __init__(self, srv_name='/crash_direction_service'):\n",
    "        self._srv_name = srv_name\n",
    "        self._imu_reader_object = ImuTopicReader()\n",
    "        self.detection_dict = {\"front\":False, \"left\":False, \"right\":False, \"back\":False}\n",
    "        self._my_service = rospy.Service(self._srv_name, Trigger , self.srv_callback)\n",
    "\n",
    "    def srv_callback(self, request):\n",
    "        self.detection_dict = self._imu_reader_object.four_sector_detection()\n",
    "        \n",
    "        message = self.direction_to_move()\n",
    "        \n",
    "        rospy.logdebug(\"[LEFT=\"+str(self.detection_dict[\"left\"])+\", FRONT=\"+str(self.detection_dict[\"front\"])+\", RIGHT=\"+str(self.detection_dict[\"right\"])+\"]\"+\", BACK=\"+str(self.detection_dict[\"back\"])+\"]\")\n",
    "        rospy.logdebug(\"DIRECTION ==>\"+message)\n",
    "        \n",
    "        response = TriggerResponse()\n",
    "        \"\"\"\n",
    "        ---                                                                                                 \n",
    "        bool success   # indica si ha chocado                                      \n",
    "        string message # Dirección\n",
    "        \"\"\"\n",
    "        response.success = self.has_crashed()\n",
    "        response.message = message\n",
    "        \n",
    "        return response\n",
    "\n",
    "    \n",
    "    def has_crashed(self):\n",
    "        for key, value in self.detection_dict.iteritems():\n",
    "            if value:\n",
    "                return True\n",
    "        \n",
    "        return False\n",
    "    \n",
    "    def direction_to_move(self):\n",
    "\n",
    "        if not self.detection_dict[\"front\"]:\n",
    "            message = \"forwards\"\n",
    "        \n",
    "        else:\n",
    "            if not self.detection_dict[\"back\"]:\n",
    "                    message = \"backwards\"\n",
    "            else:\n",
    "                if not self.detection_dict[\"left\"]:\n",
    "                    message = \"left\"\n",
    "                else:\n",
    "                    if not self.detection_dict[\"right\"]:\n",
    "                        message = \"right\"\n",
    "                    else:\n",
    "                        message = \"un_stuck\"\n",
    "\n",
    "        \n",
    "        return message\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    rospy.init_node('crash_direction_service_server', log_level=rospy.INFO) \n",
    "    dir_serv_object = CrashDirectionService()\n",
    "    rospy.spin() # mantain the service open."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Y el Client:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from std_srvs.srv import Trigger, TriggerRequest\n",
    "import sys \n",
    "\n",
    "rospy.init_node('crash_direction_service_client') # inicializa un nodo ROS con el nombre service_client\n",
    "service_name = \"/crash_direction_service\"\n",
    "rospy.wait_for_service(service_name) # espera a que el servicio /gazebo/delete_model esté corriendo\n",
    "direction_service = rospy.ServiceProxy(service_name, Trigger) # crea la conexión al servicio\n",
    "request_object = TriggerRequest()\n",
    "\n",
    "rate = rospy.Rate(5)\n",
    "\n",
    "ctrl_c = False\n",
    "def shutdownhook():\n",
    "    # funciona mejor que rospy.is_shut_down()\n",
    "    global ctrl_c\n",
    "    print \"shutdown time!\"\n",
    "    ctrl_c = True\n",
    "\n",
    "rospy.on_shutdown(shutdownhook)\n",
    "\n",
    "while not ctrl_c:\n",
    "    result = direction_service(request_object) # envía a través de la conexión el nombre del model a borrar\n",
    "    \"\"\"\n",
    "    ---                             \n",
    "    bool success   # indicate succes\n",
    "    string message # informational, \n",
    "    \"\"\"\n",
    "    if result.success:\n",
    "        rospy.logwarn(\"Success ==\"+str(result.success)) # imprime el resultado dado por el servicio llamado\n",
    "        rospy.logwarn(\"Direction To Go==\"+str(result.message)) # imprime el resultado dado por el servicio llamado\n",
    "    else:\n",
    "        rospy.loginfo(\"Success ==\"+str(result.success)) # imprime el result dado por el servicio llamado\n",
    "        rospy.loginfo(\"Direction To Go==\"+str(result.message)) # imprime el result dado por el servicio llamado\n",
    "    rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step3\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Paso 3: Usa tópicos a través de Actions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ahora necesitas crear una action que cuando la llames empiece a guardar datos de odometría y comprueba si el robot ha salido del laberinto.<br>\n",
    "Para conseguir esto, tienes que medir la distancia del punto incial a la posición actual. Si la distancia es mayor que el tamaño del laberinto, estás fuera. Algo más elaborado sería considerando también el vector, y por tanto saber si has salido correctamente o simplemente has saltado por un muro.<br>\n",
    "La Action debe también pararse en el caso de que haya pasado un cierto período de tiempo sin salir del laberinto."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">1. Crea un action server que finaliza cuando detecta que el robot ha salido del laberinto o ha pasado un cierto periodo de tiempo. Usa sólo el subscriber del tópico /odom.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Así que lo primero que has de pensar es qué tipo de mensaje necesitas para que la action funcione como debe.<br>\n",
    "Necesitas llamar a esta action, sin ningún input.<br>\n",
    "No necesita dar un feedback porque lo único que importa es que retorne los datos necesarios para poder evaluar la distancia.\n",
    "Necesita devolver los datos necesarios para calcular la distancia posteriormente.<br>\n",
    "Así que el mensaje de la action debería ser algo así:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "#goal, empty                \n",
    "---                             \n",
    "#result, Odometry array             \n",
    "nav_msgs/Odometry[] result_odom_array                \n",
    "---                             \n",
    "#feedback, empty"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Este mensaje es, como puedes ver, personalizado. Por lo tanto deberás compilar un mensaje de action.<br>\n",
    "Los pasos para hacer esto son los siguientes:<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: green\">Paso 1</span>: Crea un nuevo paquete llamado <i>my_sphero_actions</i>, para colocar todos los action servers y el mensaje.<br>\n",
    "<span style=\"color: green\">Paso 2</span>: Crea un directorio <i>action</i>, y dentro, un mensaje de action llamado <span style=\"color: blue\">record_odom.action</span>.<br>\n",
    "<span style=\"color: green\">Paso 3</span>: Modifica todo lo necesario en los ficheros package.xml y CMakeLists.txt, para poder compilar correctamente el mensaje. Estos son los 2 ficheros tal y como deberían ser si la única dependencia externa de mi paquete <i>my_sphero_actions</i> fuese <i>nav_msgs:</i><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: orange\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">CMakeLists.txt</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "cmake_minimum_required(VERSION 2.8.3)\n",
    "project(my_sphero_actions)\n",
    "\n",
    "## Find catkin macros and libraries\n",
    "## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n",
    "## is used, also find other catkin packages\n",
    "## Here go the packages needed to COMPILE the messages of topic, services and actions.\n",
    "## in package.xml you have to state them as build\n",
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  actionlib_msgs\n",
    "  nav_msgs\n",
    ")\n",
    "\n",
    "## Generate actions in the 'action' folder\n",
    "add_action_files(\n",
    "   FILES\n",
    "   record_odom.action\n",
    ")\n",
    "\n",
    "## Generate added messages and services with any dependencies listed here\n",
    "generate_messages(\n",
    "   DEPENDENCIES\n",
    "   actionlib_msgs\n",
    "   nav_msgs\n",
    " )\n",
    "\n",
    "###################################\n",
    "## catkin specific configuration ##\n",
    "###################################\n",
    "## Declare things to be passed to dependent projects\n",
    "## State here all the packages that will be needed by someone that executes something from your package\n",
    "## All the packages stated here must be in the package.xml as run_depend\n",
    "catkin_package(\n",
    "  CATKIN_DEPENDS rospy nav_msgs\n",
    ")\n",
    "\n",
    "###########\n",
    "## Build ##\n",
    "###########\n",
    "\n",
    "## Specify additional locations of header files\n",
    "## Your package locations should be listed before other locations\n",
    "# include_directories(include)\n",
    "include_directories(\n",
    "  ${catkin_INCLUDE_DIRS}\n",
    ")\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: orange\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">package.xml</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<?xml version=\"1.0\"?>\n",
    "<package>\n",
    "  <name>my_sphero_actions</name>\n",
    "  <version>0.0.0</version>\n",
    "  <description>The my_sphero_actions package</description>\n",
    "\n",
    "  <maintainer email=\"user@todo.todo\">user</maintainer>\n",
    "  <license>TODO</license>\n",
    "\n",
    "  <buildtool_depend>catkin</buildtool_depend>\n",
    "  <build_depend>actionlib_msgs</build_depend>\n",
    "  <build_depend>nav_msgs</build_depend>\n",
    "  \n",
    "  <run_depend>rospy</run_depend>\n",
    "  <run_depend>nav_msgs</run_depend>\n",
    "\n",
    "  <export>\n",
    "  </export>\n",
    "</package>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Cuando creas que ya lo tienes, ejecuta los siguientes comandos en el WebShell:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "roscd;cd ..<br>\n",
    "catkin_make<br>\n",
    "source deve/setup.bash<br>\n",
    "rosmsg list | grep record_odom\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br><br>\n",
    "\n",
    "\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Salida del WebShell #1</p><br>\n",
    "my_sphero_actions/record_odomAction<br>\n",
    "my_sphero_actions/record_odomActionFeedback<br>\n",
    "my_sphero_actions/record_odomActionGoal<br>\n",
    "my_sphero_actions/record_odomActionResult<br>\n",
    "my_sphero_actions/record_odomFeedback<br>\n",
    "my_sphero_actions/record_odomGoal<br>\n",
    "my_sphero_actions/record_odomResult<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br><br><br><br><br>\n",
    "\n",
    "<span style=\"color: green\">rosmsg list | grep record_odom</span>: Este comando lista todos los rosmsgs definidos en el sistema y en el directorio <i>devel</i> y filtrandolos mediante el comando <i>grep</i> para solo mostrar aquellos con el nombre <i>record_odom</i>. En el directorio devel se guardan todos los mensajes compilados de tu paquete.<br>\n",
    "Esta es una de las mejores maneras de saber que tu mensaje de action ha sido correctamente compilado y es accesible por todo tu sistema ROS."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: green\">Paso 4</span>: Crea el action server. Este action server tiene que empezar a grabar el tópico /odom y detenerse cuando haya pasado un cierto periodo de tiempo o la distancia desplazada alcance un cierto valor.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: red; color: white\"> Cuando necesites usar un objeto de otro fichero python, tiene que estar en el mismo paquete. Usar módulos python de otros paquetes no es tan fácil como parece.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Copia tu subscriber del tópico <span style=\"color: blue\">/odom</span> a tu paquete <span style=\"color: blue\">my_sphero_actions</span>. De esta manera tu server podrá usarlo fácilmente.<br>\n",
    "Ahora crea el action server y el action client. Lo mismo aquí que con servicios: No necesitas el client pero es muy útil para testear el server y también te da un template de cómo usarlo más tarde en el programa core. Este es un ejemplo de cómo podría hacerse:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: red; color: white\">ImportError: No module named my_sphero_actions.msg</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br>\n",
    "\n",
    "Este error es bastante común cuando generas tus propios mensajes. No encuentra <i>my_sphero_actions.msg</i>. Pero tú lo has compilado y haciendo el rosmsg list retorna la salida correcta. Entonces por qué? Por que con tal de que tu programa sea capaz de de encontrar los mensajes tienes que compilaros y ejecutar el comando \"source devel/setup.bash\". Este script configura no sólo el entorno ROS pero también otros sistemas relacionados con la generación de mensajes.<br>\n",
    "Así que con tal de hacer que tus mensajes siempre funcionen haz lo siguiente:<br>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "catkin_make<br>\n",
    "source devel/setup.bash\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ahora estás listo para trabajar en tu action server. Este es un ejemplo de cómo podría hacerse:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import actionlib\n",
    "from my_sphero_actions.msg import record_odomFeedback, record_odomResult, record_odomAction\n",
    "from nav_msgs.msg import Odometry\n",
    "from odom_topic_subscriber import OdomTopicReader\n",
    "from odometry_analysis import check_if_out_maze\n",
    "\n",
    "\n",
    "class RecordOdomClass(object):\n",
    "    \n",
    "    def __init__(self, goal_distance):\n",
    "        \"\"\"\n",
    "        Inicia un action Server. Para testear que se ha creado correctamente, haz rostopic list y busca /rec_odom_as/...\n",
    "        Cuando lo lances, ten en cuenta que deberías haber hecho:\n",
    "        $catkin_make\n",
    "        $source devel/setup.bash\n",
    "        \"\"\"\n",
    "        # crea el action server\n",
    "        self._as = actionlib.SimpleActionServer(\"/rec_odom_as\", record_odomAction, self.goal_callback, False)\n",
    "        self._as.start()\n",
    "        \n",
    "        # Crea un objeto que lee del tópico Odom\n",
    "        self._odom_reader_object = OdomTopicReader()\n",
    "        \n",
    "        # crea mensajes que son usados para publicar el result\n",
    "        self._result   = record_odomResult()\n",
    "        \n",
    "        self._seconds_recording = 120\n",
    "        self._goal_distance = goal_distance\n",
    "    \n",
    "    def goal_callback(self, goal):\n",
    "    \n",
    "        success = True\n",
    "        rate = rospy.Rate(1)\n",
    "        \n",
    "        for i in range(self._seconds_recording):\n",
    "            rospy.loginfo(\"Recording Odom index=\"+str(i))\n",
    "            # comprueba que el preempt (cancelación) no ha sido solicitado por el action client\n",
    "            if self._as.is_preempt_requested():\n",
    "                rospy.logdebug('The goal has been cancelled/preempted')\n",
    "                # la siguiente linía, setea el client en estado de preempt (goal cancelado)\n",
    "                self._as.set_preempted()\n",
    "                success = False\n",
    "                # terminamos el bucle\n",
    "                break\n",
    "            \n",
    "            else:# construye el siguiente mensaje de feedback a enviar\n",
    "                if not self.reached_distance_goal():\n",
    "                    rospy.logdebug('Reading Odometry...')\n",
    "                    self._result.result_odom_array.append(self._odom_reader_object.get_odomdata())\n",
    "                else:\n",
    "                    rospy.logwarn('Reached distance Goal')\n",
    "                    # terminamos el bucle\n",
    "                    break\n",
    "            rate.sleep()\n",
    "        \n",
    "        # en este punto, o bien el goal ha sido conseguido (success==true)\n",
    "        # o el client ha hecho preempt del goal (success==false)\n",
    "        # si es success, publicamos el result final\n",
    "        # si no es success, no publicamos nada en el result\n",
    "        if success:\n",
    "            self._as.set_succeeded(self._result)\n",
    "            # limpia la variable Result\n",
    "        \n",
    "        self.clean_variables()\n",
    "    \n",
    "    def clean_variables(self):\n",
    "        \"\"\"\n",
    "        Cleans variables for the next call\n",
    "        \"\"\"\n",
    "        self._result   = record_odomResult()\n",
    "    \n",
    "    def reached_distance_goal(self):\n",
    "        \"\"\"\n",
    "        Returns True if the distance moved from the first instance of recording till now has reached the self._goal_distance\n",
    "        \"\"\"\n",
    "        return check_if_out_maze(self._goal_distance, self._result.result_odom_array)\n",
    "    \n",
    "    \n",
    "      \n",
    "if __name__ == '__main__':\n",
    "  rospy.init_node('record_odom_action_server_node')\n",
    "  RecordOdomClass(goal_distance=2.0)\n",
    "  rospy.spin()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Aquí hay un ejemplo de cómo la odometría podría ser procesada para saber si el Sphero ha salido del laberinto:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from nav_msgs.msg import Odometry\n",
    "from geometry_msgs.msg import Vector3\n",
    "import math \n",
    "\n",
    "\n",
    "class OdometryAnalysis(object):\n",
    "    def __init__(self):\n",
    "        pass\n",
    "    \n",
    "    def get_distance_moved(self, odmetry_data_list):\n",
    "        \n",
    "        distance = None\n",
    "        \n",
    "        if len(odmetry_data_list) >= 2 :\n",
    "            start_odom = odmetry_data_list[0]\n",
    "            end_odom = odmetry_data_list[len(odmetry_data_list)-1]\n",
    "            \n",
    "            start_position = start_odom.pose.pose.position\n",
    "            end_position = end_odom.pose.pose.position\n",
    "            \n",
    "            rospy.loginfo(\"start_position ==>\"+str(start_position))\n",
    "            rospy.loginfo(\"end_position ==>\"+str(end_position))\n",
    "            \n",
    "            \n",
    "            distance_vector = self.create_vector(start_position, end_position)\n",
    "            rospy.loginfo(\"Distance Vector ==>\"+str(distance_vector))\n",
    "            \n",
    "            distance = self.calculate_legth_vector(distance_vector)\n",
    "            rospy.loginfo(\"Distance ==>\"+str(distance))\n",
    "        \n",
    "        else:\n",
    "            rospy.logerr(\"Odom array doesnt have the minimum number of elements = \"+str(len(odmetry_data_list)))\n",
    "        \n",
    "        return distance\n",
    "        \n",
    "    def create_vector(self, p1, p2):\n",
    "        \n",
    "        distance_vector = Vector3()\n",
    "        distance_vector.x = p2.x - p1.x\n",
    "        distance_vector.y = p2.y - p1.y\n",
    "        distance_vector.z = p2.z - p1.z\n",
    "        \n",
    "        return distance_vector\n",
    "    \n",
    "    def calculate_legth_vector(self,vector):\n",
    "        \n",
    "        length = math.sqrt(math.pow(vector.x,2)+math.pow(vector.y,2)+math.pow(vector.z,2))\n",
    "        return length\n",
    "\n",
    "\n",
    "def check_if_out_maze(goal_distance, odom_result_array):\n",
    "    odom_analysis_object = OdometryAnalysis()\n",
    "    distance = odom_analysis_object.get_distance_moved(odom_result_array)\n",
    "    rospy.loginfo(\"Distance Moved=\"+str(distance))\n",
    "    # Para salir consideraremos que cada cuadrado en el suelo mide alrededor de 0.5m, así que para considerar que ha salido\n",
    "    # la distancia debe ser sqrt (6*5 + 5*4) = 7.8\n",
    "    return distance > goal_distance"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Y aquí el ejemplo del client:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import time\n",
    "import actionlib\n",
    "from my_sphero_actions.msg import record_odomGoal, record_odomFeedback, record_odomResult, record_odomAction\n",
    "from nav_msgs.msg import Odometry\n",
    "\n",
    "\n",
    "# definición de la función feedback_callback. Será llamada cuando se reciba feedback\n",
    "# del action server\n",
    "# simplemente imprime un mensaje indicaando que un nuevo mensaje ha llegado\n",
    "def feedback_callback(feedback):\n",
    "    rospy.loginfo(\"Rec Odom Feedback feedback ==>\"+str(feedback))\n",
    "\n",
    "\n",
    "def count_seconds(seconds):\n",
    "    for i in range(seconds):\n",
    "        rospy.loginfo(\"Seconds Passed =>\"+str(i))\n",
    "        time.sleep(1)\n",
    "\n",
    "# inicializa el nodo del action client\n",
    "rospy.init_node('record_odom_action_client_node')\n",
    "\n",
    "# crea la conexión al action server action server\n",
    "client = actionlib.SimpleActionClient('/rec_odom_as', record_odomAction)\n",
    "\n",
    "rate = rospy.Rate(1)\n",
    "\n",
    "# espera hasta que el action server está corriendo\n",
    "rospy.loginfo('Waiting for action Server')\n",
    "client.wait_for_server()\n",
    "rospy.loginfo('Action Server Found...')\n",
    "\n",
    "# crea un goal para enviarlo al action server\n",
    "goal = record_odomGoal()\n",
    "\n",
    "# envía el goal al action server, especificando a qué función de feedback\n",
    "# llamar cuando se reciba feedback\n",
    "client.send_goal(goal, feedback_cb=feedback_callback)\n",
    "\n",
    "\n",
    "\n",
    "# simple_state será 1 si es active, y 2 cuando haya terminado. Es una variable, mejor usa una función como get_state.\n",
    "# state = client.simple_state\n",
    "# state_result dará el FINAL STATE. Será 1 si Active, 2 si NO ERROR, 3 si hay Warning, y 3 si ERROR\n",
    "state_result = client.get_state()\n",
    "\n",
    "\"\"\"\n",
    "class SimpleGoalState:\n",
    "    PENDING = 0\n",
    "    ACTIVE = 1\n",
    "    DONE = 2\n",
    "    WARN = 3\n",
    "    ERROR = 4\n",
    "\n",
    "\"\"\"\n",
    "\n",
    "rospy.loginfo(\"state_result: \"+str(state_result))\n",
    "\n",
    "while state_result < 2:\n",
    "    rospy.loginfo(\"Waiting to finish: \")\n",
    "    rate.sleep()\n",
    "    state_result = client.get_state()\n",
    "    rospy.loginfo(\"state_result: \"+str(state_result))\n",
    "    \n",
    "\n",
    "state_result = client.get_state()\n",
    "rospy.loginfo(\"[Result] State: \"+str(state_result))\n",
    "if state_result == 4:\n",
    "    rospy.logerr(\"Something went wrong in the Server Side\")\n",
    "if state_result == 3:\n",
    "    rospy.logwarn(\"There is a warning in the Server Side\")\n",
    "\n",
    "rospy.loginfo(\"[Result] State: \"+str(client.get_result()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lánzalo:<br>\n",
    "rosrun my_sphero_actions rec_odom_action_server.py<br>\n",
    "\n",
    "Comprueba que está funcionando:<br>\n",
    "rosnode list | grep record_odom_action_server_node<br>\n",
    "rostopic list | grep rec_odom_as\n",
    "\n",
    "Lanza el client para testear que realmente funciona.\n",
    "<ol>\n",
    "<li>Déjalo corriendo hasta que el tiempo termine. Debería retornar en el lado del client todas las lecturas de /odom hasta el momento</li>\n",
    "<li>Fuerza que la distancia del goal sea alcanzada. Usa el roslaunch turtlebot_teleop keyboard_teleop.launch. Debería retornar las lecturas del tópico /odom guardadas hasta entonces.</li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step4\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### Paso 4: Crea un programa principal para gestionarlo todo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finalmente tienes todas las herramientas necesarias para crear el programa principal que haga lo siguiente:<br>\n",
    "<ol>\n",
    "<li>Llama a un servicio que dice si se ha chocado y en qué dirección debería moverse.</li>\n",
    "<li>Mueve el Sphero basándose en la response del servicio</li>\n",
    "<li>Comprueba si ha salido del laberinto o el tiempo se ha agotado. Si es así, el programa termina.</li>\n",
    "</ol>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: green\">Paso 1</span>: Crea un paquete llamado <span style=\"color: blue\">my_sphero_main</span>, que contiene el programa principal y los ficheros python que necesita. Quizás necesites copiar algunos ficheros de otros paquetes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: green\">Paso 2</span>: Crea un fichero launch que lanza el action_server, el service server y el programa principal. Prueba primero con los tags <i>node</i> y si funciona genera otros ficheros launch para el action_server y el service_server y usa el tag <i>include</i>. Aquí hay un ejemplo de como podría hacerse:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "<launch>\n",
    "\n",
    "  <node pkg =\"my_sphero_actions\"\n",
    "        type=\"rec_odom_action_server.py\"\n",
    "        name=\"record_odom_action_server_node\"\n",
    "        output=\"screen\">\n",
    "  </node>\n",
    "  \n",
    "  <node pkg =\"my_sphero_services\"\n",
    "        type=\"direction_service_server.py\"\n",
    "        name=\"crash_direction_service_server\"\n",
    "        output=\"screen\">\n",
    "  </node>\n",
    "\n",
    "  <node pkg =\"my_sphero_main\"\n",
    "        type=\"sphero_main.py\"\n",
    "        name=\"sphero_main_node\"\n",
    "        output=\"screen\">\n",
    "  </node>\n",
    "\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color: green\">Paso 3</span>: Crea un programa principal. Usa todos los datos e información que has extraído de los clients del servicio y la action para reutilizar tanto código como sea psobille. Aquí hay un ejemplo de cómo podría hacerse:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import actionlib\n",
    "from std_srvs.srv import Trigger, TriggerRequest\n",
    "from exam_action_rec_odom.msg import record_odomGoal, record_odomFeedback, record_odomResult, record_odomAction\n",
    "from cmd_vel_publisher import CmdVelPub\n",
    "from odometry_analysis import OdometryAnalysis\n",
    "from odometry_analysis import check_if_out_maze\n",
    "\n",
    "class ControlSphero(object):\n",
    "    def __init__(self, goal_distance):\n",
    "        self._goal_distance = goal_distance\n",
    "        self.init_direction_service_client()\n",
    "        self.init_rec_odom_action_client()\n",
    "        self.init_move_sphero_publisher()\n",
    "        \n",
    "    def init_direction_service_client(self, service_name = \"/crash_direction_service\"):\n",
    "        rospy.loginfo('Waiting for Service Server')\n",
    "        rospy.wait_for_service(service_name) # espera a que el servicio /gazebo/delete_model esté corriendo\n",
    "        rospy.loginfo('Service Server Found...')\n",
    "        self._direction_service = rospy.ServiceProxy(service_name, Trigger) # crea la conexión al servicio\n",
    "        self._request_object = TriggerRequest()\n",
    "        \n",
    "    def make_direction_request(self):\n",
    "        \n",
    "        result = self._direction_service(self._request_object)\n",
    "        return result.message\n",
    "    \n",
    "    def init_rec_odom_action_client(self):\n",
    "        self._rec_odom_action_client = actionlib.SimpleActionClient('/rec_odom_as', record_odomAction)\n",
    "        # espera hasta que el action server está corriendo\n",
    "        rospy.loginfo('Waiting for action Server')\n",
    "        self._rec_odom_action_client.wait_for_server()\n",
    "        rospy.loginfo('Action Server Found...')\n",
    "        self._rec_odom_action_goal = record_odomGoal()\n",
    "    \n",
    "    def send_goal_to_rec_odom_action_server(self):\n",
    "        self._rec_odom_action_client.send_goal(self._rec_odom_action_goal, feedback_cb=self.rec_odom_feedback_callback)\n",
    "        \n",
    "    def rec_odom_feedback_callback(self,feedback):\n",
    "        rospy.loginfo(\"Rec Odom Feedback feedback ==>\"+str(feedback))\n",
    "    \n",
    "    def rec_odom_finished(self):\n",
    "        \n",
    "        has_finished = ( self._rec_odom_action_client.get_state() >= 2 )\n",
    "        \n",
    "        return has_finished\n",
    "    \n",
    "    def get_result_rec_odom(self):\n",
    "        return self._rec_odom_action_client.get_result()\n",
    "        \n",
    "    def init_move_sphero_publisher(self):\n",
    "        self._cmdvelpub_object = CmdVelPub()\n",
    "\n",
    "    def move_sphero(self, direction):\n",
    "        self._cmdvelpub_object.move_robot(direction)\n",
    "\n",
    "    def got_out_maze(self, odom_result_array):\n",
    "        return check_if_out_maze(self._goal_distance, odom_result_array)\n",
    "\n",
    "rospy.init_node(\"sphero_main_node\", log_level=rospy.INFO)\n",
    "controlsphero_object = ControlSphero(goal_distance=2.0)\n",
    "rate = rospy.Rate(10)\n",
    "\n",
    "controlsphero_object.send_goal_to_rec_odom_action_server()\n",
    "\n",
    "while not controlsphero_object.rec_odom_finished():\n",
    "    direction_to_go = controlsphero_object.make_direction_request()\n",
    "    rospy.loginfo(direction_to_go)\n",
    "    controlsphero_object.move_sphero(direction_to_go)\n",
    "    rate.sleep()\n",
    "\n",
    "\n",
    "odom_result = controlsphero_object.get_result_rec_odom()\n",
    "odom_result_array = odom_result.result_odom_array\n",
    "\n",
    "if controlsphero_object.got_out_maze(odom_result_array):\n",
    "    rospy.loginfo(\"Out of Maze\")\n",
    "else:\n",
    "    rospy.loginfo(\"In Maze\")\n",
    "\n",
    "rospy.loginfo(\"Sphero Maze test Finished\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"stepExtra\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Paso EXTRA: Cómo usar módulos Python de diferentes paquetes"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Como debes haber visto, si querías usar algo declarado en un módulo python que estba en otro paquete, no podías. Esto es porque en ROS, importar módulos Python de otros paquetes no es tan fácil como parece."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para aprender cómo hacerlo, sigue el siguiente ejemplo:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Digamos que tienes un paquete llamado <i style=\"color: green\">common_pkg</i> y otro llamado <i style=\"color: green\">testing</i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Crea estos 2 paquetes</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Cuando los tengas, edítalos para sean capaces de usar una clase Python definida en <i style=\"color: blue\">common_pkg</i>, en un programa Python que esté en <i style=\"color: blue\">testing</i>. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### common_pkg"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Para preparar el paquete <i>common_pkg</i> para que cualquiera pueda usar los ficheros Python que hay en él, sigue estos pasos:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>\n",
    "Ejecuta los siguientes comandos en el WebShell #1:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "roscd; cd ..\n",
    "cd src/common_pkg<br>\n",
    "mkdir src/common_dir<br>\n",
    "touch src/common_dir/__init__.py<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br>\n",
    "Esto generará un directorio extra llamado <i style=\"color: blue\">common_dir</i>, y dentro un fichero especial Python para ser capaces de encontrarlo a través de un Python llamado <i style=\"color: blue\">__init__.py</i>,.\n",
    "\n",
    "</li>\n",
    "\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>\n",
    "Cuando termines creas tu fichero Python dentro del directorio <i style=\"color: blue\">common_dir</i>.<br>\n",
    "A continuación tienes un ejemplo de qué podrías poner en él:\n",
    "</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"common_things\">Programa Python {common_things.py}: common_things.py</p><br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "from geometry_msgs.msg import Twist\n",
    "import time\n",
    "\n",
    "def cool(name):\n",
    "    print('Cool ' + name)\n",
    "\n",
    "class CmdVelPub(object):\n",
    "    def __init__(self):\n",
    "        self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)\n",
    "        self._twist_object = Twist()\n",
    "        self.linearspeed = 0.2\n",
    "        self.angularspeed = 0.5\n",
    "        \n",
    "    def move_robot(self, direction):\n",
    "        if direction == \"forwards\":\n",
    "            self._twist_object.linear.x = self.linearspeed\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        elif direction == \"right\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = self.angularspeed\n",
    "        elif direction == \"left\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = -self.angularspeed\n",
    "        elif direction == \"backwards\":\n",
    "            self._twist_object.linear.x = -self.linearspeed\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        elif direction == \"stop\":\n",
    "            self._twist_object.linear.x = 0.0\n",
    "            self._twist_object.angular.z = 0.0\n",
    "        else:\n",
    "            pass\n",
    "        \n",
    "        self._cmd_vel_pub.publish(self._twist_object)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"common_things\">FIN del Programa Python {common_things.py}: common_things.py</p><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>\n",
    "Ve a la raíz del paquete <i style=\"color: green\">common_pkg</i><br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "roscd common_pkg<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br>\n",
    "</li>\n",
    "\n",
    "\n",
    "<li>Crea un fichero llamado <i style=\"color: green\">setup.py</i>, con el siguiente contenido:</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "## ! No INVOQUES ESTE FICHERO setup.py MANUALMENTE, USA CATKIN\n",
    "\n",
    "from distutils.core import setup\n",
    "from catkin_pkg.python_setup import generate_distutils_setup\n",
    "\n",
    "# recupera los valores de package.xml\n",
    "setup_args = generate_distutils_setup(\n",
    "    packages=['common_dir'],\n",
    "    package_dir={'': 'src'},\n",
    ")\n",
    "\n",
    "setup(**setup_args)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>Ahora edita el fichero <i style=\"color: green\">CMakeLists.txt</i>, y descomenta la siguiente línea:</li>\n",
    "<ul>"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "## Descomenta si el paquete tiene un setup.py\n",
    "catkin_python_setup()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>\n",
    "Compila y comprueba que no haya errores:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "roscd; cd ..<br>\n",
    "catkin_make<br>\n",
    "source devel/setup.bash<br>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br>\n",
    "</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Testeo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Una vez hayas preparado el paquete common_pkg, simplemente tienes que importar los elementos de la siguiente manera:<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from common_dir.common_things import cool, CmdVelPub"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Date cuenta que haces el import con el nombre del <i style=\"color: green\">directorio dentro del directorio src de tu paquete</i>.<br>\n",
    "nombre del paquete = <i style=\"color: green\">common_pkg</i><br>\n",
    "nombre del directorio del que importas = <i style=\"color: green\">common_dir</i><br>\n",
    "Esto es muy importante porque puede llevar a errores."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Aquí tienes un ejemplo del fichero Python en el directorio src del paquete testing:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"test_import\">Programa Python {test_import.py}: test_import.py</p><br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "import rospy\n",
    "import time\n",
    "\n",
    "from common_dir.common_things import cool, CmdVelPub\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    cool('TheConstruct')\n",
    "    \n",
    "    stop_time = 1\n",
    "    move_time = 3\n",
    "    \n",
    "    rospy.init_node('test_import', log_level=rospy.INFO)\n",
    "    \n",
    "    move_object = CmdVelPub()\n",
    "    rospy.loginfo(\"Starting...\")\n",
    "    move_object.move_robot(direction=\"stop\")\n",
    "    time.sleep(stop_time)\n",
    "    \n",
    "    rospy.loginfo(\"Forwards...\")\n",
    "    move_object.move_robot(direction=\"forwards\")\n",
    "    time.sleep(move_time)\n",
    "    \n",
    "    rospy.loginfo(\"Stopping...\")\n",
    "    move_object.move_robot(direction=\"stop\")\n",
    "    time.sleep(stop_time)\n",
    "    \n",
    "    rospy.loginfo(\"Forwards...\")\n",
    "    move_object.move_robot(direction=\"backwards\")\n",
    "    time.sleep(move_time)\n",
    "    \n",
    "    rospy.loginfo(\"Stopping...\")\n",
    "    move_object.move_robot(direction=\"stop\")\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"test_import\">FIN del Programa Python {test_import.py}: test_import.py</p><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<ul>\n",
    "<li>\n",
    "Ahora ejecuta el test_import.py:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: #FFFFFF\">Ejecuta en el WebShell #1</p><br>\n",
    "rosrun testing test_import.py\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br><br><br><br><br><br>\n",
    "</li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Deberías ver el robot moviéndose."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Conclusión"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ahora trata de optimizar tu sistema. Juega con los ratios y la estrategia de deteccion de obstáculos. Juega con el movimiento y la\"IA\" empleada para decidir qué hacer.<br>\n",
    "Especialmente, debes entender perfectamente este proyecto porque el <span style=\"color:red\">examen será muy similar</span>."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "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.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}