Course Project.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS IN 5 DAYS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Course Project"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Sphero Robot",
    "image": true,
    "name": "sphero_real_sim",
    "width": "12cm"
   },
   "source": [
    "<img src=\"img/sphero_real_sim.jpg\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Estimated time to completion:</b> 10 hours\n",
    "<br><br>\n",
    "<b>What will you learn in this unit?</b>\n",
    "\n",
    "* Practice everything you learn through the course\n",
    "* Put together everything you learn into a big project\n",
    "* Create a main program"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Win the Sphero Race!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this project, you will have to make a <a href=\"http://www.sphero.com/sphero\">Sphero robot</a> move through a maze faster than the other students. The fastest one will win a prize.<br>\n",
    "\n",
    "For this goal, you will have to apply all the things that you have learned throughout the course. It is really important that you complete this race because all of the structures that you create for this project will be asked about in our <span class=\"ign_red\"> <b>official exam</b></span>.\n",
    "<br><br>\n",
    "<b>Note: Our offical exam is only available for on-site and virtual classes at present.</b>\n",
    "\n",
    "Get the robot out of the maze as fast as possible and you will get the prize. The ideal would be for Spheros to come out cleanly, but it is possible that your robot may collide with the maze. You can use the collision detection to get data and help you in your strategy to get out.\n",
    "\n",
    "Basically, in this project, you will have to:<br>\n",
    "\n",
    "1. Apply all the theory that is given in the course\n",
    "2. Decide on a strategy to solve the problem\n",
    "3. Implement this strategy in the simulation environment\n",
    "4. Do as many tests as required in the simulation environment until it works\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To achieve success in this project, please follow the 5 steps below that will provide clear instructions, and even solutions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, remember to:\n",
    "\n",
    "* Create your packages and code in the simulation environment as you have been doing throughout the course.\n",
    "* Use the consoles to gather information about the status of the simulated robot.\n",
    "* Use the IDE to create your programs and execute them through the consoles, observing the results on the simulation screen. You can use other consoles to watch calls to topics, services, or action servers.\n",
    "* Everything that you create in this unit will be automatically saved in your space. You can come back to this unit at any time and continue with your work from where you left off.\n",
    "* <span class=\"ign_green\">Every time you need to reset the position of the robot, just press the restart button in the simulation window.</span>\n",
    "* Use the debugging tools to try to find what is not working and why (for instance, the rviz tool is very useful for this purpose).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>One final note</b>: Because the program that you create should work with a real robot, if needed, you can't move the Sphero in a closed loop. This is because in reality, the circuit could be different, the robot could not be as responsive, there might be errors in the readings, and so on. So in this project, you should create a program that can cope with all of those uncertainties."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What does Sphero Provide to Program It?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So the main question is, what can you do with Sphero from a ROS programming point of view? Which sensors and actuators does Sphero provide that will allow you to do the maze test?<br>\n",
    "\n",
    "Good question! Sphero provides the following sensors and actuators:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sensors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* **IMU sensor**: Sphero has an <a href=\"https://en.wikipedia.org/wiki/Inertial_measurement_unit\">Inertial Measurement Unit</a> (IMU), which provides information about acceleration and orientation. The value of the sensor is provided through the <span class=\"ign_green\"><i>/sphero/imu/data3</i></span> topic.\n",
    "* **Odometry**: The odometry of the robot can be accessed through the <span class=\"ign_green\"><i>/odom</i></span> topic."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Actuators"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* **Speed**: You can send speed commands to move the robot through the <span class=\"ign_green\"><i>/cmd_vel</i></span> topic.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that you know the relevant topics of the robot, it is your job to figure out the types of messages, and how to use them, in order to make the robot do what you want it to do."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Ideas to Start Working On"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is a list of things that you can start with. You do not have to follow them. They are provided just in case you don't know where to start."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "1. Start watching some of the messages the sensor topics are publishing. Try to get an idea about the information that they are providing. Move the robot in the simulation and see how those messages change their values. It is very important that you understand how changes in the robot produce changes in the topics.\n",
    "2. Try to move the robot by sending messages to the <i>/cmd_vel</i> (either through the console or through python programs).\n",
    "3. Observe how the topic messages change when the robot hits an obstacle.\n",
    "4. Is the odometry trustworthy? Can you move the robot the exact amount, even when it collides with something?\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Steps You Should Cover"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These are the steps that you should follow throughout the duration of the project. These steps will ensure that you have practised and created all of the structures that will be asked about in the final exam of this course. If you perform all of the steps mentioned here, you will find completion of the exam to be achievable.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "1. <a href=\"#step1\">Step 1: Read and Write Topics (Dedicate 2 hours)</a>\n",
    "2. <a href=\"#step2\">Step 2: Use topics through Services (Dedicate 3 hours)</a>\n",
    "3. <a href=\"#step3\">Step 3: Use topics through Actions (Dedicate 4 hours)</a>\n",
    "4. <a href=\"#step4\">Step 4: Create a main program to manage everything (Dedicate 1 hour)</a>\n",
    "5. <a href=\"#stepExtra\">EXTRA Step: How to use python modules from different packages (Not required, included here for information purposes only)</a>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>NOTE 1</b>: We provide the solutions for each step. Do not watch unless you are really stuck."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>NOTE 2</b>: The fifth step may not be required, but we have found that some students organize their code in such a way that they need this step. We have provided it here just in case you need it, but you don't have to use it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step1\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 1: Read and Write Topics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This step requires 3 actions:<br>\n",
    "\n",
    "1. Create a package called <span class=\"ign_green\"><i>my_sphero_topics</i></span> that will contain all of the programs related to the topics\n",
    "2. Create a <span class=\"ign_green\">topic publisher</span> that allows you to move the Sphero.\n",
    "3. Create <span class=\"ign_green\">two topic subscribers</span> that extract the data that you need from Odometry and the IMU.\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's get start on them!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">1. Create package my_sphero_topics, with <i>rospy</i> as dependency.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">2. Create the Topic Publisher to move Sphero.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move Sphero, you need to publish in the topic **<i>/cmd_vel</i>**.\n",
    "<br><br>\n",
    "It is important that you always encapsulate your topic subscribers and publishers inside classes. This will allow you to store values and manage the publishing callbacks easily.<br>\n",
    "\n",
    "First, you have to see if there is a topic like <i>/cmd_vel</i> running. \n",
    "<br><br>\n",
    "<b>Note</b>: It will not always be this simple. With real robots, you will need to access the code to see what the name of the topic is that moves the robot, or even use <i>rostopic info /name_of_topic</i> to know which one it could be.\n",
    "\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic list"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, there is a /cmd_vel topic.<br>\n",
    "You, then, have to extract the type of message that /cmd_vel uses.<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic info /cmd_vel"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Test it to see if it works py publishing different values:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic pub /cmd_vel message_type_of_cmd_vel [TAB][TAB]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have the information, you are ready to create the python class.<br>\n",
    "Create a python file in the src folder of the package that you just created, \"my_sphero_topics\".<br>\n",
    "This file has to have not only the class, but also a way of testing that the class works.\n",
    "\n",
    "Here is an example of a way in which this could be done (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "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",
    "        # works better than the 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 class=\"transparent\">3. Create Two Topic Subscribers that extract the data that you need from the Odometry and the IMU.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To get the Odometry data and the IMU data, you need to read the appropriate topics. Try to find them on your own by typing:<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic list"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Have you found them? What type of message do they use? Are they the same as the ones listed here?<br>\n",
    "\n",
    "<span style=\"color:blue;\">/sphero/imu/data3</span>, type = <span style=\"color:green;\">sensor_msgs/Imu</span><br>\n",
    "<span style=\"color:blue;\">/odom</span>, type = <span style=\"color:green;\">nav_msgs/Odometry</span><br>\n",
    "\n",
    "Are they publishing? What's the data like?<br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic echo name_of_topic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have the information, you are ready to create the python classes for each one.\n",
    "<br><br>\n",
    "Create two different python files in the src folder of the package that you just created, <b>\"my_sphero_topics.\"</b><br>\n",
    "These files have to have not only the class, but also a way of testing the class objects.\n",
    "\n",
    "<span class=\"ign_red\">Remember that you need to move the Sphero to see the changes in both the <b>/odom</b> and the <b>/sphero/imu/data3</b> topics. Use the previously created program to move it.</span>\n",
    "\n",
    "Here is an example of how this could be done (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "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",
    "        Returns the newest odom data\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",
    "        # works better than the 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": [
    "The **<i>/sphero/imu/data3</i>** topic will be done in exactly the same way. So try it yourself!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step2\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 2: Use Topics through Services"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you need to take it a step further. Instead of having a topic subscriber/publisher on its own, you need to create a service that reads from topics.<br>\n",
    "You have to do the following:\n",
    "<br><br>\n",
    "Create a service that, when called, tells you if the robot has crashed or not, through the imu data. It also has to return some extra information, such as in what direction to move now that it has crashed.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We divided this into 3 tasks:\n",
    "\n",
    "1. Determine the data\n",
    "2. Modify the subscriber\n",
    "3. Create the Server and Client\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">1. Determine data</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "1. Determine what data input you need ( <span class=\"ign_blue\"><b>request</b></span> )\n",
    "2. Determine what data you want the service to return ( <span class=\"ign_blue\"><b>response</b></span> )\n",
    "\n",
    "\n",
    "Then, you have to search for a service message that is already done in the system. You can find them in the <span class=\"ign_blue\">std_srvs</span> or <span class=\"ign_blue\">rospy_tutorials</span> package. You can also find other service messages created in non-standard packages. Bear in mind that using packages that might not be installed in ROS systems, or third party packages, is not recommened at this point. In this case, it is better to just generate your own service messages and use the <i>std_msgs</i> message types.<br>\n",
    "It is always preferable to use messages that are already done, just because it's faster and you don't have to deal with compilation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this case, you need a service message with the following structure (<b>DO NOT watch unless you are stuck</b>):<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# request, Empty because no data is needed\n",
    "---\n",
    "#response\n",
    "bool movement_successfull\n",
    "string extra_data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this case, you have a service message that has this exact structure.<br>\n",
    "It's in the <span class=\"ign_blue\"><b>std_srvs</b> package</span> and is called <span class=\"ign_blue\">Trigger.srv</span>. This is no coincidence. This service structure is very useful because normally you would ask a service to give you data, without providing any input."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So you just need to create a service that uses this <b>Trigger.srv</b>, that reads from the imu topic and tells you if you have crashed or not. It will also tell you based in the imu crash data, in which direction to move to now.\n",
    "<br><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">2. Modify the /sphero/imu/data3 topic susbcriber</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you have to modify the <b>/sphero/imu/data3</b> topic susbcriber for it to be able to tell you from which direction the crash occurred. Here is how it could be done (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: red; color: white\"> When you need to use an object from another python file, it has to be in the same package. Using python modules from other packages is not as easy as it may seem.</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/data3'):\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",
    "        Returns the newest imu data\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",
    "        Detects in which four directions there is an obstacle that made the robot crash\n",
    "        Based on the imu data\n",
    "        Axis:\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",
    "                # Winner is in the x axis, therefore its a side crash left/right\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",
    "                # Winner is the Y axis, therefore its a forn/back crash\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",
    "                # Z Axixs is the winner, therefore its a crash that made it jump\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",
    "        Converts the fiven message to a dictionary telling in which direction there is a detection\n",
    "        \"\"\"\n",
    "        detect_dict = {}\n",
    "        # We consider that when there is a big Z axis component there has been a very big 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",
    "        # works better than the 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": [
    "A way of evaluating the threshold that you consider relevant crash, or how the axis may be placed is by executing the code, and then controlling the Sphero with the:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch sphero_gazebo keyboard_teleop.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">3. Create the Server and client that tells you if there was a crash and what direction to move</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Why do you need to create a client,too? Well, this is not needed for your core program to run, but its highly recommended because it allows you to test the server. So one way that it could be done is the following. Bear in mind that there is not only one way of doing things, and this is just a basic example (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>The Server</b>:"
   ]
  },
  {
   "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   # indicate if crashed                                       \n",
    "        string message # Direction\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": [
    "<b>The Client</b>:"
   ]
  },
  {
   "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') # initialise a ROS node with the name service_client\n",
    "service_name = \"/crash_direction_service\"\n",
    "rospy.wait_for_service(service_name) # wait for the service client /gazebo/delete_model to be running\n",
    "direction_service = rospy.ServiceProxy(service_name, Trigger) # create the connection to the service\n",
    "request_object = TriggerRequest()\n",
    "\n",
    "rate = rospy.Rate(5)\n",
    "\n",
    "ctrl_c = False\n",
    "def shutdownhook():\n",
    "    # works better than the 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) # send through the connection the name of the object to be deleted by the service\n",
    "    \"\"\"\n",
    "    ---                             \n",
    "    bool success   # indicate succes\n",
    "    string message # informational, \n",
    "    \"\"\"\n",
    "    if result.success:\n",
    "        rospy.logwarn(\"Success ==\"+str(result.success)) # print the result given by the service called\n",
    "        rospy.logwarn(\"Direction To Go==\"+str(result.message)) # print the result given by the service called\n",
    "    else:\n",
    "        rospy.loginfo(\"Success ==\"+str(result.success)) # print the result given by the service called\n",
    "        rospy.loginfo(\"Direction To Go==\"+str(result.message)) # print the result given by the service called\n",
    "    rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step3\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 3: Use Topics through Actions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you need to create an action that, when called, starts to save odometry data and checks if the robot has exited the maze.<br>\n",
    "To accomplish that, you have to measure the distance from the starting point to the current position. If the distance is bigger that the maze distance, you are out. A more elaborate one would be to consider the vector, and therefore know if you exited correctly or you just jumped over a wall.\n",
    "<br><br>\n",
    "The Action should also stop if a certain amount of time has passed without the robot exiting the maze. The task is then:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Create an action server that finishes when it has detected that the robot has exited the maze, or has been working for a certain period of time. Use only the /odom topic subscriber.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We divided it into 3 subtasks:\n",
    "\n",
    "1. Define the action message\n",
    "2. Create the action server, action client, and algorithm to exit the maze\n",
    "3. Test it\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">1. The first thing you have to think about is what kind of message you need for this action to work as intended.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You need to call this action, without any input.\n",
    "<br><br>\n",
    "It doesn't need to give feedback because the only thing that matters is that it returns the needed data to evaluate the distance.\n",
    "It needs to return the data used to calculate the distance, for post-completion calculations.\n",
    "<br><br>\n",
    "So the action message should look something like this (<b>DO NOT watch this unless you are stuck</b>):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#goal, empty                \n",
    "---                             \n",
    "#result, Odometry array             \n",
    "nav_msgs/Odometry[] result_odom_array                \n",
    "---                             \n",
    "#feedback, empty"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This message is, as you can see, customized. Therefore, you will need to compile the package.<br>\n",
    "The steps to do this are as follows:<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span class=\"ign_green\">Step 1</span>: Create a new package called <i>my_sphero_actions</i> to store all of the action servers and the message.<br>\n",
    "<span class=\"ign_green\">Step 2</span>: Create an <i>action</i> directory, and within, an action message called <span class=\"ign_blue\">record_odom.action</span>.<br>\n",
    "<span class=\"ign_green\">Step 3</span>: Make all of the needed changes to the package.xml and CMakeLists.txt files in order to correctly compile the action message. These are the two files as they should be, if the only external dependency of your <i>my_sphero_actions package</i> is:<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: orange\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">CMakeLists.txt</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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 class=\"transparent\">package.xml</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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": [
    "Once you think you have it, execute the following commands in the WebShell:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roscd;cd ..\n",
    "catkin_make\n",
    "source deve/setup.bash\n",
    "rosmsg list | grep record_odom"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Output in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "my_sphero_actions/record_odomAction\n",
    "my_sphero_actions/record_odomActionFeedback\n",
    "my_sphero_actions/record_odomActionGoal\n",
    "my_sphero_actions/record_odomActionResult\n",
    "my_sphero_actions/record_odomFeedback\n",
    "my_sphero_actions/record_odomGoal\n",
    "my_sphero_actions/record_odomResult"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span class=\"ign_green\"><b>rosmsg list | grep record_odom</b></span>: This command is listing all of the rosmsgs defined in the system and in your <i>devel</i> folder, and only filtering with <i>grep</i> command the ones with the name <i>record_odom</i>. All of the messages compiled from your packages are stored in the devel folder.<br>\n",
    "This is one of the best ways to know that your action message has been correctly compiled and is accesible for all of your ROS system."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">2. Create the action server, the client, and the program to compute out of the maze.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This action server has to start recording the <i>/odom</i> topic and stop when a certain time period has passed or the distance moved reaches a certain value."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left\">\n",
    "<tr>\n",
    "<th>\n",
    "<p style=\"background: red; color: white\"> When you need to use an object from another python file, it has to be in the same package. Using python modules from other packages is not as easy as it may seem.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Move your <span class=\"ign_blue\"><i>/odom</i></span> topic susbcriber to your <span class=\"ign_blue\"><i>my_sphero_actions package</i></span>. This way, your server can use it easily.<br>\n",
    "Now create the action server and action client. It is the same here as it is in services: you don't need the client, but it's very useful to test the server and it also gives you a template for how to use it later in the core program. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<b>Note</b>: It may happen that when you test it, you get the following error:\n",
    "\n",
    "<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><br>\n",
    "\n",
    "This error is quite common when you generate your own messages. It doesn't find the <i>my_sphero_actions.msg</i>. But you have compiled it and doing the rosmsg list returns the correct output. Then, why? Because in order to allow your program to find the messages, you have to compile them and execute the source devel/setup.bash. This script sets not only the ROS environment, but also other systems related to message generation.<br>\n",
    "So, in order to always make your messages work, do the following:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_make\n",
    "source devel/setup.bash"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you are ready to work on your action server. This is an example of how it could be done (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "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",
    "        It starts an action Server. To test it was created correctly, just rostopic the list and search for /rec_odom_as/...\n",
    "        When launching, bear in mind that you should have:\n",
    "        $catkin_make\n",
    "        $source devel/setup.bash\n",
    "        \"\"\"\n",
    "        # creates the action server\n",
    "        self._as = actionlib.SimpleActionServer(\"/rec_odom_as\", record_odomAction, self.goal_callback, False)\n",
    "        self._as.start()\n",
    "        \n",
    "        # Create an object that reads from the topic Odom\n",
    "        self._odom_reader_object = OdomTopicReader()\n",
    "        \n",
    "        # create messages that are used to publish 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",
    "            # check that the preempt (cancelation) has not been requested by the action client\n",
    "            if self._as.is_preempt_requested():\n",
    "                rospy.logdebug('The goal has been cancelled/preempted')\n",
    "                # the following line sets the client in a preempted state (goal cancelled)\n",
    "                self._as.set_preempted()\n",
    "                success = False\n",
    "                # we end the action loop\n",
    "                break\n",
    "            \n",
    "            else:# builds the next feedback msg to be sent\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",
    "                    # we end the action loop\n",
    "                    break\n",
    "            rate.sleep()\n",
    "        \n",
    "        # at this point, either the goal has been achieved (success==true)\n",
    "        # or the client preempted the goal (success==false)\n",
    "        # If successful, then we publish the final result\n",
    "        # If not successful, we do not publish anything in the result\n",
    "        if success:\n",
    "            self._as.set_succeeded(self._result)\n",
    "            # Clean the Result Variable\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 until 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": [
    "Here is an example of how the odometry could be processed to know if the Sphero has exited the maze:"
   ]
  },
  {
   "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",
    "    # To exit we consider that each square in the floor is around 0.5m, there fore to exit correctly\n",
    "    # distance has to be sqrt (6*5 + 5*4) = 7.8\n",
    "    return distance > goal_distance"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And here is the client example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "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",
    "# definition of the feedback callback. This will be called when feedback\n",
    "# is received from the action server\n",
    "# it just prints a message indicating a new message has been received\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",
    "# initializes the action client node\n",
    "rospy.init_node('record_odom_action_client_node')\n",
    "\n",
    "# create the connection to the action server\n",
    "client = actionlib.SimpleActionClient('/rec_odom_as', record_odomAction)\n",
    "\n",
    "rate = rospy.Rate(1)\n",
    "\n",
    "# waits until the action server is up and running\n",
    "rospy.loginfo('Waiting for action Server')\n",
    "client.wait_for_server()\n",
    "rospy.loginfo('Action Server Found...')\n",
    "\n",
    "# creates a goal to send to the action server\n",
    "goal = record_odomGoal()\n",
    "\n",
    "# sends the goal to the action server, specifying which feedback function\n",
    "# to call when feedback received\n",
    "client.send_goal(goal, feedback_cb=feedback_callback)\n",
    "\n",
    "\n",
    "\n",
    "# simple_state will be 1 if active, and 2 when finished. It's a variable, better use a function like get_state.\n",
    "#state = client.simple_state\n",
    "# state_result will give the FINAL STATE. Will be 1 when Active, and 2 if NO ERROR, 3 If Any Warning, and 3 if 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": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">3. Test it.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Launch it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun my_sphero_actions rec_odom_action_server.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that it's working:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosnode list | grep record_odom_action_server_node"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rostopic list | grep rec_odom_as"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Launch the client to test if it really works.\n",
    "\n",
    "1. Leave it running until the time runs out. It should return to the client side all of the <i>/odom</i> recordings until then.\n",
    "2. Force that the distance goal gets reached. Use the **<i>roslaunch sphero_gazebo keyboard_teleop.launch</i>** to move the robot. It should also return the <i>/odom</i> topics recorded until then."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div id=\"step4\"></div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### Step 4: Create a Main Program to Manage Everything"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, you finally have all the tools needed to create a main program that does the following:<br>\n",
    "\n",
    "1. Calls a service that tells you if it has crashed and in what direction you should move.\n",
    "2. Moves the sphero based on the service response.\n",
    "3. Checks if it has exited the maze or the time given has run out. If so, the program ends.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have divided this into 3 sub-steps:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">1. Create a package called <span class=\"ign_blue\"><i>my_sphero_main</i></span>.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This package has to contain the main program and the python files that it needs. You might need to copy some files from other packages."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">2. Create a launch file that launches the Action Server, the Service Server, and the main program.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try it first with the <i>node</i> tags, and if it works, then generate a launch for the action_server and the service_server and use the <i>include</i> tag. Here is an example of how it could be done (<b>DO NOT watch unless you are stuck</b>):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">3. Create the main program.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Use all of the data and knowledge that you have extracted from the clients of the server and action to reuse as much code as possible. This is an example of how it could be done: (<b>DO NOT watch unless you are stuck</b>)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "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) # wait for the service client /gazebo/delete_model to be running\n",
    "        rospy.loginfo('Service Server Found...')\n",
    "        self._direction_service = rospy.ServiceProxy(service_name, Trigger) # create the connection to the service\n",
    "        self._request_object = TriggerRequest()\n",
    "        \n",
    "    def make_direction_request(self):\n",
    "        \n",
    "        result = self._direction_service(self._request_object) # send the name of the object to be deleted by the service through the connection\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",
    "        # waits until the action server is up and running\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": [
    "### Step 5: How to Use Python Modules from Different Packages"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you might have noticed, if you wanted to use something declared in a python module that was from another package, you had to copy it into your code. This is because in ROS, python module importing from other packages is not as easy as it might seem."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To learn how to do it, go through this example, divided in 4 sub-steps:\n",
    "\n",
    "1. Create packages\n",
    "2. Prepare <i>common_pkg</i>\n",
    "3. Import in <i>testing</i>\n",
    "4. Test everything\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's say that you have a package named <i class=\"ign_green\">common_pkg</i> and another package named <i class=\"ign_green\">testing</i>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">1. Create those 2 packages.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once done, edit both of them to be able to use a python class defined in <i class=\"ign_blue\">common_pkg</i>, in a python program located in the package <i class=\"ign_blue\">testing</i>. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">2. Prepare common_pkg.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To prepare <i>common_pkg</i> so that anyone can use the python files in it, follow these steps:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* Execute the following commands on WebShell #1:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "roscd; cd ..\n",
    "cd src/common_pkg\n",
    "mkdir src/common_dir\n",
    "touch src/common_dir/_\\_\\_init\\_\\_.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This will generate an extra directory named <i class=\"ign_blue\">common_dir</i> inside a special python file, which you are able to find through the python named <i class=\"ign_blue\">__init__.py</i>,."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* Once done, you create your python file inside the <i class=\"ign_blue\">common_dir</i> folder.<br>\n",
    "Here is an example of what you could put in it:\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"common_things\">**Python Program {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\">**END Python Program {common_things.py}: common_things.py**</p><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Go to the root of the <i class=\"ign_green\">common_pkg</i><br>\n",
    "\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roscd common_pkg"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Create a file named <i class=\"ign_green\">setup.py</i>, with the following content:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD\n",
    "\n",
    "from distutils.core import setup\n",
    "from catkin_pkg.python_setup import generate_distutils_setup\n",
    "\n",
    "# fetch values from 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": [
    "\n",
    "* Now edit the <i class=\"ign_green\">CMakeLists.txt</i> file, and uncomment the following line:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "## Uncomment if the package has a setup.py\n",
    "catkin_python_setup()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* Compile and check that there is no error:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roscd; cd ..\n",
    "catkin_make\n",
    "source devel/setup.bash"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">3. Import in testing.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have set up the common_pkg, you just have to import the elements in the following way:<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": [
    "Note that you have imported with the name of the <i class=\"ign_green\">directory inside the src directory of your package</i>.<br>\n",
    "name of the package = <i class=\"ign_green\">common_pkg</i><br>\n",
    "name of the directory and where you import = <i class=\"ign_green\">common_dir</i><br>\n",
    "This is very important because it can lead to errors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So here you have an example of the python file in the src directory of the testing package:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#3B8F10;color:white;\" id=\"test_import\">**Python Program {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\">**END Python Program {test_import.py}: test_import.py**</p><br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">4. Test everything.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now execute the **test_import.py** file:<br>\n",
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>\n",
    "<br><br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun testing test_import.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see the robot moving."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Conclusion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now try to optimise your system. Play with the rates and the detection strategy. Play with the movement and the \"AI\" used to decide what to do.<br>\n",
    "You especially need to know this project (except STEP 5) by heart because the <span class=\"ign_red\">exam will be very similar</span>."
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  },
  "latex_metadata": {
   "chapter": "Course Project",
   "chapter_title": "Course Project",
   "course_title": "ROS BASICS IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}