Add and use parking slot parking parameters

diff --git a/incl/pslot.hh b/incl/pslot.hh
index c45e3806d0eded14e9839418280f73979c3fb12e..8602e891beae8654ed3eac63098bf8aba604f829 100644 (file)
--- 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,15 @@ public:
        /*! Return parking slot's orientation. */
        double h() 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,9 +97,8 @@ 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<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
+       std::vector<BicycleCar> drive_in_slot(BicycleCar c);
 
        /*! \brief Steer car `c` into the parking slot `this`.
         *
@@ -101,9 +112,8 @@ public:
        /*! \brief Find entry.
         *
         * \param c For which `BicycleCar` should entry be found?
-        * \param max Maximum number of backward-forward direction changes.
         */
-       PoseRange fe(BicycleCar c, unsigned int& max);
+       PoseRange fe(BicycleCar c);
 
        /*! \brief Recompute zero slot's `PoseRange` entry for `this`.
         *

diff --git a/src/pslot.cc b/src/pslot.cc
index ec6462776b71842d65313b7c107e2c250db140e6..5f25efd52d703256bf8ad632c3756381e9f674e1 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -95,6 +95,24 @@ ParkingSlot::h() const
        return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
 }
 
+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
 {
@@ -145,20 +163,20 @@ ParkingSlot::collide(BicycleCar const& c) const
 }
 
 std::vector<BicycleCar>
-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<BicycleCar> path;
-       path.reserve(max + 2);
+       path.reserve(this->max_cusp_ + 2);
        path.push_back(c);
        unsigned int cusp = 0;
-       while (cusp < max + 1) {
+       while (cusp < this->max_cusp_ + 1) {
                if (this->parked(c)) {
-                       if (cusp < max) {
-                               max = cusp;
+                       if (cusp < this->max_cusp_) {
+                               this->max_cusp_ = cusp;
                        }
                        path.push_back(c);
                        return path;
@@ -196,7 +214,7 @@ ParkingSlot::steer_in_slot(BicycleCar c)
 }
 
 PoseRange
-ParkingSlot::fe(BicycleCar c, unsigned int& max)
+ParkingSlot::fe(BicycleCar c)
 {
        assert(this->parallel());
        assert(this->right());
@@ -206,7 +224,7 @@ ParkingSlot::fe(BicycleCar c, unsigned int& max)
        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.001);
+       c.sp(this->parking_speed_);
        auto const rc = c.rf();
        this->curb_.intersects_with(rc, c.len());
        double max_to_slot;
@@ -221,13 +239,13 @@ ParkingSlot::fe(BicycleCar c, unsigned int& max)
        std::vector<BicycleCar> starts;
        double a_to_slot = 0.0;
        while (a_to_slot < max_to_slot) {
-               a_to_slot += 0.0001;
-               c.rotate(rc, 0.0001);
+               a_to_slot += this->delta_angle_to_slot_;
+               c.rotate(rc, this->delta_angle_to_slot_);
                starts.push_back(c);
        }
        std::vector<std::vector<BicycleCar>> entries;
        for (auto s: starts) {
-               auto r = this->drive_in_slot(s, max);
+               auto r = this->drive_in_slot(s);
                if (r.size() > 0) {
                        entries.push_back(r);
                }