Added BSD headers to all source and header files
Filled out and cleaned up package.xml