Cleanup and print a failure message if the robot is stuck instead of failure