Fix findt() for multiple goals

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5bb1385616c49b3b0826a3409bf8ae303338524f..dbcf838f74f7887133a1ab34b03cc3f476241e38 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1026,7 +1026,14 @@ bool RRTBase::rebase(RRTNode *nr)
 
 std::vector<RRTNode *> RRTBase::findt()
 {
-        return this->findt(this->goal_);
+        RRTNode *goal = this->goal_;
+        for (auto g: this->goals()) {
+                if (goal->parent() == nullptr || g->ccost() < g->ccost())
+                        goal = g;
+        }
+        if (goal->parent() == nullptr)
+                this->goal_found(false);
+        return this->findt(goal);
 }
 
 std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
@@ -1060,6 +1067,17 @@ int RRTBase::YI(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::defaultSamplingInfo()
+{
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
+}
+
 void RRTBase::setSamplingInfo(SamplingInfo si)
 {
         this->ndx_ = std::normal_distribution<float>(si.x0, si.x);