Add support for rviz