use rostime instead of systemtime


this is important for simulation and might be important for synchronisation

This fix needs tests! I have only tested this in simulation(simtime) by now and not with real robot-hardware