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 DataWe'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.
Testing Laser Code - Single QueryTo 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 WayTo 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.
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 ClimbingTesting 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
Hill Climbing TestingThere are two ways to test this code, both using localization/src/hill_climb.py To test it with no visualization, just run
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.
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.