From ba55cdb43e39e775c2963cfa35a9ee1fda42cc19 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 10 Jun 2019 16:17:58 +0200
Subject: [PATCH] Add goal_found method for two RRTNodes

---
 base/main.cc                   |  2 +-
 base/rrtbase.cc                | 57 ++++++++++++++++++++++++++++++++++
 decision_control/rrtplanner.cc |  7 +----
 incl/rrtbase.h                 |  4 +++
 4 files changed, 63 insertions(+), 7 deletions(-)

diff --git a/base/main.cc b/base/main.cc
index e028b28..8c544c1 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -199,8 +199,8 @@ int main()
         }
         if (ps.cusp().size() > 0) {
                 p.goal(ps.getMidd());
-                p.goals(ps.goals());
                 p.slot_cusp(ps.cusp().front()); // use first found solution
+                p.goals(ps.goals());
                 jvo["midd"][0] = p.goal()->x();
                 jvo["midd"][1] = p.goal()->y();
                 jvo["midd"][2] = p.goal()->h();
diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index b21340f..da51b5a 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -513,6 +513,63 @@ bool RRTBase::goal_found(
         return false;
 }
 
+bool RRTBase::goal_found(
+        RRTNode *node,
+        RRTNode *goal
+)
+{
+        if (IS_NEAR(node, goal)) {
+                if (this->goal_found_) {
+                        if (
+                                goal->ccost() != -1
+                                && node->ccost() + this->cost(node, goal)
+                                < goal->ccost()
+                        ) {
+                                RRTNode *op; // old parent
+                                float oc; // old cumulative cost
+                                float od; // old direct cost
+                                op = goal->parent();
+                                oc = goal->ccost();
+                                od = goal->dcost();
+                                node->add_child(goal,
+                                                this->cost(node, goal));
+                                if (this->collide(node, goal)) {
+                                        node->children().pop_back();
+                                        goal->parent(op);
+                                        goal->ccost(oc);
+                                        goal->dcost(od);
+                                } else {
+                                        op->rem_child(goal);
+                                        return true;
+                                }
+                        } else {
+                                return false;
+                        }
+                } else {
+                        node->add_child(
+                                goal,
+                                this->cost(node, goal)
+                        );
+                        if (this->collide(node, goal)) {
+                                node->children().pop_back();
+                                goal->remove_parent();
+                                return false;
+                        }
+                        this->goal_found_ = true;
+                        // Update ccost of goal's children
+                        goal->update_ccost();
+                        // Update ccost of goals
+                        for (auto g: this->goals()) {
+                                if (g == goal)
+                                        break;
+                                g->ccost(-1);
+                        }
+                        return true;
+                }
+        }
+        return false;
+}
+
 bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 {
         std::vector<RRTEdge *> edges;
diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 0723be8..b0cb089 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -604,14 +604,9 @@ bool T2::next()
                                         this->add_ixy(ns);
                                         this->ocost(ns);
                                         pn = ns;
-                                        RRTNode *oldgoal = this->goal();
-                                        this->goal(go);
-                                        if (this->goal_found(pn, CO)) {
-                                                this->goal_cost();
+                                        if (this->goal_found(pn, go)) {
                                                 this->tlog(this->findt());
                                                 en_add = false;
-                                        } else {
-                                                this->goal(oldgoal);
                                         }
                                 }
                         }
diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 84ca580..1f1b6f4 100644
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -132,6 +132,10 @@ class RRTBase {
                 bool goal_found(
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
+                bool goal_found(
+                        RRTNode *node,
+                        RRTNode *goal
+                );
                 bool collide(RRTNode *init, RRTNode *goal);
                 bool optp_dijkstra(
                                 std::vector<RRTNode *> &cusps,
-- 
2.39.2