Add friend class
Modify keepSecurityDistance() flow
Add hasObstacle() function
Cleanup code