Add goal_found method for two RRTNodes

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

diff --git a/base/main.cc b/base/main.cc
index 7c5f41655bfc0cfbafbc94beadc72a67d5294dca..8c544c198ec498930ace88e3fac16a0d7935e75f 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -17,6 +17,8 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <algorithm>
 #include <chrono>
+#include <cmath>
+#include <cstdlib>
 #include <iostream>
 #include <jsoncpp/json/json.h>
 #include <pthread.h>
@@ -109,8 +111,18 @@ void *next_run(void *arg)
 }
 #endif
 
+RRTNode *sa_tmp()
+{
+        float new_x = 1 + static_cast<float>(rand()) /
+                static_cast<float>(RAND_MAX / (6.6 - 1 - 1));
+        float new_y = 1;
+        float new_h = M_PI / 2;
+        return new RRTNode(new_x, new_y, new_h);
+}
+
 int main()
 {
+        srand(static_cast<unsigned>(time(0)));
 #if USE_GL > 0
         init();
 #endif
@@ -131,6 +143,9 @@ int main()
                                 jvi["goal"][0].asFloat(),
                                 jvi["goal"][1].asFloat(),
                                 jvi["goal"][2].asFloat()));
+        jvo["init"][0] = p.root()->x();
+        jvo["init"][1] = p.root()->y();
+        jvo["init"][2] = p.root()->h();
         std::vector<CircleObstacle> co;
         std::vector<SegmentObstacle> so;
         for (auto o: jvi["obst"]) {
@@ -169,21 +184,23 @@ int main()
                         ));
                 }
         }
-        if (ps.slot().bnodes().size() > 0) {
-                ps.setAll();
-                p.samplingInfo_ = ps.getSamplingInfo();
-        }
 #ifdef USE_SLOTPLANNER
         TSTART();
         if (ps.slot().bnodes().size() > 0)
-                ps.fipr(ps.getFP());
+                ps.fip(co, so);
         TEND();
         jvo["ppse"] = ELAPSED;
         TPRINT("ParallelSlot");
 #endif
+        if (ps.slot().bnodes().size() > 0) {
+                ps.setAll();
+                p.samplingInfo_ = ps.getSamplingInfo();
+                p.useSamplingInfo_ = true;
+        }
         if (ps.cusp().size() > 0) {
-                p.goal(ps.cusp().front().front());
+                p.goal(ps.getMidd());
                 p.slot_cusp(ps.cusp().front()); // use first found solution
+                p.goals(ps.goals());
                 jvo["midd"][0] = p.goal()->x();
                 jvo["midd"][1] = p.goal()->y();
                 jvo["midd"][2] = p.goal()->h();
@@ -196,6 +213,15 @@ int main()
                 jvo["goal"][2] = p.goal()->h();
         }
         TSTART();
+        std::cerr << "Slot Info:" << std::endl;
+        if (ps.slotSide() == LEFT)
+                std::cerr << "- LEFT" << std::endl;
+        else
+                std::cerr << "- RIGHT" << std::endl;
+        if (ps.slotType() == PARALLEL)
+                std::cerr << "- PARALLEL" << std::endl;
+        else
+                std::cerr << "- PERPENDICULAR" << std::endl;
 #ifdef USE_LOADF
         std::vector<RRTNode *> steered;
         for (auto jn: jvi["traj"][0]) {
@@ -242,10 +268,7 @@ int main()
                 p.next();
                 p.tend();
                 if (p.opt_path()) {
-                        if (ps.cusp().size() > 0)
-                                p.tlog(p.findt(p.slot_cusp().back()));
-                        else
-                                p.tlog(p.findt());
+                        p.tlog(p.findt());
                 }
         }
 #elif defined USE_PTHREAD