Update RRT sampling with right side parking

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)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 99cb1f136011394ec5627c673ee36f419d6bad55..3516726ace5bdc55c86c16b4c593f63a3a5b2eb7 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -473,14 +473,20 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
-        BicycleCar *CC = new BicycleCar(
-                this->fposecenter()->x(),
-                this->fposecenter()->y() - 0.01,
-                this->slotHeading()
-        );
+        BicycleCar *CC = this->getEPC();
         si.x = this->slot().bnodes()[0]->x();
         si.y = this->slot().bnodes()[0]->y();
+        if (this->slotSide() == RIGHT) {
+                si.dx = 1;
+                si.dy = 1;
+                si.dh = 1;
+        } else {
+                si.dx = -1;
+                si.dy = -1;
+                si.dh = -1;
+        }
         si.r = CC->diag_radi();
+        si.sh = this->slotHeading();
         si.h = this->slotHeading() - acos(EDIST( // TODO generalize
                 this->slot().bnodes()[0],
                 this->slot().bnodes()[1]

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index d4f7ba06d6c34d328c29535ed2e885c94367372d..ee45122315c55f8052425190a3e9a06f4eb3d24a 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -39,9 +39,13 @@ enum SlotType {
 
 struct SamplingInfo {
         float x;
+        float dx;
         float y;
+        float dy;
         float r;
+        float sh;
         float h;
+        float dh;
 };
 
 class ParallelSlot {