Cleaning up line_planner. Demo in sdu_pichi_demining