minor weights tweaks
bringing back some stuff to fix a bug
fixed costmap visualization