Now we seem to add a correct id but the key field doesnt work and that makes rviz crash