Split init from reverse parallel psp

diff --git a/src/psp.cc b/src/psp.cc
index 9258012c78dfe693dc6bb3e93b82fc46ee6696c7..5b657cd638bc7344a670ed16acf68e6129e89b7a 100644 (file)
--- 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())