Generate samples randomly based on SamplingInfo

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d074418886c802f7d62b58383005d9b74a94dc04..692a2093cf23ac6d1c13da401b0d1bd42b312c9b 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -863,7 +863,24 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 // RRT Framework
 RRTNode *RRTBase::sample()
 {
-        return sa1();
+        std::normal_distribution<float> xdist(
+                0,
+                BCAR_WIDTH / 3
+        );
+        float x = this->samplingInfo_.x
+                + BCAR_WIDTH / 2
+                + std::abs(xdist(this->gen_));
+        std::normal_distribution<float> ydist(
+                0,
+                BCAR_LENGTH / 3
+        );
+        float y = this->samplingInfo_.y + ydist(this->gen_);
+        std::normal_distribution<float> hdist(
+                0,
+                this->samplingInfo_.h / 3
+        );
+        float h = std::abs(hdist(this->gen_));
+        return new RRTNode(x, y, M_PI / 2 - h);
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)