Temp fix route calc
Merge branch 'robot-test' of https://github.com/plantener/CDIO into robot-test

merge