Add sampling info struct

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index bc4ff1b44f863a7d1178afde9d84885444987adc..8b3ae7be5c3380f3bd070e3455efbc88e7f033a2 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -261,3 +261,21 @@ void ParallelSlot::fpose()
                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
         }
 }
+
+struct SamplingInfo ParallelSlot::getSamplingInfo()
+{
+        struct SamplingInfo si;
+        BicycleCar *CC = new BicycleCar(
+                this->fposecenter()->x(),
+                this->fposecenter()->y() - 0.01,
+                M_PI / 2
+        );
+        si.x = this->slot().bnodes()[0]->x();
+        si.y = this->slot().bnodes()[0]->y();
+        si.r = CC->diag_radi();
+        si.h = 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 b1c75aa2929f241be11cb502803c67f272e46370..3cbc044588bcfa6343ba12d9bef329753a5fea81 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -32,6 +32,13 @@ enum SlotType {
         PERPENDICULAR
 };
 
+struct SamplingInfo {
+        float x;
+        float y;
+        float r;
+        float h;
+};
+
 class ParallelSlot {
         private:
                 float DH_ = 0.01;
@@ -68,6 +75,8 @@ class ParallelSlot {
                 );
                 /** Test possible init poses */
                 void fpose();
+                /** Return values to set sampling function of RRT */
+                struct SamplingInfo getSamplingInfo();
 };
 
 #endif