Update ccost of goal's parents

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9f14eed4644664b24dfa72f2c1fecf898c6e531e..b21340f491755448c4adba4f657433f9ebe6e94d 100644 (file)
--- 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;
                 }
         }