7 std::chrono::high_resolution_clock::time_point TSTART_;
8 std::chrono::high_resolution_clock::time_point TEND_;
9 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
10 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
13 std::chrono::duration<double> DT_;
14 DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
19 inline void TPRINT(const char *what)
21 std::cerr << what << ": " << TDIFF() << std::endl;
26 Json::Value jvi; // JSON input
27 Json::Value jvo; // JSON output
30 if (jvi["init"] == Json::nullValue) {
31 std::cerr << "I need `init` in JSON input scenario";
32 std::cerr << std::endl;
36 if (jvi["goal"] == Json::nullValue) {
37 std::cerr << "I need `goal` in JSON input scenario";
38 std::cerr << std::endl;
42 if (jvi["goals"] == Json::nullValue) {
43 std::cerr << "I need `goals` in JSON input scenario";
44 std::cerr << std::endl;
48 if (jvi["obst"] == Json::nullValue) {
49 std::cerr << "I need `obst` in JSON input scenario";
50 std::cerr << std::endl;
55 rrts.nodes().front().x(jvi["init"][0].asDouble());
56 rrts.nodes().front().y(jvi["init"][1].asDouble());
57 rrts.nodes().front().h(jvi["init"][2].asDouble());
60 tmp_node.x(jvi["goal"][0].asDouble());
61 tmp_node.y(jvi["goal"][1].asDouble());
62 tmp_node.h(jvi["goal"][2].asDouble());
63 rrts.goals().push_back(tmp_node);
64 for (auto g: jvi["goals"]) {
65 tmp_node.x(g[0].asDouble());
66 tmp_node.y(g[1].asDouble());
67 tmp_node.h(g[2].asDouble());
68 rrts.goals().push_back(tmp_node);
72 Obstacle tmp_obstacle;
73 for (auto o: jvi["obst"]) {
74 tmp_obstacle.poly().clear();
76 double tmp_x = c[0].asDouble();
77 double tmp_y = c[1].asDouble();
78 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
79 tmp_obstacle.poly().push_back(tmp_tuple);
81 rrts.obstacles().push_back(tmp_obstacle);
86 double edist_init_goal = sqrt(
88 rrts.nodes().front().x()
89 - rrts.goals().front().x(),
93 rrts.nodes().front().y()
94 - rrts.goals().front().y(),
99 rrts.nodes().front().x(), edist_init_goal,
100 rrts.nodes().front().y(), edist_init_goal,
107 while (rrts.next()) {}
111 jvo["time"] = TDIFF();
114 jvo["iterations"] = rrts.icnt();
117 if (rrts.path().size() > 0) {
118 jvo["cost"] = cc(*rrts.path().back());
124 jvo["init"][0] = rrts.nodes().front().x();
125 jvo["init"][1] = rrts.nodes().front().y();
126 jvo["init"][2] = rrts.nodes().front().h();
129 unsigned int ocnt = 0;
130 for (auto o: rrts.obstacles()) {
131 unsigned int ccnt = 0;
132 for (auto c: o.poly()) {
133 jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
134 jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
141 // unsigned int ncnt = 0;
142 // for (auto n: rrts.nodes()) {
143 // jvo["nodes_x"][ncnt] = n.x();
144 // jvo["nodes_y"][ncnt] = n.y();
145 // //jvo["nodes_h"][ncnt] = n.h();
150 jvo["nodes"] = (unsigned int) rrts.nodes().size();
153 unsigned int gcnt = 0;
154 for (auto g: rrts.goals()) {
155 jvo["goals"][gcnt][0] = g.x();
156 jvo["goals"][gcnt][1] = g.y();
157 jvo["goals"][gcnt][2] = g.h();
162 if (rrts.path().size() > 0) {
163 jvo["goal"][0] = rrts.path().back()->x();
164 jvo["goal"][1] = rrts.path().back()->y();
165 jvo["goal"][2] = rrts.path().back()->h();
169 unsigned int pcnt = 0;
170 for (auto n: rrts.path()) {
171 jvo["path"][pcnt][0] = n->x();
172 jvo["path"][pcnt][1] = n->y();
173 jvo["path"][pcnt][2] = n->h();
174 if (n->t(RRTNodeType::cusp))
176 if (n->t(RRTNodeType::connected))
180 jvo["cusps-in-path"] = cu;
181 jvo["connecteds-in-path"] = co;
184 std::cout << jvo << std::endl;