Add ce2 test

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 08fcb27803748e5f9c60449fe83e73d59c37ae6a..716ff873e3e3a3c02af4837536278d15cd52bb20 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,3 +39,4 @@ add_executable(test1 src/test1.cc)
 add_executable(test2 src/test2.cc)
 add_executable(test3 src/test3.cc)
 add_executable(test4 src/test4.cc)
+add_executable(test5 src/test5.cc)

diff --git a/README.md b/README.md
index bb6acb15283887c05212e1ebf2631f75a075bd9e..b788f5960f396e45fa13f86da175d7e572f47cae 100644 (file)
--- a/README.md
+++ b/README.md
@@ -60,6 +60,8 @@ The following are expected in the output along with *plan*:
 
 ## Test files
 There is a list of test files and what they include:
+- `test5.cc`: cute collision, Reeds and Shepp for build cost, Euclidean
+  distance for search cost.
 - `test4.cc`: cute collision, Reeds and Shepp for build cost, Matej's
   heuristics for search cost.
 - `test3.cc`: RRT\* with [cute c2][] library for collision detection.

diff --git a/src/test5.cc b/src/test5.cc
new file mode 100644 (file)
index 0000000..29d77f6
--- /dev/null
+++ b/src/test5.cc
@@ -0,0 +1,186 @@
+#include <chrono>
+#include <iostream>
+#include <jsoncpp/json/json.h>
+
+#include "rrtce.h"
+
+std::chrono::high_resolution_clock::time_point TSTART_;
+std::chrono::high_resolution_clock::time_point TEND_;
+inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
+inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
+inline double TDIFF()
+{
+        std::chrono::duration<double> DT_;
+        DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
+                TEND_ - TSTART_
+        );
+        return DT_.count();
+}
+inline void TPRINT(const char *what)
+{
+        std::cerr << what << ": " << TDIFF() << std::endl;
+}
+
+int main()
+{
+        Json::Value jvi; // JSON input
+        Json::Value jvo; // JSON output
+        std::cin >> jvi;
+
+        if (jvi["init"] == Json::nullValue) {
+                std::cerr << "I need `init` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
+        if (jvi["goal"] == Json::nullValue) {
+                std::cerr << "I need `goal` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
+        if (jvi["goals"] == Json::nullValue) {
+                std::cerr << "I need `goals` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
+        if (jvi["obst"] == Json::nullValue) {
+                std::cerr << "I need `obst` in JSON input scenario";
+                std::cerr << std::endl;
+                return 1;
+        }
+
+        RRTCE2 rrts;
+        rrts.nodes().front().x(jvi["init"][0].asDouble());
+        rrts.nodes().front().y(jvi["init"][1].asDouble());
+        rrts.nodes().front().h(jvi["init"][2].asDouble());
+        {
+                RRTNode tmp_node;
+                tmp_node.x(jvi["goal"][0].asDouble());
+                tmp_node.y(jvi["goal"][1].asDouble());
+                tmp_node.h(jvi["goal"][2].asDouble());
+                rrts.goals().push_back(tmp_node);
+                for (auto g: jvi["goals"]) {
+                        tmp_node.x(g[0].asDouble());
+                        tmp_node.y(g[1].asDouble());
+                        tmp_node.h(g[2].asDouble());
+                        rrts.goals().push_back(tmp_node);
+                }
+        }
+        {
+                Obstacle tmp_obstacle;
+                for (auto o: jvi["obst"]) {
+                        tmp_obstacle.poly().clear();
+                        for (auto c: o) {
+                                double tmp_x = c[0].asDouble();
+                                double tmp_y = c[1].asDouble();
+                                auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
+                                tmp_obstacle.poly().push_back(tmp_tuple);
+                        }
+                        rrts.obstacles().push_back(tmp_obstacle);
+                }
+        }
+
+        {
+                double edist_init_goal = sqrt(
+                        pow(
+                                rrts.nodes().front().x()
+                                - rrts.goals().front().x(),
+                                2
+                        )
+                        + pow(
+                                rrts.nodes().front().y()
+                                - rrts.goals().front().y(),
+                                2
+                        )
+                );
+                rrts.set_sample(
+                        rrts.nodes().front().x(), edist_init_goal,
+                        rrts.nodes().front().y(), edist_init_goal,
+                        0, 2 * M_PI
+                );
+        }
+
+        rrts.init();
+        TSTART();
+        while (rrts.next()) {}
+        TEND();
+
+        {
+                jvo["time"] = TDIFF();
+        }
+        {
+                jvo["iterations"] = rrts.icnt();
+        }
+        {
+                if (rrts.path().size() > 0) {
+                        jvo["cost"] = cc(*rrts.path().back());
+                } else {
+                        jvo["cost"] = -1;
+                }
+        }
+        {
+                jvo["init"][0] = rrts.nodes().front().x();
+                jvo["init"][1] = rrts.nodes().front().y();
+                jvo["init"][2] = rrts.nodes().front().h();
+        }
+        {
+                unsigned int ocnt = 0;
+                for (auto o: rrts.obstacles()) {
+                        unsigned int ccnt = 0;
+                        for (auto c: o.poly()) {
+                                jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
+                                jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
+                                ccnt++;
+                        }
+                        ocnt++;
+                }
+        }
+        //{
+        //        unsigned int ncnt = 0;
+        //        for (auto n: rrts.nodes()) {
+        //                jvo["nodes_x"][ncnt] = n.x();
+        //                jvo["nodes_y"][ncnt] = n.y();
+        //                //jvo["nodes_h"][ncnt] = n.h();
+        //                ncnt++;
+        //        }
+        //}
+        {
+                jvo["nodes"] = (unsigned int) rrts.nodes().size();
+        }
+        {
+                unsigned int gcnt = 0;
+                for (auto g: rrts.goals()) {
+                        jvo["goals"][gcnt][0] = g.x();
+                        jvo["goals"][gcnt][1] = g.y();
+                        jvo["goals"][gcnt][2] = g.h();
+                        gcnt++;
+                }
+        }
+        {
+                if (rrts.path().size() > 0) {
+                        jvo["goal"][0] = rrts.path().back()->x();
+                        jvo["goal"][1] = rrts.path().back()->y();
+                        jvo["goal"][2] = rrts.path().back()->h();
+                }
+                unsigned int cu = 0;
+                unsigned int co = 0;
+                unsigned int pcnt = 0;
+                for (auto n: rrts.path()) {
+                        jvo["path"][pcnt][0] = n->x();
+                        jvo["path"][pcnt][1] = n->y();
+                        jvo["path"][pcnt][2] = n->h();
+                        if (n->t(RRTNodeType::cusp))
+                                cu++;
+                        if (n->t(RRTNodeType::connected))
+                                co++;
+                        pcnt++;
+                }
+                jvo["cusps-in-path"] = cu;
+                jvo["connecteds-in-path"] = co;
+        }
+
+        std::cout << jvo << std::endl;
+        return 0;
+}