From edd71da8759032fcd1c72d2bb5fd273e481f0677 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 20 May 2019 13:32:27 +0200
Subject: [PATCH] Add random init position

---
 base/main.cc | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

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"]) {
-- 
2.39.2