Fix possible goals for right parallel parking slot

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9b2bd19c07fc1d8b1898d374f1b0312466b17f35..dcd727c29ffe269600c54be5c4c85da004c57a29 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,9 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ### Added
 - Workaround to not working `fe()` for perpendicular parking slot.
 
+### Fixed
+- Possible goals for right parallel parking slot.
+
 ## 0.3.0 - 2020-02-04
 ### Added
 - Guess goal car method.

diff --git a/src/psp.cc b/src/psp.cc
index 7fe4ea88b658caa7cf54a72100a5c0a88c9305a0..583bad203cdd0b67541542f10dd5e9ce6ef58a44 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -207,12 +207,13 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
 )
 {
         std::vector<BicycleCar> pi;
-        if (this->cc().sp() > 0)
+        if (this->ps().parallel())
                 this->cc().sp(1);
         else
                 this->cc().sp(-1);
         this->cc().sp(this->cc().sp() * dist);
-        this->cc().st(this->cc().st() * 1);
+        if (this->ps().right())
+                this->cc().st(this->cc().st() * -1);
         BicycleCar orig_cc(this->cc());
         for (unsigned int i = 0; i < cnt; i++) {
                 this->cc().next();