Set use_nn when sampling case 3

This sampling select with 0.5 probability if random is chosen from steered1 or
steered2.

diff --git a/api/rrts.h b/api/rrts.h
index 5632bfa52a0a02506d9519fbdd9e95f7734c86c4..f64dc33b98570067b193cf29216365d93cef8607 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -129,7 +129,7 @@ class Obstacle {
 normal, 1 - uniform, 2 - uniform circle)
 */
 class RRTS {
-        private:
+        protected:
                 unsigned int icnt_ = 0;
                 std::chrono::high_resolution_clock::time_point tstart_;
                 double scnt_ = 0;
@@ -168,7 +168,7 @@ class RRTS {
                 /*! \brief Set uniform circle distribution for sampling.
                 */
                 void set_sample_uniform_circle();
-        protected:
+                RRTNode* use_nn; // Used for RRTExt12.
                 std::vector<RRTNode> tmp_steered_;
                 bool finishit = false;
                 double path_cost_before_opt_ = 9999;

diff --git a/src/rrts.cc b/src/rrts.cc
index 911e9e297573067d5fa625481634314bb2fe01e5..217e44ad747172fb0a2befaae3b9acd3863d0771 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -225,6 +225,7 @@ void RRTS::sample()
                         x = this->nodes().front().x();
                         y = this->nodes().front().y();
                         h = this->nodes().front().h();
+                        this->use_nn = &this->nodes().front();
                 } else {
                         this->udi1_ = std::uniform_int_distribution<unsigned int>(
                                 0,
@@ -243,6 +244,7 @@ void RRTS::sample()
                                 x = n1->x();
                                 y = n1->y();
                                 h = n1->h();
+                                this->use_nn = this->steered1_[ind1];
                         } else if (
                                 this->steered1_.size() == 0
                         ) {
@@ -250,6 +252,7 @@ void RRTS::sample()
                                 x = n2->x();
                                 y = n2->y();
                                 h = n2->h();
+                                this->use_nn = this->steered2_[ind2];
                         } else {
                                 auto n1 = this->steered1_[ind1];
                                 auto n2 = this->steered2_[ind2];
@@ -258,10 +261,12 @@ void RRTS::sample()
                                         x = n1->x();
                                         y = n1->y();
                                         h = n1->h();
+                                        this->use_nn = this->steered1_[ind1];
                                 } else {
                                         x = n2->x();
                                         y = n2->y();
                                         h = n2->h();
+                                        this->use_nn = this->steered2_[ind2];
                                 }
                         }
                 }