added simple ground truth generator as well as a planner based on the policy
still need to recompute the strategy if we fail to park somewhere