Course_SummitXLInstructionManual_Unit_2_old.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 2: Set Outdoors Navigation"
   ]
  },
  {
   "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.<br>\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 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 in case of Summit XL, because it moves by sliding across, the odometry can have quite a lot of error. This is because odometry normaly relies on the fact that the wheels dont slide on the floor and there is 1:1 ratio of movement. This is not the case when you lose traction.  Bare 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.\n"
   ]
  },
  {
   "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.<br>\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": [
    "### Step 1: Disconnect AMCL"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Of course the first thing you have to do is disconnect the localization based on map, because now it wont be reliable when you get out of the mapped area. So you will have to comment the launch of AMCL"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_movebase_new.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",
    "    <!-- Run the map server -->\n",
    "    <include file=\"$(find sumit_xl_tools)/launch/start_map_server.launch\" />\n",
    "    <!--- Run AMCL -->\n",
    "    <!--\n",
    "    <include file=\"$(find sumit_xl_tools)/launch/start_localization.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 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": [
    "This should be your new move_move launch system. As you can see you still launch the map server, but you comment the launch **start_localization.launch** that launches the amcl.<br>\n",
    "The reason you leave the map server is that move base needs a map to plan all its paths.<br>\n",
    "So the next step will be to adapt your old map to a new one that can be used in a larger area."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Upgrate your old map to have a huge free area outside the mapped area"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So this could be your old map image:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/old_clean_map.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see its a huge image but only a small part is white ( free). You have to change that, make it all the grey ( unknown ) area, **free**.<br>\n",
    "For that you can use any software that can biarise an image and turn the grey colour to white. An exaple is this page: https://manytools.org/image/colorize-filter/. You can select the **DualTone** and maximise the contrast to get an image like thisone:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/new_clean_map.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Nice. Save it as **new_clean_map.pgm** and create a new yaml file based on the old one. Because the image is the exact same size and has the same features, you just have to make a copy.<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "image: /absolute_path_to_your_file/new_clean_map.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\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that you have the new image, change that in the map server configuration so that this map is the one loaded:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_map_server.launch**"
   ]
  },
  {
   "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 sumit_xl_tools)/maps/new_clean_map.yaml\"/>\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": {
    "collapsed": true
   },
   "source": [
    "As you can se the map appears in a default position, because there is no transform from the **/map** frame. Why? Because you dont have the AMCL which conected the **map** frame to the odom. So the next step is to publish the necessary TFS that position each element where it should, based in the GPS data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Get the position of the robot based on GPS,Odometry and IMU data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In indoor navigation you had as origin of the TF tree the **/map** frame. And from there you positioned the **odom** frame of the robot thanks to **AMCL** localization.<br>\n",
    "Now there is NO AMCL, and therefore who will position the robot in the map?<br>\n",
    "More over, who will position now the **map**? Because it has to be positioned based on GPS coordinates."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And here is where the package <a href=\"http://docs.ros.org/kinetic/api/robot_localization/html/index.html\">robot_localization</a> comes into play.<br>\n",
    "This package allows you to localise a Robot based on infinite number of inputs. You can merge the data from AMCL, GPS, Vision.. To get the best localization through Kalman Filters.<br>\n",
    "In this course we will only talk about the tool <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 soe 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": [
    "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 to move Haro</p><br>\n",
    "rosrun your_package 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 to move Haro</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 XL based only on its GPS, Odometry and IMU.<br>\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 publishers for MAP and Odom frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will have to publish the position of the robot in from of TF, so that afterwards , the move_base planner can generate atransform from the robots location to the map location.<br>\n",
    "So now you have to create a python script that publishes that TF."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**publish_world_to_odom_and_map.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python \n",
    "\n",
    "import rospy\n",
    "import tf\n",
    "from nav_msgs.msg import Odometry\n",
    "\n",
    "class GPSXY(object):\n",
    "    def __init__(self):\n",
    "        rospy.Subscriber(\"/odometry/gps\", Odometry, self.callback)\n",
    "        self.x = 0.0\n",
    "        self.y= 0.0\n",
    "\n",
    "    def callback(self,msg):\n",
    "        self.x = msg.pose.pose.position.x\n",
    "        self.y= msg.pose.pose.position.y\n",
    "        \n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('utm_to_map_node')\n",
    "    br = tf.TransformBroadcaster()\n",
    "    rate = rospy.Rate(3.0)\n",
    "        \n",
    "    gps_pos = GPSXY()\n",
    "    \n",
    "    while not rospy.is_shutdown():\n",
    "        br.sendTransform((gps_pos.x, gps_pos.y, 0.0),\n",
    "                         (0.0, 0.0, 0.0, 1.0),\n",
    "                         rospy.Time.now(),\n",
    "                         \"odom\",\n",
    "                         \"gps_origin\")\n",
    "                         \n",
    "        rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So here you now can publish the position of the robot based on its GPS.<br>\n",
    "Note that you are publishing from **odom** to **gps_origin**.This is where you want to consider the local GPS origin. You could instead of using this, publish the UTM origin, but for practical reasons, its better to publish one that its in close range of your location.<br>\n",
    "And where is this Origin? Well in this case you are going to set it to the latitude and logintude of the officies of <a href=\"http://www.robotnik.es/\">Robotnik</a>. This is latitude = 39.5080331, and longitude = -0.4619816. 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": [
    "So now the next step is to position the **/map** frame relative to the **/gps_origin** frame. This way you will now have a TF connectig the Robot with the map in someway, which is what the move_base needs.<br>\n",
    "Just update the previous **publish_world_to_odom_and_map.py** version to this new one."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "NEW **publish_world_to_odom_and_map.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",
    "    # Robotnik position at Fuente del Jarro\n",
    "    olat = 39.5080331\n",
    "    olon = -0.4619816\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 ==>\"+str(xg2)+\",\"+str(yg2))\n",
    "    rospy.loginfo(\"COORDINATES ==>\"+str(xa)+\",\"+str(ya))\n",
    "    rospy.loginfo(\"COORDINATES ==>\"+str(utmx)+\",\"+str(utmy))\n",
    "\n",
    "    return xg2, yg2\n",
    "\n",
    "class GPSXY(object):\n",
    "    def __init__(self):\n",
    "        rospy.Subscriber(\"/odometry/gps\", Odometry, self.callback)\n",
    "        self.x = 0.0\n",
    "        self.y= 0.0\n",
    "\n",
    "    def callback(self,msg):\n",
    "        \n",
    "        self.x = msg.pose.pose.position.x\n",
    "        self.y= msg.pose.pose.position.y\n",
    "        \n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    rospy.init_node('utm_to_map_node')\n",
    "    br = tf.TransformBroadcaster()\n",
    "    rate = rospy.Rate(3.0)\n",
    "    xg2, yg2 = get_xy_based_on_lat_long(lat=39.5080331,lon=-0.4619816, name=\"MAP\")\n",
    "    \n",
    "    gps_pos = GPSXY()\n",
    "    \n",
    "    while not rospy.is_shutdown():\n",
    "        br.sendTransform((gps_pos.x, gps_pos.y, 0.0),\n",
    "                         (0.0, 0.0, 0.0, 1.0),\n",
    "                         rospy.Time.now(),\n",
    "                         \"odom\",\n",
    "                         \"gps_origin\")\n",
    "                         \n",
    "        br.sendTransform((xg2, yg2, 0.0),\n",
    "                         (0.0, 0.0, 0.0, 1.0),\n",
    "                         rospy.Time.now(),\n",
    "                         \"map\",\n",
    "                         \"gps_origin\")\n",
    "\n",
    "        rate.sleep()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you are using a new ROS package called <a href=\"http://wiki.ros.org/geonav_transform\">geonav_transform</a>. This package gives you tools to work with all the tranformations from GPS to Cartesian space and to UTM space. More information about UTM <a href=\"https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system\">here</a>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Comment on some code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "br.sendTransform((xg2, yg2, 0.0),\n",
    "                         (0.0, 0.0, 0.0, 1.0),\n",
    "                         rospy.Time.now(),\n",
    "                         \"map\",\n",
    "                         \"gps_origin\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is the core. Here you are publishing the Tranform from the map frame to the already published gps_origin. This will position the map."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "xg2, yg2 = get_xy_based_on_lat_long(lat=39.5080331,lon=-0.4619816, name=\"MAP\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function is the one responsible of converting latitude and longitude to the Cartesian. It sets as origin olat = 39.5080331 and olon = -0.4619816. But you could set it alse where. In this particular case the map is set exactly in the same place as the **gps_origin** but thats just because the map is considered to be generated there. In reality it will be probably different."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_xy_based_on_lat_long(lat,lon, name):\n",
    "    # Define a local orgin, latitude and longitude in decimal degrees\n",
    "    # Robotnik position at Fuente del Jarro\n",
    "    olat = 39.5080331\n",
    "    olon = -0.4619816\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 ==>\"+str(xg2)+\",\"+str(yg2))\n",
    "    rospy.loginfo(\"COORDINATES ==>\"+str(xa)+\",\"+str(ya))\n",
    "    rospy.loginfo(\"COORDINATES ==>\"+str(utmx)+\",\"+str(utmy))\n",
    "\n",
    "    return xg2, yg2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can see that there are other conversions made. One of them is utm, which would be if you desire to use UTM frame as refference."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U2-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remove the AMCL launch from your move_base launch, and add:\n",
    "* The **start_navsat.launch** to start the publications of /odometry/gps\n",
    "* The launch of the python script **publish_world_to_odom_and_map.py** to publish the TF of map and odom.\n",
    "The objective is to get in RVIZ the same exact appearance as when AMCL was working."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U2-1</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": [
    "So you should have got something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_manual_unit2_gps_localization2.gif\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And you should get something similar to this when setting a pose to move to:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/summit_xl_manual_unit2_gps_localization_pose.gif\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Tha launch file for the move base should be something similar to this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<?xml version=\"1.0\"?>\n",
    "<!-- NEW SUMIT XL NAVIGATION -->\n",
    "<launch>\n",
    "\n",
    "    <node pkg=\"my_sumit_xl_tools\" type=\"publish_world_to_odom_and_map.py\" respawn=\"false\" name=\"utm_to_map_node\" output=\"screen\"/>\n",
    "\n",
    "    <!-- Start GPS to XY publication of the robot position -->\n",
    "    \n",
    "\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_navsat.launch\" />\n",
    "\n",
    "    \n",
    "    <!-- Run the map server -->\n",
    "    <include file=\"$(find my_sumit_xl_tools)/launch/start_map_server.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 my_sumit_xl_tools)/new_config/planner.yaml\" command=\"load\"/>\n",
    "        \n",
    "        <!-- observation sources located in costmap_common.yaml -->\n",
    "        <rosparam file=\"$(find my_sumit_xl_tools)/new_config/costmap_common.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "        <rosparam file=\"$(find my_sumit_xl_tools)/new_config/costmap_common.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "        \n",
    "        <!-- local costmap, needs size -->\n",
    "        <rosparam file=\"$(find my_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 my_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 my_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>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One Important improvement is the fact that because GPS has such a high level of error, you will have to change the tolerance to error of the move_base system. THats why you have to eddit the file **planner.yaml** and change the following part:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "  # Goal Tolerance Parameters  \n",
    "  yaw_goal_tolerance: 0.1\n",
    "  xy_goal_tolerance: 3.0\n",
    "  # xy_goal_tolerance: 0.2\n",
    "  latch_xy_goal_tolerance: false"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The xy_goal_tolerance for planar position. The yaw should be quite precise because it comes from the IMU."
   ]
  },
  {
   "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>\n",
    "<img src=\"img/summit_xl_instruction_manual_unit2_gpsmap2.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But this is not what you should see, because if you retrieve the address from the <a href=\"https://www.gps-coordinates.net/\">Google page</a> based on the origin latitude and longitude (olat = 39.5080331 olon = -0.4619816), 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_config/gps.rviz**<br>\n",
    "* You can set it manually as follows:\n",
    "    ![Camera Icon Image](img/summit_xl_unit2_add_mapbox.png)<br>\n",
    "    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/.\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If everythin went well , RVIZ should show something like this:\n",
    "![RViz Gps General](img/summit_xl_unit2_gpsimage_rviz.png)<br>\n",
    "![RViz Gps General](img/summit_xl_unit2_gpsimage_rviz2.png)<br>"
   ]
  },
  {
   "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": [
    "<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 already done for the map TF publication.\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.\n",
    "You migh have to move by keyboard the robot until it gets out of the house, depending on the value syou set in the planner. If it can go through doors, then no need for this step."
   ]
  },
  {
   "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": {
    "collapsed": true
   },
   "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
}