Update UD_rviz_interaction.cpp