Added some code for obstacle management. Not finished yet.