From e30f48e1c5c35d8c48014135f0eda4e03b8cdee9 Mon Sep 17 00:00:00 2001
From: Elias Leon Foramitti <mitti.leon@gmail.com>
Date: Sun, 26 Feb 2023 09:47:17 +0100
Subject: [PATCH] Fix A* node ordering (#249)

---
 include/heuristic/HeuristicMapper.hpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/include/heuristic/HeuristicMapper.hpp b/include/heuristic/HeuristicMapper.hpp
index 32c4132c9..da3ce8aff 100644
--- a/include/heuristic/HeuristicMapper.hpp
+++ b/include/heuristic/HeuristicMapper.hpp
@@ -339,8 +339,10 @@ inline bool operator<(const HeuristicMapper::Node& x,
 
 inline bool operator>(const HeuristicMapper::Node& x,
                       const HeuristicMapper::Node& y) {
-  const auto xcost = static_cast<double>(x.costFixed) + x.lookaheadPenalty;
-  const auto ycost = static_cast<double>(y.costFixed) + y.lookaheadPenalty;
+  const auto xcost =
+      static_cast<double>(x.costFixed) + x.lookaheadPenalty + x.costHeur;
+  const auto ycost =
+      static_cast<double>(y.costFixed) + y.lookaheadPenalty + y.costHeur;
   if (std::abs(xcost - ycost) > 1e-6) {
     return xcost > ycost;
   }