В последнем if
, если snode не находится в open_set (не каламбур!), Вы не должны устанавливать только эвристику, но эвристику плюс стоимость текущего узла.И если snode равно в открытом наборе, вы должны проверить минимум между текущим значением и текущим (если есть два или более способов добраться до одного и того же узла, следует рассмотреть наименее дорогостоящий).
Это означает, что вам необходимо хранить как «фактическую» стоимость узла, так и «расчетную» стоимость.Фактическая стоимость начального узла равна нулю.Для каждого нового узла это минимум для каждой входящей дуги между стоимостью другой вершины плюс стоимость дуги (другими словами, стоимость последнего узла плюс стоимость перехода от этого к текущему).Ориентировочная стоимость должна была бы суммировать оба значения: фактическую стоимость на данный момент плюс эвристическую функцию.
Я не знаю, как узлы представлены в вашем коде, поэтому я не могу дать совет более конкретный, чемтот.Если у вас все еще есть сомнения, отредактируйте свой вопрос, предоставив более подробную информацию.