From b1ccf604491ddc27c189a42dbf2fe0aad405b1ed Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Mon, 2 Jul 2018 15:01:21 +0200
Subject: [PATCH] Add alternative steering procedure

---
 incl/steer.h              |  4 ++++
 vehicle_platform/steer.cc | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/incl/steer.h b/incl/steer.h
index deb23c9..d9a0a3f 100644
--- a/incl/steer.h
+++ b/incl/steer.h
@@ -25,4 +25,8 @@ std::vector<RRTNode *> st1(
                 RRTNode *init,
                 RRTNode *goal);
 
+std::vector<RRTNode *> st2(
+                RRTNode *init,
+                RRTNode *goal);
+
 #endif
diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 03ff31e..e1e4120 100644
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -18,6 +18,10 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <cmath>
 #include "steer.h"
 
+#define ST2WHEELBASE 2.450
+#define ST2TURNINGRADI 10.82
+#define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
+
 std::vector<RRTNode *> st1(
                 RRTNode *init,
                 RRTNode *goal)
@@ -37,3 +41,32 @@ std::vector<RRTNode *> st1(
         }
         return nodes;
 }
+
+std::vector<RRTNode *> st2(
+                RRTNode *init,
+                RRTNode *goal)
+{
+        float speed = 1;
+        float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
+        float sa = angl - init->h(); // steering angle
+        float co = cos(angl - init->h());
+        float si = sin(angl - init->h());
+        if (co < 0) {
+                speed = -speed;
+                if (si > 0)
+                        sa = -sa;
+        } else {
+                if (si < 0)
+                        sa = -sa;
+        }
+        if (sa > ST2MAXSTEERING)
+                sa = ST2MAXSTEERING;
+        if (sa < -ST2MAXSTEERING)
+                sa = -ST2MAXSTEERING;
+        float new_h = init->h() + (speed / ST2WHEELBASE) * tan(sa);
+        float new_x = init->x() + speed * cos(new_h);
+        float new_y = init->y() + speed * sin(new_h);
+        std::vector<RRTNode *> nodes;
+        nodes.push_back(new RRTNode(new_x, new_y, new_h));
+        return nodes;
+}
-- 
2.39.2