X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/92f24b15d81036649a9492e5a1d88a22005a880b..4b31eb4c045adc3ac8e53076e3e5173b566ee1c2:/src/template-with-reset.cc diff --git a/src/template-with-reset.cc b/src/template-with-reset.cc index 2266a3e..c3a8253 100644 --- a/src/template-with-reset.cc +++ b/src/template-with-reset.cc @@ -2,7 +2,8 @@ #include #include #include -#include "fiat_punto.h" +// When the file with precomputed entries is ready. +//#include "fiat_punto.h" #include "pslot.hh" #include "rrtsp.hh" @@ -19,12 +20,36 @@ int main() 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(); @@ -35,6 +60,7 @@ int main() 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()); @@ -44,7 +70,7 @@ int main() c.h(pr.h()); c.sp(-0.1); c.st(0.0); - std::vector ispath; + std::vector ispath; if (s.right()) { while (!c.rf().on_right_side_of(s.entry())) { c.next(); @@ -58,25 +84,57 @@ int main() } 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()) {} @@ -86,23 +144,24 @@ int main() 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"]; @@ -113,8 +172,54 @@ int main() 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;