- Integrate obstacle detection to detect static obstacles - Obstacle avoidance can be done using current replanning approach