Add compute entries ut, impl

[hubacji1/bcar.git] / incl / pslot.hh

diff --git a/incl/pslot.hh b/incl/pslot.hh
index c669d23e7e81d653bce5f18558bc373819a4aa6d..51fd7c83ee38dfefaeb307c22b3aabc2e23ea86e 100644 (file)
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 /*! \file */
 #ifndef BCAR_PSLOT_H
 #define BCAR_PSLOT_H
@@ -16,13 +22,18 @@ namespace bcar {
  */
 class ParkingSlot {
 private:
-       double offset_ = 0.001; // to avoid collision during init
-       Point border_[4];
-       Line entry_;
-       Line rear_;
-       Line curb_;
-       Line front_;
+       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;
+       Line _curb;
+       Line _front;
 public:
+       std::vector<std::vector<BicycleCar>> _ispaths;
+       std::vector<BicycleCar> _entries;
        /*! \brief Set parking slot.
 
        \param p Point with `x`, `y` coordinates of entry side's corner.
@@ -67,6 +78,40 @@ 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. */
+       unsigned int get_max_cusp() const;
+       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;
 
@@ -82,15 +127,62 @@ public:
        /*! Return `true` if `c`'s car frame collide with `this` border. */
        bool collide(BicycleCar const& c) const;
 
+       /*! \brief Set car `c` to the start position.
+        *
+        * \param c Bicycle car.
+        */
+       void set_to_start(BicycleCar& c);
+
        /*! \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 Drive car `c` from slot.
+        *
+        * \param c Starting bicycle car.
+        */
+       std::vector<BicycleCar> drive_of_slot(BicycleCar c);
+
+       /*! \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<Pose> steer_in_slot(BicycleCar c);
+
+       /*! \brief Find entry.
+        *
+        * \param c For which `BicycleCar` should entry be found?
+        */
+       PoseRange fe(BicycleCar c);
+
+       /*! \brief Compute entries from `this->_ispaths`.
+        *
+        * The problem with in-slot paths is that car can't leave the parking
+        * slot with full rate to the right (for right sided parking slot.)
+        *
+        * The idea is to move the first cars of in-slot paths (the one that is
+        * computed as entry to the parking slot) with the full rate to the left
+        * until they can leave the parking slot with full rate to the right.
+        *
+        * The computed cars are stored in `this->_entries`.
+        */
+       void compute_entries();
+
+       /*! \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);
 
-       /*! Find entry. */
-       PoseRange fe();
+       /*! Generate output for plotting with gnuplot. */
+       void gen_gnuplot_to(std::ostream& out);
 
        friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
 };