I am writing some unit tests.  At some point I should try this in rviz and verify this visially, but I have not gotten around to that yet.