*/
#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
#include <iostream>
#include <jsoncpp/json/json.h>
+#include <pthread.h>
#include <signal.h>
+#include <unistd.h>
#include "compile.h"
#include "obstacle.h"
#include "rrtplanner.h"
+#include "slotplanner.h"
+
+#if USE_GL > 0
// OpenGL
#include <GL/gl.h>
#include <GL/glu.h>
#include <SDL2/SDL.h>
+#endif
+
+// debug
+//#define JSONLOGEDGES
+//#define JSONLOGSAMPLES
+
+#if USE_GL > 0
+ #define USE_INTERRUPT
+#else
+ // choose
+ //#define USE_INTERRUPT
+ // or
+ #define USE_TMAX
+ // or
+ //#define USE_LOADF
+ // or
+ //#define USE_PTHREAD
+#endif
-#define USE_GL
-#define USE_INTERRUPT
-#define USE_TMAX
-#define USE_TMAX_1ST_CUSPS
+// OpenGL window size
+#define SCREEN_WIDTH 1000
+#define SCREEN_HEIGHT 1000
+
+std::chrono::high_resolution_clock::time_point TSTART_;
+std::chrono::high_resolution_clock::time_point TEND_;
+float TELAPSED = 0;
+float ELAPSED = 0;
+void TSTART() {TSTART_ = std::chrono::high_resolution_clock::now();}
+void TEND() {
+ std::chrono::duration<float> DT_;
+ TEND_ = std::chrono::high_resolution_clock::now();
+ DT_ = std::chrono::duration_cast<std::chrono::duration<float>>(
+ TEND_ - TSTART_
+ );
+ TELAPSED += DT_.count();
+ ELAPSED = DT_.count();
+}
+void TPRINT(const char *what) {
+ std::chrono::duration<float> DT_;
+ DT_ = std::chrono::duration_cast<std::chrono::duration<float>>(
+ TEND_ - TSTART_
+ );
+ std::cerr << what << ": " << DT_.count() << std::endl;
+}
bool run_planner = true;
+#if USE_GL > 0
SDL_Window* gw = NULL;
SDL_GLContext gc;
bool init();
bool initGL();
+#endif
void hint(int)
{
run_planner = false;
}
+#ifdef USE_PTHREAD
+struct next_arg {
+ bool *gf;
+ T2 *p;
+};
+
+void *next_run(void *arg)
+{
+ struct next_arg *na = (struct next_arg *) arg;
+ T2 *lp = (T2 *) na->p;
+ bool *gf = na->gf;
+ while (!*gf && lp->elapsed() < TMAX) {
+ if (lp->next())
+ *gf = true;
+ lp->tend();
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+#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()
{
-#ifdef USE_GL
+ srand(static_cast<unsigned>(time(0)));
+#if USE_GL > 0
init();
#endif
std::cin >> jvi;
std::string encoding = jvi.get("encoding", "UTF-8" ).asString();
+ 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;
+ }
+
PLANNER p(
- new RRTNode(
- jvi["init"][0].asFloat(),
- jvi["init"][1].asFloat(),
- jvi["init"][2].asFloat()),
- new RRTNode(
- jvi["goal"][0].asFloat(),
- jvi["goal"][1].asFloat(),
- jvi["goal"][2].asFloat()));
+ new RRTNode(
+ jvi["init"][0].asFloat(),
+ jvi["init"][1].asFloat(),
+ jvi["init"][2].asFloat()
+ ),
+ new RRTNode(
+ 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();
+ jvo["goal"][0] = jvi["goal"][0].asFloat();
+ jvo["goal"][1] = jvi["goal"][1].asFloat();
+ jvo["goal"][2] = jvi["goal"][2].asFloat();
+
+ if (jvi["goals"] != Json::nullValue) {
+ for (auto g: jvi["goals"]) {
+ p.goals().push_back(new RRTNode(
+ g[0].asFloat(),
+ g[1].asFloat(),
+ g[2].asFloat()
+ ));
+ }
+ }
+
std::vector<CircleObstacle> co;
std::vector<SegmentObstacle> so;
+ p.HMIN = p.root()->x();
+ p.HMAX = p.root()->x();
+ p.VMIN = p.root()->y();
+ p.VMAX = p.root()->y();
+ j = 0;
for (auto o: jvi["obst"]) {
- if (o["circle"] != Json::nullValue) {
- co.push_back(CircleObstacle(
- o["circle"][0].asFloat(),
- o["circle"][1].asFloat(),
- o["circle"][2].asFloat()));
- }
- if (o["segment"] != Json::nullValue) {
+ float tmpx;
+ float tmpy;
+ float tmpr;
+ float tmps;
+ for (i = 0; i < o.size() - 1; i++) {
+ tmpx = o[i][0].asFloat();
+ tmpy = o[i][1].asFloat();
+ tmpr = o[i + 1][0].asFloat();
+ tmps = o[i + 1][1].asFloat();
so.push_back(SegmentObstacle(
- new RRTNode(
- o["segment"][0][0].asFloat(),
- o["segment"][0][1].asFloat(),
- 0),
- new RRTNode(
- o["segment"][1][0].asFloat(),
- o["segment"][1][1].asFloat(),
- 0)));
+ new RRTNode(tmpx, tmpy, 0),
+ new RRTNode(tmpr, tmps, 0)
+ ));
+ p.frame().add_bnode(so.back().init());
+ if (tmpx < p.HMIN) p.HMIN = tmpx;
+ if (tmpx > p.HMAX) p.HMAX = tmpx;
+ if (tmpy < p.VMIN) p.VMIN = tmpy;
+ if (tmpy > p.VMAX) p.VMAX = tmpy;
+ if (tmpr < p.HMIN) p.HMIN = tmpr;
+ if (tmpr > p.HMAX) p.HMAX = tmpr;
+ if (tmps < p.VMIN) p.VMIN = tmps;
+ if (tmps > p.VMAX) p.VMAX = tmps;
+
+ // output
+ jvo["obst"][j][i][0] = tmpx;
+ jvo["obst"][j][i][1] = tmpy;
}
+ jvo["obst"][j][i][0] = tmpr;
+ jvo["obst"][j][i][1] = tmps;
+ j++;
}
+ p.defaultSamplingInfo();
p.link_obstacles(&co, &so);
p.ocost(p.root());
p.ocost(p.goal());
- RRTNode *nn;
- std::vector<RRTNode *> tr;
-
-#ifdef DBG_LOADF
+#ifdef USE_LOADF
std::vector<RRTNode *> steered;
for (auto jn: jvi["traj"][0]) {
steered.push_back(new RRTNode(
std::reverse(steered.begin(), steered.end());
RRTNode *pn = p.root();
for (auto n: steered) {
+ if (IS_NEAR(pn, n))
+ continue;
pn->add_child(n, p.cost(pn, n));
pn = n;
+ p.glplot();
}
pn->add_child(p.goal(), p.cost(pn, p.goal()));
+ p.goal_found(true);
p.tlog(p.findt());
- if (p.opt_path())
+ if (p.opt_path()) {
p.tlog(p.findt());
-#else
-#ifdef USE_INTERRUPT
+ p.glplot();
+ }
+ p.glplot();
+ sleep(2);
+#elif defined USE_INTERRUPT
signal(SIGINT, hint);
signal(SIGTERM, hint);
p.tstart();
p.tend();
if (p.opt_path())
p.tlog(p.findt());
-#ifdef USE_GL
p.glplot();
-#endif
}
-#if JSONLOGEDGES > 0
- p.logr(p.root());
-#endif
-#elif defined USE_TMAX // USE TMAX
+#elif defined USE_TMAX
p.tstart();
p.tend();
while (!p.goal_found() && p.elapsed() < TMAX) {
p.next();
p.tend();
- if (p.opt_path())
+ if (p.opt_path()) {
p.tlog(p.findt());
+ }
}
-#if JSONLOGEDGES > 0
- p.logr(p.root());
-#endif
-#else // NOT USE_INTERRUPT
- i = 0;
- while (!p.goal_found() && i < 20) {
- p.tstart();
- p.tend();
- while (p.elapsed() < TMAX) {
- p.next();
+#elif defined USE_PTHREAD
+ bool gf = false;
+ RRTNode *ron = nullptr;
+ RRTNode *gon = nullptr;
+ float mc = 9999;
+ pthread_t rt; // root thread
+ pthread_t gt; // goal thread
+ pthread_t ct; // connect thread
+
+ struct next_arg ra;
+ ra.gf = &gf;
+ ra.p = &p.p_root_;
+
+ struct next_arg ga;
+ ga.gf = &gf;
+ ga.p = &p.p_goal_;
+
+ p.tstart();
+ p.p_root_.tstart();
+ p.p_goal_.tstart();
+ pthread_create(&rt, NULL, &next_run, (void *) &ra);
+ pthread_create(>, NULL, &next_run, (void *) &ga);
+ int tol = 0;
+ int ndl = 0;
+ bool ndone = true;
+ while (!gf && p.elapsed() < TMAX &&
+ p.p_root_.nodes().size() < NOFNODES &&
+ p.p_goal_.nodes().size() < NOFNODES) {
+ // overlap trees
+ ndone = true;
+ for (int i = 0; i < IXSIZE; i++) {
+ for (int j = 0; j < IYSIZE; j++) {
+ if (p.p_root_.ixy_[i][j].changed() &&
+ p.p_goal_.ixy_[i][j].changed()) {
+ndone = false;
+for (auto rn: p.p_root_.ixy_[i][j].nodes()) {
+for (auto gn: p.p_goal_.ixy_[i][j].nodes()) {
+ if (rn->ccost() + gn->ccost() < mc &&
+ IS_NEAR(rn, gn)) {
+ gf = true;
+ p.goal_found(true);
+ ron = rn;
+ gon = gn;
+ mc = rn->ccost() + gn->ccost();
+ }
+}}
+ }
+ tol++;
+ if (ndone)
+ ndl++;
p.tend();
- }
- if (!p.goal_found()) {
-#if NNVERSION > 1
- nn = p.nn(p.iy_, p.goal(), p.cost);
-#else
- nn = p.nn(p.nodes(), p.goal(), p.cost);
+ if (p.elapsed() >= TMAX)
+ goto escapeloop;
+ }}
+ // end of overlap trees
+ p.tend();
+ }
+escapeloop:
+ pthread_join(rt, NULL);
+ pthread_join(gt, NULL);
+ float nodo = ((float) ndl / (float) tol);
+ std::cerr << "nothing done is " << 100.0 * nodo;
+ std::cerr << "%" << std::endl;
+ //std::cerr << "rgf is " << p.p_root_.goal_found() << std::endl;
+ //std::cerr << "ggf is " << p.p_goal_.goal_found() << std::endl;
+ //std::cerr << "cgf is " << p.goal_found() << std::endl;
+ if (p.p_root_.goal_found() && p.p_root_.goal()->ccost() < mc) {
+ ron = p.p_root_.goal()->parent();
+ gon = p.p_root_.goal();
+ mc = p.p_root_.goal()->ccost();
+ }
+ if (p.p_goal_.goal_found() && p.p_goal_.goal()->ccost() < mc) {
+ ron = p.p_goal_.goal();
+ gon = p.p_goal_.goal()->parent();
+ mc = p.p_goal_.goal()->ccost();
+ }
+ p.root()->remove_parent(); // needed if p.p_goal_.goal_found()
+ p.root()->ccost(0);
+ p.goal()->children().clear();
+ // connect trees
+ if (gf) {
+ while (gon != p.goal()) {
+ p.p_root_.nodes().push_back(new RRTNode(
+ gon->x(),
+ gon->y(),
+ gon->h()));
+ ron->add_child(
+ p.p_root_.nodes().back(),
+ p.p_root_.cost(
+ ron,
+ p.p_root_.nodes().back()));
+ ron = p.p_root_.nodes().back();
+ gon = gon->parent();
+ }
+ ron->add_child(p.goal(), p.p_root_.cost(ron, p.goal()));
+ }
+ // end of connect trees
+ if (gf)
+ p.tlog(p.findt());
+ if (p.opt_path())
+ p.tlog(p.findt());
#endif
- tr = p.findt(nn);
- } else {
- nn = p.goal();
- tr = p.findt();
- }
- p.tlog(tr);
-#if JSONLOGEDGES > 0
- p.logr(p.root());
+ TEND();
+ TPRINT("RRT");
+ jvo["rrte"] = ELAPSED;
+#ifdef JSONLOGEDGES
+ p.logr(p.root());
#endif
- if (p.opt_path())
- p.tlog(p.findt(nn));
- if (!p.goal_found()) {
- nn = p.tlog().back().back();
- for (j = p.tlog().back().size() - 1; j > 0; j--) {
- float s1 = p.tlog().back()[j]->s();
- float s2 = p.tlog().back()[j - 1]->s();
- if (sgn(s1) != sgn(s2)) {
- nn = p.tlog().back()[j - 1];
- break;
- }
- }
- p.tlog(p.findt(nn));
- p.rebase(nn);
- }
- i++;
- }
- std::cerr << "#iterations is " << i << std::endl;
-#endif // USE_INTERRUPT
-#endif // DBG_LOADF
+ // statistics to error output
+ std::cerr << "TELAPSED is " << TELAPSED << std::endl;
std::cerr << "Elapsed is " << p.elapsed() << std::endl;
std::cerr << "Goal found is " << p.goal_found() << std::endl;
std::cerr << "#nodes is " << p.nodes().size() << std::endl;
std::cerr << "- " << edges.size() << std::endl;
// JSON output
- jvo["elap"] = p.elapsed();
+ jvo["elap"] = TELAPSED;
+#ifdef USE_PTHREAD
+ jvo["nodo"][0] = nodo;
+#endif
// log cost
for (j = 0; j < p.clog().size(); j++)
jvo["cost"][j] = p.clog()[j];
jvo["traj"][j][i][4] = n->s();
i++;
}
+ if (j == p.tlog().size() - 1) {
+ i = 0;
+ for (auto n: traj) {
+ jvo["path"][i][0] = n->x();
+ jvo["path"][i][1] = n->y();
+ jvo["path"][i][2] = n->h();
+ i++;
+ }
+ }
j++;
}
-#if JSONLOGEDGES > 0
- // log edges
+#ifdef JSONLOGEDGES
i = 0;
j = 0;
for (auto edges: p.rlog()) {
i++;
}
#endif
-#if JSONLOGSAMPLES > 0
- // log samples
+#ifdef JSONLOGSAMPLES
i = 0;
j = 0;
for (auto s: p.samples()) {
// print output
std::cout << jvo << std::endl;
-#ifdef USE_GL
+#if USE_GL > 0
SDL_DestroyWindow(gw);
SDL_Quit();
#endif
return 0;
}
+#if USE_GL > 0
bool init()
{
if (SDL_Init(SDL_INIT_VIDEO) < 0) {
}
return true;
}
+#endif
projects / hubacji1 / iamcar.git / blobdiff