Update mpu_rviz.cpp