fix(freespace_planner): fix a bug that goal goes out of the costmap while parking.

fix(freespace_planner): fix a bug that goal goes out of the costmap while parking. #34897