X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/47251fa186ec2b823baa806ee02272fd03c3f4cb..802b4fe9bfc4f3e75b28cebf3ce5013b436a5362:/src/psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index df6b58e..31d4a74 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -152,6 +152,46 @@ void PSPlanner::guess_gc()
         this->gc().h(h);
 }
 
+std::vector<BicycleCar> PSPlanner::last_maneuver()
+{
+        std::vector<BicycleCar> lm;
+        if (this->ps().parallel()) {
+                // zig-zag out from the slot
+                this->cc() = BicycleCar(this->gc());
+                this->cc().sp(0.1);
+                while (!this->left()) {
+                        while (!this->collide() && !this->left()) {
+                                this->cc().next();
+                                lm.push_back(BicycleCar(this->cc()));
+                        }
+                        if (this->left() && !this->collide()) {
+                                break;
+                        } else {
+                                lm.pop_back();
+                                this->cc().sp(this->cc().sp() * -1);
+                                this->cc().next();
+                                this->cc().st(this->cc().st() * -1);
+                                this->c_++;
+                                lm.push_back(BicycleCar(this->cc()));
+                        }
+                }
+                if (this->cc().st() < 0) {
+                        this->c_++;
+                        lm.push_back(BicycleCar(this->cc()));
+                }
+        } else {
+                // go 1 m forward
+                this->cc().sp(0.1);
+                BicycleCar orig_cc(this->cc());
+                for (unsigned int i = 0; i < 10; i++) {
+                        this->cc().next();
+                        lm.push_back(BicycleCar(this->cc()));
+                }
+                this->cc() = BicycleCar(orig_cc);
+        }
+        return lm;
+}
+
 bool PSPlanner::left()
 {
 	double lfx = this->cc().lfx();
@@ -306,16 +346,6 @@ void PSPlanner::fe_parallel()
         while (!q.empty() && iter_cntr < 30) {
                 this->cc() = BicycleCar(q.front());
                 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)
-                )
-                        this->cc().next();
                 this->cc().sp(this->cc().sp() * -1);
                 this->cc().next();
                 this->gc() = BicycleCar(this->cc());