#include <iostream>
#include <json/json.h>
#include <vector>
-#include "fiat_punto.h"
+// When the file with precomputed entries is ready.
+//#include "fiat_punto.h"
#include "pslot.hh"
#include "rrtsp.hh"
std::cout << std::fixed;
std::cerr << std::fixed;
assert(jvi["slot"] != Json::nullValue);
+ rrts::Ter ip_time;
+ ip_time.start();
bcar::ParkingSlot s(
jvi["slot"][0][0].asDouble(), jvi["slot"][0][1].asDouble(),
jvi["slot"][1][0].asDouble(), jvi["slot"][1][1].asDouble(),
jvi["slot"][2][0].asDouble(), jvi["slot"][2][1].asDouble(),
jvi["slot"][3][0].asDouble(), jvi["slot"][3][1].asDouble());
bcar::BicycleCar c;
+ // wang 2017
+ //c.ctc(8.411896337925237);
+ //c.w(1.81);
+ //c.wb(2.7);
+ //c.df(3.7);
+ //c.len(4.85);
+ // jhang 2020 https://en.wikipedia.org/wiki/Chrysler_Pacifica_(minivan)
+ //c.ctc(9.557619159602458);
+ //c.w(2.022);
+ //c.wb(3.089);
+ //c.df(4.236);
+ //c.len(5.171);
+ // feng 2018
+ //p.eta(0.1);
+ // PES Porsche Cayenne
+ //c.ctc(11.2531); // this is guess
+ //c.w(1.983);
+ //c.wb(2.895);
+ //c.df(2.895 + 0.9); // this is guess
+ //c.len(4.918);
+ c.become("porsche cayenne");
+
bool swapped = false;
if (s.parallel() && !s.right()) {
s.swap_side();
s.swap_side();
pr.reflect(s.entry());
}
+ //std::cerr << ip_time.scnt() << std::endl;
// The following uses precomputed entries and needs `"fiat_punto.h"` to
// be included.
//auto pr = get_fiat_punto_entry(s.len(), s.w());
c.h(pr.h());
c.sp(-0.1);
c.st(0.0);
- std::vector<bcar::Pose> ispath;
+ std::vector<bcar::BicycleCar> ispath;
if (s.right()) {
while (!c.rf().on_right_side_of(s.entry())) {
c.next();
}
if (s.parallel()) {
c.set_max_steer();
- if (!s.right()) {
- c.st(c.st() * -1.0);
- }
- for (auto p: s.steer_in_slot(c)) {
- c.next();
- ispath.push_back(c);
+ for (auto &n : s.drive_in_slot(c)) {
+ ispath.push_back(n);
}
}
jvi["goal"][0] = pr.x();
jvi["goal"][1] = pr.y();
jvi["goal"][2] = pr.b();
jvi["goal"][3] = pr.e();
+ jvi["goal_inside"][0] = ispath.back().x();
+ jvi["goal_inside"][1] = ispath.back().y();
+ jvi["goal_inside"][2] = ispath.back().h();
+
+ // Opel Corsa
+ //p.bc().ctc(9.900);
+ //p.bc().w(1.532);
+ //p.bc().wb(2.343);
+ //p.bc().df(3.212);
+ //p.bc().len(3.622);
+ // wang 2017
+ //p.bc().ctc(8.411896337925237);
+ //p.bc().w(1.81);
+ //p.bc().wb(2.7);
+ //p.bc().df(3.7);
+ //p.bc().len(4.85);
+ // jhang 2020 https://en.wikipedia.org/wiki/Chrysler_Pacifica_(minivan)
+ //p.bc().ctc(9.557619159602458);
+ //p.bc().w(2.022);
+ //p.bc().wb(3.089);
+ //p.bc().df(4.236);
+ //p.bc().len(5.171);
+ //p.eta(0.3);
+ // feng 2018
+ //p.eta(0.1);
+ // PES Porsche Cayenne
+ //p.bc().ctc(11.2531); // this is guess
+ //p.bc().w(1.983);
+ //p.bc().wb(2.895);
+ //p.bc().df(2.895 + 0.9); // this is guess
+ //p.bc().len(4.918);
+ //p.eta(0.1);
+ p.set_bc_to_become("porsche cayenne");
+
p.json(jvi);
unsigned int icnt = 0;
unsigned int rcnt = 0;
unsigned int bcnt = 0;
+ unsigned int ncnt = 0; // not better counter
Json::Value best_path;
Json::Value pj;
double cost = 0.0;
+ p.reset();
while (icnt < 1000) {
p.icnt(icnt);
while (p.next()) {}
double gc = pj["goal_cc"].asDouble();
assert(gc > 0.0);
if (cost == 0.0 || gc < cost) {
+ if (gc < 0.75 * cost) {
+ ncnt = 0;
+ }
best_path = pj["path"];
cost = gc;
bcnt += 1;
+ } else {
+ ncnt++;
}
}
+ if (ncnt > 5) {
+ break;
+ }
p.reset();
rcnt += 1;
}
double elapsed = p.scnt();
auto jvo = p.json();
- unsigned int i = 0;
- for (auto p: ispath) {
- jvo["ispath"][i][0] = p.x();
- jvo["ispath"][i][1] = p.y();
- jvo["ispath"][i][2] = p.h();
- i += 1;
- }
jvo["init"] = jvi["init"];
jvo["slot"] = jvi["slot"];
jvo["obst"] = jvi["obst"];
jvo["time"] = elapsed;
jvo["icnt"] = p.icnt();
jvo["rcnt"] = rcnt;
+ jvo["ncnt"] = ncnt;
jvo["bcnt"] = bcnt;
jvo["path"] = best_path;
+ unsigned int best_path_size = best_path.size();
+ double best_path_gx = best_path[best_path_size - 1][0].asDouble();
+ double best_path_gy = best_path[best_path_size - 1][1].asDouble();
+ double best_path_gh = best_path[best_path_size - 1][2].asDouble();
+ // FIXME for perpendicular parking.
+ //assert(std::abs(best_path_gx - ispath[0].x()) < 1e-3
+ // && std::abs(best_path_gy - ispath[0].y()) < 1e-3
+ // && std::abs(best_path_gh - ispath[0].h()) < 1e-3);
+ unsigned int i = 0;
+ for (auto p: ispath) {
+ jvo["ispath"][i][0] = p.x();
+ jvo["ispath"][i][1] = p.y();
+ jvo["ispath"][i][2] = p.h();
+ if (i == 0) {
+ // The first pose of in-slot path is entry
+ // configuration, i.e. the last pose of the path.
+ // We just need to check if the last pose of the path
+ // (goal) is cusp.
+ int best_path_goal_sgn = bcar::sgn(
+ jvo["path"][best_path_size - 1][3].asDouble());
+ int ispath_goal_sgn = bcar::sgn(ispath[0].sp());
+ if (best_path_goal_sgn != ispath_goal_sgn) {
+ jvo["path"][best_path_size - 1][5] = true;
+ }
+ i++;
+ continue;
+ }
+ jvo["path"][best_path_size + i - 1][0] = p.x();
+ jvo["path"][best_path_size + i - 1][1] = p.y();
+ jvo["path"][best_path_size + i - 1][2] = p.h();
+ jvo["path"][best_path_size + i - 1][3] = p.sp();
+ int segment_type = 0;
+ if (p.st() > 0) {
+ segment_type = 1;
+ } else if (p.st() < 0) {
+ segment_type = -1;
+ }
+ jvo["path"][best_path_size + i - 1][4] = segment_type;
+ bool is_cusp = false;
+ if (bcar::sgn(p.sp()) != bcar::sgn(ispath[i - 1].sp())) {
+ is_cusp = true;
+ }
+ jvo["path"][best_path_size + i - 1][5] = is_cusp;
+ i++;
+ }
jvo["goal_cc"] = cost;
std::cout << jvo << std::endl;
return 0;
projects / hubacji1 / iamcar2.git / blobdiff