Major restructuring

Reduced code redundancy, integrated more code from ROS repository.
Functionality probably not actually improved much.
Dummy change

Line-ending problem forcing a commit..