service ready for A*, topic added to waypoints, rviz rdy
univector + new heuristic functiosn written, path_planner now has waypoint simplification