Add default sampling info setter

diff --git a/base/main.cc b/base/main.cc
index effd757a0c7cf12d8d8e1f560cbdeb8943312360..c327d78c2c69af7ff602b7f9f6679b5f6b8654ed 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -191,6 +191,7 @@ int main()
                         if (tmps > p.VMAX) p.VMAX = tmps;
                 }
         }
+        p.defaultSamplingInfo();
         p.link_obstacles(&co, &so);
         p.ocost(p.root());
         p.ocost(p.goal());

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5bb1385616c49b3b0826a3409bf8ae303338524f..bf366dd056624663be91466f8dfdc95a38d2670e 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1060,6 +1060,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);

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 44dc06de2784b6ac5fc5022c64adfdebdbe46849..9b17d44e673c772bc1d7501f215fb8940b2565eb 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -181,6 +181,7 @@ class RRTBase {
                 int YI(RRTNode *n);
 
                 // RRT Framework
+                void defaultSamplingInfo();
                 void setSamplingInfo(SamplingInfo si);
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);