fix #762
Merge pull request #766 from ros-visualization/hydro_backport_764

Hydro backport 764