Use nn in RRTBase

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d8299d2aaaef9e7d3e4fb83cec3e0674ff376fa0..41197b301a6732c0632f8ac67fccb8c47fe8dec5 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -95,6 +95,9 @@ RRTBase::RRTBase()
         : root_(new RRTNode())
         , goal_(new RRTNode())
         , gen_(std::random_device{}())
+        , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
+        , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
@@ -106,6 +109,9 @@ RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
         : root_(init)
         , goal_(goal)
         , gen_(std::random_device{}())
+        , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
+        , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
@@ -393,12 +399,52 @@ bool RRTBase::glplot()
                 glVertex2f(GLVERTEX(this->samples_.back()));
                 glEnd();
         }
-        // Plot nodes
+        // Plot nodes (position, orientation
         std::vector<RRTNode *> s; // DFS stack
         std::vector<RRTNode *> r; // reset visited_
         RRTNode *tmp;
         glBegin(GL_LINES);
         s.push_back(this->root_);
+        while (s.size() > 0) {
+                tmp = s.back();
+                s.pop_back();
+                if (!tmp->visit()) {
+                        r.push_back(tmp);
+                        for (auto ch: tmp->children()) {
+                                s.push_back(ch);
+                                glColor3f(0.5, 0.5, 0.5);
+                                BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
+                                glVertex2f(
+                                        bc.lfx() * GLPLWSCALE,
+                                        bc.lfy() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.lrx() * GLPLWSCALE,
+                                        bc.lry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.lrx() * GLPLWSCALE,
+                                        bc.lry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rrx() * GLPLWSCALE,
+                                        bc.rry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rrx() * GLPLWSCALE,
+                                        bc.rry() * GLPLHSCALE
+                                );
+                                glVertex2f(
+                                        bc.rfx() * GLPLWSCALE,
+                                        bc.rfy() * GLPLHSCALE
+                                );
+                        }
+                }
+        }
+        glEnd();
+        // Plot nodes
+        glBegin(GL_LINES);
+        s.push_back(this->root_);
         while (s.size() > 0) {
                 tmp = s.back();
                 s.pop_back();
@@ -465,7 +511,7 @@ bool RRTBase::goal_found(
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode* ))
 {
-        if (IS_NEAR(node, this->goal_)) {
+        if (GOAL_IS_NEAR(node, this->goal_)) {
                 if (this->goal_found_) {
                         if (node->ccost() + this->cost(node, this->goal_) <
                                         this->goal_->ccost()) {
@@ -523,7 +569,7 @@ bool RRTBase::goal_found(
         RRTNode *goal
 )
 {
-        if (IS_NEAR(node, goal)) {
+        if (GOAL_IS_NEAR(node, goal)) {
                 if (this->goal_found_) {
                         if (
                                 goal->ccost() != -1
@@ -742,6 +788,8 @@ bool RRTBase::optp_dijkstra(
                         ch_cost = dnodes[tmp.ni].c +
                                 this->cost(cusps[tmp.ni], cusps[i]);
                         steered = this->steer(cusps[tmp.ni], cusps[i]);
+                        if (steered.size() <= 0)
+                                break;
                         for (unsigned int j = 0; j < steered.size() - 1; j++)
                                 steered[j]->add_child(steered[j + 1], 1);
                         if (this->collide(
@@ -877,7 +925,10 @@ bool RRTBase::opt_path()
                 return false;
         std::vector<RRTNode *> cusps;
         for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
-                if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
+                if (!IS_NEAR(
+                        tmp_cusps[i],
+                        tmp_cusps[(i + 1) % tmp_cusps.size()]
+                ))
                         cusps.push_back(tmp_cusps[i]);
         }
         std::reverse(cusps.begin(), cusps.end());
@@ -984,25 +1035,19 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::setSamplingInfo(SamplingInfo si)
+{
+        this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
+        this->ndy_ = std::normal_distribution<float>(si.y0, si.y);
+        this->ndh_ = std::normal_distribution<float>(si.h0, si.h);
+}
+
 RRTNode *RRTBase::sample()
 {
-        if (this->useSamplingInfo_ && this->nodes().size() % 2 == 0) {
-                float x = static_cast<float>(rand());
-                x /= static_cast<float>(RAND_MAX / this->samplingInfo_.x);
-                x -= this->samplingInfo_.x / 2;
-                x += this->samplingInfo_.x0;
-                float y = static_cast<float>(rand());
-                y /= static_cast<float>(RAND_MAX / this->samplingInfo_.y);
-                y -= this->samplingInfo_.y / 2;
-                y += this->samplingInfo_.y0;
-                float h = static_cast<float>(rand());
-                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
-                h -= this->samplingInfo_.h / 2;
-                h += this->samplingInfo_.h0;
-                return new RRTNode(x, y, h);
-        } else {
-                return sa1();
-        }
+        float x = this->ndx_(this->gen_);
+        float y = this->ndy_(this->gen_);
+        float h = this->ndh_(this->gen_);
+        return new RRTNode(x, y, h);
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)
@@ -1012,8 +1057,39 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
-        return nn4(this->iy_, rs, nullptr);
-        //return nn3(this->iy_, rs, nullptr);
+        int iy = IYI(rs->y());
+        struct mcnn nn;
+        nn.nn = nullptr;
+        nn.mc = 9999;
+        unsigned int i = 0; // vector step
+        unsigned int j = 0; // array step
+        int iyj = 0;
+        while (nn.mc > j * IYSTEP) {
+                iyj = (int) (iy + j);
+                if (iyj >= IYSIZE)
+                        iyj = IYSIZE - 1;
+                #pragma omp parallel for reduction(minn: nn)
+                for (i = 0; i < this->iy_[iyj].size(); i++) {
+                        if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                nn.nn = this->iy_[iyj][i];
+                        }
+                }
+                if (j > 0) {
+                        iyj = (int) (iy - j);
+                        if (iyj < 0)
+                                iyj = 0;
+                        #pragma omp parallel for reduction(minn: nn)
+                        for (i = 0; i < this->iy_[iyj].size(); i++) {
+                                if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                        nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                        nn.nn = this->iy_[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn.nn;
 }
 
 std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)