Adding the goal and the obstacles points. Also, changing the code to read a laserscan data.