From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 4 Mar 2019 18:05:46 +0000 (+0100)
Subject: Add sampling info struct
X-Git-Tag: feature/e-rs-comparison~12
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/2e5469bbf77de77adbd40f3e5803325edf134a23

Add sampling info struct
---

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b030bfa..f712d05 100644
--- 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 6703e94..7d08e98 100644
--- 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