References and Prerequisites

Microstrain GPS/IMU Sensor

• We will be integrating the Microstrain 3DM-GX5-45 sensors on each robot.
• The ROS driver node should be installed on each Pioneer.  You will want to be familiar with the interface (published topics, services, etc.) provided by the node which is documented on the ROS wiki: http://wiki.ros.org/microstrain_3dm_gx5_45

Geonav Local Reference Frame

• We will also be using the geonav_transform node to provide localization information in a local reference frame.
• Documentation of the node is at http://wiki.ros.org/geonav_transform
• Some general documentation on 2D local reference frames is at Local Coordinate Frames

RFID Reader

• We will be integrating the Thingmagic USB Pro RFID reader: http://www.thingmagic.com/usb-pro-rfid-reader
• The ROS driver node should be installed on each Pioneer, but again, you will want to be familiar with the interface provided by the ROS node: http://wiki.ros.org/thingmagic_rfid

Git Repository / ROS Package

As in the last couple assignments, your submission for this assignment will be a Git repository that contains a ROS package.  The name of the repository on the gitlab.nps.edu server should be mrc_hw7 and it should be a ROS package (catkin_crate_pkg).

Setup

Multimaster Setup - One Robot

We will continue to use the multimaster setup from the last assignment.   You will want to make a launch file, as you did previously, so that your laptop communicates with only one robot.  The launch file will look something like this...

<?xml version="1.0"?>
<launch>
 <node name="master_discovery" pkg="master_discovery_fkie" type="master_discovery" output="screen">
   <param name="mcast_group" value="224.0.0.1" />
 </node>
 <node name="master_sync" pkg="master_sync_fkie" type="master_sync" output="screen">
   <!-- Ignore all hosts by default -->
   <rosparam param="ignore_hosts">['*']</rosparam>
   <!-- Add selective hosts to sync with -->
   <rosparam param="sync_hosts">[192.168.11.60]</rosparam>-->
 </node>
</launch>

 were you will adjust the sync_hosts parameter so that your system only synchronizes with a single robot.  Ignore all avoids your multmaster setup to connect to all the other laptops and computers on the network.

Navigation Sensor Check

There are nodes on the Pioneer that will provide the GPS-based navigation estimate

• microstrain_3dm_gx5_45_node (http://wiki.ros.org/microstrain_3dm_gx5_45) provides the nav/odom and nav/status topics. 
  
  • the Odometry message on the /nav/odom topic provides the position in Lat/Lon. 
  • the /nav/status topic provides three integer values that indicate the current state of the navigation estimate - i.e., if we should trust it or not.
• geonav_transform_node (http://wiki.ros.org/geonav_transform) transforms the Lat/Lon position into a local reference frame and reports the Odometry position in local x/y coordinates.
  • provides the the geonav/odom which is the navigation estimate in the local coordinate frame.
  • the local origin, the Lat/Lon position we declare as 0,0 in our local coordinate frame, is selected as 36.596 deg. N, -121.878 deg E.

Make sure that you can see these nodes and topics.  If you can't see them from your laptop, ssh into the robot and use the rosnode and rostopic commands to investigate.

Joystick Update

You should have a local copy of our nre_joy ROS package https://github.com/bsb808/nre_joy

You should git pull that repository to make sure you have the latest changes.

Launch the joystick driver with a command like this

roslaunch nre_joy teleop_ugv_joy_p3at.launch namespace:=p0 autorepeat_rate:=10

where the namespace argument is the name of your Pioneer.

The autorepeat option will cause the joystick to continuously publish Twist messages at the given rate (in Hz).  This will help provide for smooth driving of the robot.

Driving Outside

At this point you should have verified the the navigation nodes are running and that you can drive the robot with the joystick.  Now drive the robot outside to where the GPS antenna has a clear view of the sky (you can also move it by hand if that is easier).  Once the GPS fix becomes valid (should only take a minute or two) the microstrain Kalman filter should start to provide valid information.  Examine the /nav/status messages.  If the navigation solution is valid you should see the following values on

The documentation for this message (which is brief) is at http://wiki.ros.org/microstrain_3dm_gx5_45#Nav_Status_message_description

The numbers correspond to

1. 2 = Filter running, solution valid
2. 1 = Dynamics mode is "portable".
3. 0 = No status flags, i.e., no errors.

You will want to make sure that your sensor is able to produce a valid navigation estimate.  If you have a good GPS fix and the filter won't converge, you can reset the Kalman filter with a ROS service call such as

rosservice call /p5/reset_kf

You can also try restarting the microstrain node.  If you kill the node it should automatically restart

rosnode kill /p5/microstrain_3dm_gx5_45_node

Troubleshooting Microstrain Kalman Filter

Making sure that the /nav/status messages indicate that the navigation solution is valid is critical.  The first number in the array, the (2 in the example above) is the filter state.  The possible values are

• 0x00 – Startup  
• 0x01 – Initialization (see status flags)  
• 0x02 – Running, Solution Valid  
• 0x03 – Running, Solution Error (see status flags)

We want to see a number 2, indicating a valid solution.  Some scenarios we have seen so far include:

• nav/status = (1, 1, 12290): The first number indicates that the filter is initializing, so it may just take some time (no more than a couple minutes) for the filter to initialize.
• nav/status = (3, 1, 64): The first number indicates an error.  The last number is the a status flag and indicates the attitude uncertainty is high.  If you drive the vehicle around a bit, it should converge quickly (10 seconds or so).

Full documentation on the filter status message is in the Microstrain documentation 3dm-gx5-45_dcp_manual_8500-0064_0.pdf - specifically page 155.

Exercise 1: Driving the Perimeter

Once you can drive around via joystick and you verify that the microstrain provides valid data,

1. Start a recording a bag file of all the data from your one robot.  You can do the recording on your laptop.  Please don't include the bag file with your mrc_hw7 repository (large binary files are a poor fit for version control, something like (drob)box is a better solution).
2. Using the joystick, drive the perimeter of the operating area.  To start we'll use the small grass area outside CAVR indicated in this image (see the small grey area with red boundary).
   
   This will test your wireless reception around the area.
3. Once you can drive around in the area, post process the data to generate the images shown below.  A prototype of the script to generate the plots from the bag file is provided at https://gitlab.nps.edu/bsbingha/mrc_hw7/blob/master/matlab/post_odom.m.  You may need to make some small changes (topic names for your robot, bag file name, etc.) but the script should be fairly complete.

To assess the quality of of the navigation sensor, you will generate the six figures below.  Save each figure as a PNG image in mrc_hw7/images with the filenames indicated below.  Again, the hard work should be in getting setup to record the bag file, the MATLAB script above should do 99% of the work for you to generate the images.

Raw Odometry (odom.png)

This plot shows the odometry estimate coming from the Pioneer encoder measurements.  This is the navigation we have been using thus far in the course.

Geonav Odometry (geonav.png)

Plot of the Odometry messages published by the geonav_transform in our local coordinate frame.

Geonav Odometry Quiver (quiver.png)

The quiver plot is very useful because it provides a quick way to determine if there are heading offsets.  In the figure below you might notice that all the arrows appear to point slightly more to the right of the robot that we would like.  This could indicate (and in this case did indicate) that the mounting of the sensor was a few degrees off with respect to the bow/stern of the robot.

Kalman Filter Status (kf.png)

The time series of the messages from the nav/status topic will help identify any problems the navigation estimate.  In the figure below we see that the FilterState is 2 for the entire run, telling us the filter is valid and the StatusFlags are 0.  The DynamicsMode is set to 1 for the the "Portable" mode.  (We may experiment with setting the sensor up in "Automotive" mode to see how that affects performance of the estimate, so it is fine if you see either mode.)

Navigation Estimate Uncertainty (geonav_var.png)

Navigation is always uncertain.  The messages on the nav/odom topic include a measurement of navigation estimate uncertainty - the quality of the estimate.  In the image below we see that the x and y variance are below 6 cm during our experiment.

Odometry vs. Geonav (odom_vs_geonav.png)

In the image below we plot both the Odometry navigation estimate and the geonav local coordinate navigation estimate.  We have translated and rotated the Odometry record so that it starts with the same position and orientation.

The interpretation of the above figure is that the GPS/IMU-based coordinates provide a fixed frame for repeatable common navigation.  The resolution of the solution provided by the sensor is very good (~6 cm), but we have to pay attention to the state of the internal filter to make sure we know when to trust the solution.  And ideal solution would be to combine the odometry and GPS/IMU estimates to provide the best of both worlds.  The robot_localization ROS package is frequently used for this type of application (http://wiki.ros.org/robot_localization), but is beyond the scope of this course.

Next Steps

Once you have this all working you should be in a position to start having your single robot, and multiple robots, navigate in a fixed frame.  Possible next steps towards the Final Project: DTE Robotics Challenge would be to

1. Have a single Pioneer navigate to a fixed set of waypoints in the geonav local coordinate frame
   1. You might select a series of waypoints based on the geonav coordinates for the perimeter run you made in this assignment.
2. Repeat the leader-follower exercise from the previous assignment, but use the common, fixed geonav coordinates for navigating the Pioneers.  For example, based on the geonav coordinates from the perimeter run illustrated above, the following waypoint coordinates should keep the robots in the operating regio
   
   1. (87,15)
      (83,17)
      (94,27)
      (96,24)
   2. You made need to adjust the the location of these waypoints and/or the gains on your controller to keep your robot within the small area.
3. A working example in Gazebo is provided that uses object-oriented programming in MATLAB. Pioneer in Gazebo, GPS/IMU Navigation, MATLAB OOP Waypoint Guidance
   This method has some advantages over using globals as we did in the previous exercise.  If you have some familiarity with object-oriented methods (or MATLAB "handle" objects") this example may be helpful.