Various bugfixes to ROS
Merge branch 'HALDevelopment' of https://github.com/AgileManufacturing/HUniversal-Production-Utrecht into HALDevelopment