From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 4 Feb 2020 09:49:14 +0000 (+0100)
Subject: Fix rotation of init for bfs paralell parking
X-Git-Tag: v0.3.0~1^2~4
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/ad65215575e3d4e792e566b6a71a84eadbb55436

Fix rotation of init for bfs paralell parking
---

diff --git a/src/psp.cc b/src/psp.cc
index f8b5950..9bcd7bb 100644
--- 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`