Do sampling based on sample dist type

diff --git a/api/rrts.h b/api/rrts.h
index fbd3d3428c52b6e214db9f9232c41afa22969e3e..c9205b852a40dedf49b96add7e0b07c91744c3ae 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -96,6 +96,20 @@ class RRTS {
                 /*! \brief Update and return elapsed time.
                 */
                 double elapsed();
+                /*! \brief Set normal distribution for sampling.
+                */
+                void set_sample_normal(
+                        double x1, double x2,
+                        double y1, double y2,
+                        double h1, double h2
+                );
+                /*! \brief Set uniform distribution for sampling.
+                */
+                void set_sample_uniform(
+                        double x1, double x2,
+                        double y1, double y2,
+                        double h1, double h2
+                );
         protected:
                 /*! \brief Store RRT node to tree data structure.
                 */
@@ -113,6 +127,9 @@ class RRTS {
                         std::normal_distribution<double> ndx_;
                         std::normal_distribution<double> ndy_;
                         std::normal_distribution<double> ndh_;
+                        std::uniform_real_distribution<double> udx_;
+                        std::uniform_real_distribution<double> udy_;
+                        std::uniform_real_distribution<double> udh_;
                 virtual RRTNode *nn(RRTNode &t);
                 virtual std::vector<RRTNode *> nv(RRTNode &t);
                 void steer(RRTNode &f, RRTNode &t);
@@ -163,20 +180,30 @@ class RRTS {
                 bool next();
                 /*! \brief Set sampling info.
 
-                There is normal distribution sampling for `x`, `y`, and
-                `h` parameters of RRT node.
-
-                \param mx Mean x value.
-                \param dx Standard deviation of x.
-                \param my Mean y value.
-                \param dy Standard deviation of y.
-                \param mh Mean h value.
-                \param dh Standard deviation of h.
+                Based on `sample_dist_type`, set proper distribution
+                parameters. The distribution parameters are relative to `front`
+                node in `nodes` (init).
+
+                For normal sampling:
+                \param x1 Mean x value.
+                \param x2 Standard deviation of x.
+                \param y1 Mean y value.
+                \param y2 Standard deviation of y.
+                \param h1 Mean h value.
+                \param h2 Standard deviation of h.
+
+                For uniform sampling:
+                \param x1 Minimum x value.
+                \param x2 Maximum x value.
+                \param y1 Minimum y value.
+                \param y2 Maximum y value.
+                \param h1 Minimum h value.
+                \param h2 Maximum h value.
                 */
                 void set_sample(
-                        double mx, double dx,
-                        double my, double dy,
-                        double mh, double dh
+                        double x1, double x2,
+                        double y1, double y2,
+                        double h1, double h2
                 );
                 /*! \brief Generate JSON output.
                 */

diff --git a/src/rrts.cc b/src/rrts.cc
index d69b2a3d6f589ccdcf34b9d05816905df78eeda8..1877ff28aeeeac62a0bb8bb1cbc2f5271ebf9fb3 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -140,9 +140,20 @@ double RRTS::cost_search(RRTNode &f, RRTNode &t)
 
 void RRTS::sample()
 {
-        double x = this->ndx_(this->gen_);
-        double y = this->ndy_(this->gen_);
-        double h = this->ndh_(this->gen_);
+        double x = 0;
+        double y = 0;
+        double h = 0;
+        switch (this->sample_dist_type()) {
+        case 1:
+                x = this->udx_(this->gen_);
+                y = this->udy_(this->gen_);
+                h = this->udh_(this->gen_);
+                break;
+        default:
+                x = this->ndx_(this->gen_);
+                y = this->ndy_(this->gen_);
+                h = this->ndh_(this->gen_);
+        }
         this->samples().push_back(RRTNode());
         this->samples().back().x(x);
         this->samples().back().y(y);
@@ -349,7 +360,7 @@ bool RRTS::next()
         return next;
 }
 
-void RRTS::set_sample(
+void RRTS::set_sample_normal(
         double mx, double dx,
         double my, double dy,
         double mh, double dh
@@ -359,6 +370,34 @@ void RRTS::set_sample(
         this->ndy_ = std::normal_distribution<double>(my, dy);
         this->ndh_ = std::normal_distribution<double>(mh, dh);
 }
+void RRTS::set_sample_uniform(
+        double xmin, double xmax,
+        double ymin, double ymax,
+        double hmin, double hmax
+)
+{
+        this->udx_ = std::uniform_real_distribution<double>(xmin,xmax);
+        this->udy_ = std::uniform_real_distribution<double>(ymin,ymax);
+        this->udh_ = std::uniform_real_distribution<double>(hmin,hmax);
+}
+void RRTS::set_sample(
+        double x1, double x2,
+        double y1, double y2,
+        double h1, double h2
+)
+{
+        switch (this->sample_dist_type()) {
+        case 1:
+                x1 += this->nodes().front().x();
+                x2 += this->nodes().front().x();
+                y1 += this->nodes().front().y();
+                y2 += this->nodes().front().y();
+                this->set_sample_uniform(x1, x2, y1, y2, h1, h2);
+                break;
+        default:
+                this->set_sample_normal(x1, x2, y1, y2, h1, h2);
+        }
+}
 
 Json::Value RRTS::json()
 {