From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 23 Aug 2019 16:39:19 +0000 (+0200)
Subject: Stop when parked an headings are similar
X-Git-Tag: v0.3.0~6^2~2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/86bebc739ab93178c3a07f893f7311bfbd2e7d51

Stop when parked an headings are similar
---

diff --git a/src/psp.cc b/src/psp.cc
index b60e9d1..a4b1440 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -204,6 +204,9 @@ void PSPlanner::fe_parallel()
                 q.pop();
                 while (
                         !this->collide()
+                        && (std::abs(
+                                this->cc().h() - this->ps().heading()
+                        ) > M_PI / 32)
                         && (std::abs(
                                 this->cc().h() - this->ps().heading()
                         ) < M_PI / 2)