Cleaned up code and added roslaunch file