Ошибка в описании маршрута.
Вместо расстояния от текущей позиции до следующей, везде было расстояние от исходной позиции.
Поэтому билась только первая промежуточная точка (сосна с полосой).