Add random init position

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