Course_Project_Nav.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ROS Navigation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Course Project"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Navigation Project Environment",
    "image": true,
    "name": "project1",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/project1.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "",
    "image": true,
    "name": "rosject",
    "width": "5cm"
   },
   "source": [
    "<img src=\"img/rosject.png\" width=\"250\" style=\"float:left;\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* ROSJect Link: http://bit.ly/2n5zEOW\n",
    "\n",
    "* Package Name: **summit_xl_gazebo**\n",
    "\n",
    "* Launch File: **main.launch**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**SUMMARY**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: **1'5 hours**<br><br>What will you learn with this unit?\n",
    "\n",
    "  * Practice everything you've learned through the course\n",
    "  * Put together everything that you learned into a big project\n",
    "  * Create a launch file that launches each part of the Navigation Stack\n",
    "  * \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">**END OF SUMMARY**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Navigate the Summit Robot!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this project, you will have to make a <a href=\"http://www.robotnik.eu/mobile-robots/summit-xl/\">Summit robot</a> navigate  around a room autonomously.<br>\n",
    "\n",
    "For this goal, you will have to apply all of the things that you are learning along the course. It's really important that you complete it because all of the structures that you create for this project will be asked about in our <span class=\"ign_red\"> **official exam**</span>.\n",
    "<br><br>\n",
    "**Note: Our offical exam is only available, at present, for on-site or virtual classes.**\n",
    "\n",
    "Basically, in this project, you will have to:<br>\n",
    "\n",
    "  * Apply all of the theory given in the course\n",
    "  * Follow the steps provided below without looking at the provided solutions (unless you get really stuck)\n",
    "  * Make as many tests as required in the simulation environment until it works\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To finish this project successfully, we provide the 5 steps you should follow with clear instructions, and even solutions, below."
   ]
  },
  {
   "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 files 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 the point 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": [
    "## What does Summit provide for programming?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sensors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "  * **Laser sensor**: Summit has a <a href=\"https://en.wikipedia.org/wiki/Inertial_measurement_unit\">Hokuyo Laser,</a> which provides information about the objects that it detects in its range. The value of the sensor is provided through the topic <span class=\"ign_green\"><i>/hokuyo_base/scan</i></span>\n",
    "  * **Odometry**: The odometry of the robot can be accessed through the <span class=\"ign_green\"><i>/odom</i></span> topic \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Actuators"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "  * **Speed**: You can send speed commands to move the robot through the topic <span class=\"ign_green\"><i>/summit_xl_control/cmd_vel</i></span>.\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 assure you that you have practised and created all of the structures asked about in the final exam of this course. If you perform all of the steps mentioned here, you will find the exam passable.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "* <a href=\"#step1\">Step 1: Generate a Map of the environment (Dedicate 2 hours)</a>\n",
    "* <a href=\"#step2\">Step 2: Make the Robot Localize itself in the Map that you've created (Dedicate 2 hours)</a>\n",
    "* <a href=\"#step3\">Step 3: Set Up a Path Planning System (Dedicate 3 hours)</a>\n",
    "* <a href=\"#stepExtra\">Step 4: Create a ROS program that interacts with the Navigation Stack (Dedicate 3 hours)</a>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "**NOTE 1**: We do provide the solution for each step. Do not watch unless you are really stuck."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 1: Generate a Map of the Environment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you've learned in the first 2 chapters of the course, the first thing you need in order to navigate autonomously with a robot is a map of the environment. Where else would this project start?\n",
    "<br><br>\n",
    "Therefore, the first step would not be anything other than creating a map of the environment that we want to navigate.\n",
    "In order to achieve this, we've divided this first step into 7 sub-steps:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Make sure that the Summit robot is publishing it's transform data correctly.\n",
    "* Create a package called **my_summit_mapping** that will contain all of the files related to mapping.\n",
    "* Create a **launch file** that will launch the slam_gmapping node and add the necessary parameters in order to properly configure the slam_gmapping node.\n",
    "* Launch the slam_gmapping node and create a map of the simulated environment.\n",
    "* Save the map that you've just created.\n",
    "* Create a script that automatically saves maps.\n",
    "* Create a **launch file** that will provide the created map to other nodes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's start doing them!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**1. Generate the necessary files in order to visualize the frames tree.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get a file like this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Summit Frame Tree",
    "image": true,
    "name": "summit_frame_tree",
    "width": "15cm"
   },
   "source": [
    "<img src=\"img/summit_frame_tree.png\" width=\"900\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**2. Create a package called <i>my_summit_mapping.</i>**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create the package, adding **rospy** as the only dependency."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**3. Create the launch file for the gmapping node.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the mapping section (Chapater 2) of this course, you've seen how to create a launch file for the slam_gmapping node. You've also seen some of the most important parameters to set. So, in this step, you'll have to create a launch file for the slam_gmapping node and add the parameters that you think you need to set."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you can see a full list of parameters that you can set to configure the slam_gmapping node: http://docs.ros.org/hydro/api/gmapping/html/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example launch file for the gmapping node:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <node pkg=\"gmapping\" type=\"slam_gmapping\" name=\"slam_gmapping\" output=\"screen\">\n",
    "      <!-- simulation remap from=\"scan\" to=\"/hokuyo_laser_topic\"/ -->\n",
    "      <!-- real -->\n",
    "      <!-- remap from=\"scan\" to=\"/scan_filtered\"/ -->\n",
    "      <remap from=\"scan\" to =\"/hokuyo_base/scan\"/>\n",
    "      <!-- param name=\"map_udpate_interval\" value=\"5.0\"/ -->\n",
    "      <param name=\"base_frame\" value=\"base_footprint\"/>\n",
    "      <param name=\"odom_frame\" value=\"odom\"/>\n",
    "      <param name=\"map_udpate_interval\" value=\"5.0\"/>\n",
    "      <param name=\"maxUrange\" value=\"2.0\"/>\n",
    "      <param name=\"sigma\" value=\"0.05\"/>\n",
    "      <param name=\"kernelSize\" value=\"1\"/>\n",
    "      <param name=\"lstep\" value=\"0.05\"/>\n",
    "      <param name=\"astep\" value=\"0.05\"/>\n",
    "      <param name=\"iterations\" value=\"5\"/>\n",
    "      <param name=\"lsigma\" value=\"0.075\"/>\n",
    "      <param name=\"ogain\" value=\"3.0\"/>\n",
    "      <param name=\"lskip\" value=\"0\"/>\n",
    "      <param name=\"srr\" value=\"0.1\"/>\n",
    "      <param name=\"srt\" value=\"0.2\"/>\n",
    "      <param name=\"str\" value=\"0.1\"/>\n",
    "      <param name=\"stt\" value=\"0.2\"/>\n",
    "      <param name=\"linearUpdate\" value=\"0.2\"/>\n",
    "      <param name=\"angularUpdate\" value=\"0.1\"/>\n",
    "      <param name=\"temporalUpdate\" value=\"3.0\"/>\n",
    "      <param name=\"resampleThreshold\" value=\"0.5\"/>\n",
    "      <param name=\"particles\" value=\"100\"/>\n",
    "      <param name=\"xmin\" value=\"-50.0\"/>\n",
    "      <param name=\"ymin\" value=\"-50.0\"/>\n",
    "      <param name=\"xmax\" value=\"50.0\"/>\n",
    "      <param name=\"ymax\" value=\"50.0\"/>\n",
    "      <param name=\"delta\" value=\"0.05\"/>\n",
    "      <param name=\"llsamplerange\" value=\"0.01\"/>\n",
    "      <param name=\"llsamplestep\" value=\"0.01\"/>\n",
    "      <param name=\"lasamplerange\" value=\"0.005\"/>\n",
    "      <param name=\"lasamplestep\" value=\"0.005\"/>\n",
    "    </node>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**4. Launch the node using the launch file that you've just created, and create a map of the environment.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to move the robot around the environment, you can use the keyboard teleop. To launch the keyboard teleop, just execute the following command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roslaunch summit_xl_gazebo keyboard_teleop.launch"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, remember to launch Rviz and add the proper displays in order to visualize the map you are generating. You should get something like this in RViz:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Difference between the real laser length and the length that is actually taken into account to build the map",
    "image": true,
    "name": "max_urange_2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/max_urange_2.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, start moving the robot around the room to create the map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What's happening? Are you having difficulty generating a proper map? Is there anything strange in the mapping process? Do you know what could be the reason behind it?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What's happening here is the following:\n",
    "<br>\n",
    "\n",
    "* Your maxUrange parameter in the launch file of the slam_gmapping node is set to 2. This parameter sets the maximum range of the laser scanner that is used for map building. Since this is a small value, the robot can't get enough data to know where it is, so it may get lost. This will cause some strange issues during the mapping process, such as the robot readjusting its position incorrectly.\n",
    "<br>\n",
    "* In order to solve it, you should set the maxUrange parameter to some higher value, for instance, 7. This way, the robot will be able to get more data from the environemnt, so that it won't get lost. Now, you'll be able to finalize the mapping process correctly.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you modify this parameter and relaunch the slam_gmapping node, you should get something like this in RViz:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Effect of changing the urange parameter value",
    "image": true,
    "name": "max_urange_7",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/max_urange_7.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you will be able to map the environment much more easily."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**5. Save the map.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a directory in your package named **maps**, and save the map files there. Your map image file should look something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Map of the environment",
    "image": true,
    "name": "fetch_map",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_map.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**6. Create a launch file that launches the map_server node.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you've also seen in the course, the map you've just created will be used by other nodes (this means, it'll be used in further steps) in order to perform navigation. \n",
    "<br><br>\n",
    "Therefore, you'll need to create a launch file in order to provide the map. As you know, this is done through the map_server node."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, launch this file and check that it's really providing the map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 2: Make the Robot Localize itself in the Map"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This step has 4 actions for you to do:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Create a package called **my_summit_localization** that will contain all of the files related to localization.\n",
    "* Create a **launch file** that will launch the amcl node and add the necessary parameters in order to properly configure the amcl node.\n",
    "* Launch the node and check that the robot properly localizes itself in the environment.\n",
    "* Create a table with three different spots.\n",
    "* Create a ROS Service to save the different spots into a file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**1. Create a package called my_summit_localization.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create the package adding **rospy** as the only dependency."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**2. Create the launch file for the amcl node.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the localization section (Chapater 3) of this course, you saw how to create a launch file for the amcl node. You've also seen some of the most important parameters to set. So, in this step, you'll have to create a launch file for the amcl node and add the parameters you think you need to set."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you can see a full list of parameters that you can set to configure the amcl node: http://wiki.ros.org/amcl"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that before setting the amcl node parameters, you'll need to load the map created in Step 1. For that, you'll just need to include the launch you've created in Step 1 in the amcl node launch file in order to provide the map to other nodes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**3. Launch the node and check that the Summit robot correctly localizes itself in the map.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Launch the node and check in RViz that the Summit robot correctly localizes itseld in the map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you've done all of the previous steps correctly, you should see something like this in Rviz:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "The robot initial localization",
    "image": true,
    "name": "summit_localization",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_localization.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to check that the localization works fine, move the robot around the environment and check that the particle cloud keeps getting smaller the more you move the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Dispersed particles:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Particles should get closer as the robot moves",
    "image": true,
    "name": "summit_localization2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_localization2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Closer particles:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Most of the particles are together when the localization is correct",
    "image": true,
    "name": "summit_localization3",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_localization3.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**4. Create the spots table.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you've checked that localization is working fine, you'll need to create a table with 3 different spots in the environemnt. For each point, assign a tag (with the name of the spot) alongside its coordinates in the map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These are the 3 spots that you'll have to register into the table:\n",
    "\n",
    "Fetch Room Center (**label: room**):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Robot inside a room",
    "image": true,
    "name": "fetch_room",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/fetch_room.png\" width=\"500\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Facing the Turtle's Poster (**label: turtle**):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Robot facing the poster",
    "image": true,
    "name": "turtle_poster",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/turtle_poster.png\" width=\"500\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Besides the Table (**label: table**):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Robot facing the table",
    "image": true,
    "name": "besides_table",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/besides_table.png\" width=\"500\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can access the coordinates of each position by looking into the topics that the amcl node publishes on. The only data that you really need to save are the position and orientation. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, you have a screenshot of the <i>/amcl_pose</i> topic:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Showing the current pose of the robot on the map",
    "image": true,
    "name": "echo_amcl_pose",
    "width": "16cm"
   },
   "source": [
    "<img src=\"img/echo_amcl_pose.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a file named **spots.yaml** and write in the pose data that you've gotten from the 3 spots."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**5. Create a ROS Service that saves the different spots into a file.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous section, you created a file with the coordinates of the 3 spots. But, the whole process was too cumbersome, don't you think? Why don't we create a ROS Service that does the work for us? Sounds like a good idea, right? Then, let's do it!\n",
    "\n",
    "So, now you'll create a ROS program that will do the following:\n",
    "\n",
    "* It will launch a node named **spot_recorder**.\n",
    "* This node will contain a service server named **/save_spot** that will take a string as input.\n",
    "* When this service is called, it will store the current coordinates of the robot (position and orientation values) with a label that will be the string provided on the service call.\n",
    "* When the <i>end</i> string is provided in the service call, the node will write all of the values stored into a file named **spots.txt**.\n",
    "* Finally, the service will return a message indicating if the file has been saved correctly.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to achieve this, let's divide it into smaller parts."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 1 Service Message"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, you'll have to determine the kind of data you need for your service message.\n",
    "\n",
    "* Determine what input data you need ( <span class=\"ign_blue\">**request**</span> )\n",
    "* Determine what data you want the service to return ( <span class=\"ign_blue\">**response**</span> )\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, see if there is an already-built message in the system that suits your needs. If there isn't, then you'll have to create your own custom message with the data you want."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If this is the case, then do the following. Inside the pacakge you just created, create a new directory named **srv**. Inside this directory, create a file named **MyServiceMessage.srv** that will contain the definition of your service message. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This file could be something like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# request\n",
    "string label\n",
    "---\n",
    "#response\n",
    "bool navigation_successfull\n",
    "string message"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you'll have to modify the **package.xml** and **CMakeLists.txt** files of your package in order to compile the new message. Assuming that the only dependency you added to your package was **rospy**, you should do the following modifications to these files:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In CMakeLists.txt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will have to edit/uncomment four functions inside this file:\n",
    "\n",
    "* find_package()\n",
    "* add_service_files()\n",
    "* generate_messages()\n",
    "* catkin_package()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  rospy\n",
    "  std_msgs\n",
    "  message_generation\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "add_service_files(\n",
    "  FILES\n",
    "  MyCustomServiceMessage.srv\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  std_msgs\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_package(\n",
    "      CATKIN_DEPENDS\n",
    "      rospy\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In package.xml:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add these 2 lines to the file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<build_depend>message_generation</build_depend>\n",
    "<run_depend>message_runtime</run_depend>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you've finshed with the modifications, compile your package and check that your message has been correctly compiled and is ready to use. In order to check this, you can use the command **<i>rossrv show MyServiceMessage</i>**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "catkin_make output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Output while compilating",
    "image": true,
    "name": "catkin_make_output",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/catkin_make_output.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "rossrv show output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Showing the structure of a service message",
    "image": true,
    "name": "service_msg_output",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/service_msg_output.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 2 Service Code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once your message is created, you're ready to use it in your service! So, let's write the code for our service. Inside the src directory of your package, create a file named **spots_to_file.py**. Inside this file, write the code necessary for you service."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 3 Launch file"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a launch file for the node you've just created. You can also launch this node in the same launch file you've created to launch the slam_gmapping node. It's up to you."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 4 Test it"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the keyboard teleop, move the robot to the 3 different spots shown above. At each one of these spots, perform a service call to the service you've just created. In the service call, provide the string with the name that you want to give to each spot.\n",
    "<br><br>\n",
    "For example, when you are at the center of the Fetch Room, call your service like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosservice call /record_spot \"label: fetch_room\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Repeat this for each of the 3 spots. When finished, do one last service call, providing <i>end</i> as the string in order to create the file."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 3: Set Up the Path Planning System"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This step has 5 actions for you to take:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Create a package called **my_summit_path_planning** that will contain all of the files related to Path Planning.\n",
    "* Create a **launch file** that will launch the move_base node.\n",
    "* Create the necessary parameter files in order to properly configure the move_base node.\n",
    "* Create the necessary parameter files in order to properly configure the global and local costmaps.\n",
    "* Create the necessary parameter files in order to properly configure the global and local planners.\n",
    "* Launch the node and check that everything works fine."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**1. Create a package named my_summit_path_planning.**</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 launch file for the move_base node.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the Path Planning section (Chapter 4) of this course, you saw how to create a launch file for the move_base node. So, in this step, you'll have to create a launch file for the move_base node and add the parameters and the parameter files required."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the move_base.launch file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <!--- Run AMCL -->\n",
    "  <include file=\"$(find my_summit_localization)/launch/amcl.launch\" />\n",
    "  <remap from=\"cmd_vel\" to=\"/move_base/cmd_vel\" />\n",
    "\n",
    "  <!-- Run move_base -->\n",
    "  <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/move_base_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/costmap_common_params.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/costmap_common_params.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/local_costmap_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/global_costmap_params_map.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/dwa_local_planner_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_summit_path_planning)/config/global_planner_params.yaml\" command=\"load\" />\n",
    "\n",
    "    <param name=\"base_global_planner\" value=\"navfn/NavfnROS\" />\n",
    "    <param name=\"base_local_planner\" value=\"dwa_local_planner/DWAPlannerROS\" />\n",
    "    <param name=\"controller_frequency\" value=\"5.0\" />\n",
    "    <param name=\"controller_patience\" value=\"15.0\" />\n",
    "  </node>\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 move_base_params.yaml file**</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\">**4. Create the costmap_common_params.yaml, the global_costmap_params.yaml, and the local_costmap_params.yaml files**</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\">**5. Create the navfn_global_planner_params.yaml and dwa_local_planner_params.yaml files**</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\">**6. Launch the node and check that everything works fine**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you think your parameter files are correct, launch the node and test that the path planning works fine. Check that the robot can plan trajectories and navigate to different points in the environment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Room 1:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Example of the Summit planning in the map",
    "image": true,
    "name": "summit_planning1",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_planning1.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Room 2:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Another example of planning",
    "image": true,
    "name": "summit_planning2",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_planning2.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Main Hall:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "caption": "Another example of planning",
    "image": true,
    "name": "summit_planning3",
    "width": "10cm"
   },
   "source": [
    "<img src=\"img/summit_planning3.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 4: Create a ROS program that interacts with the Navigation Stack"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In Step 2 of this project, you created a table with 3 different spots, each one associated with a label. You may have been wondering why you were asked to do this, right? Well, now you'll have the answer!\n",
    "<br><br>\n",
    "In this final step, you will have to create a ROS node that will interact with the navigation stack. This node will contain a service that will take a string as input. This string will indicate one of the labels that you selected in Step 2 for the different spots in the simulation. When this service receives a call with the label, it will get the coordinates (position and orientation) associated with that label, and will send these coordinates to the move_base action server. If it finished correctly, the service will return an \"OK\" message."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Summarizing, you will have to create a node that contains:\n",
    "\n",
    "* A service that will take a string as input. Those strings will be the labels you selected in Step 2 for each one of the spots.\n",
    "* An action client that will send goals to the move_base action server.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to achieve this, follow the next steps:<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**1. Create the package**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create the package called <i>my_summit_navigation</i> that will contain all of the files related to this project. Remember to specify the **rospy** dependency."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**2. Create the service message**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, you'll have to determine the kind of data you need for your message.\n",
    "\n",
    "* Determine what input data you need ( <span class=\"ign_blue\">**request**</span> )\n",
    "* Determine what data you want the service to return ( <span class=\"ign_blue\">**response**</span> )\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, see if there is an already-built message in the system that suits your needs. If there isn't, then you'll have to create your own custom message with the data you want."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If this is the case, then do the following. Inside the package that you just created, create a new directory named **srv**. Inside this directory, create a file named **MyServiceMessage.srv** that will contain the definition of your service message. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This file could be something like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# request\n",
    "string label\n",
    "---\n",
    "#response\n",
    "bool navigation_successfull\n",
    "string message"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you'll have to modify the **package.xml** and **CMakeLists.txt** files of your package in order to compile the new message. Assuming the only dependency you added to your package was **rospy**, you should do the following modifications to these files:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In CMakeLists.txt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will have to edit/uncomment four functions inside this file:\n",
    "\n",
    "* find_package()\n",
    "* add_service_files()\n",
    "* generate_messages()\n",
    "* catkin_package()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  rospy\n",
    "  std_msgs\n",
    "  message_generation\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "add_service_files(\n",
    "  FILES\n",
    "  MyCustomServiceMessage.srv\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  std_msgs\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "catkin_package(\n",
    "      CATKIN_DEPENDS\n",
    "      rospy\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In package.xml:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add these 2 lines to the file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<build_depend>message_generation</build_depend>\n",
    "<run_depend>message_runtime</run_depend>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you've finshed with the modifications, compile your package and check that your message has been correctly compiled and is ready to use. In order to check this, you can use the command **<i>rossrv show MyServiceMessage</i>**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**3. Create the Service Server.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once your message is created, you're ready to use it in your service! So, let's write the code for our service. Inside the src directory of your package, create a file named **get_coordinates_service_server.py**. Inside this file, write the code necessary for your service."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**4. Create the Action Client**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside the src directory, create a file named **send_coordinates_action_client.py** that will contain the code of an action client."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you've finished writting your code, it's time to test it! Call your service by providing one of the labels, and check that the Summit robot navigates to that spot. For example, if your label is \"table,\" type the following into a WebShell:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosservice call /get_coordinates \"label: 'table'\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">**5. Create a launch file that manages everything.**</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you may have noticed in the previous step, you need to have the **move_base** node running in order to make everything work. So, let's create a launch file that starts the **move_base** node alongside with the Service Server you have just created above."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <p style=\"background:red;color:white;\">Solutions</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Please Try to do it by yourself unless you get stuck or need some inspiration. You will learn much more if you fight for each exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Follow this link to open the solutions for the Navigation Project:[Navigation Project Solutions](extra_files/nav_project_solutions.ipynb)"
   ]
  }
 ],
 "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": "6 - Project",
   "chapter_title": "Unit 6. Project",
   "course_title": "ROS NAVIGATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}