Switched new thigh rosserial code to CMake for comipling, dosn't work yet.