From 47251fa186ec2b823baa806ee02272fd03c3f4cb Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 15 Apr 2020 13:08:45 +0200 Subject: [PATCH] Add more possible goals for perpendicular slot --- src/psp.cc | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/psp.cc b/src/psp.cc index c9a9140..df6b58e 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -115,8 +115,6 @@ void PSPlanner::guess_gc() this->ps().x2() - this->ps().x1() ); while (h < 0) h += 2 * M_PI; - x += 2 * cos(h); - y += 2 * sin(h); //// This is for backward parking only. //double entry_width = edist( @@ -225,6 +223,22 @@ std::vector PSPlanner::possible_goals( pi.push_back(BicycleCar(this->cc())); } this->cc() = BicycleCar(orig_cc); + } else { + if (!this->ps().right()) { + this->cc().set_max_steer(); + for (unsigned int i = 0; i < cnt; i++) { + this->cc().next(); + pi.push_back(BicycleCar(this->cc())); + } + } else { + this->cc().set_max_steer(); + this->cc().st(this->cc().st() * -1); + 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