Switch to primitives with reverse
Update rviz config