Update RRT sampling with right side parking

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index ab7cf25173c5a7701c3c40aef84a6d91751b843a..fab537ff691f2c8eaedba030bad00e13c209c561 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -865,24 +865,34 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 // RRT Framework
 RRTNode *RRTBase::sample()
 {
+        float x = this->samplingInfo_.x;
+        float y = this->samplingInfo_.y;
         std::normal_distribution<float> xdist(
                 0,
                 BCAR_WIDTH / 3
         );
-        float x = this->samplingInfo_.x
-                + BCAR_WIDTH / 2
-                + std::abs(xdist(this->gen_));
+        x += this->samplingInfo_.dx
+                * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
+                * cos(this->samplingInfo_.sh + M_PI / 2);
+        y += this->samplingInfo_.dx
+                * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
+                * sin(this->samplingInfo_.sh + M_PI / 2);
         std::normal_distribution<float> ydist(
                 0,
                 BCAR_LENGTH / 3
         );
-        float y = this->samplingInfo_.y + ydist(this->gen_);
+        x += ydist(this->gen_) * cos(this->samplingInfo_.sh);
+        y += ydist(this->gen_) * sin(this->samplingInfo_.sh);
         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);
+        return new RRTNode(
+                x,
+                y,
+                this->samplingInfo_.sh + this->samplingInfo_.dh * h
+        );
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)