#define SLOTPLANNER_H
#include <vector>
+#include "aux.h"
#include "bcar.h"
#include "obstacle.h"
-#define EDIST(a, b) ({ __typeof__ (a) _a = (a); \
- __typeof__ (b) _b = (b); \
- pow(pow((_b)->x() - (_a)->x(), 2) + \
- pow((_b)->y() - (_a)->y(), 2), 0.5); })
-
enum SlotSide {
LEFT,
RIGHT
};
struct SamplingInfo {
+ float x0;
+ float y0;
+ float h0;
float x;
float y;
- float r;
float h;
};
private:
float DH_ = 0.01;
std::vector<std::vector<RRTNode *>> cusp_;
+ std::vector<RRTNode *> goals_;
PolygonObstacle slot_;
+ float slotHeading_;
SlotSide slotSide_;
SlotType slotType_;
+ float poseHeading_;
public:
ParallelSlot();
// getter
+ /** Get slot entry point */
+ std::vector<RRTNode *> &goals();
+ RRTNode *getMidd();
std::vector<std::vector<RRTNode *>> &cusp();
float DH() const;
PolygonObstacle &slot();
float slotHeading();
SlotSide slotSide();
SlotType slotType();
+ float poseHeading();
// setter
void DH(float dh);
+ void setAll();
// other
/** BFS to _Find Init Pose_. */
- void fip();
+ void fip(
+ std::vector<CircleObstacle>& co,
+ std::vector<SegmentObstacle>& so
+ );
+ void fipf(
+ std::vector<CircleObstacle>& co,
+ std::vector<SegmentObstacle>& so
+ ); // perpendicular forward parking
+ /** _Find Last Not Colliding_ BicycleCar pose
+
+ @param B Find from?
+ */
+ BicycleCar *flnc(
+ BicycleCar *B,
+ std::vector<CircleObstacle>& co,
+ std::vector<SegmentObstacle>& so
+ );
/** _Find Init Pose by Reverse_ approach, see Vorobieva2015
@param B Last pose of vehicle when it is parked.
*/
+ void fipr(RRTNode *n);
void fipr(BicycleCar *B);
/** _Find Last Not Colliding for Reverse_ BicycleCar pose
@param B Find from?
*/
BicycleCar *flncr(BicycleCar *B);
- /** _Find Last Not Colliding_ BicycleCar pose
-
- @param B Find from?
- */
- BicycleCar *flnc(BicycleCar *B);
RRTNode *fposecenter();
/** Recursive function to find out the moves */
bool flast(
);
/** Test possible init poses */
void fpose();
+ /** Return start pose for fip method */
+ BicycleCar *getEP();
+ /** Return center of rotation for start pose for fip method */
+ BicycleCar *getEPC();
+ /** Return start pose for fipr method */
+ BicycleCar *getFP();
+ BicycleCar *getFPf(); // parked forward, only for perpendicular
+ /** In slot perpendicular pose getter
+
+ This method returns a pose of perpendicular parking slot from
+ where it is possible to get out of the slot with full steer to
+ farther corner side (i.e. right corner for the RIGHT side).
+
+ @param B The pose to start from.
+ */
+ BicycleCar *getISPP(BicycleCar *B);
+ BicycleCar *getISPPf(BicycleCar *B); // perp. forward parking
/** Return true if car is inside slot */
bool isInside(BicycleCar *c);
/** Return values to set sampling function of RRT */