CSE550

Mobile Robotics

Assignment 4: Localization

In this lab, we will implement a simplified version of AMCL, in which you try to find the where in a given map a particular laser scan originates.

Note, due to the ongoing git issues, you will have to add the assignment04 folder to your assignments repo yourself.

The Data

We'll be attempting to localize in two environments, a fairly empty "easy" map and the CSE550 map from assignment02. The data is stored in bag files. The map itself is stored in localization/EasyMap.bag and localization/CSE550Map.bag. The laser scan and true pose of the robot are stored in localization/easy-#.bag and localization/cse550-#.bag.

Laser Operations

  1. Modify your bresenham's implementation from assignment 2 to complete the ray_tracing operation in assignment04/src/assignment04/laser.py. This code takes in a x,y coordinate (integers in map space), an angle, and a map. The function should return the x,y coordinate of the first cell in the map along the ray (starting at the input x,y at the specified angle) that has the value 100. This is meant to figure out what the laser would hit from the pose specified, given the map. If you reach the end of the map, return None.
  2. expected_scan takes in a pose (x,y,theta) the properties of a laser scanner (min_angle, increment, n_readings, max_range) and the map. Your goal is to return an array of floats that represents what ranges you would expect for the entire laser scan. You should use your ray_tracing function. You should return max_range if ray_tracing returns None, or if the ray traced is longer than the max range.
  3. scan_similarity takes in two arrays of floats that represent the data from the laser scanner. Assuming both were taken from the same position, come up with a metric that represents the scans' similarity to each other, with higher numbers representing more similar scans. The actual metric is up to you, but it should return a number between 0 and 1.

Testing Laser Code - Single Query

To check your laser code, you can use the localization/src/query_pose.py script to check the resulting scan for one pose at a time. This code can only be run on a local machine, not the server. Start the roscore, and then run rviz with the argument "-d localization/hill_climb.rviz". Then run the script src/query_pose.py MAPBAG DATABAG. Then in the rviz window, draw an arrow on the map using the "2D Pose Estimate Tool" (like you did in the last lab for specifying the goal). This will display the expected scan, and the score will print out in the terminal.

Testing Laser Code - The Brute Force Way

To localize the robot, you could calculate the likelihood that the robot is in each individual pose in the map. That is what the script in localization/src/find_pose.py does. Your provide two bag files, one with the map, and one that contains a laser scan and the actual pose. The script will score each pose with your laser.py methods and print out the best ones. This takes quite a bit of time.

Note you can run this code on the server, but since it takes so long, don't do it more than necessary.

Sample Commands:

  • python src/find_pose.py EasyMap.bag easy-1.bag
  • python src/find_pose.py CSE550Map.bag cse550-1.bag -resolution 8
    (This only searches every 8th column and row in the original map, which causes the runtime to actually be manageable.)

Due to the resolution of the maps, you may not get the exact original pose. For example, for the above commands, I get (0.5, 0.5, 0.0) and (-6.460000079125166, -3.900000136345625, 0.0).

Hill Climbing

Testing every possible pose takes too long. So we're going to use an AMCL-like approach to find the actual pose. Things you need to implement in assignment04/src/assignment04/particle.py
  1. random_particle(the_map) - Returns a tuple (x,y,theta) for a random pose (in real world coordinates) in the map. You may find you get better results if you only return points that are in unoccupied cells.
  2. new_particle(particle) - Given a tuple (x,y,theta), returns a similar particle with slightly altered coordinates. You can add additional parameters if you desire.
  3. resample(scores, n_particles) - Given an array of tuples (score, (x,y,theta)), do importance sampling to create n_particles new particles (using your new_particle method). You should return an array of tuples (x,y,theta) that represent your new particles.

Hill Climbing Testing

There are two ways to test this code, both using localization/src/hill_climb.py To test it with no visualization, just run
  • python src/hill_climb.py MAPBAG DATABAG
This defaults to 100 particles and 5 iterations. You can increase that number with the arguments -particles 1000 -iterations 10 .

To run it with visualizations, start roscore and rviz as described in the Single Query Testing, and then run the same command with the -ros flag.

Hints

Note that EasyMap.bag has 1-meter resolution, which means that the expected scans can be off by as much as a meter. To get the hill climbing to work well for the "Easy" data, use EasyMap2.bag, which has a ten-fold increase in resolution.

Due Date: April 15, 2015, 11:59pm CST.