3 #include <jsoncpp/json/json.h>
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
29 if (jvi["init"] == Json::nullValue) {
30 std::cerr << "I need `init` in JSON input scenario";
31 std::cerr << std::endl;
34 if (jvi["goal"] == Json::nullValue) {
35 std::cerr << "I need `goal` in JSON input scenario";
36 std::cerr << std::endl;
39 if (jvi["goals"] == Json::nullValue) {
40 std::cerr << "I need `goals` in JSON input scenario";
41 std::cerr << std::endl;
44 if (jvi["obst"] == Json::nullValue) {
45 std::cerr << "I need `obst` in JSON input scenario";
46 std::cerr << std::endl;
50 std::vector<RRTNode> final_path;
51 double final_path_cost = 9999;
52 unsigned int final_path_steps = 0;
53 double cost_from_orig_init = 0;
54 unsigned int steps_from_orig_init = 0;
56 init_node.x(jvi["init"][0].asDouble());
57 init_node.y(jvi["init"][1].asDouble());
58 init_node.h(jvi["init"][2].asDouble());
60 init_node.st(M_PI / 32); // only for sc2_4
61 RRTNode last_node(init_node);
62 RRTNode orig_init(init_node);
64 steps_from_orig_init++;
66 double angl_diff = std::abs(last_node.h() - init_node.h());
68 cost_from_orig_init += sqrt(
69 pow(last_node.x() - init_node.x(), 2)
70 + pow(last_node.y() - init_node.y(), 2)
73 cost_from_orig_init += angl_diff * init_node.mtr();
77 rrts.nodes().front().x(init_node.x());
78 rrts.nodes().front().y(init_node.y());
79 rrts.nodes().front().h(init_node.h());
82 tmp_node.x(jvi["goal"][0].asDouble());
83 tmp_node.y(jvi["goal"][1].asDouble());
84 tmp_node.h(jvi["goal"][2].asDouble());
85 rrts.goals().push_back(tmp_node);
86 for (auto g: jvi["goals"]) {
87 tmp_node.x(g[0].asDouble());
88 tmp_node.y(g[1].asDouble());
89 tmp_node.h(g[2].asDouble());
90 rrts.goals().push_back(tmp_node);
94 Obstacle tmp_obstacle;
95 for (auto o: jvi["obst"]) {
96 tmp_obstacle.poly().clear();
98 double tmp_x = c[0].asDouble();
99 double tmp_y = c[1].asDouble();
100 auto tmp_tuple = std::make_tuple(tmp_x, tmp_y);
101 tmp_obstacle.poly().push_back(tmp_tuple);
103 rrts.obstacles().push_back(tmp_obstacle);
107 double edist_init_goal = sqrt(
109 rrts.nodes().front().x()
110 - rrts.goals().front().x(),
114 rrts.nodes().front().y()
115 - rrts.goals().front().y(),
120 rrts.nodes().front().x(), edist_init_goal,
121 rrts.nodes().front().y(), edist_init_goal,
127 while (rrts.next()) {}
129 if (rrts.path().size() > 0) {
131 cc(*rrts.path().back())
132 + cost_from_orig_init
136 cc(*rrts.path().back())
137 + cost_from_orig_init
139 final_path_steps = steps_from_orig_init;
141 for (auto n: rrts.path()) {
142 final_path.push_back(RRTNode());
143 final_path.back().x(n->x());
144 final_path.back().y(n->y());
145 final_path.back().h(n->h());
149 std::cerr << "finished 0, cost is " << final_path_cost << std::endl;
151 jvo[rcnt++] = rrts.json();
152 for (int i = 1; i < 5; i++) {
153 last_node = RRTNode(init_node);
155 steps_from_orig_init++;
157 double angl_diff = std::abs(last_node.h() - init_node.h());
158 if (angl_diff < 0.001)
159 cost_from_orig_init += sqrt(
160 pow(last_node.x() - init_node.x(), 2)
161 + pow(last_node.y() - init_node.y(), 2)
164 cost_from_orig_init += angl_diff * init_node.mtr();
167 rrts2.goals() = rrts.goals();
168 rrts2.obstacles() = rrts.obstacles();
169 rrts2.nodes().front().x(init_node.x());
170 rrts2.nodes().front().y(init_node.y());
171 rrts2.nodes().front().h(init_node.h());
173 double edist_init_goal = sqrt(
175 rrts2.nodes().front().x()
176 - rrts2.goals().front().x(),
180 rrts2.nodes().front().y()
181 - rrts2.goals().front().y(),
186 rrts2.nodes().front().x(), edist_init_goal,
187 rrts2.nodes().front().y(), edist_init_goal,
192 while (rrts2.next()) {}
194 if (rrts2.path().size() > 0) {
196 cc(*rrts2.path().back())
197 + cost_from_orig_init
201 cc(*rrts2.path().back())
202 + cost_from_orig_init
204 final_path_steps = steps_from_orig_init;
206 for (auto n: rrts2.path()) {
207 final_path.push_back(RRTNode());
208 final_path.back().x(n->x());
209 final_path.back().y(n->y());
210 final_path.back().h(n->h());
214 std::cerr << "finished " << i << ", cost is " << final_path_cost;
215 std::cerr << std::endl;
216 jvo[rcnt++] = rrts2.json();
219 jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"];
220 jvo[rcnt]["cost"] = final_path_cost;
221 unsigned int ncnt = 0;
222 for (unsigned int i = 0; i < final_path_steps; i++) {
223 jvo[rcnt]["path"][ncnt][0] = orig_init.x();
224 jvo[rcnt]["path"][ncnt][1] = orig_init.y();
225 jvo[rcnt]["path"][ncnt][2] = orig_init.h();
229 for (auto n: final_path) {
230 jvo[rcnt]["path"][ncnt][0] = n.x();
231 jvo[rcnt]["path"][ncnt][1] = n.y();
232 jvo[rcnt]["path"][ncnt][2] = n.h();
236 std::cout << jvo << std::endl;