vehicle_platform/steer.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "bcar.h"
  20 #include "reeds_shepp.h"
  21 #include "steer.h"
  22
  23 #define ST1COUNT 10
  24 #define ST1STEP 0.2
  25
  26 #define ST2COUNT 1
  27 #define ST2STEP 1
  28 #define ST2WHEELBASE 2.450
  29 #define ST2TURNINGRADI 10.82
  30 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
  31
  32 #define ST4COUNT 10
  33 #define ST4KP 1
  34 #define ST4KI 0
  35 #define ST4KD 0
  36
  37 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
  38 {
  39         float angl;
  40         float new_x;
  41         float new_y;
  42         float new_h;
  43         std::vector<RRTNode *> nodes;
  44         int i;
  45         for (i = 1; i < ST1COUNT + 1; i++) {
  46                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  47                 new_x = init->x() + i * ST1STEP * cos(angl);
  48                 new_y = init->y() + i * ST1STEP * sin(angl);
  49                 new_h = goal->h();
  50                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  51         }
  52         return nodes;
  53 }
  54
  55 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
  56 {
  57         float speed;
  58         float angl;
  59         float steer; // steering angle
  60         float co;
  61         float si;
  62         float new_h;
  63         float new_x;
  64         float new_y;
  65         std::vector<RRTNode *> nodes;
  66         int i;
  67         for (i = 1; i < ST2COUNT + 1; i++) {
  68                 speed = ST2STEP;
  69                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  70                 steer = angl - init->h(); // steering angle
  71                 co = cos(angl - init->h());
  72                 si = sin(angl - init->h());
  73                 if (co < 0) {
  74                         speed = -speed;
  75                         if (si > 0)
  76                                 steer = -steer;
  77                 } else {
  78                         if (si < 0)
  79                                 steer = -steer;
  80                 }
  81                 if (steer > ST2MAXSTEERING)
  82                         steer = ST2MAXSTEERING;
  83                 if (steer < -ST2MAXSTEERING)
  84                         steer = -ST2MAXSTEERING;
  85                 new_h = init->h() + (speed / ST2WHEELBASE) * tan(steer);
  86                 new_x = init->x() + speed * cos(new_h);
  87                 new_y = init->y() + speed * sin(new_h);
  88                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  89         }
  90         return nodes;
  91 }
  92
  93 int cb_st3(double q[3], void *user_data)
  94 {
  95         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
  96         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
  97         return 0;
  98 }
  99
 100 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
 101 {
 102         std::vector<RRTNode *> nodes;
 103         double q0[] = {init->x(), init->y(), init->h()};
 104         double q1[] = {goal->x(), goal->y(), goal->h()};
 105         ReedsSheppStateSpace rsss(10.82); // TODO const param
 106         rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
 107         return nodes;
 108 }
 109
 110 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
 111 {
 112         std::vector<RRTNode *> nodes;
 113         BicycleCar bc(init->x(), init->y(), init->h());
 114
 115         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
 116         float co = cos(angl - init->h());
 117         //float si = sin(angl - init->h());
 118
 119         float speed = 1; // desired
 120         float gx = goal->x();
 121         float gy = goal->y();
 122         if (co < 0)
 123                 speed = -speed;
 124         float cerr = 0;
 125         float lerr = 0;
 126         float rx = 0; // recomputed goal x
 127         float ry = 0; // recomputed goal y
 128         float r = 0;
 129         int i = 0;
 130         for (i = 0; i < ST4COUNT; i++) {
 131                 // speed controller
 132                 cerr = speed - bc.speed();
 133                 bc.speed(ST4KP * cerr + ST4KI * lerr);
 134                 lerr += cerr;
 135                 // steer controller
 136                 rx = (gx - bc.x()) * cos(-bc.h()) -
 137                         (gy - bc.y()) * sin(-bc.h());
 138                 ry = (gx - bc.x()) * sin(-bc.h()) -
 139                         (gy - bc.y()) * cos(-bc.h());
 140                 if (ry != 0) {
 141                         r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
 142                         bc.steer(1 / r);
 143                 }
 144                 // next iteration
 145                 bc.next();
 146                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
 147         }
 148         return nodes;
 149 }