Add more car dimensions, remove unused comments

[hubacji1/iamcar2.git] / src / template-with-reset.cc

diff --git a/src/template-with-reset.cc b/src/template-with-reset.cc
index bd9d97288c3bb7e07de603fc11b7046550906b56..0d17b0603357028118236a2fb08fc304d18fba43 100644 (file)
--- a/src/template-with-reset.cc
+++ b/src/template-with-reset.cc
@@ -28,26 +28,6 @@ int main()
                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;
@@ -70,7 +50,7 @@ int main()
        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();
@@ -96,35 +76,7 @@ int main()
        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);
-       c.become("porsche cayenne");
+       p.set_bc_to_become("porsche cayenne");
 
        p.json(jvi);
        unsigned int icnt = 0;
@@ -162,13 +114,6 @@ int main()
        }
        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"];
@@ -182,6 +127,51 @@ int main()
        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;