Fixed path length calculation in the local planner.
Minor changes in local planner.