Added Hydro-compatible sbpl lattice planner code