Course_JackalManual_Unit1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Unit 1: Navigation Indoor"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_intro.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/Clearpath-Logo.png\" width=\"300\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"300\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So in this unit you will learn how to create your own navigation script, generate maps and save them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first thing you will need for a robot that patroles is the ability to move around without crashing into everything. For this you will learn:<br>\n",
    "* How to move the robot\n",
    "* How to navigate without map.\n",
    "* How to map the environment\n",
    "* How to set the navigation stack to be able to send him a location and be able to reach that destination.\n",
    "* How to send it a sequence of waypoints and execute those movements."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How to Move the Robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To move Jackal, as you saw in the introduction you can do it many ways, but in reality all do the same thing: **publish into the /cmd_vel topic**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Publish through the web shell in the mentioned topic and see if you can make Jackal move forwards, backwards, left, right and in circles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-1</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should have done something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p><br>\n",
    "rostopic pub /cmd_vel geometry_msgs/Twist [TAB][TAB]\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With this command you can then fill in the structure that appears after hitting TAB key two consecutive times, to publish the message."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a package that you  will use along the rest of this course where you can have all your programs. Use the name **my_jackal_tools** for future refference.<br>\n",
    "Then create a program that moves jackal around publishing in the **/cmd_vel** topic. See how it moves and its limitations. This is crucial to know what you can do with your robot.<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-2</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How to navigate without map"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you will learn how to reproduce the system that you launched in the Demo to make Jackal navigate only with the map generated localy by the detection of obstacles."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we will go over the files and only point out particular elements for Jackal. If you want to know more details on how navigation is performed in ROS, please do consider to do **RobotIgnites Navigation in ROS Course** before continuing."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create the launch file for move base"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is the core of how to make Jackal move around. The package <a href=\"http://wiki.ros.org/move_base\">move_base</a> makes all the path planning for you.<br>\n",
    "You will use **move_base** for all the navigation, but here it only has Jackals sensors to detect obstacles.<br>\n",
    "Here you have an example of how **move_base** should be launched."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**odom_navigation.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    " <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/global_costmap_params.yaml\" command=\"load\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/local_costmap_params.yaml\" command=\"load\" />\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/base_local_planner_params.yaml\" command=\"load\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/move_base_params.yaml\" command=\"load\" />\n",
    "\n",
    "  <param name=\"base_global_planner\" type=\"string\" value=\"navfn/NavfnROS\" />\n",
    "  <param name=\"base_local_planner\" value=\"base_local_planner/TrajectoryPlannerROS\"/>\n",
    "\n",
    "  <remap from=\"odom\" to=\"odometry/filtered\" />\n",
    " </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we have some elements to comment on:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically **move_base** takes in configuration files of .yaml format that define everything. From which **planner** its going to use, to how the maps are used or sensors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**costmap_common_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "map_type: costmap\n",
    "origin_z: 0.0\n",
    "z_resolution: 1\n",
    "z_voxels: 2\n",
    "\n",
    "obstacle_range: 2.5\n",
    "raytrace_range: 3.0\n",
    "\n",
    "publish_voxel_map: false\n",
    "transform_tolerance: 0.5\n",
    "meter_scoring: true\n",
    "\n",
    "footprint: [[-0.21, -0.165], [-0.21, 0.165], [0.21, 0.165], [0.21, -0.165]]\n",
    "footprint_padding: 0.1\n",
    "\n",
    "plugins:\n",
    "- {name: obstacles_layer, type: \"costmap_2d::ObstacleLayer\"}\n",
    "- {name: inflater_layer, type: \"costmap_2d::InflationLayer\"}\n",
    "\n",
    "obstacles_layer:\n",
    "  observation_sources: scan\n",
    "  scan: {sensor_frame: front_laser, data_type: LaserScan, topic: front/scan, marking: true, clearing: true, min_obstacle_height: -2.0, max_obstacle_height: 2.0, obstacle_range: 2.5, raytrace_range: 3.0}\n",
    "\n",
    "inflater_layer:\n",
    " inflation_radius: 0.30\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Just note the **obstacles_layer**, that uses the topic **/front/scan** to read the laser readings. The camera is not used for navigation.<br>\n",
    "Also note the ray trace range is only 3 meters , when its <a href=\"https://www.sick.com/media/pdf/2/42/842/dataSheet_LMS111-10100_1041114_en.pdf\">SICK laser</a> has a range up to 20 meters. This is just to make detections faster and not have in mind areas that arent close enough."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**global_costmap_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_costmap:\n",
    "  global_frame: odom\n",
    "  robot_base_frame: base_link\n",
    "  update_frequency: 20\n",
    "  publish_frequency: 5\n",
    "  width: 40.0\n",
    "  height: 40.0\n",
    "  resolution: 0.05\n",
    "  origin_x: -20.0\n",
    "  origin_y: -20.0\n",
    "  static_map: true\n",
    "  rolling_window: false"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**local_costmap_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "local_costmap:\n",
    "   global_frame: odom\n",
    "   robot_base_frame: base_link\n",
    "   update_frequency: 20.0\n",
    "   publish_frequency: 5.0\n",
    "   width: 10.0\n",
    "   height: 10.0\n",
    "   resolution: 0.05\n",
    "   static_map: false\n",
    "   rolling_window: true"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**base_local_planner_params**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "TrajectoryPlannerROS:\n",
    "\n",
    "  # Robot Configuration Parameters\n",
    "  acc_lim_x: 10.0\n",
    "  acc_lim_theta:  20.0\n",
    "\n",
    "  max_vel_x: 0.5\n",
    "  min_vel_x: 0.1\n",
    "\n",
    "  max_vel_theta: 1.57\n",
    "  min_vel_theta: -1.57\n",
    "  min_in_place_vel_theta: 0.314\n",
    "\n",
    "  holonomic_robot: false\n",
    "  escape_vel: -0.5\n",
    "\n",
    "  # Goal Tolerance Parameters\n",
    "  yaw_goal_tolerance: 0.157\n",
    "  xy_goal_tolerance: 0.25\n",
    "  latch_xy_goal_tolerance: false\n",
    "\n",
    "  # Forward Simulation Parameters\n",
    "  sim_time: 2.0\n",
    "  sim_granularity: 0.02\n",
    "  angular_sim_granularity: 0.02\n",
    "  vx_samples: 6\n",
    "  vtheta_samples: 20\n",
    "  controller_frequency: 20.0\n",
    "\n",
    "  # Trajectory scoring parameters\n",
    "  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).\n",
    "  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01\n",
    "  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6\n",
    "  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8\n",
    "\n",
    "  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories\n",
    "  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false\n",
    "  heading_scoring_timestep: 0.8   #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)\n",
    "  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout\n",
    "  simple_attractor: false\n",
    "  publish_cost_grid_pc: true\n",
    "\n",
    "  #Oscillation Prevention Parameters\n",
    "  oscillation_reset_dist: 0.05 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)\n",
    "  escape_reset_dist: 0.1\n",
    "  escape_reset_theta: 0.1\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note some parameters like:\n",
    "* holonomic_robot: Jackal is non holonomic\n",
    "* yaw_goal_tolerance and xy_goal_tolerance are relatively low."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**move_base_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "shutdown_costmaps: false\n",
    "\n",
    "controller_frequency: 20.0\n",
    "controller_patience: 15.0\n",
    "\n",
    "planner_frequency: 20.0\n",
    "planner_patience: 5.0\n",
    "\n",
    "oscillation_timeout: 0.0\n",
    "oscillation_distance: 0.5\n",
    "\n",
    "recovery_behavior_enabled: true\n",
    "clearing_rotation_allowed: true"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the **clearing_rotation_allowed** which meand that it will turn around to clear space if for some reason it gets lost to try and recover itself. Its also usefull for situations when a moving obstacle was detected in its path. By turning it gives time to the obstacle to move out the way and therefore clear the localcostmap."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For **localPlanner** the **TrajectoryPlannerROS** is used. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For **globalPlanner** the **navfn/NavfnROS** is used. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Inside the new **my_jackal_tools** create the files given examples and the launch file **odom_navigation.launch**.\n",
    "* Then launch rviz through the command:"
   ]
  },
  {
   "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",
    "rosrun rviz rviz\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Add the RVIZ elements: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_rvizelements_odomnav.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Note the two paths, one for the local and another for the global planner.\n",
    "* One map only for the **LocalCostmap**. In this case both local and global maps are the same.\n",
    "* **move_base_simple/goal** will be the value of the topic for the **Pose**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember to save the configuration of RVIZ when finished to use it afterwards. Name it **navigation** and save it in a folder called **rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get the same result as in the Demo "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit0_nav_odom.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Navigate with a MAP"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you are going to learn how to navigate with a map. But first you have to generate that map:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create a Map"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To create a Map you need to launch two elements: **GMapping** and **MoveBase**.\n",
    "* **GMapping**: in the one in charge of generating and saving the map. More info <a href=\"http://wiki.ros.org/gmapping\">here</a>.\n",
    "* **MoveBase**: The one in charge of sending the commands to /cmd_vel to move the robot around based on sensors and now generating map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So you can create a launch file that launches both:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_mapping.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "  <!--- Run gmapping -->\n",
    "  <include file=\"$(find my_jackal_tools)/launch/gmapping.launch\" />\n",
    "\n",
    "  <!--- Run Move Base -->\n",
    "  <include file=\"$(find my_jackal_tools)/launch/with_map_move_base.launch\" />\n",
    "\n",
    "</launch>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So first, take a look at the **gmapping.launch**. It should be something similar to this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "  <arg name=\"scan_topic\" default=\"front/scan\" />\n",
    "\n",
    "  <node pkg=\"gmapping\" type=\"slam_gmapping\" name=\"slam_gmapping\" output=\"screen\">\n",
    "\n",
    "    <param name=\"odom_frame\" value=\"odom\"/>\n",
    "    <param name=\"base_frame\" value=\"base_link\"/>\n",
    "    <param name=\"map_frame\" value=\"map\"/>\n",
    "\n",
    "    <!-- Process 1 out of every this many scans (set it to a higher number to skip more scans)  -->\n",
    "    <param name=\"throttle_scans\" value=\"1\"/>\n",
    "\n",
    "    <param name=\"map_update_interval\" value=\"5.0\"/> <!-- default: 5.0 -->\n",
    "\n",
    "    <!-- The maximum usable range of the laser. A beam is cropped to this value.  -->\n",
    "    <param name=\"maxUrange\" value=\"5.0\"/>\n",
    "\n",
    "    <!-- The maximum range of the sensor. If regions with no obstacles within the range of the sensor should appear as free space in the map, set maxUrange < maximum range of the real sensor <= maxRange -->\n",
    "    <param name=\"maxRange\" value=\"10.0\"/>\n",
    "\n",
    "    <param name=\"sigma\" value=\"0.05\"/>\n",
    "    <param name=\"kernelSize\" value=\"1\"/>\n",
    "    <param name=\"lstep\" value=\"0.05\"/>\n",
    "    <param name=\"astep\" value=\"0.05\"/>\n",
    "    <param name=\"iterations\" value=\"5\"/>\n",
    "    <param name=\"lsigma\" value=\"0.075\"/>\n",
    "    <param name=\"ogain\" value=\"3.0\"/>\n",
    "    <param name=\"minimumScore\" value=\"0.0\"/>\n",
    "    <!-- Number of beams to skip in each scan. -->\n",
    "    <param name=\"lskip\" value=\"0\"/>\n",
    "\n",
    "    <param name=\"srr\" value=\"0.01\"/>\n",
    "    <param name=\"srt\" value=\"0.02\"/>\n",
    "    <param name=\"str\" value=\"0.01\"/>\n",
    "    <param name=\"stt\" value=\"0.02\"/>\n",
    "\n",
    "    <!-- Process a scan each time the robot translates this far  -->\n",
    "    <param name=\"linearUpdate\" value=\"0.1\"/>\n",
    "\n",
    "    <!-- Process a scan each time the robot rotates this far  -->\n",
    "    <param name=\"angularUpdate\" value=\"0.05\"/>\n",
    "\n",
    "    <param name=\"temporalUpdate\" value=\"-1.0\"/>\n",
    "    <param name=\"resampleThreshold\" value=\"0.5\"/>\n",
    "\n",
    "    <!-- Number of particles in the filter. default 30        -->\n",
    "    <param name=\"particles\" value=\"10\"/>\n",
    "\n",
    "<!-- Initial map size  -->\n",
    "    <param name=\"xmin\" value=\"-10.0\"/>\n",
    "    <param name=\"ymin\" value=\"-10.0\"/>\n",
    "    <param name=\"xmax\" value=\"10.0\"/>\n",
    "    <param name=\"ymax\" value=\"10.0\"/>\n",
    "\n",
    "    <!-- Processing parameters (resolution of the map)  -->\n",
    "    <param name=\"delta\" value=\"0.02\"/>\n",
    "\n",
    "    <param name=\"llsamplerange\" value=\"0.01\"/>\n",
    "    <param name=\"llsamplestep\" value=\"0.01\"/>\n",
    "    <param name=\"lasamplerange\" value=\"0.005\"/>\n",
    "    <param name=\"lasamplestep\" value=\"0.005\"/>\n",
    "\n",
    "    <remap from=\"scan\" to=\"$(arg scan_topic)\"/>\n",
    "  </node>\n",
    "</launch>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here are basically all the pareameters related to the map generation defined.<br>\n",
    "Note the maxUrange = 5.0 meters and maxRange = 10 meters. As you see here the laser is also cut of to emprove performance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**with_map_move_base.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    " \n",
    "  <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "\n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "    \n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/map_nav_params/local_costmap_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/map_nav_params/global_costmap_params.yaml\" command=\"load\" />\n",
    "    \n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/base_local_planner_params.yaml\" command=\"load\" />\n",
    "    <rosparam file=\"$(find my_jackal_tools)/params/move_base_params.yaml\" command=\"load\" />\n",
    "    \n",
    "    <param name=\"base_global_planner\" type=\"string\" value=\"navfn/NavfnROS\" />\n",
    "    <param name=\"base_local_planner\" value=\"base_local_planner/TrajectoryPlannerROS\"/>\n",
    "    \n",
    "    <remap from=\"odom\" to=\"odometry/filtered\" />\n",
    "  </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**odom_navigation.launch**"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "<launch>\n",
    "\n",
    " <node pkg=\"move_base\" type=\"move_base\" respawn=\"false\" name=\"move_base\" output=\"screen\">\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"global_costmap\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/costmap_common_params.yaml\" command=\"load\" ns=\"local_costmap\" />\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/global_costmap_params.yaml\" command=\"load\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/local_costmap_params.yaml\" command=\"load\" />\n",
    "\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/base_local_planner_params.yaml\" command=\"load\" />\n",
    "  <rosparam file=\"$(find my_jackal_tools)/params/move_base_params.yaml\" command=\"load\" />\n",
    "\n",
    "  <param name=\"base_global_planner\" type=\"string\" value=\"navfn/NavfnROS\" />\n",
    "  <param name=\"base_local_planner\" value=\"base_local_planner/TrajectoryPlannerROS\"/>\n",
    "\n",
    "  <remap from=\"odom\" to=\"odometry/filtered\" />\n",
    " </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you can see there are some differences with the **odom_navigation.launch**.<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<rosparam file=\"$(find my_jackal_tools)/params/map_nav_params/local_costmap_params.yaml\" command=\"load\" />\n",
    "<rosparam file=\"$(find my_jackal_tools)/params/map_nav_params/global_costmap_params.yaml\" command=\"load\" />\n",
    "\n",
    "*****************\n",
    "\n",
    "<rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/global_costmap_params.yaml\" command=\"load\" />\n",
    "<rosparam file=\"$(find my_jackal_tools)/params/odom_nav_params/local_costmap_params.yaml\" command=\"load\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **only** difference is in the **global and local costmaps**. Which is understandable because outside that, the res is only related to move base internals and sensors. These are the only ones that talk about maps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**local_costmap_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "local_costmap:\n",
    "   global_frame: map\n",
    "   robot_base_frame: base_link\n",
    "   update_frequency: 20.0\n",
    "   publish_frequency: 5.0\n",
    "   width: 10.0\n",
    "   height: 10.0\n",
    "   resolution: 0.05\n",
    "   static_map: false\n",
    "   rolling_window: true"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_frame: map\n",
    "    \n",
    "***\n",
    "\n",
    "global_frame: odom"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The only difference is that you are changing of **global frame**. Now you calculate all the paths based on the map frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**global_costmap_params.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_costmap:\n",
    "   global_frame: map\n",
    "   robot_base_frame: base_link\n",
    "   update_frequency: 20.0\n",
    "   publish_frequency: 5.0\n",
    "   width: 40.0\n",
    "   height: 40.0\n",
    "   resolution: 0.05\n",
    "   origin_x: -20.0\n",
    "   origin_y: -20.0\n",
    "   static_map: true\n",
    "   rolling_window: false\n",
    "\n",
    "   plugins:\n",
    "   - {name: static_layer, type: \"costmap_2d::StaticLayer\"}\n",
    "   - {name: obstacles_layer, type: \"costmap_2d::ObstacleLayer\"}\n",
    "   - {name: inflater_layer, type: \"costmap_2d::InflationLayer\"}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "global_frame: map\n",
    "\n",
    "plugins:\n",
    "- {name: static_layer, type: \"costmap_2d::StaticLayer\"}\n",
    "- {name: obstacles_layer, type: \"costmap_2d::ObstacleLayer\"}\n",
    "- {name: inflater_layer, type: \"costmap_2d::InflationLayer\"}\n",
    "\n",
    "****\n",
    "\n",
    "global_frame: odom"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The only differences are:\n",
    "* The Global frame used.\n",
    "* Use of plugins: These are plugins that their only function is to generate the costmaps. See <a href=\"http://wiki.ros.org/costmap_2d\">here</a> for more infomation ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Inside the new **my_jackal_tools** create the files given examples and the launch file **start_mapping.launch**.\n",
    "* Then launch rviz through the command:"
   ]
  },
  {
   "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",
    "rosrun rviz rviz\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Add the RVIZ elements: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_rvizelements_gmappingnav.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Note the two Maps, one reads the **/map** topic. The Other the **GlobalCostmap**.<br>\n",
    "The other values are the same as the No map configuration."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember to save the configuration of RVIZ when finished to use it afterwards. Name it **gmapping** and save it in a folder called **rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-4</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get smething similar to this if everything is ok:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_gmapping1.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You see that instantly it started to generate the map. No its time to move Jackal around.<br>\n",
    "You can move it through the **start_teleop.launch** used in the Demo, by the **InteractiveMarkers** or better: **through poses in the map**!<br>\n",
    "This works because you are navigating in the same way as you were in the **No Map navigation**. So you can use 2D Poses to generate the map.<br>\n",
    "Just **Be Carefull** not to give very far away poses, because remmember you dont have a map and the path planning is not as good."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_mapcreation.gif\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you have all the map as you would like, then you have to save it to be able to use it for navigation.<br>\n",
    "For this you have to execute the following command where you want the map files to be saved. You have a sugesstion on where to put it but its up to you:"
   ]
  },
  {
   "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",
    "roscd my_jackal_tools<br>\n",
    "mkdir maps<br>\n",
    "cd maps<br>\n",
    "rosrun map_server map_saver -f mymap\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This should have generated two files: **mymap.pgm and mymap.yaml**.<br>\n",
    "You will use the **pgm** image for cleaning the map generated, and the **yaml** is the file directly used by the PathPLanning afterwards. It also contains valueble information like the path to the image of the maps dimmensions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**mymap.yaml**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "image: 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",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see here, the file **mymap.pgm** is in the same location as the **mymap.yaml**. Thats why the imgae variable only has the name of the image. But here can go a full absolute path."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Navigate with your map"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And finally you now have to launch the package **amcl** to localise Jackal with the map. For that you create the following launch:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_navigation_with_map.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "    <!-- Run the map server -->\n",
    "    <arg name=\"map_file\" default=\"$(find jackal_tools)/maps/mymap.yaml\"/>\n",
    "    <node name=\"map_server\" pkg=\"map_server\" type=\"map_server\" args=\"$(arg map_file)\" />\n",
    "    \n",
    "    <!--- Run AMCL -->\n",
    "    <include file=\"$(find jackal_tools)/launch/amcl.launch\" />\n",
    "    \n",
    "    <!--- Run Move Base -->\n",
    "    <include file=\"$(find jackal_tools)/launch/with_map_move_base.launch\" />\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here there three main parts:\n",
    "* **map_server**: The Node that publishes the map saved.\n",
    "* **amcl**: The node that localises Jackal based on map readings\n",
    "* **move_base**: This move base has the configuration of moving with a map."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So really there is only one launch file to comment on which is:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**amcl.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "\n",
    "  <arg name=\"use_map_topic\" default=\"false\"/>\n",
    "  <arg name=\"scan_topic\" default=\"front/scan\" />\n",
    "\n",
    "  <node pkg=\"amcl\" type=\"amcl\" name=\"amcl\">\n",
    "    <param name=\"use_map_topic\" value=\"$(arg use_map_topic)\"/>\n",
    "    <!-- Publish scans from best pose at a max of 10 Hz -->\n",
    "    <param name=\"odom_model_type\" value=\"diff\"/>\n",
    "    <param name=\"odom_alpha5\" value=\"0.1\"/>\n",
    "    <param name=\"gui_publish_rate\" value=\"10.0\"/>\n",
    "    <param name=\"laser_max_beams\" value=\"720\"/>\n",
    "    <param name=\"laser_min_range\" value=\"0.1\"/>\n",
    "    <param name=\"laser_max_range\" value=\"30.0\"/>\n",
    "    <param name=\"min_particles\" value=\"500\"/>\n",
    "    <param name=\"max_particles\" value=\"2000\"/>\n",
    "    <!-- Maximum error between the true distribution and the estimated distribution. -->\n",
    "    <param name=\"kld_err\" value=\"0.05\"/>\n",
    "    <param name=\"kld_z\" value=\"0.99\"/>\n",
    "    <param name=\"odom_alpha1\" value=\"0.2\"/>\n",
    "    <param name=\"odom_alpha2\" value=\"0.2\"/>\n",
    "    <!-- translation std dev, m -->\n",
    "    <param name=\"odom_alpha3\" value=\"0.2\"/>\n",
    "    <param name=\"odom_alpha4\" value=\"0.2\"/>\n",
    "    <param name=\"laser_z_hit\" value=\"0.5\"/>\n",
    "    <param name=\"laser_z_short\" value=\"0.05\"/>\n",
    "    <param name=\"laser_z_max\" value=\"0.05\"/>\n",
    "    <param name=\"laser_z_rand\" value=\"0.5\"/>\n",
    "    <param name=\"laser_sigma_hit\" value=\"0.2\"/>\n",
    "    <param name=\"laser_lambda_short\" value=\"0.1\"/>\n",
    "    <param name=\"laser_model_type\" value=\"likelihood_field\"/>\n",
    "    <!-- Maximum distance to do obstacle inflation on map, for use in likelihood_field model. -->\n",
    "    <param name=\"laser_likelihood_max_dist\" value=\"2.0\"/>\n",
    "    <!-- Translational movement required before performing a filter update.  -->\n",
    "    <param name=\"update_min_d\" value=\"0.1\"/>\n",
    "    <!--Rotational movement required before performing a filter update. -->\n",
    "    <param name=\"update_min_a\" value=\"0.314\"/>\n",
    "    <param name=\"odom_frame_id\" value=\"odom\"/>\n",
    "    <param name=\"base_frame_id\" value=\"base_link\"/>\n",
    "    <param name=\"global_frame_id\" value=\"map\"/>\n",
    "    <!-- Number of filter updates required before resampling. -->\n",
    "    <param name=\"resample_interval\" value=\"1\"/>\n",
    "    <!-- Increase tolerance because the computer can get quite busy -->\n",
    "    <param name=\"transform_tolerance\" value=\"1.0\"/>\n",
    "    <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001. -->\n",
    "    <param name=\"recovery_alpha_slow\" value=\"0.0\"/>\n",
    "    <!--Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1. -->\n",
    "    <param name=\"recovery_alpha_fast\" value=\"0.1\"/>\n",
    "    <!-- Initial pose mean -->\n",
    "    <param name=\"initial_pose_x\" value=\"0.0\" />\n",
    "    <param name=\"initial_pose_y\" value=\"0.0\" />\n",
    "    <param name=\"initial_pose_a\" value=\"0.0\" />\n",
    "    <!-- When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->\n",
    "    <param name=\"receive_map_topic\" value=\"true\"/>\n",
    "    <!--  When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received. -->\n",
    "    <param name=\"first_map_only\" value=\"false\"/>\n",
    "    <remap from=\"scan\" to=\"$(arg scan_topic)\"/>\n",
    "  </node>\n",
    "\n",
    "</launch>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-5</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Inside the new **my_jackal_tools** create the files given examples and the launch file **start_navigation_with_map.launch**.\n",
    "* Then launch rviz through the command:"
   ]
  },
  {
   "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",
    "rosrun rviz rviz\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Add the RVIZ elements: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_rvizelements_navwithmap.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Note the two Paths, **local** and **global**<br>\n",
    "* Note the particle Cloud, that indicates where Jackal thinks he is. Its reading from the topic **/particlecloud**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember to save the configuration of RVIZ when finished to use it afterwards. Name it **lacalization** and save it in a folder called **rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">END Exercise U1-5</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When launching this could happen:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_errorinestimate.png\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is because Jackal doesn t know where it is initialy, so you have to indicate the position and orientation with the **2D Estimate Pose** icon. Once done, it should navigate without problems. It will localize itself after a few seconds of movement. You know it is well localised because all the arrows are conscentrated into the position of the robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_finalnav.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add WayPoints"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So there you have it. Now your Jackal can navigate around one position at a time. But what if you want to generate in an easy way a sequence of waypoints so that it patroles around the **main interest points of the area**?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are going to use a ROS package called **follow_waypoints**, you can get more information http://wiki.ros.org/follow_waypoints.\n",
    "This package basically tracks the **Estimate pose** that you place in RVIZ and stores it. Then when you have finished you just have to publish in a topic that starts sending those positions to the movebase system."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Steps to do it:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to start the **start_navigation_with_map.launch** if you don't have it running:"
   ]
  },
  {
   "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_jackal_tools start_navigation_with_map.launch\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Second step is to start the waypoint server. This server will listen to publications into the topic **/initialpose** and store those poses until its instructed to send them to **move_base** to be executed:"
   ]
  },
  {
   "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",
    "roslaunch follow_waypoints follow_waypoints.launch\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then you have to start RVIZ, and add a **PoseArray** element, susbcribed to the topic **/waypoints**, and change its name to WayPoints. This will now show all the waypoints you set from now on."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can change the colour also if you want. In this example its changed to **green** to see it better and differenciate from the posearray of **psoition estimates**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you have to set the waypoints. For this you have to select the **PoseEstimate** and set it where you want it. Because this will move the estimated pose of the robot around its important that **the last waypoint is where the robot is right now**. Otherwise it will be harder for the robot to get to the place. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_waypoints2.gif\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_waypoints1.png\" width=\"600\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should also get in webshell where you launched the waypoint server a message stating that it recieved the waypoint:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "[INFO][139816504842048][/follow_waypoints/execute:92]: Waiting to recieve waypoints via Pose msg on topic /initialpose         \n",
    "[INFO][139816504842048][/follow_waypoints/execute:93]: To start following waypoints: 'rostopic pub /path_ready std_msgs/Empty -\n",
    "1'                                                                                                                             \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint                                                  \n",
    "[INFO][139816504842048][/follow_waypoints/execute:104]: Recieved new waypoint"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now you just have to publish in the topic **/path_ready** to start sending waypoints to the movebase."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you want to see the global path generated just activate it in RVIZ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #3</p><br>\n",
    "rostopic pub /path_ready std_msgs/Empty -1\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should get something similar to this for each waypoint executed. Bare in mind that because you move the robot around, it will probably first do a recovery behaviour to clean up all the false obstacles generated through changing the estipate pose:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_waypoints3.gif\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After the recovery behaviour will come the execution of each way point:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_waypoints4.gif\"></img>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, its a nice way to do it , but its a bit cumbersome and messy. Specially because it has to change the initial position of the robot which is not very desirable. This is because this waypoint manager listenes to the topic **/initialpose**. Th eproblem is that **amlc localiser** also. Thats why when changed, the robot localises itself also in another place.<br>\n",
    "So the next step is creating your own version. So you will part from the code that you just used that is installed in the system and is courtsesy of <a href=\"https://github.com/danielsnider\">Daniel Snider</a> ,who created the original Waypoints manager <a href=\"https://github.com/danielsnider/follow_waypoints\">Program</a>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to avoid touching the **/initialpose**, the topic that the **follow_points** listens to, this has to be changed. Not only that put it has to be configurable. See here the final code:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**custom_follow_waypoints.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "\n",
    "import threading\n",
    "import rospy\n",
    "import actionlib\n",
    "\n",
    "from smach import State,StateMachine\n",
    "from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal\n",
    "from geometry_msgs.msg import PoseWithCovarianceStamped, PoseArray\n",
    "from std_msgs.msg import Empty\n",
    "\n",
    "waypoints = []\n",
    "\n",
    "class FollowPath(State):\n",
    "    def __init__(self):\n",
    "        State.__init__(self, outcomes=['success'], input_keys=['waypoints'])\n",
    "        self.frame_id = rospy.get_param('~goal_frame_id','map')\n",
    "        # Get a move_base action client\n",
    "        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)\n",
    "        rospy.loginfo('Connecting to move_base...')\n",
    "        self.client.wait_for_server()\n",
    "        rospy.loginfo('Connected to move_base.')\n",
    "\n",
    "    def execute(self, userdata):\n",
    "        global waypoints\n",
    "        # Execute waypoints each in sequence\n",
    "        for waypoint in waypoints:\n",
    "            # Break if preempted\n",
    "            if waypoints == []:\n",
    "                rospy.loginfo('The waypoint queue has been reset.')\n",
    "                break\n",
    "            # Otherwise publish next waypoint as goal\n",
    "            goal = MoveBaseGoal()\n",
    "            goal.target_pose.header.frame_id = self.frame_id\n",
    "            goal.target_pose.pose.position = waypoint.pose.pose.position\n",
    "            goal.target_pose.pose.orientation = waypoint.pose.pose.orientation\n",
    "            rospy.loginfo('Executing move_base goal to position (x,y): %s, %s' %\n",
    "                    (waypoint.pose.pose.position.x, waypoint.pose.pose.position.y))\n",
    "            rospy.loginfo(\"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'\")\n",
    "            self.client.send_goal(goal)\n",
    "            self.client.wait_for_result()\n",
    "        return 'success'\n",
    "\n",
    "def convert_PoseWithCovArray_to_PoseArray(waypoints):\n",
    "    \"\"\"Used to publish waypoints as pose array so that you can see them in rviz, etc.\"\"\"\n",
    "    poses = PoseArray()\n",
    "    poses.header.frame_id = 'map'\n",
    "    poses.poses = [pose.pose.pose for pose in waypoints]\n",
    "    return poses\n",
    "\n",
    "class GetPath(State):\n",
    "    def __init__(self):\n",
    "        State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints'])\n",
    "        # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc.\n",
    "        self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1)\n",
    "        self._custom_waypointstopic = rospy.get_param('~custom_waypointstopic','/my_waypoints_list')\n",
    "        # Start thread to listen for reset messages to clear the waypoint queue\n",
    "        def wait_for_path_reset():\n",
    "            \"\"\"thread worker function\"\"\"\n",
    "            global waypoints\n",
    "            while not rospy.is_shutdown():\n",
    "                data = rospy.wait_for_message('/path_reset', Empty)\n",
    "                rospy.loginfo('Recieved path RESET message')\n",
    "                self.initialize_path_queue()\n",
    "                rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches\n",
    "                               # for three seconds and wait_for_message() in a\n",
    "                               # loop will see it again.\n",
    "        reset_thread = threading.Thread(target=wait_for_path_reset)\n",
    "        reset_thread.start()\n",
    "\n",
    "    def initialize_path_queue(self):\n",
    "        global waypoints\n",
    "        waypoints = [] # the waypoint queue\n",
    "        # publish empty waypoint queue as pose array so that you can see them the change in rviz, etc.\n",
    "        self.poseArray_publisher.publish(convert_PoseWithCovArray_to_PoseArray(waypoints))\n",
    "\n",
    "    def execute(self, userdata):\n",
    "        global waypoints\n",
    "        self.initialize_path_queue()\n",
    "        self.path_ready = False\n",
    "\n",
    "        # Start thread to listen for when the path is ready (this function will end then)\n",
    "        def wait_for_path_ready():\n",
    "            \"\"\"thread worker function\"\"\"\n",
    "            data = rospy.wait_for_message('/path_ready', Empty)\n",
    "            rospy.loginfo('Recieved path READY message')\n",
    "            self.path_ready = True\n",
    "        ready_thread = threading.Thread(target=wait_for_path_ready)\n",
    "        ready_thread.start()\n",
    "\n",
    "        topic = self._custom_waypointstopic\n",
    "        rospy.loginfo(\"Waiting to recieve waypoints via Pose msg on topic %s\" % topic)\n",
    "        rospy.loginfo(\"To start following waypoints: 'rostopic pub /path_ready std_msgs/Empty -1'\")\n",
    "\n",
    "        # Wait for published waypoints\n",
    "        while not self.path_ready:\n",
    "            try:\n",
    "                pose = rospy.wait_for_message(topic, PoseWithCovarianceStamped, timeout=1)\n",
    "            except rospy.ROSException as e:\n",
    "                if 'timeout exceeded' in e.message:\n",
    "                    continue  # no new waypoint within timeout, looping...\n",
    "                else:\n",
    "                    raise e\n",
    "            rospy.loginfo(\"Recieved new waypoint\")\n",
    "            waypoints.append(pose)\n",
    "            # publish waypoint queue as pose array so that you can see them in rviz, etc.\n",
    "            self.poseArray_publisher.publish(convert_PoseWithCovArray_to_PoseArray(waypoints))\n",
    "\n",
    "        # Path is ready! return success and move on to the next state (FOLLOW_PATH)\n",
    "        return 'success'\n",
    "\n",
    "class PathComplete(State):\n",
    "    def __init__(self):\n",
    "        State.__init__(self, outcomes=['success'])\n",
    "\n",
    "    def execute(self, userdata):\n",
    "        rospy.loginfo('###############################')\n",
    "        rospy.loginfo('##### REACHED FINISH GATE #####')\n",
    "        rospy.loginfo('###############################')\n",
    "        return 'success'\n",
    "\n",
    "def main():\n",
    "    rospy.init_node('custom_follow_waypoints')\n",
    "\n",
    "    sm = StateMachine(outcomes=['success'])\n",
    "\n",
    "    with sm:\n",
    "        StateMachine.add('GET_PATH', GetPath(),\n",
    "                           transitions={'success':'FOLLOW_PATH'},\n",
    "                           remapping={'waypoints':'waypoints'})\n",
    "        StateMachine.add('FOLLOW_PATH', FollowPath(),\n",
    "                           transitions={'success':'PATH_COMPLETE'},\n",
    "                           remapping={'waypoints':'waypoints'})\n",
    "        StateMachine.add('PATH_COMPLETE', PathComplete(),\n",
    "                           transitions={'success':'GET_PATH'})\n",
    "\n",
    "    outcome = sm.execute()\n",
    "\n",
    "if __name__ == \"__main__\":\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The new topic is retrived through a parameter setting."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self._custom_waypointstopic = rospy.get_param('~custom_waypointstopic','/my_waypoints_list')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This means that when this node is launched, it will have to be set this parameter if we don't want it to have its default value **/my_waypoints_list**.<br>\n",
    "Here you have an example of how it should be launched:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**start_follow_waypoints.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "    <env name=\"ROSCONSOLE_FORMAT\" value=\"[${severity}][${thread}][${node}/${function}:${line}]: ${message}\"/>\n",
    "     <arg name=\"waypoints_topic\" default=\"/my_jackal_waypoints\"/>\n",
    "    \n",
    "    <node pkg=\"my_jackal_tools\" type=\"custom_follow_waypoints.py\" name=\"custom_follow_waypoints\" output=\"screen\" clear_params=\"true\">\n",
    "        <param name=\"goal_frame_id\" value=\"map\"/>\n",
    "        <param name=\"custom_waypointstopic\" value=\"$(arg waypoints_topic)\"/>\n",
    "    </node>\n",
    "\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets comment on various elements here:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<env name=\"ROSCONSOLE_FORMAT\" value=\"[${severity}][${thread}][${node}/${function}:${line}]: ${message}\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This line changes the way the logs are shown, in this case it will show which node, function and line in the code is that log executed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<arg name=\"waypoints_topic\" default=\"/my_jackal_waypoints\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you state which topic where you will have to publish the waypoint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<param name=\"goal_frame_id\" value=\"map\"/>\n",
    "<param name=\"custom_waypointstopic\" value=\"$(arg waypoints_topic)\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you state which is the frame and the topic params values."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now create a python script that generates those waypoints and when finished executes the moving system to follow those waypoints.<br>\n",
    "To do so, you will need to publish directly into the topic **/my_jackal_waypoints** or the value that you stated in tha launch file **start_follow_waypoints.launch**.<br>\n",
    "Then you will have to publish to start in the topic **/path_ready**.<br>\n",
    "Tasks:\n",
    "* Make it generate circular paths\n",
    "* Eight shaped paths\n",
    "* Dynamic waypoints based on the position of the standing person. For this last one you will need to acces the TF data of the standingPerson."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#EE9023;color:white;\">Exercise U1-3</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">Solution Exercise U.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": [
    "You should get something similar to this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/jackal_clearpath_instruction_manual_unit1_waypoints_example.gif\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of a publisher that generates a straight line waypoint path and executes it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**waypoint_publisher.py**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#!/usr/bin/env python\n",
    "# license removed for brevity\n",
    "import rospy\n",
    "from std_msgs.msg import String\n",
    "from geometry_msgs.msg import PoseWithCovarianceStamped\n",
    "import time\n",
    "import tf\n",
    "from std_msgs.msg import Empty\n",
    "\n",
    "\"\"\"\n",
    "user:~$ rosmsg show geometry_msgs/PoseWithCovarianceStamped                                                                                                               \n",
    "std_msgs/Header header                                                                                                                                                    \n",
    "  uint32 seq                                                                                                                                                              \n",
    "  time stamp                                                                                                                                                              \n",
    "  string frame_id                                                                                                                                                         \n",
    "geometry_msgs/PoseWithCovariance pose                                                                                                                                     \n",
    "  geometry_msgs/Pose pose                                                                                                                                                 \n",
    "    geometry_msgs/Point position                                                                                                                                          \n",
    "      float64 x                                                                                                                                                           \n",
    "      float64 y                                                                                                                                                           \n",
    "      float64 z                                                                                                                                                           \n",
    "    geometry_msgs/Quaternion orientation                                                                                                                                  \n",
    "      float64 x                                                                                                                                                           \n",
    "      float64 y                                                                                                                                                           \n",
    "      float64 z                                                                                                                                                           \n",
    "      float64 w                                                                                                                                                           \n",
    "  float64[36] covariance \n",
    "\"\"\"\n",
    "\n",
    "def talker():\n",
    "    pub_wp = rospy.Publisher('/my_jackal_waypoints', PoseWithCovarianceStamped, queue_size=1)\n",
    "    \n",
    "    pub_init_wp = rospy.Publisher('/path_ready', Empty, queue_size=1)\n",
    "    \n",
    "    rospy.init_node('waypoint_publisher', anonymous=True)\n",
    "    rate = rospy.Rate(10) # 10hz\n",
    "    \n",
    "    my_wp = PoseWithCovarianceStamped()\n",
    "    my_wp.header.stamp = rospy.Time.now()\n",
    "    my_wp.header.frame_id = \"/map\"\n",
    "    \n",
    "    \n",
    "    init_value = 2.0\n",
    "    \n",
    "    roll = 0\n",
    "    pitch = 0\n",
    "    yaw = 0.7\n",
    "    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)\n",
    "    #type(pose) = geometry_msgs.msg.Pose\n",
    "    my_wp.pose.pose.orientation.x = quaternion[0]\n",
    "    my_wp.pose.pose.orientation.y = quaternion[1]\n",
    "    my_wp.pose.pose.orientation.z = quaternion[2]\n",
    "    my_wp.pose.pose.orientation.w = quaternion[3]\n",
    "\n",
    "\n",
    "    for i in range(5):\n",
    "        rospy.loginfo(str(i))\n",
    "        my_wp.pose.pose.position.x = float(i) + init_value \n",
    "        my_wp.pose.pose.position.y = float(i) + init_value\n",
    "        \n",
    "        while not rospy.is_shutdown():\n",
    "            connections = pub_wp.get_num_connections()\n",
    "            if connections > 0:\n",
    "                pub_wp.publish(my_wp)\n",
    "                break\n",
    "            rospy.loginfo(\"Waiting for /my_jackal_waypoints topic\")\n",
    "            rate.sleep()\n",
    "        rospy.loginfo(\"Published waypoint number =\"+str(i))\n",
    "        time.sleep(2)\n",
    "    \n",
    "    start_command = Empty()\n",
    "    \n",
    "    while not rospy.is_shutdown():\n",
    "            connections = pub_init_wp.get_num_connections()\n",
    "            if connections > 0:\n",
    "                pub_init_wp.publish(start_command)\n",
    "                rospy.loginfo(\"Sent waypoint list execution command\")\n",
    "                break\n",
    "            rospy.loginfo(\"Waiting for /path_ready topic\")\n",
    "            rate.sleep()\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    try:\n",
    "        talker()\n",
    "    except rospy.ROSInterruptException:\n",
    "        pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extra: Launch your RVIZ configs easily"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here you have an example of how to launch RVIZ sessions faster. You just have to create this tinny launch that retrieves the name of the **rviz** configuration file saved in the **rviz** folder. You will use it in the next units for sure."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**view_robot.launch**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<launch>\n",
    "  <param name=\"use_gui\" value=\"true\"/>\n",
    "  <arg name=\"config\" default=\"complete\" />\n",
    "  <node name=\"rviz\" pkg=\"rviz\" type=\"rviz\" args=\"-d $(find my_jackal_tools)/rviz/$(arg config).rviz\" />\n",
    "</launch>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And you can use it like so in any launch file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<!-- Start RVIZ for Localization -->\n",
    "<include file=\"$(find my_jackal_tools)/launch/view_robot.launch\">\n",
    "    <arg name=\"config\" value=\"localization\" />\n",
    "</include>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This one will load in RVIZ the file called **localization.rviz**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Congratulations!! You are now capable of creating your own navigation program for indoor with your Jackal robot."
   ]
  }
 ],
 "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
}