Added robot model to rviz config
Changed z-height of gridcells so they don't cover the robotmodel