first revision of graph implementation with a service node

added necessary messages and serives. graph structure tested.
first attempt to decrease odometry drift with ir sensors