Change guided sampling

Sampling info contains information of bounding box where to sample:
- `x0 - x/2 ... x0 + x/2`
- `y0 - y/2 ... y0 + y/2`
- `h0 - h/2 ... h0 + h/2`

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9c98b3d5204ddfad50a8198dfe2be6e21b471484..cb14c2b09651ca4b486670888ad027b751199405 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -894,23 +894,19 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 RRTNode *RRTBase::sample()
 {
         if (this->useSamplingInfo_ && this->nodes().size() % 2 == 0) {
-                float sar = static_cast<float>(rand());
-                sar /= static_cast<float>(RAND_MAX / this->samplingInfo_.r);
-                sar += this->samplingInfo_.mr;
-                float sah = static_cast<float>(rand());
-                sah /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
-                sah *= this->samplingInfo_.dh;
-                sah += this->samplingInfo_.sh;
+                float x = static_cast<float>(rand());
+                x /= static_cast<float>(RAND_MAX / this->samplingInfo_.x);
+                x -= this->samplingInfo_.x / 2;
+                x += this->samplingInfo_.x0;
+                float y = static_cast<float>(rand());
+                y /= static_cast<float>(RAND_MAX / this->samplingInfo_.y);
+                y -= this->samplingInfo_.y / 2;
+                y += this->samplingInfo_.y0;
                 float h = static_cast<float>(rand());
-                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.mh);
-                h += this->samplingInfo_.mmh;
-                h *= this->samplingInfo_.dh;
-                h += this->samplingInfo_.sh;
-                return new RRTNode(
-                        this->samplingInfo_.x + sar * cos(sah),
-                        this->samplingInfo_.y + sar * sin(sah),
-                        h
-                );
+                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
+                h -= this->samplingInfo_.h / 2;
+                h += this->samplingInfo_.h0;
+                return new RRTNode(x, y, h);
         } else {
                 return sa1();
         }

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index c107693bfc4a939ff9441678b8411b527145a971..0685f71d6ac20beaa29e15e111d258c1f14e572c 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -107,6 +107,7 @@ void ParallelSlot::fip(
         std::vector<SegmentObstacle>& so
 )
 {
+        this->setAll();
         if (this->slotType() == PERPENDICULAR) {
                 // TODO different slot headings
                 // this is jus for slot heading = pi / 2
@@ -662,30 +663,22 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
+        RRTNode *n = this->getMidd();
         if (this->slotType() == PARALLEL) {
-                si.x = this->slot().bnodes().front()->x();
-                si.y = this->slot().bnodes().front()->y();
-                si.mr = BCAR_WIDTH / 2;
-                si.mmh = 0;
+                if (n != nullptr) {
+                        si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
+                        si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
+                        si.h0 = n->h();
+                } else {
+                        si.x0 = this->slot().bnodes().front()->x();
+                        si.y0 = this->slot().bnodes().front()->y();
+                        si.h0 = this->slotHeading();
+                }
+                si.x = BCAR_WIDTH;
+                si.y = BCAR_WIDTH;
+                si.h = M_PI / 8;
         } else {
-                si.x = this->slot().bnodes().back()->x();
-                si.x -= this->slot().bnodes().front()->x();
-                si.x /= 2;
-                si.x += this->slot().bnodes().front()->x();
-                si.y = this->slot().bnodes().back()->y();
-                si.y -= this->slot().bnodes().front()->y();
-                si.y /= 2;
-                si.y += this->slot().bnodes().front()->y();
-                si.mr = 0;
-                si.mmh = (M_PI - M_PI / 6) / 2;
+                // TODO
         }
-        si.r = BCAR_LENGTH;
-        si.h = M_PI;
-        si.mh = M_PI / 2;
-        si.sh = this->slotHeading();
-        if (this->slotSide() == RIGHT)
-                si.dh = 1;
-        else
-                si.dh = -1;
         return si;
 }

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index f10b7def354653ce59f983d1fa2f26ab68ab3f89..343888da6aa64f14ba04420f27141aef5da48ede 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -34,15 +34,12 @@ enum SlotType {
 };
 
 struct SamplingInfo {
+        float x0;
+        float y0;
+        float h0;
         float x;
         float y;
-        float r; // max radi from [x, y] for random sample
-        float mr; // minimum r added to random r
-        float h; // max angle for random sample
-        float mh; // max angle for heading of random sample
-        float mmh; // minimum mh added to mh
-        float sh; // slot heading
-        float dh; // direction to compute random heading from slot heading
+        float h;
 };
 
 class ParallelSlot {