Fix RobotFootprintModel visualization bug
Fix RobotFootprintModel visualization bug

Fixed a bug where the visualization marker array is incorrectly modified.

(cherry picked from commit a465ae1784e71c8f0600992f57a80fbedf1826a9)

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