-#ifndef PSLOT_H
-#define PSLOT_H
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
-#include <ostream>
+/*! \file */
+#ifndef BCAR_PSLOT_H
+#define BCAR_PSLOT_H
-/*! \brief Parking slot basic class.
+#include <ostream>
+#include <vector>
+#include "bcar.hh"
-This class contains some geometrical computations of parking slot. Parking slot
-consists of 4 cartesian coordinates `border` representing the border of the
-parking slot.
+namespace bcar {
-\param border Array of 4 `x`, `y` values - the borderd of the parking slot.
-*/
+/*! \brief Parking slot basic class.
+ *
+ * This class contains some geometrical computations of parking slot. Parking
+ * slot consists of 4 cartesian coordinates `border` representing the border of
+ * the parking slot.
+ */
class ParkingSlot {
private:
- double border_[4][2] = {
- {0, 0},
- {1, 0},
- {1, 2},
- {0, 2}
- };
+ 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:
- /*! \brief Reverse order of border coordinates.
+ 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.
+ \param h Direction of the entry side.
+ \param W The width of the slot.
+ \param L The length of the slot.
*/
- void reverse_border();
+ ParkingSlot(Point p, double h, double W, double L);
+ ParkingSlot(double lrx, double lry, double rrx, double rry, double rfx,
+ double rfy, double lfx, double lfy);
- // slot info
- /*! \brief Return orientation of the parking slot.
+ /*! Get slot's length. */
+ double len() const;
- The orientation of the parking slot is computed as the
- direction from the first to the last border coordinates.
- */
- double heading() const;
- /*! \brief Return `true` if slot is parallel.
+ /*! Get slot's width. */
+ double w() const;
- There are two slot types - parallel and perpendicular.
- */
+ /*! Get slot's left front x coordinate. */
+ double lfx() const;
+
+ /*! Get slot's left front y coordinate. */
+ double lfy() const;
+
+ /*! Get slot's left rear x coordinate. */
+ double lrx() const;
+
+ /*! Get slot's left rear y coordinate. */
+ double lry() const;
+
+ /*! Get slot's right rear x coordinate. */
+ double rrx() const;
+
+ /*! Get slot's right rear y coordinate. */
+ double rry() const;
+
+ /*! Get slot's right front x coordinate. */
+ double rfx() const;
+
+ /*! Get slot's right front y coordinate. */
+ double rfy() const;
+
+ /*! 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;
- /*! \brief Return `true` if slot is on the right.
- The slot could be on right or the left side.
- */
+ /*! Return `true` for the parking slot on the right side. */
bool right() const;
- // getters, setters
- double x1() const { return this->border_[0][0]; }
- double y1() const { return this->border_[0][1]; }
- double x2() const { return this->border_[1][0]; }
- double y2() const { return this->border_[1][1]; }
- double x3() const { return this->border_[2][0]; }
- double y3() const { return this->border_[2][1]; }
- double x4() const { return this->border_[3][0]; }
- double y4() const { return this->border_[3][1]; }
- /*! \brief Set parking slot border.
-
- \param x1 First `x` coordinate.
- \param y1 First `y` coordinate.
- \param x2 Second `x` coordinate.
- \param y2 Second `y` coordinate.
- \param x3 Third `x` coordinate.
- \param y3 Third `y` coordinate.
- \param x4 The last (fourth) `x` coordinate.
- \param y4 The last (fourth) `y` coordinate.
- */
- void border(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3,
- double x4, double y4
- ) {
- this->border_[0][0] = x1;
- this->border_[0][1] = y1;
- this->border_[1][0] = x2;
- this->border_[1][1] = y2;
- this->border_[2][0] = x3;
- this->border_[2][1] = y3;
- this->border_[3][0] = x4;
- this->border_[3][1] = y4;
- };
- /*! \brief Set parking slot.
+ /*! Change side of the parking slot. */
+ void swap_side();
- \param x Horizontal coordinate of entry side center.
- \param y Vertical coordinate of entry side center.
- \param h The heading towards slot.
- \param w The width of the slot.
- \param l The length of the slot.
- */
- void set_slot(
- double x,
- double y,
- double h,
- double w,
- double l
- );
-
- ParkingSlot();
- friend std::ostream &operator<<(
- std::ostream &out,
- const ParkingSlot &ps
- )
- {
- out << "[[" << ps.x1() << "," << ps.y1() << "]";
- out << ",[" << ps.x2() << "," << ps.y2() << "]";
- out << ",[" << ps.x3() << "," << ps.y3() << "]";
- out << ",[" << ps.x4() << "," << ps.y4() << "]";
- out << "]";
- return out;
- }
-};
+ /*! Return `true` if car `c` is parking in slot `this`. */
+ bool parked(BicycleCar const& c) const;
-template <typename T> int sgn(T val) {
- return (T(0) < val) - (val < T(0));
-}
+ /*! 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.
+ */
+ 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);
+
+ /*! Generate output for plotting with gnuplot. */
+ void gen_gnuplot_to(std::ostream& out);
+
+ friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
+};
-#endif /* PSLOT_H */
+} // namespace bcar
+#endif /* BCAR_PSLOT_H */
projects / hubacji1 / bcar.git / blobdiff