Add method for setting sampling info

diff --git a/api/rrts.h b/api/rrts.h
index b18858643c9806b0ea71e35f1b2fcbd605aee6d2..2e7f53da46b9fd483edccfca7de43e50a72c6a77 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -64,6 +64,23 @@ class RRTS {
                 /*! \brief Run next RRT* iteration.
                 */
                 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.
+                */
+                void set_sample(
+                        double mx, double dx,
+                        double my, double dy,
+                        double mh, double dh
+                );
 
                 // getters, setters
                 std::vector<RRTNode> &goals() { return this->goals_; }

diff --git a/src/rrts.cc b/src/rrts.cc
index d5faf1f75ebf9f45205119c2a2b3f5900d19c94f..3968da41fb18f4408473db28069351afb3f423b5 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -135,6 +135,17 @@ bool RRTS::next()
         return next;
 }
 
+void RRTS::set_sample(
+        double mx, double dx,
+        double my, double dy,
+        double mh, double dh
+)
+{
+        this->ndx_ = std::normal_distribution<double>(mx, dx);
+        this->ndy_ = std::normal_distribution<double>(my, dy);
+        this->ndh_ = std::normal_distribution<double>(mh, dh);
+}
+
 RRTS::RRTS()
         : gen_(std::random_device{}())
 {