Draw classified objects in rviz

Currently restricted to cube,sphere and cylinder