Add sampling info struct

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b030bfa5d104cecfee4433fa9395b3c60a39ed12..f712d057568a5ef597989028e51f60070e6c690d 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -261,3 +261,16 @@ void ParallelSlot::fpose()
                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
         }
 }
+
+struct SamplingInfo ParallelSlot::getSamplingInfo()
+{
+        struct SamplingInfo si;
+        si.x = this->slot().bnodes()[0]->x();
+        si.y = this->slot().bnodes()[0]->y();
+        si.r = BCAR_LENGTH;
+        si.h = 3 * M_PI / 2 - acos(EDIST( // TODO generalize
+                this->slot().bnodes()[0],
+                this->slot().bnodes()[1]
+        ) / BCAR_LENGTH);
+        return si;
+}

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 6703e944a5459f80a3f9a8aac19dc932fc37cd96..7d08e98e6e702d8f8a0f7c2a07677859f5155bbf 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -33,6 +33,13 @@ enum SlotType {
         PERPENDICULAR
 };
 
+struct SamplingInfo {
+        float x;
+        float y;
+        float r;
+        float h;
+};
+
 class ParallelSlot {
         private:
                 float DH_ = 0.01;
@@ -69,6 +76,8 @@ class ParallelSlot {
                 );
                 /** Test possible init poses */
                 void fpose();
+                /** Return values to set sampling function of RRT */
+                struct SamplingInfo getSamplingInfo();
 };
 
 #endif