Add pose heading method to SlotPlanner

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index e4a61ebdbd88215c56b950be72d809c01b0ed54b..7482dbe86119e1b04e2ead8deae1dcd163a70671 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -68,6 +68,11 @@ SlotType ParallelSlot::slotType()
         return this->slotType_;
 }
 
+float ParallelSlot::poseHeading()
+{
+        return this->poseHeading_;
+}
+
 // setter
 void ParallelSlot::DH(float dh)
 {
@@ -84,9 +89,13 @@ void ParallelSlot::setAll()
         float dy = y3 - y0;
         float dx = x3 - x0;
         this->slotHeading_ = atan2(dy, dx);
-        // slot side
+        // pose heading
         float y1 = this->slot().bnodes()[1]->y();
         float x1 = this->slot().bnodes()[1]->x();
+        dy = y0 - y1;
+        dx = x0 - x1;
+        this->poseHeading_ = atan2(dy, dx);
+        // slot side
         if (sgn((x1 - x0) * (y3 - y0) - (y1 - y0) * (x3 - x0)) < 0)
                 this->slotSide_ = LEFT;
         else

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 9a845d0fbb17f1646b675aea9f6d6445e7a77f19..4fc77de942ce598ad606620aaa70762777822e32 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -51,6 +51,7 @@ class ParallelSlot {
                 float slotHeading_;
                 SlotSide slotSide_;
                 SlotType slotType_;
+                float poseHeading_;
         public:
                 ParallelSlot();
 
@@ -64,6 +65,7 @@ class ParallelSlot {
                 float slotHeading();
                 SlotSide slotSide();
                 SlotType slotType();
+                float poseHeading();
 
                 // setter
                 void DH(float dh);