Extend possible goal for parallel parking slot

diff --git a/src/psp.cc b/src/psp.cc
index f1965bf49974b7f81c318af398dd88d4a1aa4803..c9a91408d684ff811a9683d0c98fa5b7c0894bbc 100644 (file)
--- 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;
 }