Integrated MapReader, RoutePlanning, Fixed Trajectory Planning

Route Planning still incompatible with rest