From 5002feb6ca26bbabaa00b2c921aa7b6a7ee2b429 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 15 Apr 2020 11:09:27 +0200
Subject: [PATCH] Extend possible goal for parallel parking slot

---
 src/psp.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/psp.cc b/src/psp.cc
index f1965bf..c9a9140 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -218,6 +218,14 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
                 pi.push_back(BicycleCar(this->cc()));
         }
         this->cc() = BicycleCar(orig_cc);
+        if (this->ps().parallel()) {
+                this->cc().st(0);
+                for (unsigned int i = 0; i < cnt; i++) {
+                        this->cc().next();
+                        pi.push_back(BicycleCar(this->cc()));
+                }
+                this->cc() = BicycleCar(orig_cc);
+        }
         return pi;
 }
 
-- 
2.39.2