Add alternative steering procedure

diff --git a/incl/steer.h b/incl/steer.h
index deb23c94cfa48ef4196a3736c54f7d15a14275ae..d9a0a3f14d23cba478b04b2ede2cd6da6abc9f03 100644 (file)
--- 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 03ff31e3d5f13f3ae539216a29b2692f5c2ab8c0..e1e4120f55000041345da16b155a771bfea19d73 100644 (file)
--- 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;
+}