Add goal_found method for two RRTNodes

diff --git a/base/main.cc b/base/main.cc
index e028b2899b01a17c16c1e24cd4d3498f92c79a57..8c544c198ec498930ace88e3fac16a0d7935e75f 100644 (file)
--- 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 b21340f491755448c4adba4f657433f9ebe6e94d..da51b5af802f5274dca94915b7657c883301d153 100644 (file)
--- 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 0723be8a59fdebd48441839fa5e9e32fcd0cad4e..b0cb089f75c201c194c18c6521ac02ec3ee5c9de 100644 (file)
--- 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 84ca580288c21e7f99e05bbecb3f40f143a19ccf..1f1b6f479334ff7491baace6a7cb3bc44ac27b59 100644 (file)
--- 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,