Merge branch 'feature/perpendicular-slotplanner'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d942f3b2d33b9a6dd7acce8ed65cccbd07987a6b..9128562a084c599374843e383c1f530fcf17361c 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,11 +16,13 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - SlotSide (LEFT, RIGHT) and SlotType (PARALLEL, PERPENDICULAR) methods for
   SlotPlanner.
 - Information used for sampling getter.
+- Slot planner working for perpendicular parking scenario also.
 
 ### Changed
 - Test results plot.
 - Make T2 planner the default planner.
 - Sampling info for different dimensions and angles of slot.
+- Use reverse approach for finding init pose by default.
 
 ### Fixed
 - Car border.

diff --git a/base/main.cc b/base/main.cc
index af76ff3547f42058c8890bbb538ac3478cc5f094..637d272796dc4c0152d4edef2cecedeb41f96ad1 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -170,12 +170,7 @@ int main()
         ps.setAll();
         p.samplingInfo_ = ps.getSamplingInfo();
         if (ps.slot().bnodes().size() > 0)
-                ps.fpose();
-                //ps.fipr(new BicycleCar(
-                //        p.goal()->x(),
-                //        p.goal()->y(),
-                //        p.goal()->h()
-                //));
+                ps.fipr(ps.getFP());
         TEND();
         jvo["ppse"] = ELAPSED;
         TPRINT("ParallelSlot");

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 4b80a17f876447841cf19c6e41ea6d5312612468..ed1446de490c20a83eab26a9392b45d6ebe59f76 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -151,6 +151,16 @@ void ParallelSlot::fipr(BicycleCar *B)
         int di = 1;
         if (this->slotSide() == LEFT)
                 di = -1;
+        if (this->slotType() == PERPENDICULAR) {
+                cusp.push_back(new RRTNode(
+                        B->x() - di * B->length(),
+                        B->y(),
+                        B->h()
+                ));
+                std::reverse(cusp.begin(), cusp.end());
+                this->cusp().push_back(cusp);
+                return;
+        }
         this->DH(di * 0.01 / B->out_radi());
         BicycleCar *c;
         c = this->flncr(B);
@@ -408,9 +418,15 @@ BicycleCar *ParallelSlot::getEP()
                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
         }
-        // move down
-        nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
-        ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
+        if (this->slotType() == PARALLEL) {
+                // move down
+                nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
+                ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
+        } else {
+                // move down
+                nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
+                ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
+        }
         return new BicycleCar(nx, ny, CC->h());
 }
 
@@ -437,15 +453,35 @@ BicycleCar *ParallelSlot::getFP()
         float h = this->slotHeading();
         float nx;
         float ny;
-        if (this->slotSide() == LEFT) {
-                nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
-                ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
+        if (this->slotType() == PARALLEL) {
+                if (this->slotSide() == LEFT) {
+                        nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
+                        ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
+                } else {
+                        nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
+                        ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
+                }
+                x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
+                y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
         } else {
-                nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
-                ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
+                if (this->slotSide() == LEFT) {
+                        h -= M_PI / 2;
+                        nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * cos(h + M_PI);
+                        ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * sin(h + M_PI);
+                        x = nx + (BCAR_WIDTH + 0.01) * cos(h + M_PI / 2);
+                        y = ny + (BCAR_WIDTH + 0.01) * sin(h + M_PI / 2);
+                } else {
+                        h += M_PI / 2;
+                        nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * cos(h - M_PI);
+                        ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
+                                * sin(h - M_PI);
+                        x = nx + (BCAR_WIDTH / 2 + 0.01) * cos(h - M_PI / 2);
+                        y = ny + (BCAR_WIDTH / 2 + 0.01) * sin(h - M_PI / 2);
+                }
         }
-        x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
-        y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
         return new BicycleCar(x, y, h);
 }
 
@@ -489,9 +525,13 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
         }
         si.r = CC->diag_radi();
         si.sh = this->slotHeading();
-        si.h = this->slotHeading() - acos(EDIST( // TODO generalize
-                this->slot().bnodes()[0],
-                this->slot().bnodes()[1]
-        ) / BCAR_LENGTH);
+        if (this->slotType() == PARALLEL) {
+                si.h = this->slotHeading() - acos(EDIST(
+                        this->slot().bnodes()[0],
+                        this->slot().bnodes()[1]
+                ) / BCAR_LENGTH);
+        } else {
+                si.h = M_PI /2;
+        }
         return si;
 }

diff --git a/rper.json b/rper.json
index 7779170806ce725fa165d60060420783ca2703f4..846ce4ae0bb11aecfad6db19c1a2988943e7d542 100644 (file)
--- a/rper.json
+++ b/rper.json
@@ -1,6 +1,14 @@
 {
         "init": [-2, 6.5, 1.5707963267948966],
-        "goal": [0.37, 14.2, 0],
+        "goal": [3.5, 14.2, 3.14],
+        "slot" : {
+                "polygon" : [
+                        [0, 15.4],
+                        [5.3, 15.4],
+                        [5.3, 13],
+                        [0, 13]
+                ]
+        },
         "obst": [
                 {
                         "segment": [