Course_SummitXLInstructionManual_Unit_2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 2: Set Outdoors Navigation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_intro2.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotnik_logo.png\" width=\"300\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"300\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you will learn how to access the GPS data and use it to navigate around without a mapping system. You will have to relay upon the lasers to detect any obstacles and navigate around them to get to your destination in GPS coordinates.\n",
    "You will also learn how to visualise real maps in RVIZ based on GPS data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GPS basic concepts"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So you have your Summit XL Robot navigating with your map, thats great. But what happens when you want to move ouside your map? How do you localise yourself without AMCL system?<br>\n",
    "Well the answer is to use the **GPS data**.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It seems simple right? Well yes and no. GPS data has some limitations:<br>\n",
    "* GPS data is not very precise: GPS data oscilates in value and your position will move a lot. This is anavoidable and you will have to mitigate that error in your own way. **Odometry** data will mitigate some of these errors, but not all of them because Summit XL wheels slide and this produces errors in the odometry. Bear that in mind.\n",
    "* GPS gives you no orientation: GPS only gives you a point in space, no orientation. This means that you need to complement that with a **compas** of some kind. Here is where **IMU** comes in handy."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With all this in mind, you will be able to have a crude but usefull system to position and move your robot around an outdoors terrain without map.\n",
    "Of course, you can move around with GPS and while doing it, generate a new map, making bigger the area where map localization ( much more precise ) can be used next time."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Get The GPS navigation running"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First of all, please be sure to have a package where to save and launch all the files named **my_summitxl_tools**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 0: Load an empty Map"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Because we want to have the map, but we are going to move around based on the GPS, we load into the map server an empty map. For that just male a copy of the map you created in Unit1 and remove all the black areas leaving a totaly white ( which means empty) map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/old_clean_map.png\" width=300/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create its empty version called **mymap_empty.yaml**. You can create it as you wish, <a href=\"https://manytools.org/image/colorize-filter/\">here</a> is a page where you can convert an image completely to white, or monocrome... It gives you some options:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/old_clean_map_white.png\" width=300/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you have it create also a **yaml** file with the same name and referencing the image: "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "image:  mymap_empty.pgm\n",
    "resolution: 0.050000\n",
    "origin: [-50.000000, -50.000000, 0.000000]\n",
    "negate: 0\n",
    "occupied_thresh: 0.65\n",
    "free_thresh: 0.196"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a map server launch to be able to test that works:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_map_server.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <arg name=\"map_file\" default=\"$(find my_sumit_xl_tools)/maps/mymap_empty.yaml\"/>\n",
    "    <node name=\"map_server\" pkg=\"map_server\" type=\"map_server\" args=\"$(arg map_file)\" />\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now the map server will load the new map into the param server and it should be seen in RVIZ. When you launch the map server you should see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/new_clean_map_rviz_nomaptf.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step 1: Disconnect AMCL"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Of course the first thing you have to do retrieve the **map based navigation launch** which if you followed the named conventions in this course, it should have a name like **start_navigation_with_map.launch**. Create a copy of it with the name **start_navigation_with_gps_ekf.launch**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_navigation_with_gps_ekf.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "    <!-- Run the map server -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_map_server.launch\" />\n",
    "\n",
    "    <!--- Run Move Base -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_movebase_map.launch\" />\n",
    "    \n",
    "    <!-- Start RVIZ for Localization -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/view_robot.launch\">\n",
    "        <arg name=\"config\" value=\"navigation_map\" />\n",
    "    </include>\n",
    "\n",
    "</launch>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see you only have to remove the **amcl.launch** and add the launch of RVIZ with the file **navigation_map.rviz**. The rest is exactly the same."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_movebase_map.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\"?>\n",
    "<!-- NEW SUMIT XL NAVIGATION -->\n",
    "<launch>\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 sumit_xl_tools)/new_config/costmap_common.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "        <rosparam file=\"$(find sumit_xl_tools)/new_config/costmap_common.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "        \n",
    "        <!-- local costmap, needs size -->\n",
    "        <rosparam file=\"$(find sumit_xl_tools)/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 sumit_xl_tools)/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 sumit_xl_tools)/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": [
    "So what implication has removing **amcl.launch**?<br>\n",
    "Well the main function of **AMCL** is to publish the transform of **/map** frame to **/odom** frame.<br>\n",
    "Check out this frame tree with **amcl** running:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[TF tree with amcl running in the navigation with map](extra_files/summit_tf_amcl_running.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_amcltf.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you see that the node responsible of conecting **map** frame and **odom** frame is **AMCL**. So how will you do it now?<br>\n",
    "You will use something called **ekf_localization**. That node is part of a package called **robot_localization** that merges sensor data through Kalman Filters to publish more stable TFs. You are going to use it to publish also the map --> odom TF with GPS, IMU and odom data merged."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step2: Convert the GPS data into Odometry type data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Introducing the ROS package <a href=\"http://docs.ros.org/kinetic/api/robot_localization/html/index.html\">robot_localization</a>.<br>\n",
    "This package allows you to localise a Robot based on infinite number of inputs. You can merge the data from IMU, GPS, Vision.. To get the best localization through Kalman Filters.<br>\n",
    "You are going to learn the basics of how to set it up and use some extra tools GPS related like <a href=\"http://docs.ros.org/kinetic/api/robot_localization/html/integrating_gps.html\">navsat_transform_node</a>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **navsat_transform_node** alows you to convert the GPS **latitude** and **longitude** into **XY** coordinates that can be represented in space. Here is the example of the launch file you will need to launch it:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_navsat.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    " <!-- -->\n",
    "  <node pkg=\"robot_localization\" type=\"navsat_transform_node\" name=\"navsat_transform_node\" respawn=\"true\">\n",
    "\n",
    "    <param name=\"magnetic_declination_radians\" value=\"0\"/>\n",
    "    <param name=\"yaw_offset\" value=\"0\"/>\n",
    "    <param name=\"zero_altitude\" value=\"true\"/>\n",
    "    \n",
    "    \n",
    "    <param name=\"broadcast_utm_transform\" value=\"false\"/>\n",
    "    <param name=\"publish_filtered_gps\" value=\"false\"/>\n",
    "\n",
    "    \n",
    "    <param name=\"use_odometry_yaw\" value=\"false\"/>\n",
    "    <param name=\"wait_for_datum\" value=\"false\"/>\n",
    "    \n",
    "\n",
    "    <remap from=\"/imu/data\" to=\"/imu/data\" />\n",
    "    <remap from=\"/gps/fix\" to=\"/gps/fix\" />\n",
    "    <remap from=\"/odometry/filtered\" to=\"/odom\" />\n",
    "\n",
    "  </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Some element to comment on:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<param name=\"magnetic_declination_radians\" value=\"0\"/>\n",
    "<param name=\"yaw_offset\" value=\"0\"/>\n",
    "<param name=\"zero_altitude\" value=\"true\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These paraemeters are just to adjust some offsets you might have in your sensors IMU and so on. You also can consider Null the altitude."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<param name=\"broadcast_utm_transform\" value=\"false\"/>\n",
    "<param name=\"publish_filtered_gps\" value=\"false\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* broadcast_utm_transform: This is in case you want to publish the static TF of the UTM ( Origin of the GPS system ) in the TF tree. \n",
    "* publish_filtered_gps: No mystery here, if you want it to publish an improved by odometry and IMU GPS data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<param name=\"use_odometry_yaw\" value=\"false\"/>\n",
    "<param name=\"wait_for_datum\" value=\"false\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* use_odometry_yaw: This is for when you have a very reliable odometry turning system. In case of robots outdoors , the turning data in odometry is reliable enough because they skid once in a while , specially in rough terrain, thats why its turned OFF here.\n",
    "* wait_for_datum: This is to give the system directly the datum, what is considered as the origin of our GPS localization. Normally its the GPS that directly gives this data and thats why its normaly turned off. If you need to give it for some reason, you will have to add in this launch the following param setting:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<rosparam param=\"datum\">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Or through the service **set_datum service and using the robot_localization/SetDatum service message**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<remap from=\"/imu/data\" to=\"/imu/data\" />\n",
    "<remap from=\"/gps/fix\" to=\"/gps/fix\" />\n",
    "<remap from=\"/odometry/filtered\" to=\"/odom\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is the most important parameters. These are the topics from which it gets the GPS, IMU and Odometry data. With it it generates the Pose equivalent."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "How to know what GPS data means and where it is in real life?<br>\n",
    "Well in the case of this simulated GPS, it starts located in the coordenates: This is latitude = 39.5080331, and longitude = -0.4619816. These are the coordenates of the officies of <a href=\"http://www.robotnik.es/\">Robotnik</a>, creators of Summit XL.<br>\n",
    "In real life of course it would have the readings of the real place.<br>\n",
    "You can get the conversion from address to lat-longitude and the otherway round in this <a href=\"https://www.gps-coordinates.net/\">Google page</a>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have this launch ready, when launched, it will publish into the topic /odometry/gps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p><br>\n",
    "roslaunch my_sumit_xl_tools start_navsat.launch\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\">Execute in WebShell #2</p><br>\n",
    "rostopic info /odometry/gps\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see here, the message type is nav_msgs/Odometry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "## nav_msgs/Odometry                                                                                                              \n",
    "                                                                                                                                                             \n",
    "std_msgs/Header header                                                                                                                                       \n",
    "  uint32 seq                                                                                                                                                 \n",
    "  time stamp                                                                                                                                                 \n",
    "  string frame_id                                                                                                                                            \n",
    "string child_frame_id                                                                                                                                        \n",
    "geometry_msgs/PoseWithCovariance pose                                                                                                                        \n",
    "  geometry_msgs/Pose pose                                                                                                                                    \n",
    "    geometry_msgs/Point position                                                                                                                             \n",
    "      float64 x                                                                                                                                              \n",
    "      float64 y                                                                                                                                              \n",
    "      float64 z                                                                                                                                              \n",
    "    geometry_msgs/Quaternion orientation                                                                                                                     \n",
    "      float64 x                                                                                                                                              \n",
    "      float64 y                                                                                                                                              \n",
    "      float64 z                                                                                                                                              \n",
    "      float64 w                                                                                                                                              \n",
    "  float64[36] covariance                                                                                                                                     \n",
    "geometry_msgs/TwistWithCovariance twist                                                                                                                      \n",
    "  geometry_msgs/Twist twist                                                                                                                                  \n",
    "    geometry_msgs/Vector3 linear                                                                                                                             \n",
    "      float64 x                                                                                                                                              \n",
    "      float64 y                                                                                                                                              \n",
    "      float64 z                                                                                                                                              \n",
    "    geometry_msgs/Vector3 angular                                                                                                                            \n",
    "      float64 x                                                                                                                                              \n",
    "      float64 y                                                                                                                                              \n",
    "      float64 z                                                                                                                                              \n",
    "  float64[36] covariance"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see,its a standard Odometry message. This means that now you have the position of the Summit based only on its GPS, Odometry and IMU.\n",
    "So the obvious step now is convert this data into a TF that positions the robot in the correct spot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create TF publisher for MAP to Odom frame using EKF node"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So now that you have a new source of odometry information based on the GPS, its time to merge that data with extra data from IMU and the classical odometry, to generate the TF **map -> odom**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now have a look at the updated version of the **start_navigation_with_gps_ekf.launch**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "UPDATED **start_navigation_with_gps_ekf.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_navsat.launch\" />\n",
    "\n",
    "\n",
    "    <!-- Run the ekf for map to odom config -->\n",
    "    <node pkg=\"robot_localization\" type=\"ekf_localization_node\" name=\"ekf_localization_with_gps\"> \n",
    "    <rosparam command=\"load\" file=\"$(find my_sumit_xl_tools)/config/robot_localization_with_gps.yaml\" />\n",
    "    </node>\n",
    "\n",
    "    <!-- Run the map server -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_map_server.launch\" />\n",
    "\n",
    "    <!--- Run Move Base -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_movebase_map.launch\" />\n",
    "    \n",
    "    <!-- Start RVIZ for Localization -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/view_robot.launch\">\n",
    "        <arg name=\"config\" value=\"navigation_map\" />\n",
    "    </include>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So concentrate on the launch of **ekf_localization**:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<!-- Run the ekf for map to odom config -->\n",
    "    <node pkg=\"robot_localization\" type=\"ekf_localization_node\" name=\"ekf_localization_with_gps\"> \n",
    "    <rosparam command=\"load\" file=\"$(find my_sumit_xl_tools)/config/robot_localization_with_gps.yaml\" />\n",
    "  </node>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This node needs to load a bunch of parameters that will configure how many sensors are used to calculate the TF map --> odom. This is loaded through the file **robot_localization_with_gps.yaml**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**robot_localization_with_gps.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#Configuation for robot odometry EKF\n",
    "#\n",
    "frequency: 50\n",
    "odom0: /odom\n",
    "odom0_config: [false, false, false,\n",
    "               false, false, false,\n",
    "               true, true, true,\n",
    "               false, false, true,\n",
    "               false, false, false]\n",
    "odom0_differential: true\n",
    "\n",
    "imu0: /imu/data\n",
    "imu0_config: [false, false, false,\n",
    "              true, true, true,\n",
    "              false, false, false,\n",
    "              true, true, true,\n",
    "              false, false, false]\n",
    "imu0_differential: false\n",
    "\n",
    "odom1: /odometry/gps\n",
    "odom1_config: [false, false, false,\n",
    "               false, false, false,\n",
    "               true, true, true,\n",
    "               false, false, true,\n",
    "               false, false, false]\n",
    "odom1_differential: false\n",
    "\n",
    "odom_frame: odom\n",
    "base_link_frame: base_footprint\n",
    "world_frame: map\n",
    "map_frame: map\n",
    "\n",
    "process_noise_covariance\": [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,\n",
    "                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]\n",
    "\n",
    "\n",
    "initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,\n",
    "                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This node ekf has many more parameters that can be set and for sure you will be able to upgrade the performance of the system by tuning the values. These are the basic:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Essentially what you have to set up is all the sources of data that will be merged to generate th TF. In this case you are going to use:\n",
    "* Basic Summit Wheel Odometry: this is published in the topic **/odom**.\n",
    "* Summit XL IMU: this data isn published in the topic **/imu/data**.\n",
    "* Odometry generated by the GPS to Odometry conversion made by the navsat_transform_node: This data will be published in the topic **/odometry/gps**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "All this data is set by stating the type of input ( odom or imu ), followed by a number. Thats why you have **odom0** and **odom1**, but **imu0** only. There you state the topic to read from."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then you state the configuration. This indicates what data you will take into account an which one you will ignore.\n",
    "\n",
    "odomX_config:\n",
    "[true, true, false, --> x, y, z\tPosition Values\n",
    "  \n",
    "false, false, false, --> roll, pitch, yaw Orientation values\n",
    "  \n",
    "false, false, false, x linear-velocity, y linear-velocity, z linear-velocity\t\n",
    "  \n",
    "false, false, true, roll velocity, pitch velocity, yaw velocity\t\n",
    "  \n",
    "false, false, false] x accel., y accel., z accel.\t"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this example, you are stating that you take into account:\n",
    "* X and Y Position.\n",
    "* Pitch velocity"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What you consider depend on whether that data is more or less reliable or not. In the case of your file **robot_localization_with_gps.yaml** you state that:\n",
    "* In the **odometry sensors** you only consider the **linear velocities** and ignore the rest.\n",
    "* In the case of the **imu** you only consider all the Roll-Pitch-Yaw orientation data ( orientation and speed )."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As for the **odomX_differential** , you state False if you want to consider the values as they are, or you consider only their diference in time. Its advisable that you combine both if you have more than one sensor to have more stable readings. You normally put the one thnat has maybe a constant error in differential = True to remove that error."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U2-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Implement all the launch files and configuration files to be able to launch it and have Summit XL appear in RVIZ, fully localised and with the global and local map working. It should be able to move around using **2D Pose** signals in RVIZ. Just like using map navigation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Steps to follow:\n",
    "* Create the **start_map_server.launch** and check that the empty map you created is ok.\n",
    "* Create the **start_navsat.launch** and check it works and makes the conversion. Move the Summit XL robot around to see thet the values change accordingly.\n",
    "* Create Updated version of **start_navigation_with_gps_ekf.launch** with the included ekf launching and navsat.\n",
    "* Launch it and check that Summit XL can navigate."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try to reach the **construction barrel** outside. Be carefull with the exit of the building."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U2-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "You should get something similar to this in RVIZ:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_GPSnav1.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_nav_gps.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now check the TF tree that you have. You should see that the **map** to **odom** is published by your **ekf_localization_with_gps** node."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[TF tree ekf running in the navigation with empty map](extra_files/summit_tf_ekf_gps.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_GPSnav_tf.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Represent the MAP position in RVIZ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Because you have now GPS data, you can represent the robot in the real place where the GPS is positioning it. Getting results like:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_gpsmap1.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_instruction_manual_unit2_gpsmap2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "But this is not what you should see, because if you retrieve the address from the Google page based on the origin latitude and longitude (olat = 49.9 olon = 8.9), you should get a map similar to the one shown in Google maps: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_unit2_googlepam_robotnik.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So to get this in RVIZ you have to:\n",
    "* Represent the GPS Current Position in RVIZ using a plugin: This plugin allows you to position the summit XL based in the GPS data published in /gps/fix topic in the corresponding real place on Earth https://github.com/gareth-cross/rviz_satellite.<br>\n",
    "    You can copy a rviz file that is already set for you in The path is **/opt/ros/indigo/share/sumit_xl_tools/rviz/gps.rviz**<br>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* You can set it manually as follows:   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Camera Icon Image](img/summit_xl_unit2_add_mapbox.png)<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then set the topic where the GPS data is published (For example:  /gps/fix ) , the RobotFrame ( for example /odom )and set the URL where to fetch the MapTiles. This URL has to have the following structure:<br>\n",
    "    http://a.tiles.mapbox.com/v4/mapbox.satellite/{z}/{x}/{y}.jpg?access_token=YOUR_ACCESS_TOKEN<br>\n",
    "    The Only thing that has to be changed is the YOUR_ACCESS_TOKEN which its given by MapBox https://www.mapbox.com/install/js/cdn-add/."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this case the correct topic candidates for the GPS data are:\n",
    "* /navsat/fix: This is the GPS data published directly by the GPS.\n",
    "* /gps/filtered: This topic is published by the Nav_sat_stranform node. This means that is the GPS data published by **/navsat/fix** but merged with the odometry and imu data, so it should be more precise.\n",
    "So the logical choice has to be the **/gps/filtered**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As for the Robot frame, **odom** is a good choice."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also note that you have to select as **fixed_frame** in the global_options the **map** frame to see when the robot moves around the map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If all went well you should see something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_unit2_gpsimage_rviz.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_unit2_gpsimage_rviz2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And save it as **gps.rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U2-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try different settings for the RVIZ **AerialMapDisplay**, see how it might change its visualization."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U2-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How to convert GPS Coordinates to Pose Data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This skill is very important because you need to be able to sent poses to the move base to move the robot. But normally when you move outdoors, you have GPS coordenates to go to. So you have to use the <a href=\"http://wiki.ros.org/geonav_transform\">geonav_transform</a> package.<br>\n",
    "This pacakge allows you to convert Latitude and longitude to XYZ coordinates, based on the GPS origon that you set. In this case the GPS origin is:\n",
    "* olat = 49.9 olon = 8.9"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**gps_to_xyz.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python \n",
    "\n",
    "# Import geonav tranformation module\n",
    "import geonav_transform.geonav_conversions as gc\n",
    "reload(gc)\n",
    "# Import AlvinXY transformation module\n",
    "import alvinxy.alvinxy as axy\n",
    "reload(axy)\n",
    "import rospy\n",
    "import tf\n",
    "from nav_msgs.msg import Odometry\n",
    "\n",
    "\n",
    "def get_xy_based_on_lat_long(lat,lon, name):\n",
    "    # Define a local orgin, latitude and longitude in decimal degrees\n",
    "    # GPS Origin\n",
    "    olat = 49.9\n",
    "    olon = 8.9\n",
    "    \n",
    "    xg2, yg2 = gc.ll2xy(lat,lon,olat,olon)\n",
    "    utmy, utmx, utmzone = gc.LLtoUTM(lat,lon)\n",
    "    xa,ya = axy.ll2xy(lat,lon,olat,olon)\n",
    "\n",
    "    rospy.loginfo(\"#########  \"+name+\"  ###########\")  \n",
    "    rospy.loginfo(\"LAT COORDINATES ==>\"+str(lat)+\",\"+str(lon))  \n",
    "    rospy.loginfo(\"COORDINATES XYZ ==>\"+str(xg2)+\",\"+str(yg2))\n",
    "    rospy.loginfo(\"COORDINATES AXY==>\"+str(xa)+\",\"+str(ya))\n",
    "    rospy.loginfo(\"COORDINATES UTM==>\"+str(utmx)+\",\"+str(utmy))\n",
    "\n",
    "    return xg2, yg2\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('gps_to_xyz_node')\n",
    "    xg2, yg2 = get_xy_based_on_lat_long(lat=49.9,lon=8.9, name=\"MAP\")\n",
    "    xg2, yg2 = get_xy_based_on_lat_long(lat=50.9,lon=8.9, name=\"MAP\")\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that you have different conversions, depending on the model used. You will use here the standard geonav transform **gn**. But you can als use the **AlvinXY** converter , which is a simple rectilinear conversion, or get the UTM values."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "See the output of the two tests. Just changing in une unit in latitude, the efect in meters is huge. Obvious because the distace between two lines of latitude is around **111 kilometers**. See that in AvinXY the result is exactly that: 111227.305636 meters or 111.227305636 Kilo metres. In the GN is not exactly that because is taking into acound more factors."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[INFO] [WallTime: 1505572823.286398] [0.000000] #########  MAP  ###########                                                                                               \n",
    "[INFO] [WallTime: 1505572823.286697] [0.000000] LAT COORDINATES ==>49.9,8.9                                                                                               \n",
    "[INFO] [WallTime: 1505572823.286876] [0.000000] COORDINATES XYZ ==>0.0,0.0                                                                                                \n",
    "[INFO] [WallTime: 1505572823.287080] [0.000000] COORDINATES AXY==>0.0,0.0                                                                                                 \n",
    "[INFO] [WallTime: 1505572823.287267] [0.000000] COORDINATES UTM==>492818.438732,5527517.13624                                                                             \n",
    "[INFO] [WallTime: 1505572823.287644] [0.000000] #########  MAP  ###########                                                                                               \n",
    "[INFO] [WallTime: 1505572823.287837] [0.000000] LAT COORDINATES ==>50.9,8.9                                                                                               \n",
    "[INFO] [WallTime: 1505572823.289169] [0.000000] COORDINATES XYZ ==>149.529522407,111192.232732                                                                            \n",
    "[INFO] [WallTime: 1505572823.289821] [0.000000] COORDINATES AXY==>0.0,111227.305636                                                                                       \n",
    "[INFO] [WallTime: 1505572823.290013] [0.000000] COORDINATES UTM==>492967.968254,5638709.36898"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U2-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create now a program that moves the robot to three different GPS positions in space. Steps to follow:\n",
    "* Create a GPS to Pose conversor: This can be accomplished copying what you have just learned on how to use the GPS to coordinates conversor **gps_to_xyz.py**.\n",
    "* Create a client of move_base : This client will be the one in charge of sending to the move_base server the poses. Make a system that allows it to know when it has reached its destination."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Bare in mind that your origin is different from the example, the origin of the GPS is:\n",
    "* origin latitude = 39.5080331\n",
    "* origin longitude = -0.4619816"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U2-3</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": [
    "Here you have an example of move_base client that migh come in handy."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**move_base_client.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#! /usr/bin/env python\n",
    "\n",
    "import rospy\n",
    "import time\n",
    "import actionlib\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseFeedback\n",
    "\n",
    "\"\"\"\n",
    "class SimpleGoalState:\n",
    "    PENDING = 0\n",
    "    ACTIVE = 1\n",
    "    DONE = 2\n",
    "    WARN = 3\n",
    "    ERROR = 4\n",
    "\n",
    "\"\"\"\n",
    "# We create some constants with the corresponing vaules from the SimpleGoalState class\n",
    "PENDING = 0\n",
    "ACTIVE = 1\n",
    "DONE = 2\n",
    "WARN = 3\n",
    "ERROR = 4\n",
    "\n",
    "\"\"\"\n",
    "/move_base/goal\n",
    "### PYTHON MESSAGE\n",
    "\n",
    "rosmsg show move_base_msgs/MoveBaseGoal                                                          \n",
    "geometry_msgs/PoseStamped target_pose                                                                                          \n",
    "  std_msgs/Header header                                                                                                       \n",
    "    uint32 seq                                                                                                                 \n",
    "    time stamp                                                                                                                 \n",
    "    string frame_id                                                                                                            \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",
    "               \n",
    "\n",
    "\n",
    "/move_base/cancel                                                                                                                                                         \n",
    "/move_base/cmd_vel                                                                                                                                                        \n",
    "/move_base/current_goal                                                                                                                                                   \n",
    "/move_base/feedback\n",
    "\"\"\"\n",
    "\n",
    "# definition of the feedback callback. This will be called when feedback\n",
    "# is received from the action server\n",
    "# it just prints a message indicating a new message has been received\n",
    "def feedback_callback(feedback):\n",
    "    rospy.loginfo(str(feedback))\n",
    "\n",
    "# initializes the action client node\n",
    "rospy.init_node('move_base_gps_node')\n",
    "\n",
    "action_server_name = '/move_base'\n",
    "client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)\n",
    "\n",
    "# waits until the action server is up and running\n",
    "rospy.loginfo('Waiting for action Server '+action_server_name)\n",
    "client.wait_for_server()\n",
    "rospy.loginfo('Action Server Found...'+action_server_name)\n",
    "\n",
    "# creates a goal to send to the action server\n",
    "goal = MoveBaseGoal()\n",
    "\n",
    "goal.target_pose.header.frame_id = \"/map\"\n",
    "goal.target_pose.header.stamp    = rospy.get_rostime()\n",
    "goal.target_pose.pose.position.x = 0.0\n",
    "goal.target_pose.pose.orientation.z = 0.0\n",
    "goal.target_pose.pose.orientation.w = 1.0\n",
    "\n",
    "client.send_goal(goal, feedback_cb=feedback_callback)\n",
    "\n",
    "\n",
    "# You can access the SimpleAction Variable \"simple_state\", that will be 1 if active, and 2 when finished.\n",
    "#Its a variable, better use a function like get_state.\n",
    "#state = client.simple_state\n",
    "# state_result will give the FINAL STATE. Will be 1 when Active, and 2 if NO ERROR, 3 If Any Warning, and 3 if ERROR\n",
    "state_result = client.get_state()\n",
    "\n",
    "rate = rospy.Rate(1)\n",
    "\n",
    "rospy.loginfo(\"state_result: \"+str(state_result))\n",
    "\n",
    "while state_result < DONE:\n",
    "    rospy.loginfo(\"Doing Stuff while waiting for the Server to give a result....\")\n",
    "    rate.sleep()\n",
    "    state_result = client.get_state()\n",
    "    rospy.loginfo(\"state_result: \"+str(state_result))\n",
    "    \n",
    "rospy.loginfo(\"[Result] State: \"+str(state_result))\n",
    "if state_result == ERROR:\n",
    "    rospy.logerr(\"Something went wrong in the Server Side\")\n",
    "if state_result == WARN:\n",
    "    rospy.logwarn(\"There is a warning in the Server Side\")\n",
    "\n",
    "#rospy.loginfo(\"[Result] State: \"+str(client.get_result()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations! You are now able to navigate with GPS to unmapped places."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}