From 2061c480add35281fba95307a95435b5c0737e62 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 3 Apr 2019 10:02:03 +0200 Subject: [PATCH] Change `fipr` method for perpendicular slot --- decision_control/slotplanner.cc | 35 ++++++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc index 819a44c..5f9880b 100644 --- a/decision_control/slotplanner.cc +++ b/decision_control/slotplanner.cc @@ -152,11 +152,36 @@ void ParallelSlot::fipr(BicycleCar *B) if (this->slotSide() == LEFT) di = -1; if (this->slotType() == PERPENDICULAR) { - cusp.push_back(new RRTNode( - B->x() - di * B->length() * cos(B->h()), - B->y() - di * B->length() * sin(B->h()), - B->h() - )); + this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h() + RRTNode *cc; + if (this->slotSide() == LEFT) + cc = BicycleCar(B->x(), B->y(), B->h()).ccl(); + else + cc = BicycleCar(B->x(), B->y(), B->h()).ccr(); + BicycleCar *p; + int i = 1; + p = B->move(cc, i * this->DH()); + while ( + !this->slot().collide(p->frame()) + && this->slot().collide(p) + ) { + delete p; + i += 10; + p = B->move(cc, i * this->DH()); + } + i -= 10; + p = B->move(cc, i * this->DH()); + while ( + !this->slot().collide(p->frame()) + && this->slot().collide(p) + ) { + delete p; + i += 1; + p = B->move(cc, i * this->DH()); + } + i -= 1; + p = B->move(cc, i * this->DH()); + cusp.push_back(new RRTNode(p->x(), p->y(), p->h())); std::reverse(cusp.begin(), cusp.end()); this->cusp().push_back(cusp); return; -- 2.39.2