Add setter for slot side, type

diff --git a/base/main.cc b/base/main.cc
index 122ba4174f1776f7e0005c5c7b85e90c05836c72..af76ff3547f42058c8890bbb538ac3478cc5f094 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -167,6 +167,7 @@ int main()
                         xy[1].asFloat()
                 ));
         }
+        ps.setAll();
         p.samplingInfo_ = ps.getSamplingInfo();
         if (ps.slot().bnodes().size() > 0)
                 ps.fpose();

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 3516726ace5bdc55c86c16b4c593f63a3a5b2eb7..4b80a17f876447841cf19c6e41ea6d5312612468 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -53,37 +53,11 @@ float ParallelSlot::slotHeading()
 
 SlotSide ParallelSlot::slotSide()
 {
-        if (!this->slotSide_) {
-                float y0 = this->slot().bnodes()[0]->y();
-                float x0 = this->slot().bnodes()[0]->x();
-                float y1 = this->slot().bnodes()[1]->y();
-                float x1 = this->slot().bnodes()[1]->x();
-                float y3 = this->slot().bnodes()[3]->y();
-                float x3 = this->slot().bnodes()[3]->x();
-                if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
-                        this->slotSide_ = LEFT;
-                else
-                        this->slotSide_ = RIGHT;
-        }
         return this->slotSide_;
 }
 
 SlotType ParallelSlot::slotType()
 {
-        if (!this->slotType_) {
-                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_;
 }
 
@@ -93,6 +67,34 @@ void ParallelSlot::DH(float dh)
         this->DH_ = dh;
 }
 
+void ParallelSlot::setAll()
+{
+        // slot side
+        float y0 = this->slot().bnodes()[0]->y();
+        float x0 = this->slot().bnodes()[0]->x();
+        float y1 = this->slot().bnodes()[1]->y();
+        float x1 = this->slot().bnodes()[1]->x();
+        float y3 = this->slot().bnodes()[3]->y();
+        float x3 = this->slot().bnodes()[3]->x();
+        if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
+                this->slotSide_ = LEFT;
+        else
+                this->slotSide_ = RIGHT;
+        // slot type
+        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;
+}
+
 // other
 void ParallelSlot::fip()
 {

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index ee45122315c55f8052425190a3e9a06f4eb3d24a..0931a2a9341204343144e754ca69590674a32f0b 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -68,6 +68,7 @@ class ParallelSlot {
 
                 // setter
                 void DH(float dh);
+                void setAll();
 
                 // other
                 /** BFS to _Find Init Pose_. */