From 0dba936b4333df368da25c26b33691faed013fc6 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 23 May 2019 08:34:47 +0200
Subject: [PATCH] Redefine order of slot border nodes

The node closest to init pose is the first.
---
 CHANGELOG.md                    | 2 ++
 decision_control/slotplanner.cc | 8 ++++----
 lpar.json                       | 6 +++---
 rpar.json                       | 6 +++---
 rper.json                       | 6 +++---
 5 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 613c309..fb523b2 100644
--- 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 0685f71..4d15a2d 100644
--- 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 2750a23..0e11fe2 100644
--- 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 9a176b5..07ec956 100644
--- 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 846ce4a..63f5e44 100644
--- 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": [
-- 
2.39.2