Add firsts, firsts getter

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d074418886c802f7d62b58383005d9b74a94dc04..5768286c8f3c28c665980fdea2c623aed1727da2 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -21,10 +21,14 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <queue>
 #include "bcar.h"
 #include "rrtbase.h"
+
+#if USE_GL > 0
 // OpenGL
 #include <GL/gl.h>
 #include <GL/glu.h>
 #include <SDL2/SDL.h>
+#endif
+
 // RRT
 #include "sample.h"
 #include "cost.h"
@@ -32,11 +36,15 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "nn.h"
 #include "nv.h"
 
+#if USE_GL > 0
 extern SDL_Window* gw;
 extern SDL_GLContext gc;
+#endif
 
 Cell::Cell()
-{}
+{
+        pthread_mutex_init(&this->m_, NULL);
+}
 
 bool Cell::changed()
 {
@@ -116,6 +124,11 @@ RRTNode *RRTBase::goal()
         return this->goal_;
 }
 
+std::vector<RRTNode *> &RRTBase::goals()
+{
+        return this->goals_;
+}
+
 std::vector<RRTNode *> &RRTBase::nodes()
 {
         return this->nodes_;
@@ -126,6 +139,11 @@ std::vector<RRTNode *> &RRTBase::dnodes()
         return this->dnodes_;
 }
 
+std::queue<RRTNode *> &RRTBase::firsts()
+{
+        return this->firsts_;
+}
+
 PolygonObstacle &RRTBase::frame()
 {
         return this->frame_;
@@ -189,6 +207,26 @@ float RRTBase::elapsed()
         return dt.count();
 }
 
+std::vector<RRTNode *> RRTBase::traj_cusp()
+{
+        std::vector<RRTNode *> tmp_cusps;
+        for (auto n: this->tlog().back()) {
+                if (sgn(n->s()) == 0) {
+                        tmp_cusps.push_back(n);
+                } else if (n->parent() &&
+                                sgn(n->s()) != sgn(n->parent()->s())) {
+                        tmp_cusps.push_back(n);
+                        tmp_cusps.push_back(n->parent());
+                }
+        }
+        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()])
+                        cusps.push_back(tmp_cusps[i]);
+        }
+        return cusps;
+}
+
 // setter
 void RRTBase::root(RRTNode *node)
 {
@@ -200,6 +238,19 @@ void RRTBase::goal(RRTNode *node)
         this->goal_ = node;
 }
 
+void RRTBase::goals(std::vector<RRTNode *> g)
+{
+        this->goals_ = g;
+        std::reverse(this->goals_.begin(), this->goals_.end());
+        RRTNode *pn = this->goals_.front();
+        for (auto n: this->goals_) {
+                if (n != pn) {
+                        pn->add_child(n, this->cost(pn ,n));
+                        pn = n;
+                }
+        }
+}
+
 bool RRTBase::logr(RRTNode *root)
 {
         std::vector<RRTEdge *> e; // Edges to log
@@ -315,6 +366,7 @@ void RRTBase::slot_cusp(std::vector<RRTNode *> sc)
 // other
 bool RRTBase::glplot()
 {
+#if USE_GL > 0
         glClear(GL_COLOR_BUFFER_BIT);
         glLineWidth(1);
         glPointSize(1);
@@ -405,6 +457,7 @@ bool RRTBase::glplot()
         SDL_GL_SwapWindow(gw);
         for (auto n: r)
                 n->visit(false);
+#endif
         return true;
 }
 
@@ -446,6 +499,76 @@ bool RRTBase::goal_found(
                                 return false;
                         }
                         this->goal_found_ = true;
+                        // Update ccost of goal's parents
+                        if (this->goals().size() > 0) {
+                                RRTNode *ch = this->goals().back();
+                                RRTNode *pn = this->goals().back()->parent();
+                                while (pn) {
+                                        pn->ccost(
+                                                ch->ccost()
+                                                - this->cost(pn, ch)
+                                        );
+                                        ch = pn;
+                                        pn = pn->parent();
+                                }
+                        }
+                        return true;
+                }
+        }
+        return false;
+}
+
+bool RRTBase::goal_found(
+        RRTNode *node,
+        RRTNode *goal
+)
+{
+        if (IS_NEAR(node, goal)) {
+                if (this->goal_found_) {
+                        if (
+                                goal->ccost() != -1
+                                && node->ccost() + this->cost(node, goal)
+                                < goal->ccost()
+                        ) {
+                                RRTNode *op; // old parent
+                                float oc; // old cumulative cost
+                                float od; // old direct cost
+                                op = goal->parent();
+                                oc = goal->ccost();
+                                od = goal->dcost();
+                                node->add_child(goal,
+                                                this->cost(node, goal));
+                                if (this->collide(node, goal)) {
+                                        node->children().pop_back();
+                                        goal->parent(op);
+                                        goal->ccost(oc);
+                                        goal->dcost(od);
+                                } else {
+                                        op->rem_child(goal);
+                                        return true;
+                                }
+                        } else {
+                                return false;
+                        }
+                } else {
+                        node->add_child(
+                                goal,
+                                this->cost(node, goal)
+                        );
+                        if (this->collide(node, goal)) {
+                                node->children().pop_back();
+                                goal->remove_parent();
+                                return false;
+                        }
+                        this->goal_found_ = true;
+                        // Update ccost of goal's children
+                        goal->update_ccost();
+                        // Update ccost of goals
+                        for (auto g: this->goals()) {
+                                if (g == goal)
+                                        break;
+                                g->ccost(-1);
+                        }
                         return true;
                 }
         }
@@ -863,7 +986,23 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 // RRT Framework
 RRTNode *RRTBase::sample()
 {
-        return sa1();
+        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 RRTBase::cost(RRTNode *init, RRTNode *goal)