Added robot_state.cpp to CMakeLists.txt
clean up code
Renamed some files