Add goal_found method for two RRTNodes

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

diff --git a/base/main.cc b/base/main.cc
index 1a866d184b2e1c41e74aabe6d00a25f2b799260a..8c544c198ec498930ace88e3fac16a0d7935e75f 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -16,6 +16,9 @@ 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>
@@ -24,35 +27,63 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "compile.h"
 #include "obstacle.h"
 #include "rrtplanner.h"
+#include "slotplanner.h"
+
+#if USE_GL > 0
 // OpenGL
 #include <GL/gl.h>
 #include <GL/glu.h>
 #include <SDL2/SDL.h>
+#endif
 
 // debug
 //#define JSONLOGEDGES
 //#define JSONLOGSAMPLES
 
-// choose
-//#define USE_INTERRUPT
-// or
-//#define USE_TMAX
-// or
-//#define USE_LOADF
-// or
-#define USE_PTHREAD
-
-#ifdef USE_INTERRUPT
-        #define USE_GL
+#if USE_GL > 0
+        #define USE_INTERRUPT
+#else
+        // choose
+        //#define USE_INTERRUPT
+        // or
+        #define USE_TMAX
+        // or
+        //#define USE_LOADF
+        // or
+        //#define USE_PTHREAD
 #endif
 
+std::chrono::high_resolution_clock::time_point TSTART_;
+std::chrono::high_resolution_clock::time_point TEND_;
+float TELAPSED = 0;
+float ELAPSED = 0;
+void TSTART() {TSTART_ = std::chrono::high_resolution_clock::now();}
+void TEND() {
+        std::chrono::duration<float> DT_;
+        TEND_ = std::chrono::high_resolution_clock::now();
+        DT_ = std::chrono::duration_cast<std::chrono::duration<float>>(
+                TEND_ - TSTART_
+        );
+        TELAPSED += DT_.count();
+        ELAPSED = DT_.count();
+}
+void TPRINT(const char *what) {
+        std::chrono::duration<float> DT_;
+        DT_ = std::chrono::duration_cast<std::chrono::duration<float>>(
+                TEND_ - TSTART_
+        );
+        std::cerr << what << ": " << DT_.count() << std::endl;
+}
+
 bool run_planner = true;
 
+#if USE_GL > 0
 SDL_Window* gw = NULL;
 SDL_GLContext gc;
 
 bool init();
 bool initGL();
+#endif
 
 void hint(int)
 {
@@ -80,9 +111,19 @@ 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()
 {
-#ifdef USE_GL
+        srand(static_cast<unsigned>(time(0)));
+#if USE_GL > 0
         init();
 #endif
 
@@ -102,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"]) {
@@ -128,6 +172,56 @@ int main()
         p.ocost(p.root());
         p.ocost(p.goal());
 
+        ParallelSlot ps = ParallelSlot();
+        if (
+                jvi["slot"] != Json::nullValue &&
+                jvi["slot"]["polygon"] != Json::nullValue
+        ) {
+                for (auto xy: jvi["slot"]["polygon"]) {
+                        ps.slot().add_bnode(new RRTNode(
+                                xy[0].asFloat(),
+                                xy[1].asFloat()
+                        ));
+                }
+        }
+#ifdef USE_SLOTPLANNER
+        TSTART();
+        if (ps.slot().bnodes().size() > 0)
+                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.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();
+                jvo["goal"][0] = p.slot_cusp().back()->x();
+                jvo["goal"][1] = p.slot_cusp().back()->y();
+                jvo["goal"][2] = p.slot_cusp().back()->h();
+        } else {
+                jvo["goal"][0] = p.goal()->x();
+                jvo["goal"][1] = p.goal()->y();
+                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]) {
@@ -173,8 +267,9 @@ int main()
         while (!p.goal_found() && p.elapsed() < TMAX) {
                 p.next();
                 p.tend();
-                if (p.opt_path())
+                if (p.opt_path()) {
                         p.tlog(p.findt());
+                }
         }
 #elif defined USE_PTHREAD
         bool gf = false;
@@ -278,11 +373,15 @@ escapeloop:
         if (p.opt_path())
                 p.tlog(p.findt());
 #endif
+        TEND();
+        TPRINT("RRT");
+        jvo["rrte"] = ELAPSED;
 #ifdef JSONLOGEDGES
         p.logr(p.root());
 #endif
 
         // statistics to error output
+        std::cerr << "TELAPSED is " << TELAPSED << std::endl;
         std::cerr << "Elapsed is " << p.elapsed() << std::endl;
         std::cerr << "Goal found is " << p.goal_found() << std::endl;
         std::cerr << "#nodes is " << p.nodes().size() << std::endl;
@@ -303,7 +402,7 @@ escapeloop:
                 std::cerr << "- " << edges.size() << std::endl;
 
         // JSON output
-        jvo["elap"] = p.elapsed();
+        jvo["elap"] = TELAPSED;
 #ifdef USE_PTHREAD
         jvo["nodo"][0] = nodo;
 #endif
@@ -361,7 +460,7 @@ escapeloop:
         // print output
         std::cout << jvo << std::endl;
 
-#ifdef USE_GL
+#if USE_GL > 0
         SDL_DestroyWindow(gw);
         SDL_Quit();
 #endif
@@ -374,6 +473,7 @@ escapeloop:
         return 0;
 }
 
+#if USE_GL > 0
 bool init()
 {
         if (SDL_Init(SDL_INIT_VIDEO) < 0) {
@@ -449,3 +549,4 @@ bool initGL()
         }
         return true;
 }
+#endif