Add Reeds and Shepp steering procedure

[hubacji1/iamcar.git] / vehicle_platform / cost.cc

diff --git a/vehicle_platform/cost.cc b/vehicle_platform/cost.cc
index 27a1c60894e491ef155b72680f5b63e86868d8b5..81a3edb08415f4f55eb3d9a8f1759a37ee4c86b7 100644 (file)
--- a/vehicle_platform/cost.cc
+++ b/vehicle_platform/cost.cc
@@ -19,6 +19,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define COST_H
 
 #include <cmath>
+#include "reeds_shepp.h"
 #include "rrtnode.h"
 
 float co1(RRTNode *init, RRTNode *goal)
@@ -28,4 +29,12 @@ float co1(RRTNode *init, RRTNode *goal)
         return pow(pow(dx, 2) + pow(dy, 2), 0.5);
 }
 
+float co2(RRTNode *init, RRTNode *goal)
+{
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {goal->x(), goal->y(), goal->h()};
+        ReedsSheppStateSpace rsss(10.82); // TODO const param
+        return static_cast<float>(rsss.distance(q0, q1));
+}
+
 #endif