ROSfied version of KPIECE1 Planner