Fix rotation of init for bfs paralell parking

diff --git a/src/psp.cc b/src/psp.cc
index f8b5950ade9ab744c452dcc9bb43288204e0b958..9bcd7bb754771389bb558f198fca2c246c6d6183 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -322,7 +322,7 @@ void PSPlanner::fe_parallel()
                 this->cc().rotate(
                         this->ps().x4(),
                         this->ps().y4() - 0.01,
-                        0.01
+                        ((this->ps().right()) ? 0.01 : -0.01)
                 );
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`