From b1feb96915e0e64fe8938833cbc8b053ccfb6293 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 Feb 2020 11:46:59 +0100
Subject: [PATCH] Split init from reverse parallel psp

---
 src/psp.cc | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/psp.cc b/src/psp.cc
index 9258012..5b657cd 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -373,18 +373,22 @@ void PSPlanner::fe_perpendicular()
 
 void PSPlanner::fer()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                this->cc().set_max_steer();
+                if (!this->ps().right())
+                        this->cc().st(this->cc().st() * -1);
+                this->cc().sp(0.01);
                 return this->fer_parallel();
-        else
+        } else {
                 return this->fer_perpendicular();
+        }
 }
 
 void PSPlanner::fer_parallel()
 {
-        this->cc().st(this->cc().wb() / this->cc().mtr());
-        if (!this->ps().right())
-                this->cc().st(this->cc().st() * -1);
-        this->cc().sp(0.01);
         this->cusps_.clear();
         while (!this->left()) {
                 while (!this->collide() && !this->left())
-- 
2.39.2