3 #include <jsoncpp/json/json.h>
9 std::chrono::high_resolution_clock::time_point TSTART_;
10 std::chrono::high_resolution_clock::time_point TEND_;
11 inline void TSTART() { TSTART_ = std::chrono::high_resolution_clock::now(); }
12 inline void TEND() { TEND_ = std::chrono::high_resolution_clock::now(); }
15 std::chrono::duration<double> DT_;
16 DT_ = std::chrono::duration_cast<std::chrono::duration<double>>(
21 inline void TPRINT(const char *what)
23 std::cerr << what << ": " << TDIFF() << std::endl;
26 RRTNode init_node(RRTNode *oi, double *oic)
30 r.sp(oi->sp() * TDIFF());
33 *oic = sqrt(pow(r.x() - oi->x(), 2) + pow(r.y() - oi->y(), 2));
36 std::abs(r.h() - oi->h())
37 , 2 * M_PI - std::abs(r.h() - oi->h())
49 #define EPP RRTCE7 // EPP stands for entry point planner.
54 EppStat epp_stat = FINISHED;
55 std::vector<std::thread> epp_threads;
58 , std::vector<RRTNode> *fp
70 if (epp.path().size() > 0) {
71 *fc = cc(*epp.path().back()) + *oic;
73 for (auto n: epp.path()) {
74 fp->push_back(RRTNode());
83 class EppFinished : public BrainTree::Node {
85 Status update() override
87 if (epp_stat == FINISHED)
88 return Status::Success;
89 return Status::Failure;
92 class EppGF : public BrainTree::Node {
94 std::vector<RRTNode> *fp;
97 std::vector<RRTNode> *feasible_path
102 Status update() override
105 return Status::Failure;
108 unsigned int pcnt = 0;
110 if (n.t(RRTNodeType::cusp))
112 if (n.t(RRTNodeType::connected))
116 if (cu > 5) // max cusps in feasible path
117 return Status::Failure;
118 return Status::Success;
121 class EppRestart : public BrainTree::Node {
124 std::vector<RRTNode> *fp;
131 , std::vector<RRTNode> *feasible_path
132 , double *feasible_path_cost
134 , double *orig_init_cost
138 , fpc(feasible_path_cost)
140 , oic(orig_init_cost)
143 Status update() override
145 RRTNode in = init_node(oi, oic);
146 jvi["init"][0] = in.x();
147 jvi["init"][1] = in.y();
148 jvi["init"][2] = in.h();
149 epp_threads.push_back(std::thread(
156 return Status::Success;
159 class ShouldRunBT : public BrainTree::Node {
161 Status update() override
164 return Status::Success;
165 return Status::Failure;
168 class UpdateFP : public BrainTree::Node {
170 std::vector<RRTNode> *fi;
172 std::vector<RRTNode> *fe;
176 std::vector<RRTNode> *final_path
177 , double *final_path_cost
178 , std::vector<RRTNode> *feasible_path
179 , double *feasible_path_cost
182 , fic(final_path_cost)
184 , fec(feasible_path_cost)
187 Status update() override
193 fi->push_back(RRTNode());
198 return Status::Success;
200 return Status::Failure;
206 Json::Value jvi; // JSON input
209 std::vector<RRTNode> final_path;
210 double final_path_cost = 9999;
211 std::vector<RRTNode> feasible_path;
212 double feasible_path_cost = 9999;
213 double orig_init_cost = 0;
215 orig_init.x(jvi["init"][0].asDouble());
216 orig_init.y(jvi["init"][1].asDouble());
217 orig_init.h(jvi["init"][2].asDouble());
219 //orig_init.st(M_PI / 32); // only for sc2_4
221 struct timespec sleeping_time;
222 sleeping_time.tv_sec = 0;
223 sleeping_time.tv_nsec = 50 * 1000000;
225 auto bt = BrainTree::Builder()
226 .composite<BrainTree::Sequence>()
228 .composite<BrainTree::Selector>()
229 .composite<BrainTree::Sequence>()
238 , &feasible_path_cost
243 , &feasible_path_cost
248 .composite<BrainTree::Sequence>()
253 , &feasible_path_cost
258 .decorator<BrainTree::Succeeder>()
266 while (bt->update() != BrainTree::Node::Status::Failure) {
267 nanosleep(&sleeping_time, (struct timespec *) NULL);
269 for (auto &t: epp_threads)
272 Json::Value jvo = jvi; // JSON output
274 jvo["time"] = TDIFF();
275 jvo["cost"] = final_path_cost;
276 jvo["path"][0][0] = orig_init.x();
277 jvo["path"][0][1] = orig_init.y();
278 jvo["path"][0][2] = orig_init.h();
281 unsigned int pcnt = 1;
282 for (auto n: final_path) {
283 jvo["path"][pcnt][0] = n.x();
284 jvo["path"][pcnt][1] = n.y();
285 jvo["path"][pcnt][2] = n.h();
286 if (n.t(RRTNodeType::cusp))
288 if (n.t(RRTNodeType::connected))
292 jvo["cusps-in-path"] = cu;
293 jvo["connecteds-in-path"] = co;
295 std::cout << jvo << std::endl;