From 262ca6bf5b1dedba858e0d65fd7df8b4c19e9b5e Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 26 Aug 2019 14:25:15 +0200 Subject: [PATCH] Add set goal car position method Set goal car position near 4th slot border coordinate. --- api/psp.h | 5 ++++- src/psp.cc | 20 ++++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/api/psp.h b/api/psp.h index 2520dcb..4c71d3e 100644 --- a/api/psp.h +++ b/api/psp.h @@ -19,11 +19,14 @@ class PSPlanner { BicycleCar gc_; ParkingSlot ps_; - // find entry to slot by reverse approach + // find entry to slot void fe_parallel(); void fe_perpendicular(); + // find entry to slot by reverse approach void fer_parallel(); void fer_perpendicular(); + // set goal car + void gc_to_4(); public: /*! \brief Return `true` if there is collision. diff --git a/src/psp.cc b/src/psp.cc index 7d00cc6..5992e3e 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -35,6 +35,26 @@ bool PSPlanner::forward() return false; } +void PSPlanner::gc_to_4() +{ + double angl_slot = atan2( + this->ps().y3() - this->ps().y4(), + this->ps().x3() - this->ps().x4() + ); + double angl_delta = M_PI / 2; + if (this->ps().right()) + angl_delta = -M_PI / 2; + double x = this->ps().x4(); + double y = this->ps().y4(); + x += (this->gc().dr() + 0.01) * cos(angl_slot); + y += (this->gc().dr() + 0.01) * sin(angl_slot); + x += (this->gc().w() / 2 + 0.01) * cos(angl_slot + angl_delta); + y += (this->gc().w() / 2 + 0.01) * sin(angl_slot + angl_delta); + this->gc().x(x); + this->gc().y(y); + this->gc().h(angl_slot); +} + void PSPlanner::guess_gc() { double x = this->ps().x1(); -- 2.39.2