BGL function now nicely integrated. Several other improvements as well (fixed: msg too old rviz problem, fixed: wrong gazebo hokuyo fram, and more). Next up: make services and st_navigation switch to this isntead of own bgl/dijkstra implementation