fix #4522
remove dead code
nlc
include goal waypoint in found path