]> rtime.felk.cvut.cz Git - hubacji1/iamcar.git/commitdiff
projects / hubacji1 / iamcar.git / commitdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | patch | inline | side by side (from parent 1: f444f8a)
Add next NN/NV VERSION
authorJiri Vlasak <hubacji1@fel.cvut.cz>
Mon, 3 Dec 2018 15:49:37 +0000 (16:49 +0100)
committerJiri Vlasak <hubacji1@fel.cvut.cz>
Mon, 10 Dec 2018 07:07:19 +0000 (08:07 +0100)
decision_control/rrtplanner.cc patch | blob | history
incl/rrtplanner.h patch | blob | history

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index e123fe95baa47ffe5d4a235e4066f6d812fbc8e8..94acac2f1373eade7c83424a32cdaa83db0fcd37 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -52,7 +52,9 @@ bool LaValle1998::next()
         rs = this->sample();
 #endif
         this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
         RRTNode *nn = this->nn(this->iy_, rs, this->cost);
 #else
         RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
@@ -115,7 +117,9 @@ bool Kuwata2008::next()
                 else
                         this->cost = &KUWATA2008_DCOST;
         }
-#if NNVERSION>1
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
         RRTNode *nn = this->nn(this->iy_, rs, this->cost);
 #else
         RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
@@ -201,7 +205,9 @@ bool Karaman2011::next()
         rs = this->sample();
 #endif
         this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
         RRTNode *nn = this->nn(this->iy_, rs, this->cost);
 #else
         RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
@@ -213,7 +219,17 @@ bool Karaman2011::next()
                 if (!en_add) {
                         delete ns;
                 } else {
-#if NVVERSION>1
+#if NVVERSION>2
+        nvs = this->nv(
+                        this->iy_,
+                        ns,
+                        this->cost,
+                        MIN(
+                                GAMMA_RRTSTAR(
+                                        this->nodes().size()),
+                                0.2),
+                        '0'); // TODO const
+#elif NVVERSION>1
                         nvs = this->nv(
                                         this->iy_,
                                         ns,
@@ -338,7 +354,9 @@ bool T1::next()
         else
                 rs = this->sample();
         this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
         RRTNode *nn = this->nn(this->iy_, rs, this->cost);
 #else
         RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
@@ -353,7 +371,17 @@ bool T1::next()
         // RRT* for first node
         RRTNode *ns = steered[0];
         {
-#if NVVERSION>1
+#if NVVERSION>2
+        nvs = this->nv(
+                        this->iy_,
+                        ns,
+                        this->cost,
+                        MIN(
+                                GAMMA_RRTSTAR(
+                                        this->nodes().size()),
+                                0.2),
+                        '0'); // TODO const
+#elif NVVERSION>1
                 nvs = this->nv(this->iy_, ns, this->cost, MIN(
                                         GAMMA_RRTSTAR(this->nodes().size()),
                                         0.2)); // TODO const
@@ -446,7 +474,9 @@ bool T2::next()
         rs = this->sample();
 #endif
         this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+        RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
         RRTNode *nn = this->nn(this->iy_, rs, this->cost);
 #else
         RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
@@ -468,7 +498,17 @@ bool T2::next()
                                 cusps++;
                         if (cusps > 4)
                                 en_add = false;
-#if NVVERSION>1
+#if NVVERSION>2
+        nvs = this->nv(
+                        this->iy_,
+                        ns,
+                        this->cost,
+                        MIN(
+                                GAMMA_RRTSTAR(
+                                        this->nodes().size()),
+                                0.2),
+                        '0'); // TODO const
+#elif NVVERSION>1
                         nvs = this->nv(
                                         this->iy_,
                                         ns,
@@ -548,7 +588,17 @@ bool T2::next()
 float T2::goal_cost()
 {
         std::vector<RRTNode *> nvs;
-#if NVVERSION>1
+#if NVVERSION>2
+        nvs = this->nv(
+                        this->iy_,
+                        this->goal(),
+                        this->cost,
+                        MIN(
+                                GAMMA_RRTSTAR(
+                                        this->nodes().size()),
+                                0.2),
+                        '0'); // TODO const
+#elif NVVERSION>1
         nvs = this->nv(
                         this->iy_,
                         this->goal(),
diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index 336a3e8f839e03fd98da54b53ae845c0e95cf113..bbe019bcae36e14a283bf5f938962fb530f8d297 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -37,7 +37,11 @@ class LaValle1998: public RRTBase {
                                 std::vector<RRTNode *> &nodes,
 #endif
                                 RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
+                                float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+                                , char tree
+#endif
+                                );
                 RRTNode *(*sample)();
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
@@ -58,7 +62,11 @@ class Kuwata2008: public RRTBase {
                                 std::vector<RRTNode *> &nodes,
 #endif
                                 RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
+                                float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+                                , char tree
+#endif
+                                );
                 RRTNode *(*sample)();
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
@@ -85,7 +93,11 @@ class Karaman2011: public RRTBase {
                                 std::vector<RRTNode *> &nodes,
 #endif
                                 RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
+                                float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+                                , char tree
+#endif
+                                );
                 std::vector<RRTNode *> (*nv)(
 #if NVVERSION>1
                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
@@ -94,7 +106,11 @@ class Karaman2011: public RRTBase {
 #endif
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *),
-                                float dist);
+                                float dist
+#if NVVERSION>2
+                                , char tree
+#endif
+                                );
                 RRTNode *(*sample)();
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
@@ -115,7 +131,11 @@ class T1: public RRTBase {
                                 std::vector<RRTNode *> &nodes,
 #endif
                                 RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
+                                float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+                                , char tree
+#endif
+                                );
                 std::vector<RRTNode *> (*nv)(
 #if NVVERSION>1
                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
@@ -124,7 +144,11 @@ class T1: public RRTBase {
 #endif
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *),
-                                float dist);
+                                float dist
+#if NVVERSION>2
+                                , char tree
+#endif
+                                );
                 RRTNode *(*sample)();
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,
@@ -174,7 +198,11 @@ class Klemm2015: public Karaman2011 {
                                 std::vector<RRTNode *> &nodes,
 #endif
                                 RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
+                                float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+                                , char tree
+#endif
+                                );
                 std::vector<RRTNode *> (*nv)(
 #if NVVERSION>1
                                 std::vector<RRTNode *> (&nodes)[IYSIZE],
@@ -183,7 +211,11 @@ class Klemm2015: public Karaman2011 {
 #endif
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *),
-                                float dist);
+                                float dist
+#if NVVERSION>2
+                                , char tree
+#endif
+                                );
                 RRTNode *(*sample)();
                 std::vector<RRTNode *> (*steer)(
                                 RRTNode *init,