Fixed numerous bugs; now compiles with catkin