Add object avoidance algorithm in avoid.cpp node