Redefine order of slot border nodes

The node closest to init pose is the first.

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 613c309dd7e9e4786ee47af67ade16c314bb77e5..fb523b2e4441d27a59e951df70796e35b2bd68ca 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,8 @@ The format is based on [Keep a Changelog][] and this project adheres to
 [Semantic Versioning]: http://semver.org/
 
 ## Unreleased
+### Changed
+- Paring slot in scenario starts with the point closest to the init pose.
 
 ## 0.6.1 - 2019-05-21
 ### Fixed

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 0685f71d6ac20beaa29e15e111d258c1f14e572c..4d15a2df7212d27026364f31670c3fc04d9f10b0 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -76,13 +76,13 @@ void ParallelSlot::setAll()
         float x0 = this->slot().bnodes()[0]->x();
         float y3 = this->slot().bnodes()[3]->y();
         float x3 = this->slot().bnodes()[3]->x();
-        float dy = y0 - y3;
-        float dx = x0 - x3;
+        float dy = y3 - y0;
+        float dx = x3 - x0;
         this->slotHeading_ = atan2(dy, dx);
         // slot side
         float y1 = this->slot().bnodes()[1]->y();
         float x1 = this->slot().bnodes()[1]->x();
-        if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
+        if (sgn((x1 - x0) * (y3 - y0) - (y1 - y0) * (x3 - x0)) < 0)
                 this->slotSide_ = LEFT;
         else
                 this->slotSide_ = RIGHT;
@@ -451,7 +451,7 @@ BicycleCar *ParallelSlot::flncr(BicycleCar *B)
 
 RRTNode *ParallelSlot::fposecenter()
 {
-        return this->slot().bnodes().front();
+        return this->slot().bnodes().back();
 }
 
 bool ParallelSlot::flast(

diff --git a/lpar.json b/lpar.json
index 2750a239cc5ca63d84704bf25608e2ddcc0758ca..0e11fe262f494eaa638c4ac75381cc4295dfa8d0 100644 (file)
--- a/lpar.json
+++ b/lpar.json
@@ -4,10 +4,10 @@
         "rgoal": [-0.8225, 13.665, 1.5707963267948966],
         "slot": {
                 "polygon": [
-                        [0, 17.56],
-                        [-2.2, 17.56],
+                        [0, 13],
                         [-2.2, 13],
-                        [0, 13]
+                        [-2.2, 17.56],
+                        [0, 17.56]
                 ]
         },
         "obst": [

diff --git a/rpar.json b/rpar.json
index 9a176b542fae2f3abb1e908ae9636616d6cec62d..07ec956d48c7a10dfb0c2ad5d09214b1126bf5ce 100644 (file)
--- a/rpar.json
+++ b/rpar.json
@@ -3,10 +3,10 @@
         "goal": [1.1, 14.37, 1.5707963267948966],
         "slot": {
                 "polygon": [
-                        [0, 19.5],
-                        [2.2, 19.5],
+                        [0, 13],
                         [2.2, 13],
-                        [0, 13]
+                        [2.2, 19.5],
+                        [0, 19.5]
                 ]
         },
         "obst": [

diff --git a/rper.json b/rper.json
index 846ce4ae0bb11aecfad6db19c1a2988943e7d542..63f5e44f99639f0519b42254d023e2fb639f69d0 100644 (file)
--- a/rper.json
+++ b/rper.json
@@ -3,10 +3,10 @@
         "goal": [3.5, 14.2, 3.14],
         "slot" : {
                 "polygon" : [
-                        [0, 15.4],
-                        [5.3, 15.4],
+                        [0, 13],
                         [5.3, 13],
-                        [0, 13]
+                        [5.3, 15.4],
+                        [0, 15.4]
                 ]
         },
         "obst": [