Move screen size

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

diff --git a/base/main.cc b/base/main.cc
index 538140efa6cff2becc63b4bbafdb95dbd1dc265f..035bc77fdfad71af969b7e37dbeb448eb5caaab2 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -53,6 +53,10 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
         //#define USE_PTHREAD
 #endif
 
+// OpenGL window size
+#define SCREEN_WIDTH 1000
+#define SCREEN_HEIGHT 1000
+
 std::chrono::high_resolution_clock::time_point TSTART_;
 std::chrono::high_resolution_clock::time_point TEND_;
 float TELAPSED = 0;
@@ -183,6 +187,8 @@ int main()
                                 xy[1].asFloat()
                         ));
                 }
+                for (auto e: ps.slot().frame())
+                        so.push_back(SegmentObstacle(e->init(), e->goal()));
         }
 #ifdef USE_SLOTPLANNER
         TSTART();
@@ -194,12 +200,12 @@ int main()
 #endif
         if (ps.slot().bnodes().size() > 0) {
                 ps.setAll();
-                p.samplingInfo_ = ps.getSamplingInfo();
-                p.useSamplingInfo_ = true;
+                //p.setSamplingInfo(ps.getSamplingInfo());
         }
         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();
@@ -267,12 +273,11 @@ 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());
                 }
         }
+        if (p.goal_found() && ps.slotType() == PARALLEL)
+                p.tlog(p.findt(p.slot_cusp().back()));
 #elif defined USE_PTHREAD
         bool gf = false;
         RRTNode *ron = nullptr;