From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 23 Aug 2019 19:12:49 +0000 (+0200)
Subject: Fix forward for perpendicular parking
X-Git-Tag: v0.3.0~5^2~4
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/a77a1ddba45b34058daec994ee7601006576a6c0?ds=sidebyside

Fix forward for perpendicular parking
---

diff --git a/src/psp.cc b/src/psp.cc
index 9529383..c64acd0 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,16 +21,18 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        double heading = this->ps().heading();
+        if (this->ps().parallel())
+                return false;
+        double heading = atan2(
+                this->ps().y2() - this->ps().y1(),
+                this->ps().x2() - this->ps().x1()
+        );
         while (heading < 0) heading += 2 * M_PI;
-        if (!this->ps().parallel())
-                heading -= M_PI / 2;
         double h = this->gc().h();
         while (h < 0) h += 2 * M_PI;
-        if (-0.00001 < heading - h && heading - h < 0.00001)
+        if (std::abs(heading - h) < M_PI / 4)
                 return true;
-        else
-                return false;
+        return false;
 }
 
 void PSPlanner::guess_gc()