Fix for rviz