Update changelog

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

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 04e05816480810cda6e9d2d4ffa19297b469d519..d89a06e68d2517ef25262b3c57e157e2d8e42137 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -15,8 +15,10 @@ You should have received a copy of the GNU General Public License
 along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include <algorithm>
 #include <cmath>
 #include "bcar.h"
+#include "dubins.h"
 #include "reeds_shepp.h"
 #include "steer.h"
 
@@ -30,10 +32,14 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define ST3STEP 0.2
 
 #define ST4COUNT 10
+#define ST4SPEED 0.2 // aka STEP
 #define ST4KP 1
 #define ST4KI 0
 #define ST4KD 0
 
+#define ST5TURNINGRADIUS 10.82
+#define ST5STEP 0.2
+
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
 {
         float angl;
@@ -86,20 +92,26 @@ std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
         return nodes;
 }
 
-int cb_st3(double q[3], void *user_data)
+int cb_st3(double q[4], void *user_data)
 {
         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+        nodes->back()->s(q[3]);
         return 0;
 }
 
 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
+{
+        return st3(init, goal, ST3STEP);
+}
+
+std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step)
 {
         std::vector<RRTNode *> nodes;
         double q0[] = {init->x(), init->y(), init->h()};
         double q1[] = {goal->x(), goal->y(), goal->h()};
         ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
-        rsss.sample(q0, q1, ST3STEP, cb_st3, &nodes);
+        rsss.sample(q0, q1, step, cb_st3, &nodes);
         return nodes;
 }
 
@@ -112,7 +124,7 @@ std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
         float co = cos(angl - init->h());
         //float si = sin(angl - init->h());
 
-        float speed = 1; // desired
+        float speed = ST4SPEED; // desired
         float gx = goal->x();
         float gy = goal->y();
         if (co < 0)
@@ -143,3 +155,40 @@ std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
         }
         return nodes;
 }
+
+int cb_st5(double q[3], double t, void *user_data)
+{
+        std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
+        nodes->push_back(new RRTNode(q[0], q[1], q[2]));
+        return 0;
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
+{
+        if (init->inFront(goal))
+                return st5(init, goal, ST5STEP, false);
+        return st5(init, goal, ST5STEP, true);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw)
+{
+        return st5(init, goal, ST5STEP, bw);
+}
+
+std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw)
+{
+        std::vector<RRTNode *> nodes;
+        BicycleCar *bc = new BicycleCar(init->x(), init->y(), init->h());
+        RRTNode *dg = bc->drivableNode(goal); // drivable goal
+        double q0[] = {init->x(), init->y(), init->h()};
+        double q1[] = {dg->x(), dg->y(), dg->h()};
+        DubinsPath path;
+        if (bw)
+                dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);
+        else
+                dubins_shortest_path(&path, q0, q1, ST5TURNINGRADIUS);
+        dubins_path_sample_many(&path, step, cb_st5, &nodes);
+        if (bw)
+                std::reverse(nodes.begin(), nodes.end());
+        return nodes;
+}