Add and use parking slot parking parameters

[hubacji1/bcar.git] / src / pslot.cc

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);
                }