Course_SummitXLInstructionManual_Unit_1_old.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 1: Set Indoor Navigation Stack"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first thing you will need for a robot that patroles is the ability to move around without crashing into everything. For this you will learn:<br>\n",
    "* How to move the robot\n",
    "* How to set the navigation stack to be able to send him a location and be able to reach that destination.\n",
    "* How to send it a sequence of waypoints and execute those movements."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Move Summit XL"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this case you have the standard wheel model with skid-steering. In escence this make it a normal vehicle that has manouver much more to get to a location and orientation. In th eproject you will use the omnidirectional wheels model."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move it you just have to publish in the topic **/summit_xl_control/cmd_vel**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Publish through the web shell in the mentioned topic and see if you can make SummitXL move forwards, backwards, left, right and in circles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should have done something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1 to move Haro</p><br>\n",
    "rostopic pub /summit_xl_control/cmd_vel geometry_msgs/Twist [TAB][TAB]\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With this command you can then fill in the structure that appears after hitting TAB key two consecutive times, to publish the message."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1 to move Haro</p><br>\n",
    "rostopic info /summit_xl_control/cmd_vel\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Like this you would get the type of message to publish in that topic."
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "Type: geometry_msgs/Twist\n",
    "Publishers:\n",
    " * /twist_mux (http://ip-172-31-42-0:38506/)\n",
    "Subscribers:\n",
    " * /summit_xl_robot_control (http://ip-172-31-42-0:52042/)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see the type of the message is the classical **geometry_msgs/Twist**. It will move as any differential drive that you have used. That is you move it by only publishing in the **linear.x** and the **angular.z**.<br>\n",
    "Of ocurse this is not the way you will use it. You normally need to move it manually to make tests, so the best way is to create a keyboard teleop system that can move the SummitXL through the keyboard or any other similar device.<br>\n",
    "Here you have an example of launch file that could be used for this matter:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->\n",
    "  <node pkg=\"turtlebot_teleop\" type=\"turtlebot_teleop_key.py\" name=\"turtlebot_teleop_keyboard\"  output=\"screen\">\n",
    "    <param name=\"scale_linear\" value=\"0.5\" type=\"double\"/>\n",
    "    <param name=\"scale_angular\" value=\"1.5\" type=\"double\"/>\n",
    "    <remap from=\"turtlebot_teleop_keyboard/cmd_vel\" to=\"/summit_xl_control/cmd_vel\"/>\n",
    "  </node>\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There two point to talk about here:<br>\n",
    "* turtlebot_teleop_key.py: This is the program used to capture the keys pressed in the terminal and converts them to Twist commands that are then published into the topic **turtlebot_teleop_keyboard/cmd_vel**\n",
    "\n",
    "* remap: This remap is to transfer all the information published in **turtlebot_teleop_keyboard/cmd_vel** in the topic that SummitXL controller is listening to **/summit_xl_control/cmd_vel**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Of ocurse you could publish directly into **/summit_xl_control/cmd_vel**, but then you would have to create your own version of **turtlebot_teleop_key.py**. Thanks to the ROS infraestructure, this way you recicle code, and get work done faster."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a package that you  will use along the rest of this course where you can have all your programs. Use the name **my_sumit_xl_tools** for future refference.<br>\n",
    "Then create your own launch for keyboard teleoperation based on the example and play around with SummitXL. See how it moves and its limitations. This is crucial to know what you can do with your robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Navigate the Summit Robot!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To finish this project successfully, we provide below the 5 steps you should follow with clear instructions and even solutions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What does Summit provide for programming it?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sensors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* <b>Laser sensor</b>: Summit has a <a href=\"https://en.wikipedia.org/wiki/Inertial_measurement_unit\">Hokuyo Laser</a> which provides information about the objects that detects in it's range. Value of the sensor is provided through the topic <span class=\"ign_green\"><i>/hokuyo_base/scan</i></span>\n",
    "* <b>Odometry</b>: 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": [
    "* <b>Speed</b>: you can send speed commands to move the robot through the topic <span class=\"ign_green\"><i>/summit_xl_control/cmd_vel</i></span>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Steps you should cover"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. <a href=\"#step1\">Step 1: Generate a Map of the environment (Dedicate 2 hours)</a>\n",
    "2. <a href=\"#step2\">Step 2: Make the Robot Localize itlsef in the Map you've created (Dedicate 2 hours)</a>\n",
    "3. <a href=\"#step3\">Step 3: Set Up a the Path Planning System (Dedicate 3 hours)</a>\n",
    "4. <a href=\"#stepExtra\">Step 4: Create a ROS program that interacts with the Navigation Stack (Dedicate 3 hours)</a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 1: Generate a Map of the environment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. Make sure the Summit robot is publishing it's transform data correctly.\n",
    "2. Create a package called <span class=\"ign_green\"><i>my_summit_mapping</i></span> that will contain all the files related to Mapping.\n",
    "3. Create a <span class=\"ign_green\">launch file</span> that will launch the slam_gmapping node and add the necessary parameters in order to properly configure the slam_gmapping node.\n",
    "4. Launch the slam_gmapping node and create a map of the simulated environment.\n",
    "5. Save the map you've just created.\n",
    "6. Create a script that automatically saves maps.\n",
    "7. Create a <span class=\"ign_green\">launch file</span> 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": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_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 <b>rospy</b> as 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 in this step, you'll have to create a launch file for the slam_gmapping 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 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 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": "raw",
   "metadata": {},
   "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 in RViz something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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 finding difficulties in order to generate a proper Map? Is there anything strange in the Mapping process? Do you know what could be the reason of it?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What's happening here is the following:\n",
    "\n",
    "1. 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. Tis will cause some strange issues during the mapping process, such as the robot readjusting its position in a wrong way.\n",
    "<br><br>\n",
    "2. 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 of the environemnt, so it won't get lost. Now, you'll be able to finalize the mapping process correctly."
   ]
  },
  {
   "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": {},
   "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 <b>maps</b>, and save the map files here. Your map image file should look something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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 script that automatically saves different maps.</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inside your package, create a directory called <b>src</b>. Inside this directory, create an script that will do the following:\n",
    "\n",
    "1. It will launch a node named <b>map_recorder</b>\n",
    "2. The node will create a Subscriber that will start reading to the <b>/odom</b> topic, and will check the distance moved by the robot.\n",
    "3. Each time the robot moves 5 meters, the script will call the <b>map_saver</b> in order to save the generated map until that moment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">7. Create a lanch 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": [
    "Next you have an example of how this launch fille should look like."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <node name=\"map_server\" pkg=\"map_server\" type=\"map_server\" args=\"$(find my_summit_mapping)/maps/my_map.yaml\"/>\n",
    "</launch>"
   ]
  },
  {
   "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:<br>\n",
    "\n",
    "1. Create a package called <span class=\"ign_green\"><i>my_summit_localization</i></span> that will contain all the files related to Localization.\n",
    "2. Create a <span class=\"ign_green\">launch file</span> that will launch the amcl node and add the necessary parameters in order to properly configure the amcl node.\n",
    "3. Launch the node and check that the robot properly localizes itself in the environment.\n",
    "4. Create a table with three different spots.\n",
    "5. 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 <b>rospy</b> as 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've seen how to create a launch file for the amcl node. You've also seen some of the most important parameters to set. So in 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 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 in the amcl node launch file, the launch you've created in Step 1 in order to provide the map to other nodes."
   ]
  },
  {
   "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",
    "\n",
    "<!-- Run the map server -->\n",
    "<include file=\"$(find my_summit_gma)/launch/map_server.launch\" />\n",
    "\n",
    "<node pkg=\"amcl\" type=\"amcl\" name=\"amcl\" output=\"screen\">\n",
    "\n",
    "  <remap from=\"scan\" to=\"/hokuyo_base/scan\" />\n",
    "  <remap from=\"cmd_vel\" to=\"/summit_xl_controller/cmd_vel\"/>\n",
    "\n",
    "  <!-- Publish scans from best pose at a max of 10 Hz -->\n",
    "  <param name=\"odom_model_type\" value=\"diff\"/>\n",
    "  <param name=\"odom_alpha5\" value=\"0.1\"/>\n",
    "  <param name=\"transform_tolerance\" value=\"0.2\" />\n",
    "  <param name=\"gui_publish_rate\" value=\"10.0\"/>\n",
    "  <param name=\"laser_max_beams\" value=\"30\"/>\n",
    "  <param name=\"min_particles\" value=\"500\"/>\n",
    "  <param name=\"max_particles\" value=\"5000\"/>\n",
    "  <param name=\"kld_err\" value=\"0.05\"/>\n",
    "  <param name=\"kld_z\" value=\"0.99\"/>\n",
    "  <param name=\"odom_alpha1\" value=\"0.2\"/>\n",
    "  <param name=\"odom_alpha2\" value=\"0.2\"/>\n",
    "  <param name=\"odom_alpha3\" value=\"0.8\"/>\n",
    "  <param name=\"odom_alpha4\" value=\"0.2\"/>\n",
    "  <param name=\"laser_z_hit\" value=\"0.5\"/>\n",
    "  <param name=\"laser_z_short\" value=\"0.05\"/>\n",
    "  <param name=\"laser_z_max\" value=\"0.05\"/>\n",
    "  <param name=\"laser_z_rand\" value=\"0.5\"/>\n",
    "  <param name=\"laser_sigma_hit\" value=\"0.2\"/>\n",
    "  <param name=\"laser_lambda_short\" value=\"0.1\"/>\n",
    "  <param name=\"laser_lambda_short\" value=\"0.1\"/>\n",
    "  <param name=\"laser_model_type\" value=\"likelihood_field\"/>\n",
    "  <param name=\"laser_likelihood_max_dist\" value=\"2.0\"/>\n",
    "  <param name=\"update_min_d\" value=\"0.2\"/>\n",
    "  <param name=\"update_min_a\" value=\"0.5\"/>\n",
    "  <param name=\"odom_frame_id\" value=\"odom\"/>\n",
    "  <param name=\"base_frame_id\" value=\"base_footprint\"/>\n",
    "  <param name=\"global_frame_id\" value=\"map\"/>\n",
    "  <param name=\"resample_interval\" value=\"1\"/>\n",
    "  <param name=\"transform_tolerance\" value=\"0.1\"/>\n",
    "  <param name=\"recovery_alpha_slow\" value=\"0.0\"/>\n",
    "  <param name=\"recovery_alpha_fast\" value=\"0.0\"/>\n",
    "  <param name=\"initial_pose_x\" value =\"0.0\"/>\n",
    "  <param name=\"initial_pose_y\" value =\"0.0\"/>\n",
    "  <param name=\"initial_pose_a\" value =\"0.0\"/>\n",
    "\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. 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 the previous steps correctly, you should see in Rviz something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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": [
    "<img src=\"img/summit_localization2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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": [
    "The 3 spots you'll have to register into the table are the next ones:\n",
    "\n",
    "* Fetch Room Center\n",
    "<br>\n",
    "<img src=\"img/fetch_room.png\" width=\"500\"></img>\n",
    "<br>\n",
    "* Facing the Turtle's Poster\n",
    "<br>\n",
    "<img src=\"img/turtle_poster.png\" width=\"500\"></img>\n",
    "* Besides the Table\n",
    "<br>\n",
    "<img src=\"img/besides_table.png\" width=\"500\"></img>\n",
    "<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can access the coordinates of each position by looking into the topics the amcl node publishes on. The only data 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": {},
   "source": [
    "<img src=\"img/echo_amcl_pose.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a file named <b>spots.txt</b> and write in the pose data you've get 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 so? 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",
    "1. It will launch a node named <b>spot_recorder</b>.\n",
    "2. This node will contain a Service Server named <b>/save_spot</b>, that will take an string as input.\n",
    "3. When this service is called, it will store the current coordinates of the robot (position and orientation values) alongside with a label, that will be the string provided on the service call.\n",
    "4. When the <i>end</i> string is provided in the service call, the node will write all the values stored into a file named <b>spots.txt</b>.\n",
    "5. Finally, the service will return a message indicating if the file has been saved correctly."
   ]
  },
  {
   "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",
    "1. Determine what input data 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> )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, search if there exists an already built message in the system that suits your needs. If it doesn'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 <b>srv</b>. Inside this directory, create a file named <b>SumitXLPose.srv</b>, that will contain the definition of your service message. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This file could be something like this:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "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 <b>package.xml</b> and <b>CMakeLists.txt</b> files of your package, in order to compile the new message. Assuming the only dependency you added to your package is <b>rospy</b>, 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()"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  rospy\n",
    "  std_msgs\n",
    "  message_generation\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "add_service_files(\n",
    "  FILES\n",
    "  MyCustomServiceMessage.srv\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  std_msgs\n",
    ")"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "catkin_package(\n",
    "      CATKIN_DEPENDS\n",
    "      rospy\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In package.xml:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add this 2 lines to the file:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "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 <b><i>rossrv show MyServiceMessage</i></b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "catkin_make output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/catkin_make_output.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "rossrv show output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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 <b>spots_to_file.py</b>. Inside this file, write the code necessary for you Service."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "import os\n",
    "import rospy\n",
    "from sumit_xl_tools.srv import SumitXLPose, SumitXLPoseResponse, SumitXLPoseRequest\n",
    "from geometry_msgs.msg import PoseWithCovarianceStamped\n",
    "import rospkg\n",
    "\n",
    "\"\"\"\n",
    "# topic: /amcl_pose\n",
    "# geometry_msgs/PoseWithCovarianceStamped \n",
    "\n",
    "std_msgs/Header header                                                                                                                                       \n",
    "  uint32 seq                                                                                                                                                 \n",
    "  time stamp                                                                                                                                                 \n",
    "  string 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",
    "\"\"\"\n",
    "\n",
    "class SummitXLPose(object):\n",
    "    def __init__(self):\n",
    "        self.pose_service = rospy.Service('/get_pose_sumitxl_service', SumitXLPose , self.pose_service_callback) # create the Service called my_service with the defined callback\n",
    "        rospy.Subscriber(\"/amcl_pose\", PoseWithCovarianceStamped, self.amcl_pose_callback)\n",
    "        self.pose_now = PoseWithCovarianceStamped()\n",
    "        \n",
    "        rospack = rospkg.RosPack()\n",
    "        self.spot_file_path = rospack.get_path('my_summit_localization') + \"/spots/spots_saved.txt\"\n",
    "        \n",
    "        try:\n",
    "            os.remove(self.spot_file_path)\n",
    "        except:\n",
    "            rospy.loginfo(\"File Not Found \"+str(self.spot_file_path) )\n",
    "        # Init File for Spot saving\n",
    "        file2write=open(self.spot_file_path,'a')\n",
    "        file2write.write(\"### This is the Spot Saving File ###\\n\")\n",
    "        file2write.close()\n",
    "        \n",
    "    def start_loop_service(self):\n",
    "        rospy.spin() # mantain the service open.\n",
    "        \n",
    "    def pose_service_callback(self, request):\n",
    "        spot_name = request.label\n",
    "        \n",
    "        with open(self.spot_file_path, \"a\") as myfile:\n",
    "            spot_pose = self.pose_now.pose.pose\n",
    "            myfile.write(spot_name+\":\\n\"+str(spot_pose)+\"\\n\\n\")\n",
    "\n",
    "        response = SumitXLPoseResponse()\n",
    "        response.navigation_successfull = True\n",
    "        response.message = \"Spot <\"+spot_name+\"> Saved\"\n",
    "\n",
    "        return response\n",
    "    \n",
    "    def amcl_pose_callback(self, message):\n",
    "        self.pose_now = message\n",
    "        \n",
    "\n",
    "\n",
    "def startPoseService():\n",
    "    rospy.init_node('sumitxl_poseservice_server')\n",
    "    sumitpose = SummitXLPose()\n",
    "    sumitpose.start_loop_service()\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    startPoseService()\n",
    "    "
   ]
  },
  {
   "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 showed 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 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": "raw",
   "metadata": {},
   "source": [
    "rosservice call /record_spot \"label: fetch_room\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Repeat this for the 3 different spots. When finished, do a 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 do:\n",
    "\n",
    "1. Create a package called <span class=\"ign_green\"><i>my_summit_path_planning</i></span> that will contain all the files related to Path Planning.\n",
    "2. Create a <span class=\"ign_green\">launch file</span> that will launch the move_base node.\n",
    "3. Create the necessary parameter files in order to properly configure the move_base node.\n",
    "4. Create the necessary parameter files in order to properly configure the global and local costmaps.\n",
    "5. Create the necessary parameter files in order to properly configure the global and local planners.\n",
    "6. Launch the node and check 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 (Chapater 4) of this course you've seen how to create a launch file for the move_base node. So in 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": [
    "<?xml version=\"1.0\"?>\n",
    "<!-- NEW SUMIT XL NAVIGATION -->\n",
    "<launch>\n",
    "        \n",
    "    <include file=\"$(find my_summit_localization)/launch/amcl.launch\" />\n",
    "    \n",
    "    <remap from=\"cmd_vel\" to=\"/move_base/cmd_vel\" />\n",
    "    \n",
    "    <arg name=\"no_static_map\" default=\"false\"/>\n",
    "    \n",
    "    <arg name=\"base_global_planner\" default=\"navfn/NavfnROS\"/>\n",
    "    <arg name=\"base_local_planner\" default=\"dwa_local_planner/DWAPlannerROS\"/>\n",
    "    <!-- <arg name=\"base_local_planner\" default=\"base_local_planner/TrajectoryPlannerROS\"/> -->\n",
    "    \n",
    "    <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "        \n",
    "        <param name=\"base_global_planner\" value=\"$(arg base_global_planner)\"/>\n",
    "        <param name=\"base_local_planner\" value=\"$(arg base_local_planner)\"/>  \n",
    "        <rosparam file=\"$(find sumit_xl_tools)/new_config/planner.yaml\" command=\"load\"/>\n",
    "        \n",
    "        <!-- observation sources located in costmap_common.yaml -->\n",
    "        <rosparam file=\"$(find my_summit_path_planning)/new_config/costmap_common.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "        <rosparam file=\"$(find my_summit_path_planning)/new_config/costmap_common.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "        \n",
    "        <!-- local costmap, needs size -->\n",
    "        <rosparam file=\"$(find my_summit_path_planning)/new_config/costmap_local.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "        <param name=\"local_costmap/width\" value=\"5.0\"/>\n",
    "        <param name=\"local_costmap/height\" value=\"5.0\"/>\n",
    "        \n",
    "        <!-- static global costmap, static map provides size -->\n",
    "        <rosparam file=\"$(find my_summit_path_planning)/new_config/costmap_global_static.yaml\" command=\"load\" ns=\"global_costmap\" unless=\"$(arg no_static_map)\"/>\n",
    "        \n",
    "        <!-- global costmap with laser, for odom_navigation_demo -->\n",
    "        <rosparam file=\"$(find my_summit_path_planning)/new_config/costmap_global_laser.yaml\" command=\"load\" ns=\"global_costmap\" if=\"$(arg no_static_map)\"/>\n",
    "        <param name=\"global_costmap/width\" value=\"100.0\" if=\"$(arg no_static_map)\"/>\n",
    "        <param name=\"global_costmap/height\" value=\"100.0\" if=\"$(arg no_static_map)\"/>\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 .yaml files</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the **planner.yaml** file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "controller_frequency: 5.0\n",
    "recovery_behaviour_enabled: true\n",
    "\n",
    "NavfnROS:\n",
    "  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.\n",
    "  default_tolerance: 0.1 # A tolerance on the goal point for the planner.\n",
    "\n",
    "TrajectoryPlannerROS:\n",
    "  # Robot Configuration Parameters\n",
    "  acc_lim_x: 2.5\n",
    "  acc_lim_theta:  3.2\n",
    "\n",
    "  max_vel_x: 1.0\n",
    "  min_vel_x: 0.0\n",
    "\n",
    "  max_vel_theta: 1.0\n",
    "  min_vel_theta: -1.0\n",
    "  min_in_place_vel_theta: 0.2\n",
    "\n",
    "  holonomic_robot: false\n",
    "  escape_vel: -0.1\n",
    "\n",
    "  # Goal Tolerance Parameters\n",
    "  yaw_goal_tolerance: 0.1\n",
    "  xy_goal_tolerance: 0.2\n",
    "  latch_xy_goal_tolerance: false\n",
    "\n",
    "  # Forward Simulation Parameters\n",
    "  sim_time: 2.0\n",
    "  sim_granularity: 0.02\n",
    "  angular_sim_granularity: 0.02\n",
    "  vx_samples: 6\n",
    "  vtheta_samples: 20\n",
    "  controller_frequency: 20.0\n",
    "\n",
    "  # Trajectory scoring parameters\n",
    "  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).\n",
    "  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01\n",
    "  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6\n",
    "  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8\n",
    "\n",
    "  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories\n",
    "  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false\n",
    "  heading_scoring_timestep: 0.8   #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)\n",
    "  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout\n",
    "  simple_attractor: false\n",
    "  publish_cost_grid_pc: true  \n",
    "\n",
    "  # Oscillation Prevention Parameters\n",
    "  oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)\n",
    "  escape_reset_dist: 0.1\n",
    "  escape_reset_theta: 0.1\n",
    "\n",
    "DWAPlannerROS:\n",
    "  # Robot configuration parameters  \n",
    "  acc_lim_x: 2.5\n",
    "  acc_lim_y: 0\n",
    "  acc_lim_th: 3.2\n",
    "\n",
    "  max_vel_x: 0.5\n",
    "  min_vel_x: 0.0\n",
    "  max_vel_y: 0\n",
    "  min_vel_y: 0\n",
    "\n",
    "  max_trans_vel: 0.5\n",
    "  min_trans_vel: 0.1\n",
    "  max_rot_vel: 1.0\n",
    "  min_rot_vel: 0.2\n",
    "\n",
    "  # Goal Tolerance Parameters\n",
    "  yaw_goal_tolerance: 0.1\n",
    "  xy_goal_tolerance: 0.2\n",
    "  latch_xy_goal_tolerance: false\n",
    "\n",
    "  # # Forward Simulation Parameters\n",
    "  # sim_time: 2.0\n",
    "  # sim_granularity: 0.02\n",
    "  # vx_samples: 6\n",
    "  # vy_samples: 0\n",
    "  # vtheta_samples: 20\n",
    "  # penalize_negative_x: true\n",
    "\n",
    "  # # Trajectory scoring parameters\n",
    "  # path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given\n",
    "  # goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed\n",
    "  # occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles\n",
    "  # forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters\n",
    "  # stop_time_buffer: 0.2  # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds\n",
    "  # scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s\n",
    "  # max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by\n",
    "\n",
    "  # # Oscillation Prevention Parameters\n",
    "  # oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you are not sure of what all these values are, please go over **RobotIgniteAcademy** Navigation Course in ROS."
   ]
  },
  {
   "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, global_costmap_params.yaml and local_costmap_params.yaml files</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the **costmap_common.yaml** file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "footprint: [[0.35, -0.3], [0.35, 0.3], [-0.35,0.3], [-0.35, -0.3]]\n",
    "footprint_padding: 0.01\n",
    "\n",
    "robot_base_frame: base_link\n",
    "update_frequency: 4.0\n",
    "publish_frequency: 3.0\n",
    "transform_tolerance: 0.5\n",
    "\n",
    "resolution: 0.05\n",
    "\n",
    "obstacle_range: 5.5\n",
    "raytrace_range: 6.0\n",
    "\n",
    "#layer definitions\n",
    "static:\n",
    "    map_topic: /map\n",
    "    subscribe_to_updates: true\n",
    "\n",
    "obstacles_laser:\n",
    "    observation_sources: hokuyo_laser\n",
    "    hokuyo_laser: {sensor_frame: hokuyo_base_link, data_type: LaserScan, clearing: true, marking: true, topic: hokuyo_base/scan, inf_is_valid: true}\n",
    "\n",
    "inflation:\n",
    "    inflation_radius: 1.0\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the **costmap_global_static.yaml** file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_frame: map\n",
    "rolling_window: false\n",
    "track_unknown_space: true\n",
    "\n",
    "plugins:\n",
    "  - {name: static,                  type: \"costmap_2d::StaticLayer\"}\n",
    "  - {name: inflation,               type: \"costmap_2d::InflationLayer\"}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of the **costmap_local.yaml** file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_frame: odom\n",
    "rolling_window: true\n",
    "\n",
    "plugins:\n",
    "  - {name: obstacles_laser,           type: \"costmap_2d::ObstacleLayer\"}\n",
    "  - {name: inflation,                 type: \"costmap_2d::InflationLayer\"}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of **costmap_global_laser.yaml** file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_frame: odom\n",
    "rolling_window: true\n",
    "track_unknown_space: true\n",
    "\n",
    "plugins:\n",
    "  - {name: obstacles_laser,           type: \"costmap_2d::ObstacleLayer\"}\n",
    "  - {name: inflation,                 type: \"costmap_2d::InflationLayer\"}"
   ]
  },
  {
   "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 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": [
    "<img src=\"img/summit_planning1.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_planning2.png\" width=\"800\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "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 the 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 that, right? Well, then now you'll get 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 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 to that label, and will send this 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 which contains:\n",
    "\n",
    "* A Service that will take an 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."
   ]
  },
  {
   "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 the files related to this project. Remember to specify the <b>rospy</b> 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 mesage.\n",
    "\n",
    "1. Determine what input data 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> )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, search if there exists an already built message in the system that suits your needs. If it doesn'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 <b>srv</b>. Inside this directory, create a file named <b>MyServiceMessage.srv</b>, that will contain the definition of your service message. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This file could be something like this:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "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 <b>package.xml</b> and <b>CMakeLists.txt</b> files of your package, in order to compile the new message. Assuming the only dependency you added to your package is <b>rospy</b>, 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()"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "find_package(catkin REQUIRED COMPONENTS\n",
    "  rospy\n",
    "  std_msgs\n",
    "  message_generation\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "add_service_files(\n",
    "  FILES\n",
    "  MyCustomServiceMessage.srv\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "generate_messages(\n",
    "  DEPENDENCIES\n",
    "  std_msgs\n",
    ")"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "catkin_package(\n",
    "      CATKIN_DEPENDS\n",
    "      rospy\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### In package.xml:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Add this 2 lines to the file:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "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 <b><i>rossrv show MyServiceMessage</i></b>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "catkin_make output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/catkin_make_output.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "rossrv show output:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/service_msg_output.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">3. Create the Service.</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 <b>my_navigation_program.py</b>. Inside this file, write the code necessary for you 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 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 providing one of the labels, and check that the Summit robot navigates to that spot. For example, if your label is \"fetch_room\", type in a web shell the following:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "rosservice call my_summitnavigation/MyServiceMessage \"label: 'fetch_room'\" "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add WayPoints"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So there you have it. Now your summit XL can navigate around one position at a time. But what if you want to generate in an easy way a sequence of waypoints so that it patroles around the **main interest points of the area**?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are going to use a ROS package called **follow_waypoints**, you can get more information http://wiki.ros.org/follow_waypoints.\n",
    "This package basically tracks the **Estimate pose** that you place in RVIZ and stores it. Then when you have finished you just have to publish in a topic that starts sending those positions to the movebase system."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Steps to do it:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to start the waypoint server:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1 to move Haro</p><br>\n",
    "roslaunch follow_waypoints follow_waypoints.launch\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then you have to start RVIZ, and add a PoseArray element, susbcribed to the topic /waypoints, and change its name to WayPoints. This will now show all the waypoints you set from now on."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #2 to move Haro</p><br>\n",
    "rosrun rviz rviz\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_waypoints1.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can change the colour also if you want. In this example its changed to black to see it better."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you have to set the waypoints, for this you have to select the **PoseEstimate** and set it where you want."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_waypoints3.png\" width=\"100\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_waypoints2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should also get in webshell where you launched the waypoint server a message stating that it recieved the waypoint:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[INFO][139816504842048][/follow_waypoints/execute:92]: Waiting to recieve waypoints via Pose msg on topic /initialpose         \n",
    "[INFO][139816504842048][/follow_waypoints/execute:93]: To start following waypoints: 'rostopic pub /path_ready std_msgs/Empty -\n",
    "1'                                                                                                                             \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you just have to publish in the topic **/path_ready** to start sending waypoints to the movebase."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you want to see the global path generated just activate it in RVIZ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #3 to move Haro</p><br>\n",
    "rostopic pub /path_ready std_msgs/Empty -1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this for each waypoint executed:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_waypoints4.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit1_waypoints5.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now create a python script that generates those waypoints and when finished executes the moving system to follow those waypoints.<br>\n",
    "To do so, you will need to publish directly into the topic **/initialpose** where the sserver retreives the data for the waypoint data. Then you will have to publish to start in the topic **/path_ready**.<br>\n",
    "Make it generate circular paths, eight shaped and dynamic wayponts based on the position of the standing person. For this lastone you will need to acces the TF data of the standingPerson."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extra: Launch your RVIZ configs easily"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how to launch RVIZ sessions faster. You just have to create this tinny launch that retrieves the name of the **rviz** configuration file saved in the **rviz** folder. You will use it in the next units for sure."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**view_robot.launch**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<launch>\n",
    "  <param name=\"use_gui\" value=\"true\"/>\n",
    "  <arg name=\"config\" default=\"complete\" />\n",
    "  <node name=\"rviz\" pkg=\"rviz\" type=\"rviz\" args=\"-d $(find my_summitxl_tools)/rviz/$(arg config).rviz\" />\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And you can use it like so in any launch file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<!-- Start RVIZ for Localization -->\n",
    "<include file=\"$(find my_summitxl_tools)/launch/view_robot.launch\">\n",
    "    <arg name=\"config\" value=\"localization\" />\n",
    "</include>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This one will load in RVIZ the file called **localization.rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Congratulations! You now have a fully functional indoor navigating Summit XL. In next Chapter you will learn how to navigate outdoors using GPS data."
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  },
  "latex_metadata": {
   "chapter": "6 - Course Project",
   "chapter_title": "Chapter 6. Course Project",
   "course_title": "ROS NAVIGATION IN 5 DAYS"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}