Added try/catch in rviz plugin to prevent crash