refactor AddEdgesObstacles to avoid repeating identical code chunks
refactor AddEdgesObstacles to avoid repeating identical code chunks

rst-tu-dortmund/teb_local_planner,rst-tu-dortmund/teb_local_planner