Rearanged repo, changed to catkin, added rqt interface