From: Jiri Vlasak Date: Tue, 20 Jul 2021 14:51:04 +0000 (+0200) Subject: Merge branch 'compute-pslot-table' X-Git-Tag: v0.6.0~3 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/727dec6700bd5b79f2441e7a8f17858f72ddc289?hp=6a9b2de3d871d005472023463efe69429cee7996 Merge branch 'compute-pslot-table' --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d6a77b..c5a0d6a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,9 @@ add_library(pslot STATIC src/pslot.cc) target_include_directories(pslot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl) target_link_libraries(pslot bcar) +add_executable(compute_pslot_table src/compute_pslot_table.cc) +target_link_libraries(compute_pslot_table pslot) + if (SKIP_UT) return() endif() diff --git a/incl/bcar.hh b/incl/bcar.hh index 1ca3a7a..7591d72 100644 --- a/incl/bcar.hh +++ b/incl/bcar.hh @@ -58,6 +58,12 @@ public: */ bool on_right_side_of(Line const& li) const; + /*! \brief Translate self. + * + * \param p `Point` offset to translate by. + */ + void translate(Point const& p); + /*! \brief Rotate self around the point. \param c Rotation center `Point`. @@ -65,9 +71,16 @@ public: */ void rotate(Point const& c, double const angl); + /*! \brief Compute reflection of `this` around the `Line`. + * + * \param li The plane to reflect around is given by `li`. + */ + void reflect(Line const& li); + /*! Return Euclidean distance to `p`. */ double edist(Point const& p) const; + bool operator==(Point const& p); friend std::ostream& operator<<(std::ostream& out, Point const& p); }; @@ -117,6 +130,8 @@ public: double len() const; + double h() const; + friend std::ostream& operator<<(std::ostream& out, Line const& li); }; @@ -139,27 +154,33 @@ public: void rotate(Point const& c, double const angl); + void reflect(Line const& li); + + bool operator==(Pose const& p); friend std::ostream& operator<<(std::ostream& out, Pose const& p); }; class PoseRange : public virtual Pose { private: - double e_ = 0.0; - using Pose::h; + Pose bp_; + Pose ep_; + void set_xyh(); public: + PoseRange(Pose bp, Pose ep); + PoseRange(double x, double y, double b, double e); + + Pose bp() const; + Pose ep() const; + /*! Get heading's begin in the interval [-pi, +pi] radians. */ double b() const; - /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */ - void b(double b); - /*! Get heading's end in the interval [-pi, +pi] radians. */ double e() const; - /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */ - void e(double e); - + void translate(Point const& p); void rotate(Point const& c, double const angl); + void reflect(Line const& li); friend std::ostream& operator<<(std::ostream& out, PoseRange const& p); }; @@ -283,10 +304,10 @@ class BicycleCar : public virtual Pose, public virtual CarSize, public virtual CarMove { private: public: - /*! \brief Return `false` if `bc` is not achievable. + /*! \brief Return `true` if `this` can drive to `p` trivially. * - * When `false` is returned the `bc` may still be drivable, but not - * trivially, i.e. by "line segment - circle arc - line segment". + * Trivially means that `this` can drive to `p` by line segment - circle + * arc - line segment. * * \param p `PoseRange` (resp. `Pose`) to achieve. */ diff --git a/incl/pslot.hh b/incl/pslot.hh index c669d23..655eb5e 100644 --- a/incl/pslot.hh +++ b/incl/pslot.hh @@ -17,6 +17,9 @@ namespace bcar { class ParkingSlot { private: double offset_ = 0.001; // to avoid collision during init + double parking_speed_ = -0.1; + unsigned int max_cusp_ = 10; + double delta_angle_to_slot_ = 0.001; Point border_[4]; Line entry_; Line rear_; @@ -67,6 +70,39 @@ public: /*! Return parking slot's orientation. */ double h() const; + /*! Get parking slot's left front point. */ + Point lf() const; + + /*! Get parking slot's left rear point. */ + Point lr() const; + + /*! Get parking slot's right rear point. */ + Point rr() const; + + /*! Get parking slot's right front point. */ + Point rf() const; + + /*! Get parking slot's entry side. */ + Line entry() const; + + /*! Get parking slot's rear side. */ + Line rear() const; + + /*! Get parking slot's curb side. */ + Line curb() const; + + /*! Get parking slot's front side. */ + Line front() const; + + /*! Car's next iteration distance. (Negative for backward.) */ + void set_parking_speed(double s); + + /*! Maximum allowed number of cusp inside the parking slot. */ + void set_max_cusp(unsigned int m); + + /*! Angle's increment when creating start positions. */ + void set_delta_angle_to_slot(double d); + /*! Return `true` for the parallel parking slot. */ bool parallel() const; @@ -85,12 +121,31 @@ public: /*! \brief Drive car `c` into the parking slot `this`. * * \param c Starting bicycle car. - * \param max Maximum number of backward-forward direction changes. */ - std::vector drive_in_slot(BicycleCar c, unsigned int& max); + std::vector drive_in_slot(BicycleCar c); - /*! Find entry. */ - PoseRange fe(); + /*! \brief Steer car `c` into the parking slot `this`. + * + * `steer_in_slot` returns the complete path as the list of `Pose`s, not + * just cusp `BicycleCar`s as `drive_to_slot`. + * + * \param c Starting bicycle car. + */ + std::vector steer_in_slot(BicycleCar c); + + /*! \brief Find entry. + * + * \param c For which `BicycleCar` should entry be found? + */ + PoseRange fe(BicycleCar c); + + /*! \brief Recompute zero slot's `PoseRange` entry for `this`. + * + * The _zero_ slot is the `ParkingSlot(Point(0.0, 0.0), 0.0, W, L);`. + * + * \param p Computed `PoseRange` entry. + */ + PoseRange recompute_entry(PoseRange p); friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s); }; diff --git a/src/bcar.cc b/src/bcar.cc index 8825fb7..84a3bcb 100644 --- a/src/bcar.cc +++ b/src/bcar.cc @@ -95,6 +95,13 @@ Point::on_right_side_of(Line const& li) const } } +void +Point::translate(Point const& p) +{ + this->x_ += p.x(); + this->y_ += p.y(); +} + void Point::rotate(Point const& c, double const angl) { @@ -108,12 +115,28 @@ Point::rotate(Point const& c, double const angl) this->y(ny + c.y()); } +void +Point::reflect(Line const& li) +{ + this->rotate(li.b(), -li.h()); + this->y_ -= li.b().y(); + this->y_ *= -1.0; + this->y_ += li.b().y(); + this->rotate(li.b(), li.h()); +} + double Point::edist(Point const& p) const { return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0)); } +bool +Point::operator==(Point const& p) +{ + return this->x() == p.x() && this->y() == p.y(); +} + std::ostream& operator<<(std::ostream& out, Point const& p) { @@ -222,6 +245,12 @@ Line::len() const return this->b_.edist(this->e_); } +double +Line::h() const +{ + return atan2(this->e_.y() - this->b_.y(), this->e_.x() - this->b_.x()); +} + std::ostream& operator<<(std::ostream& out, Line const& li) { @@ -266,6 +295,20 @@ Pose::rotate(Point const& c, double const angl) this->h(this->h() + angl); } +void +Pose::reflect(Line const& li) +{ + Point::reflect(li); + double dh = li.h() - this->h(); + this->h(this->h() + 2.0 * dh); +} + +bool +Pose::operator==(Pose const& p) +{ + return this->x() == p.x() && this->y() == p.y() && this->h() == p.h(); +} + std::ostream& operator<<(std::ostream& out, Pose const& p) { @@ -273,41 +316,86 @@ operator<<(std::ostream& out, Pose const& p) return out; } -double -PoseRange::b() const +void +PoseRange::set_xyh() +{ + double clen = 10.0; + double bpbx = this->bp_.x() - clen * cos(this->bp_.h()); + double bpby = this->bp_.y() - clen * sin(this->bp_.h()); + double bpfx = this->bp_.x() + clen * cos(this->bp_.h()); + double bpfy = this->bp_.y() + clen * sin(this->bp_.h()); + Line li1(Point(bpbx, bpby), Point(bpfx, bpfy)); + double epbx = this->ep_.x() - clen * cos(this->ep_.h()); + double epby = this->ep_.y() - clen * sin(this->ep_.h()); + double epfx = this->ep_.x() + clen * cos(this->ep_.h()); + double epfy = this->ep_.y() + clen * sin(this->ep_.h()); + Line li2(Point(epbx, epby), Point(epfx, epfy)); + li1.intersects_with(li2); + this->x(li1.i1().x()); + this->y(li1.i1().y()); + this->h((this->b() + this->e()) / 2.0); +} + +PoseRange::PoseRange(Pose bp, Pose ep) : bp_(bp), ep_(ep) +{ + if (this->bp_ == this->ep_) { + this->set_pose(this->ep_); + } else { + this->set_xyh(); + } +} + +PoseRange::PoseRange(double x, double y, double b, double e) + : PoseRange(Pose(x, y, b), Pose(x, y, e)) { - return this->h(); } -void -PoseRange::b(double b) +Pose +PoseRange::bp() const +{ + return this->bp_; +} + +Pose +PoseRange::ep() const { - this->h(b); + return this->ep_; +} + +double +PoseRange::b() const +{ + return std::min(this->bp_.h(), this->ep_.h()); } double PoseRange::e() const { - return this->e_; + return std::max(this->bp_.h(), this->ep_.h()); } void -PoseRange::e(double e) +PoseRange::translate(Point const& p) { - while (e < -M_PI) { - e += 2 * M_PI; - } - while (e > +M_PI) { - e -= 2 * M_PI; - } - this->e_ = e; + this->bp_.translate(p); + this->ep_.translate(p); + this->set_xyh(); } void PoseRange::rotate(Point const& c, double const angl) { - Pose::rotate(c, angl); - this->e(this->e() + angl); + this->bp_.rotate(c, angl); + this->ep_.rotate(c, angl); + this->set_xyh(); +} + +void +PoseRange::reflect(Line const& li) +{ + this->bp_.reflect(li); + this->ep_.reflect(li); + this->set_xyh(); } std::ostream& @@ -453,24 +541,18 @@ CarMove::st(double st) bool BicycleCar::drivable(Pose const& p) const { - PoseRange pr; - pr.x(p.x()); - pr.y(p.y()); - pr.b(p.h()); - pr.e(p.h()); - return this->drivable(pr); + return this->drivable(PoseRange(p, p)); } bool BicycleCar::drivable(PoseRange const& p) const { - double h = (p.b() + p.e()) / 2.0; double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h(); while (a_1 < -M_PI) a_1 += 2 * M_PI; while (a_1 > +M_PI) a_1 -= 2 * M_PI; - double h_d = h - this->h(); + double h_d = p.h() - this->h(); while (h_d < -M_PI) h_d += 2 * M_PI; while (h_d > +M_PI) @@ -481,9 +563,9 @@ BicycleCar::drivable(PoseRange const& p) const } else if (0 < a_1 && a_1 <= M_PI/2) { // left front BicycleCar z(*this); // zone border z.h(p.e()); - h_d = h - this->h(); + h_d = p.h() - this->h(); z.rotate(this->ccl(), h_d); - // assert z.h() == h + // assert z.h() == p.h() if (p.y() == z.y() && p.x() == z.x()) // p on zone border return true; a_2 = atan2(p.y() - z.y(), p.x() - z.x()); @@ -496,9 +578,9 @@ BicycleCar::drivable(PoseRange const& p) const } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear BicycleCar z(*this); // zone border z.h(p.e()); - h_d = h - this->h(); + h_d = p.h() - this->h(); z.rotate(this->ccl(), h_d); - // assert z.h() == h + // assert z.h() == p.h() if (p.y() == z.y() && p.x() == z.x()) // p on zone border return true; a_2 = atan2(p.y() - z.y(), p.x() - z.x()); @@ -512,9 +594,9 @@ BicycleCar::drivable(PoseRange const& p) const } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front BicycleCar z(*this); // zone border z.h(p.b()); - h_d = h - this->h(); + h_d = p.h() - this->h(); z.rotate(this->ccr(), h_d); - // assert z.h() == h + // assert z.h() == p.h() if (p.y() == z.y() && p.x() == z.x()) // p on zone border return true; a_2 = atan2(p.y() - z.y(), p.x() - z.x()); @@ -527,9 +609,9 @@ BicycleCar::drivable(PoseRange const& p) const } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear BicycleCar z(*this); // zone border z.h(p.b()); - h_d = h - this->h(); + h_d = p.h() - this->h(); z.rotate(this->ccr(), h_d); - // assert z.h() == h + // assert z.h() == p.h() if (p.y() == z.y() && p.x() == z.x()) // p on zone border return true; a_2 = atan2(p.y() - z.y(), p.x() - z.x()); diff --git a/src/compute_pslot_table.cc b/src/compute_pslot_table.cc new file mode 100644 index 0000000..8b060ed --- /dev/null +++ b/src/compute_pslot_table.cc @@ -0,0 +1,88 @@ +#include +#include +#include "pslot.hh" + +#define CAR "fiat_punto" +#define CAR_CURB_TO_CURB 10.820 +#define CAR_WIDTH 1.625 +#define CAR_WHEELBASE 2.450 +#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105 +#define CAR_LENGTH 3.760 + +#define SLOT_MAX_LENGTH 6.5 +#define SLOT_MAX_WIDTH 2.2 +#define SLOT_STEP_LENGTH 0.01 +#define SLOT_STEP_WIDTH 0.01 + +#define PARKING_SPEED -0.001 +#define MAX_CUSP 10 +#define DELTA_ANGLE_TO_SLOT 0.0001 + +struct entry { + double len = 0.0; + double w = 0.0; + bcar::PoseRange pr; +}; + +int main() +{ + std::cout << std::fixed; + std::cerr << std::fixed; + + bcar::BicycleCar c; + c.ctc(CAR_CURB_TO_CURB); + c.w(CAR_WIDTH); + c.wb(CAR_WHEELBASE); + c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT); + c.len(CAR_LENGTH); + + bcar::Point zp(0.0, 0.0); + double zh = 0.0; + double len = SLOT_MAX_LENGTH; + double w = SLOT_MAX_WIDTH; + + std::vector entries; + while (len > c.len()) { + if (len > c.perfect_parking_slot_len() + SLOT_STEP_LENGTH) { + len -= SLOT_STEP_LENGTH; + continue; + } + w = SLOT_MAX_WIDTH; + while (w > c.w()) { + bcar::ParkingSlot s(zp, zh, w, len); + s.set_parking_speed(PARKING_SPEED); + s.set_max_cusp(MAX_CUSP); + s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT); + auto pr = s.fe(c); + if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0 + && pr.e() == 0.0)) { + struct entry e{len, w, pr}; + entries.push_back(e); + } + w -= SLOT_STEP_WIDTH; + } + std::cerr << "Slot length " << len << " done." << std::endl; + len -= SLOT_STEP_LENGTH; + } + + using namespace std; + cout << "/* Generated by `./compute_pslot_table` */" << endl; + cout << "#ifndef PSLOT_" << CAR << "_TABLE" << endl; + cout << "#define PSLOT_" << CAR << "_TABLE" << endl; + cout << "#include \"bcar.hh\"" << endl; + cout << "bcar::PoseRange " << "get_" << CAR << "_entry(double len,"; + cout << "double w)" << endl; + cout << "{" << endl; + for (auto e: entries) { + cout << "\tif (len >= " << e.len; + cout << " && w >= " << e.w << ") {" << endl; + cout << "\t\treturn bcar::PoseRange("; + cout << e.pr.x() << "," << e.pr.y() << ","; + cout << e.pr.b() << "," << e.pr.e() << ");" << endl; + cout << "\t}" << endl; + } + cout << "\treturn bcar::PoseRange(0.0,0.0,0.0,0.0);" << endl; + cout << "}" << endl; + cout << "#endif /* PSLOT_" << CAR << "_TABLE */" << endl; + return 0; +} diff --git a/src/pslot.cc b/src/pslot.cc index 9fdf4da..91bf5e9 100644 --- a/src/pslot.cc +++ b/src/pslot.cc @@ -95,6 +95,72 @@ ParkingSlot::h() const return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx()); } +Point +ParkingSlot::lf() const +{ + return Point(this->lfx(), this->lfy()); +} + +Point +ParkingSlot::lr() const +{ + return Point(this->lrx(), this->lry()); +} + +Point +ParkingSlot::rr() const +{ + return Point(this->rrx(), this->rry()); +} + +Point +ParkingSlot::rf() const +{ + return Point(this->rfx(), this->rfy()); +} + +Line +ParkingSlot::entry() const +{ + return this->entry_; +} + +Line +ParkingSlot::rear() const +{ + return this->rear_; +} + +Line +ParkingSlot::curb() const +{ + return this->curb_; +} + +Line +ParkingSlot::front() const +{ + return this->front_; +} + +void +ParkingSlot::set_parking_speed(double s) +{ + this->parking_speed_ = s; +} + +void +ParkingSlot::set_max_cusp(unsigned int m) +{ + this->max_cusp_ = m; +} + +void +ParkingSlot::set_delta_angle_to_slot(double d) +{ + this->delta_angle_to_slot_ = d; +} + bool ParkingSlot::parallel() const { @@ -112,6 +178,10 @@ ParkingSlot::swap_side() { this->border_[1].rotate(this->border_[0], M_PI); this->border_[2].rotate(this->border_[3], M_PI); + this->entry_ = Line(this->border_[0], this->border_[3]); + this->rear_ = Line(this->border_[0], this->border_[1]); + this->curb_ = Line(this->border_[1], this->border_[2]); + this->front_ = Line(this->border_[2], this->border_[3]); } bool @@ -127,39 +197,40 @@ bool ParkingSlot::collide(BicycleCar const& c) const { return c.left().intersects_with(this->rear_) - && c.left().intersects_with(this->curb_) - && c.left().intersects_with(this->front_) - && c.rear().intersects_with(this->rear_) - && c.rear().intersects_with(this->curb_) - && c.rear().intersects_with(this->front_) - && c.right().intersects_with(this->rear_) - && c.right().intersects_with(this->curb_) - && c.right().intersects_with(this->front_) - && c.front().intersects_with(this->rear_) - && c.front().intersects_with(this->curb_) - && c.front().intersects_with(this->front_); + || c.left().intersects_with(this->curb_) + || c.left().intersects_with(this->front_) + || c.rear().intersects_with(this->rear_) + || c.rear().intersects_with(this->curb_) + || c.rear().intersects_with(this->front_) + || c.right().intersects_with(this->rear_) + || c.right().intersects_with(this->curb_) + || c.right().intersects_with(this->front_) + || c.front().intersects_with(this->rear_) + || c.front().intersects_with(this->curb_) + || c.front().intersects_with(this->front_); } std::vector -ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max) +ParkingSlot::drive_in_slot(BicycleCar c) { assert(this->parallel()); assert(this->right()); assert(c.len() < this->len()); assert(c.w() < this->w()); std::vector path; - path.reserve(max + 2); + path.reserve(this->max_cusp_ + 2); path.push_back(c); unsigned int cusp = 0; - while (cusp < max) { - if (c.h() < this->h()) { - return std::vector(); - } + while (cusp < this->max_cusp_ + 1) { if (this->parked(c)) { - if (cusp < max) { - max = cusp; + if (cusp < this->max_cusp_) { + this->max_cusp_ = cusp; } - break; + path.push_back(c); + return path; + } + if (c.h() < this->h()) { + return std::vector(); } c.next(); if (this->collide(c)) { @@ -170,68 +241,83 @@ ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max) cusp += 1; } } - path.push_back(c); + return std::vector(); +} + +std::vector +ParkingSlot::steer_in_slot(BicycleCar c) +{ + std::vector path; + while (!this->parked(c)) { + path.push_back(c); + c.next(); + if (this->collide(c)) { + c.sp(c.sp() * -1.0); + c.next(); + c.st(c.st() * -1.0); + } + } return path; } PoseRange -ParkingSlot::fe() +ParkingSlot::fe(BicycleCar c) { assert(this->parallel()); assert(this->right()); - BicycleCar c; c.h(this->h()); - double clen = this->offset_ + this->len() - c.df(); - double cw = this->offset_ + c.w() / 2.0; + double clen = -this->offset_ + this->len() - c.df(); + double cw = c.w() / 2.0; c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0)); c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0)); c.set_max_steer(); - c.sp(-0.01); - auto const& b3 = this->border_[3]; - this->curb_.intersects_with(b3, c.len()); + c.sp(this->parking_speed_); + auto const rc = c.rf(); + this->curb_.intersects_with(rc, c.len()); double max_to_slot; auto const& rr = c.rr(); auto const& i1 = this->curb_.i1(); auto const& i2 = this->curb_.i2(); if (rr.edist(i1) < rr.edist(i2)) { - max_to_slot = rr.min_angle_between(b3, i1); + max_to_slot = rr.min_angle_between(rc, i1); } else { - max_to_slot = rr.min_angle_between(b3, i2); + max_to_slot = rr.min_angle_between(rc, i2); } std::vector starts; double a_to_slot = 0.0; while (a_to_slot < max_to_slot) { - a_to_slot += 0.001; - c.rotate(b3, 0.001); + a_to_slot += this->delta_angle_to_slot_; + c.rotate(rc, this->delta_angle_to_slot_); starts.push_back(c); } std::vector> entries; - unsigned int max_cusp = 10; for (auto s: starts) { - auto r = this->drive_in_slot(s, max_cusp); + auto r = this->drive_in_slot(s); if (r.size() > 0) { entries.push_back(r); } } - assert(entries.size() > 0); + if (entries.size() == 0) { + return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0)); + } + if (entries.size() == 1) { + auto f = entries.front().front(); + return PoseRange(f, f); + } auto& c1 = entries.front().front(); auto& c2 = entries.back().front(); - double b = std::min(c1.h(), c2.h()); - double e = std::max(c1.h(), c2.h()); - clen = c.len(); - Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h())); - Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h())); - Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h())); - Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h())); - Line li1(b1, e1); - Line li2(b2, e2); - li1.intersects_with(li2); - PoseRange pr; - pr.x(li1.i1().x()); - pr.y(li1.i1().y()); - pr.b(b); - pr.e(e); - return pr; + return PoseRange(c1, c2); +} + +PoseRange +ParkingSlot::recompute_entry(PoseRange p) +{ + p.rotate(Point(0.0, 0.0), this->h()); + p.translate(this->border_[0]); + if (!this->right()) { + p.reflect(this->entry_); + } + return p; } std::ostream&