Put the speed calculation out of the publish loop to make it faster, in the robot package