Made stage cooperate for groovy
General cleanup and fixing issues with the code
Fixed include paths of p2os_driver
removed CMakeLists link and modified README.md