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[3], 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         return 0;
  95 }
  96
  97 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
  98 {
  99         std::vector<RRTNode *> nodes;
 100         double q0[] = {init->x(), init->y(), init->h()};
 101         double q1[] = {goal->x(), goal->y(), goal->h()};
 102         ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
 103         rsss.sample(q0, q1, ST3STEP, cb_st3, &nodes);
 104         return nodes;
 105 }
 106
 107 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
 108 {
 109         std::vector<RRTNode *> nodes;
 110         BicycleCar bc(init->x(), init->y(), init->h());
 111
 112         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
 113         float co = cos(angl - init->h());
 114         //float si = sin(angl - init->h());
 115
 116         float speed = ST4SPEED; // desired
 117         float gx = goal->x();
 118         float gy = goal->y();
 119         if (co < 0)
 120                 speed = -speed;
 121         float cerr = 0;
 122         float lerr = 0;
 123         float rx = 0; // recomputed goal x
 124         float ry = 0; // recomputed goal y
 125         float r = 0;
 126         int i = 0;
 127         for (i = 0; i < ST4COUNT; i++) {
 128                 // speed controller
 129                 cerr = speed - bc.speed();
 130                 bc.speed(ST4KP * cerr + ST4KI * lerr);
 131                 lerr += cerr;
 132                 // steer controller
 133                 rx = (gx - bc.x()) * cos(-bc.h()) -
 134                         (gy - bc.y()) * sin(-bc.h());
 135                 ry = (gx - bc.x()) * sin(-bc.h()) -
 136                         (gy - bc.y()) * cos(-bc.h());
 137                 if (ry != 0) {
 138                         r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
 139                         bc.steer(1 / r);
 140                 }
 141                 // next iteration
 142                 bc.next();
 143                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
 144         }
 145         return nodes;
 146 }