Add getter for final pose used in fipr

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index b0131fdfd68c560c5f26a5e9d24c4b5ca67a905f..56f5ba07a0dc7117de5fba4b9eaf681ae3d26dc5 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -378,6 +378,25 @@ void ParallelSlot::fpose()
         }
 }
 
+BicycleCar *ParallelSlot::getFP()
+{
+        float x = this->slot().bnodes()[3]->x();
+        float y = this->slot().bnodes()[3]->y();
+        float h = this->slotHeading();
+        float nx;
+        float ny;
+        if (this->slotSide() == LEFT) {
+                nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
+                ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
+        } else {
+                nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
+                ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
+        }
+        x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
+        y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
+        return new BicycleCar(x, y, h);
+}
+
 bool ParallelSlot::isInside(BicycleCar *c)
 {
         bool inside = true;

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 8bc80ee11cb46b8fbe6fc878247858fe11b731cc..0be18453da67e660b9111e9c9d3f731626856dda 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -93,6 +93,8 @@ class ParallelSlot {
                 );
                 /** Test possible init poses */
                 void fpose();
+                /** Return start pose for fipr method */
+                BicycleCar *getFP();
                 /** Return true if car is inside slot */
                 bool isInside(BicycleCar *c);
                 /** Return values to set sampling function of RRT */