rviz visualization of stomp 2d tests