From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 20 May 2019 11:32:27 +0000 (+0200)
Subject: Add random init position
X-Git-Tag: v0.6.0~2^2~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/edd71da8759032fcd1c72d2bb5fd273e481f0677

Add random init position
---

diff --git a/base/main.cc b/base/main.cc
index c36997a..c15794b 100644
--- 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"]) {