Bug fix octomap_viewer.py