Initial commit. ROS wrappers will follow soon
Modified include paths