From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 14 Dec 2018 14:28:00 +0000 (+0100)
Subject: Fix adding child in RRT* connect
X-Git-Tag: v0.4.0~13
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/4c38f35d3135b6f95ff385b2ea1670799d30bd10

Fix adding child in RRT* connect
---

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b1fa6e9..1c55967 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -236,7 +236,10 @@ bool Karaman2011::connect(
                         op = ns->parent();
                         od = ns->dcost();
                         oc = ns->ccost();
-                        nv->add_child(ns, this->cost(nv, ns));
+                        if (IS_NEAR(nv, ns))
+                                ns = nv;
+                        else
+                                nv->add_child(ns, this->cost(nv, ns));
                         if (this->collide(nv, ns)) {
                                 nv->children().pop_back();
                                 if (op)