Tune sampling coordinates

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 3d890364669b05206b9377780c02e93e7fa853a6..e049143a1413250af6535fb6a6901e2c756c3bc4 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -889,7 +889,7 @@ RRTNode *RRTBase::sample()
                 );
                 std::normal_distribution<float> hdist(
                         0,
-                        this->samplingInfo_.h / 3
+                        this->samplingInfo_.h
                 );
                 if (!this->samplingInfo_.r) {
                         float dx = BCAR_WIDTH / 2 +
@@ -915,8 +915,16 @@ RRTNode *RRTBase::sample()
                 } else {
                         float dr = std::abs(xdist(this->gen_));
                         float dh = hdist(this->gen_);
-                        x += dr * cos(this->samplingInfo_.sh + M_PI / 2 + dh);
-                        y += dr * sin(this->samplingInfo_.sh + M_PI / 2 + dh);
+                        x += dr * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
+                        y += dr * sin(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
                         h = -M_PI / 2 + dh;
                 }
                 return new RRTNode(

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 93ea4c7dfde109722e3ce93798d3cace9bdcecb8..51219061dfbd8f9da58a8f9d0d8543c861b6ac4e 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -561,7 +561,7 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
                         this->slot().bnodes()[1]
                 ) / BCAR_LENGTH);
         } else {
-                si.h = M_PI /2;
+                si.h = M_PI / 2 / 3;
         }
 #else
         si.x = this->slot().bnodes()[3]->x() - this->slot().bnodes()[0]->x();
@@ -582,7 +582,7 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
         si.r = EDIST(this->slot().bnodes()[0], this->slot().bnodes()[3]) / 2;
         si.r *= 2;
         si.sh = this->slotHeading();
-        si.h = M_PI / 2;
+        si.h = M_PI / 4;
 #endif
         return si;
 }