Fix adding child in RRT* connect

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b1fa6e98c958f8af25549d2d69bde08226290f7f..1c55967a64a594a2802e6374ee48cbeac7607b6d 100644 (file)
--- 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)