Particle Filter Localization

Note: These instructions assume you have installed the Particle Filter according to these installation instructions.

Running Localization

(1). If you have followed the installation instructions as intended, the maps the particle filter uses will be in "~/localization/localization_ws/src/maps". Assuming you have a ".pgm" file and a ".yaml" file in your "~/mapfile" folder, then you can copy all these files with: cp ~/mapfiles/* ~/localization/localization_ws/src/maps.
(2). To select which map to use for localization, you'll need to modify your "map_server.launch" file in "~/localization/localization_ws/src/launch". You may need to chmod it to edit it. Launch files essentially tell roslaunch how to run nodes in a package. The modification is simply replacing "basement_fixed.map.yaml" with the name of your ".yaml" file.

  • Debugging tip: We recommend keeping the default path "$(find particle_filter)/maps/.yaml". If roslaunch doesn't seem to find the ".yaml", double check the filename (no really, stop and do that). Then you can try putting in the full path: "/home/racecar/localization/localization_ws/src/maps/.yaml". Do not try using "~/"! roslaunch does not know what "~/" means like the terminal shell does.

(3). Now we can get cooking! In the car's terminal, run teleop.
(4). Then in another tab/window, run:

  source ~/localization/localization_ws/devel/setup.bash
  roslaunch particle_filter localize.launch
  • After the program prints "…Received first LiDAR message," it should start to print "iters per sec: 20 possible: 21" to confirm that it is getting scan data and making localization estimates.
  • Debugging tips:
    • We found that it is usually necessary for the vesc to be running completely (i.e. there’s a good Traxxis battery) in order for this to work.
    • If the roslaunch starts launching, but then returns an error message like "cannot locate node of type particle_filter", it likely means that the "particle_filter.py" file in "~/localization/localization_ws/src/" needs executable permissions. You can give it these permissions by running chmod +x particle_filter.py.
    • If the roslaunch starts launching, but then the python file returns an error message like "ImportError: No module named range_libc", it likely means that the range_libc installation failed. Try it again according to our instructions here. Maybe the "setup.py" file in "~/localization/range_libc/pywrapper/" needs executable permissions.

(5). Also, just as with cartographer, you may open RViz. Interesting topics to try will be /map, /scan, and /pf/viz/particles topics. * Wondering how to add topics in RViz? See step 7 of the "Running off of live data" section of our Cartographer page. * Again, rviz can be finicky at times. If nothing appears even after running teleop or playing the rosbag, try changing the "Fixed Frame" to "map". Then check and uncheck the the checkboxes for the topics you are interested in. If that didn't work, try re-running Rviz. Check that you are running the programs you need to run.

(6). The car likely does not know where it is starting on the map. Give it an estimate of where it is using the "2D Pose Estimate" tool.

  • Click on the map for position, drag for orientation.
  • If you want to be extra fancy, you can have the car do this itself, by publishing a PoseWithCovarianceStamped message to the /initialpose topic. You can see what these messages look like by running rostopic echo /initialpose and doing a 2D pose estimate in RViz.
  • A good idea would be to place the car in a fixed starting position and publish this known position to "/initialpose" when you press a button. Then you could press another button to change state and start running.

(7). (optional)

Don’t like your view locked to (0,0,0)? Make it follow the car by changing your frame to something on the car.

  • First use the "Focus Camera" tool and click near the pose estimates (red arrows) to center the view on the car initially.
  • Then change "Target Frame" to something on the car (like "base_link") to keep up with the car’s changes in position.

Using Pose Estimate Data in ROS

This is where you get the pose estimate of where the car is on the map!
Want to know where you are in this world while you're writing your code? Subscribe to pf/viz/inferred_pose!

  • To extract meaningful data from these messages, you can figure it out on your own.
  • Use rostopic type to see what datatype the messages are. Once you have the name, you can find more info on ros.org.
  • In your python code, remember to import the associated datatype: from geometry_msgs.msg import <datatype_of_inferred_pose>
  • If you receive a ROS message in a python program and are unsure of what it is or what it contains, try printing it out.
  • Quaternions Help (if you think angular info will help) You may have noticed the rotations for these ROS geometry messages are encoded in quaternions. Why? I really don’t know, but it allows us to track the car’s rotation from -2π to 2π. If you care to amuse yourself for a few minutes, feel free to look up quaternions and derive the conversion back to an angle. Y'all are smart. Or you may just use the ROS’s built-in transformations:
    from tf.transformations import euler_from_quaternion
    . . .
    def quatToAng3D(quat):
        euler = euler_from_quaternion((quat.x,quat.y,quat.z,quat.w))
        return euler

For reference, roll = euler[0], pitch = euler[1], yaw = euler[2], and yaw is rotation about the z-axis.

Google Cartographer Localization

To run localization in Google Cartographer, you won't need an image and an ".yaml" file, but rather this file structure called a ".pbstream". Here's how you get this thing: (1). `cd` into the folder you want your ".pbstream" stored.
(2). Run `roslaunch cartographer_ros offline_racecar_2d.launch bag_filenames:=${HOME}/bagfiles/.bag`
  Warning: this will pull up an rviz window, so whoops if you're ssh-ed in.
(3). Wait for the bag to finish playing, then watch the terminal and wait until it's done "optimizing".
Now you wanna localize. Here's how you do something like that (though it also tries to make another map, which is concerning; maybe you need to modify one of the config files to include `max_submaps_to_keep = 3`, as the [Google Cartographer website](https://google-cartographer-ros.readthedocs.io/en/latest/going_further.html) suggests).
(4). Run the localization by entering the following `roslaunch cartographer_ros demo_racecar_2d_localization.launch \ load_state_filename:=${HOME}//.pbstream`.
(5). We don't really know where to get pose data. And if you wanted to give the program pose estimated, good stinkin' luck, buddy. The best we can offer is intercepting stuff sent across the "tf" topic. While the localization is running, enter `rostopic echo tf`. The "base_link" frame may have relevant data.

Change log (how did we concoct some of those launch and configuration files):

(1). Copy the launch file demo_backpack_2d_localization.launch and rename it by entering `cp demo_backpack_2d_localization.launch demo_racecar_2d_localization.launch`.
  Within this new file change robot_description to "$(find xacro)/xacro '$(find racecar_description)/urdf/racecar.xacro'")"
  Configuration_basename becomes racecar_2d_localization.lua
  Don't remap from "echoes". Instead:
  Remap from /odom to /vesc/odom
  Remap from imu to /imu/datav (2). Delete the robag node.
(3). First, enter `cp offline_backpack_2d.launch offline_racecar_2d.launch`
Also, change the "configuration_basename" argument from backpack_2d.lua to racecar_2d.lua
Delete the "urdf_basename" parameter entirely.
Don't remap from "echoes". Instead:
remap from /odom to /vesc/odom
remap from imu to /imu/data