RBE3002 Final Project for B term of 2013 in ROS