Obstacle avoidance controlling everything and localization done