Finished Modularisation of the various sources into a more logical structure.  Need to complete responses and make ROS service