From d02603a5846e378e2765c4526ae2d67e282e256d Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 29 Mar 2021 12:27:29 +0200 Subject: [PATCH] Set use_nn when sampling case 3 This sampling select with 0.5 probability if random is chosen from steered1 or steered2. --- api/rrts.h | 4 ++-- src/rrts.cc | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/api/rrts.h b/api/rrts.h index 5632bfa..f64dc33 100644 --- 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 tmp_steered_; bool finishit = false; double path_cost_before_opt_ = 9999; diff --git a/src/rrts.cc b/src/rrts.cc index 911e9e2..217e44a 100644 --- 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( 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]; } } } -- 2.39.2