*/
#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
#include <iostream>
#include <jsoncpp/json/json.h>
#include <pthread.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
-// choose
-//#define USE_INTERRUPT
-// or
-//#define USE_TMAX
-// or
-//#define USE_LOADF
-// or
-#define USE_PTHREAD
-
-#ifdef USE_INTERRUPT
- #define USE_GL
+#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
+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)
{
}
#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
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();
std::vector<CircleObstacle> co;
std::vector<SegmentObstacle> so;
for (auto o: jvi["obst"]) {
o["segment"][1][0].asFloat(),
o["segment"][1][1].asFloat(),
0)));
+ p.frame().add_bnode(so.back().init());
}
}
p.link_obstacles(&co, &so);
p.ocost(p.root());
p.ocost(p.goal());
+ ParallelSlot ps = ParallelSlot();
+ if (
+ jvi["slot"] != Json::nullValue &&
+ jvi["slot"]["polygon"] != Json::nullValue
+ ) {
+ for (auto xy: jvi["slot"]["polygon"]) {
+ ps.slot().add_bnode(new RRTNode(
+ xy[0].asFloat(),
+ xy[1].asFloat()
+ ));
+ }
+ }
+#ifdef USE_SLOTPLANNER
+ TSTART();
+ if (ps.slot().bnodes().size() > 0)
+ ps.fip(co, so);
+ TEND();
+ jvo["ppse"] = ELAPSED;
+ TPRINT("ParallelSlot");
+#endif
+ if (ps.slot().bnodes().size() > 0) {
+ ps.setAll();
+ p.samplingInfo_ = ps.getSamplingInfo();
+ p.useSamplingInfo_ = true;
+ }
+ if (ps.cusp().size() > 0) {
+ p.goal(ps.getMidd());
+ p.slot_cusp(ps.cusp().front()); // use first found solution
+ p.goals(ps.goals());
+ jvo["midd"][0] = p.goal()->x();
+ jvo["midd"][1] = p.goal()->y();
+ jvo["midd"][2] = p.goal()->h();
+ jvo["goal"][0] = p.slot_cusp().back()->x();
+ jvo["goal"][1] = p.slot_cusp().back()->y();
+ jvo["goal"][2] = p.slot_cusp().back()->h();
+ } else {
+ jvo["goal"][0] = p.goal()->x();
+ jvo["goal"][1] = p.goal()->y();
+ jvo["goal"][2] = p.goal()->h();
+ }
+ TSTART();
+ std::cerr << "Slot Info:" << std::endl;
+ if (ps.slotSide() == LEFT)
+ std::cerr << "- LEFT" << std::endl;
+ else
+ std::cerr << "- RIGHT" << std::endl;
+ if (ps.slotType() == PARALLEL)
+ std::cerr << "- PARALLEL" << std::endl;
+ else
+ std::cerr << "- PERPENDICULAR" << std::endl;
#ifdef USE_LOADF
std::vector<RRTNode *> steered;
for (auto jn: jvi["traj"][0]) {
while (!p.goal_found() && p.elapsed() < TMAX) {
p.next();
p.tend();
- if (p.opt_path())
+ if (p.opt_path()) {
p.tlog(p.findt());
+ }
}
#elif defined USE_PTHREAD
bool gf = false;
p.p_goal_.tstart();
pthread_create(&rt, NULL, &next_run, (void *) &ra);
pthread_create(>, NULL, &next_run, (void *) &ga);
- volatile int nofrn = 0;
- volatile int nofgn = 0;
- RRTNode *rn = nullptr;
- RRTNode *gn = nullptr;
- while (!gf && p.elapsed() < TMAX) {
+ 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
- nofrn = p.p_root_.nodes().size() - 1; // TODO workaround
- nofgn = p.p_goal_.nodes().size() - 1;
- for (int i = 0; i < nofrn; i++) {
- rn = p.p_root_.nodes()[i];
- if (rn->parent() == nullptr)
- continue;
- for (int j = 0; j < nofgn; j++) {
- gn = p.p_goal_.nodes()[j];
- if (gn->parent() == nullptr)
- continue;
- if (rn->ccost() + gn->ccost() < mc &&
- IS_NEAR(rn, gn)) {
- gf = true;
- ron = rn;
- gon = gn;
- mc = rn->ccost() + gn->ccost();
- }
+ 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.elapsed() >= TMAX)
+ goto escapeloop;
+ }}
// end of overlap trees
p.tend();
}
+escapeloop:
pthread_join(rt, NULL);
pthread_join(gt, NULL);
- p.goal_found(gf);
+ 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 = 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
+ TEND();
+ TPRINT("RRT");
+ jvo["rrte"] = ELAPSED;
#ifdef JSONLOGEDGES
p.logr(p.root());
#endif
// 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];
// 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