Stop when parked an headings are similar

diff --git a/src/psp.cc b/src/psp.cc
index b60e9d1948f01d17507cc09bffd8a23c519e7c98..a4b14408e3979058e91b57975119cab886723516 100644 (file)
--- 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)