Create RoutePlanManager.c

just finish logic codes, not test