C++ API wrapper for displaying shapes and meshes in Rviz - View it on GitHub