slope_constrained_planner