From 586b349a065ab5f6c1fd046a09293ee6c4120229 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 10 Jun 2019 14:51:34 +0200
Subject: [PATCH] Update ccost of goal's parents

---
 base/rrtbase.cc | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9f14eed..b21340f 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -494,6 +494,19 @@ bool RRTBase::goal_found(
                                 return false;
                         }
                         this->goal_found_ = true;
+                        // Update ccost of goal's parents
+                        if (this->goals().size() > 0) {
+                                RRTNode *ch = this->goals().back();
+                                RRTNode *pn = this->goals().back()->parent();
+                                while (pn) {
+                                        pn->ccost(
+                                                ch->ccost()
+                                                - this->cost(pn, ch)
+                                        );
+                                        ch = pn;
+                                        pn = pn->parent();
+                                }
+                        }
                         return true;
                 }
         }
-- 
2.39.2