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 10
  27 #define ST2STEP 0.2
  28
  29 #define ST3TURNINGRADIUS 10.82
  30 #define ST3STEP 0.2
  31
  32 #define ST4COUNT 10
  33 #define ST4SPEED 0.2 // aka STEP
  34 #define ST4KP 1
  35 #define ST4KI 0
  36 #define ST4KD 0
  37
  38 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
  39 {
  40         float angl;
  41         float new_x;
  42         float new_y;
  43         float new_h;
  44         std::vector<RRTNode *> nodes;
  45         int i;
  46         for (i = 1; i < ST1COUNT + 1; i++) {
  47                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  48                 new_x = init->x() + i * ST1STEP * cos(angl);
  49                 new_y = init->y() + i * ST1STEP * sin(angl);
  50                 new_h = goal->h();
  51                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  52         }
  53         return nodes;
  54 }
  55
  56 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
  57 {
  58         std::vector<RRTNode *> nodes;
  59         BicycleCar bc(init->x(), init->y(), init->h());
  60
  61         float speed = 0;
  62         float angl = 0;
  63         float steer = 0;
  64         float co = 0;
  65         float si = 0;
  66         int i = 0;
  67         for (i = 0; i < ST2COUNT; i++) {
  68                 speed = ST2STEP;
  69                 angl = atan2(goal->y() - bc.y(), goal->x() - bc.x());
  70                 steer = angl - bc.h();
  71                 co = cos(angl - bc.h());
  72                 si = sin(angl - bc.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                 bc.speed(speed);
  82                 bc.steer(steer);
  83                 // next iteration
  84                 bc.next();
  85                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
  86         }
  87         return nodes;
  88 }
  89
  90 int cb_st3(double q[4], void *user_data)
  91 {
  92         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
  93         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
  94         nodes->back()->s(q[3]);
  95         return 0;
  96 }
  97
  98 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
  99 {
 100         return st3(init, goal, ST3STEP);
 101 }
 102
 103 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step)
 104 {
 105         std::vector<RRTNode *> nodes;
 106         double q0[] = {init->x(), init->y(), init->h()};
 107         double q1[] = {goal->x(), goal->y(), goal->h()};
 108         ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
 109         rsss.sample(q0, q1, step, cb_st3, &nodes);
 110         return nodes;
 111 }
 112
 113 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
 114 {
 115         std::vector<RRTNode *> nodes;
 116         BicycleCar bc(init->x(), init->y(), init->h());
 117
 118         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
 119         float co = cos(angl - init->h());
 120         //float si = sin(angl - init->h());
 121
 122         float speed = ST4SPEED; // desired
 123         float gx = goal->x();
 124         float gy = goal->y();
 125         if (co < 0)
 126                 speed = -speed;
 127         float cerr = 0;
 128         float lerr = 0;
 129         float rx = 0; // recomputed goal x
 130         float ry = 0; // recomputed goal y
 131         float r = 0;
 132         int i = 0;
 133         for (i = 0; i < ST4COUNT; i++) {
 134                 // speed controller
 135                 cerr = speed - bc.speed();
 136                 bc.speed(ST4KP * cerr + ST4KI * lerr);
 137                 lerr += cerr;
 138                 // steer controller
 139                 rx = (gx - bc.x()) * cos(-bc.h()) -
 140                         (gy - bc.y()) * sin(-bc.h());
 141                 ry = (gx - bc.x()) * sin(-bc.h()) -
 142                         (gy - bc.y()) * cos(-bc.h());
 143                 if (ry != 0) {
 144                         r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
 145                         bc.steer(1 / r);
 146                 }
 147                 // next iteration
 148                 bc.next();
 149                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
 150         }
 151         return nodes;
 152 }