added debug for pathplanning
added debug for pathplanning