Assignment 6

[The material in this assignment was initially developed by Dr. Nathan Sprague of James Madison University.  It has been adapted for this course with Dr. Sprague’s kind permission.]

Due Date: Friday, August 4 at 9:00

Introduction

The purpose of this assignment is to fill in some missing components of a particle filter (i.e. Monte Carlo localization) algorithm that can localize a robot using laser data and a given map of the environment.  Note that additional packages not used so far in this course will be required.  These packages have been installed in EN-1049.  To use the new packages, re-boot and select “Ubuntu 14”.  NOT YET INSTALLED IN EN-1049.  If you are attempting to run this on your own computer, you will need additional packages.  See section “Appendix: Installation”.

Code

Download the following zip file and unzip it in your ~/catkin_ws/src directory:

This directory has the following contents:

The four python scripts play the following roles:

  • map_utils.py – This is utility code for working with ROS OccupancyGrid messages. You shouldn’t need to modify this code.
  • mcl_tf.py – This node broadcasts an appropriate transform from the odom to the map coordinate frame based on the pose estimate provided by mc_localizer.py. This page on ROS coordinate frame conventions describes the relationship between the odom and map frames.
  • particle.py – This file contains the definition of a Particle class that will be used in the mcl implementation. You should not need to modify this file. But please read it carefully so that you understand how to use the Particle class.
  • mc_localizer.py – This file is unfinished.   Once you fill in the bodies of the functions described below, it will provide a complete implementation of Monte Carlo localization.  You are required to fill in the following functions which have detailed comments on their responsibilities:
    • normalize_particles – Called from apply_particle_filter to normalize particle weights.
    • resample – Called from apply_particle_filter to implement particle resampling.
    • initialize_particles_uniform – Called upon initialization to create an initial set of particles drawn from a uniform distribution.

Testing

First, launch all_but_localizer.launch to make sure your setup is working.  You should see the STDR simulator starting as well as rviz.  In the terminal you can teleoperate the robot using the keys indicated.  Drive it around for a test, but note that by default the particle filter will only work from the initial starting pose.

To run the localizer launch localizer.launch in a separate terminal.  Note that the actual particle filter algorithm will not run unless the robot is moved by a minimum distance/angle.  So you will have to move it using the other terminal (the one you launched all_but_localizer.launch from).

Take note of the parameter initialize_uniform which is read by mc_localizer.py.   This parameter, and others mentioned below are specified in localizer.launch (i.e. edit this file to change them).  The default value is False, meaning that the initial particles will be positioned around (initial_pose_x, initial_pose_y) according to a Gaussian distribution.  In this configuration, we are doing position tracking, not global localization.  You should have implemented initialize_particles_uniform which initializes particles from a uniform distribution, all over the map.  Set this parameter to True to check whether your initialize_particles_uniform function is correct.  You should also do some experiments varying the number of particles (set by num_particles) to see if the filter can converge when starting from a uniform distribution.

You might find that the filter performs poorly from a uniform distribution.  If you find a way to reliably improve the filter’s performance you can earn bonus marks to offset marks lost on other assignments.

Submission

Submit mc_localizer.py to D2L by the deadline given above.  Although you will have made changes to some launch files, it is not necessary to submit them.

Appendix: Installation

Installing the following packages should only be necessary on your home computer.

The STDR Simulator

Install all of the following with

ros-kinetic-stdr-gui – A gui in Qt for visualizing purposes in STDR Simulator.
ros-kinetic-stdr-launchers – Launch files, to easily bringup server, robots, guis
ros-kinetic-stdr-msgs – Provides msgs, services and actions for STDR Simulator.
ros-kinetic-stdr-parser – Provides a library to STDR Simulator, to parse yaml and xml description files.
ros-kinetic-stdr-resources – Provides robot and sensor descripiton files for STDR Simulator.
ros-kinetic-stdr-robot – Provides robot, sensor implementation, using nodelets for stdr_server to load them.
ros-kinetic-stdr-samples – Provides sample codes to demonstrate STDR simulator functionalities.
ros-kinetic-stdr-server – Implements synchronization and coordination functionalities of STDR Simulator.
ros-kinetic-stdr-simulator – A simple, flexible and scalable 2D multi-robot simulator.

Miscellaneous

The scipy and sklearn packages are also required install with pip (if you don’t have pip, install it first).