From 4c38f35d3135b6f95ff385b2ea1670799d30bd10 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 14 Dec 2018 15:28:00 +0100
Subject: [PATCH] Fix adding child in RRT* connect

---
 decision_control/rrtplanner.cc | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

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)
-- 
2.39.2