Add slot type getter

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index dff0fe446349e5e9623b6b3e95672f0ce0d6261e..b030bfa5d104cecfee4433fa9395b3c60a39ed12 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -40,6 +40,25 @@ PolygonObstacle &ParallelSlot::slot()
         return this->slot_;
 }
 
+SlotType ParallelSlot::slotType()
+{
+        if (this->slotType_ == NONE) {
+                float d1 = EDIST(
+                        this->slot().bnodes()[0],
+                        this->slot().bnodes()[1]
+                );
+                float d2 = EDIST(
+                        this->slot().bnodes()[1],
+                        this->slot().bnodes()[2]
+                );
+                if (d1 > d2)
+                        this->slotType_ = PERPENDICULAR;
+                else
+                        this->slotType_ = PARALLEL;
+        }
+        return this->slotType_;
+}
+
 // setter
 void ParallelSlot::DH(float dh)
 {

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 04cb96606c049dc73725a286994a0bb0c748d39e..6703e944a5459f80a3f9a8aac19dc932fc37cd96 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -22,11 +22,23 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #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 SlotType {
+        NONE,
+        PARALLEL,
+        PERPENDICULAR
+};
+
 class ParallelSlot {
         private:
                 float DH_ = 0.01;
                 std::vector<std::vector<RRTNode *>> cusp_;
                 PolygonObstacle slot_;
+                SlotType slotType_;
         public:
                 ParallelSlot();
 
@@ -34,6 +46,7 @@ class ParallelSlot {
                 std::vector<std::vector<RRTNode *>> &cusp();
                 float DH() const;
                 PolygonObstacle &slot();
+               SlotType slotType();
 
                 // setter
                 void DH(float dh);