Update sub20

Fixes bosch-ros-pkg/bosch_drivers#16. Not tested yet, so not ready for hydro-devel branch.
Also applied recent changes on hydro-devel branch.