#include <algorithm>
#include <chrono>
+#include <cmath>
+#include <cstdlib>
#include <iostream>
#include <jsoncpp/json/json.h>
#include <pthread.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
-
-// enable
-//#define USE_SLOTPLANNER
-
-// enable
-//#define USE_SLOTPLANNER
-
-#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
+// 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;
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
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();
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();
for (auto o: jvi["obst"]) {
+ float tmpx;
+ float tmpy;
+ float tmpr;
+ float tmps;
if (o["circle"] != Json::nullValue) {
- co.push_back(CircleObstacle(
- o["circle"][0].asFloat(),
- o["circle"][1].asFloat(),
- o["circle"][2].asFloat()));
+ tmpx = o["circle"][0].asFloat();
+ tmpy = o["circle"][1].asFloat();
+ tmpr = o["circle"][2].asFloat();
+ co.push_back(CircleObstacle(tmpx, tmpy, tmpr));
+ 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 (o["segment"] != Json::nullValue) {
+ tmpx = o["segment"][0][0].asFloat();
+ tmpy = o["segment"][0][1].asFloat();
+ tmpr = o["segment"][1][0].asFloat();
+ tmps = o["segment"][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;
}
}
+ p.defaultSamplingInfo();
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()
+ ));
+ }
+ for (auto e: ps.slot().frame())
+ so.push_back(SegmentObstacle(e->init(), e->goal()));
+ }
#ifdef USE_SLOTPLANNER
TSTART();
- for (auto xy: jvi["slot"]["polygon"]) {
- ps.slot().add_bnode(new RRTNode(
- xy[0].asFloat(),
- xy[1].asFloat()
- ));
- }
if (ps.slot().bnodes().size() > 0)
- ps.fpose();
+ ps.fip(co, so);
TEND();
jvo["ppse"] = ELAPSED;
TPRINT("ParallelSlot");
#endif
+ if (ps.slot().bnodes().size() > 0) {
+ ps.setAll();
+ //if (ps.getMidd() != nullptr)
+ // p.setSamplingInfo(ps.getSamplingInfo());
+ }
if (ps.cusp().size() > 0) {
- p.goal(ps.cusp().front().front());
+ 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"][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]) {
p.next();
p.tend();
if (p.opt_path()) {
- if (ps.cusp().size() > 0)
- p.tlog(p.findt(p.slot_cusp().back()));
- else
- p.tlog(p.findt());
+ p.tlog(p.findt());
}
}
+ if (p.goal_found() && ps.slotType() == PARALLEL)
+ p.tlog(p.findt(p.slot_cusp().back()));
#elif defined USE_PTHREAD
bool gf = false;
RRTNode *ron = nullptr;
// 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