milestone 3

[YiLUO2018]

• implement local path planning
• convert depth-image to laserscan
• opimization the local pathplanning
• check if the planned path is blocked by newly discoverd obstacles