impl pathPlanning and simple avoid obstacles mech