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 <algorithm>
  19 #include <cmath>
  20 #include "bcar.h"
  21 #include "dubins.h"
  22 #include "reeds_shepp.h"
  23 #include "steer.h"
  24
  25 #define ST1COUNT 10
  26 #define ST1STEP 0.2
  27
  28 #define ST2COUNT 10
  29 #define ST2STEP 0.2
  30
  31 #define ST3TURNINGRADIUS 10.82
  32 #define ST3STEP 0.2
  33
  34 #define ST4COUNT 10
  35 #define ST4SPEED 0.2 // aka STEP
  36 #define ST4KP 1
  37 #define ST4KI 0
  38 #define ST4KD 0
  39
  40 #define ST5TURNINGRADIUS 10.82
  41 #define ST5STEP 0.2
  42
  43 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
  44 {
  45         float angl;
  46         float new_x;
  47         float new_y;
  48         float new_h;
  49         std::vector<RRTNode *> nodes;
  50         int i;
  51         for (i = 1; i < ST1COUNT + 1; i++) {
  52                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  53                 new_x = init->x() + i * ST1STEP * cos(angl);
  54                 new_y = init->y() + i * ST1STEP * sin(angl);
  55                 new_h = goal->h();
  56                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  57         }
  58         return nodes;
  59 }
  60
  61 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
  62 {
  63         std::vector<RRTNode *> nodes;
  64         BicycleCar bc(init->x(), init->y(), init->h());
  65
  66         float speed = 0;
  67         float angl = 0;
  68         float steer = 0;
  69         float co = 0;
  70         float si = 0;
  71         int i = 0;
  72         for (i = 0; i < ST2COUNT; i++) {
  73                 speed = ST2STEP;
  74                 angl = atan2(goal->y() - bc.y(), goal->x() - bc.x());
  75                 steer = angl - bc.h();
  76                 co = cos(angl - bc.h());
  77                 si = sin(angl - bc.h());
  78                 if (co < 0) {
  79                         speed = -speed;
  80                         if (si > 0)
  81                                 steer = -steer;
  82                 } else {
  83                         if (si < 0)
  84                                 steer = -steer;
  85                 }
  86                 bc.speed(speed);
  87                 bc.steer(steer);
  88                 // next iteration
  89                 bc.next();
  90                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
  91         }
  92         return nodes;
  93 }
  94
  95 int cb_st3(double q[4], void *user_data)
  96 {
  97         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
  98         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
  99         nodes->back()->s(q[3]);
 100         return 0;
 101 }
 102
 103 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
 104 {
 105         return st3(init, goal, ST3STEP);
 106 }
 107
 108 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal, float step)
 109 {
 110         std::vector<RRTNode *> nodes;
 111         double q0[] = {init->x(), init->y(), init->h()};
 112         double q1[] = {goal->x(), goal->y(), goal->h()};
 113         ReedsSheppStateSpace rsss(ST3TURNINGRADIUS);
 114         rsss.sample(q0, q1, step, cb_st3, &nodes);
 115         return nodes;
 116 }
 117
 118 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
 119 {
 120         std::vector<RRTNode *> nodes;
 121         BicycleCar bc(init->x(), init->y(), init->h());
 122
 123         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
 124         float co = cos(angl - init->h());
 125         //float si = sin(angl - init->h());
 126
 127         float speed = ST4SPEED; // desired
 128         float gx = goal->x();
 129         float gy = goal->y();
 130         if (co < 0)
 131                 speed = -speed;
 132         float cerr = 0;
 133         float lerr = 0;
 134         float rx = 0; // recomputed goal x
 135         float ry = 0; // recomputed goal y
 136         float r = 0;
 137         int i = 0;
 138         for (i = 0; i < ST4COUNT; i++) {
 139                 // speed controller
 140                 cerr = speed - bc.speed();
 141                 bc.speed(ST4KP * cerr + ST4KI * lerr);
 142                 lerr += cerr;
 143                 // steer controller
 144                 rx = (gx - bc.x()) * cos(-bc.h()) -
 145                         (gy - bc.y()) * sin(-bc.h());
 146                 ry = (gx - bc.x()) * sin(-bc.h()) -
 147                         (gy - bc.y()) * cos(-bc.h());
 148                 if (ry != 0) {
 149                         r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
 150                         bc.steer(1 / r);
 151                 }
 152                 // next iteration
 153                 bc.next();
 154                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
 155         }
 156         return nodes;
 157 }
 158
 159 int cb_st5(double q[3], double t, void *user_data)
 160 {
 161         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
 162         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
 163         return 0;
 164 }
 165
 166 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal)
 167 {
 168         if (init->inFront(goal))
 169                 return st5(init, goal, ST5STEP, false);
 170         return st5(init, goal, ST5STEP, true);
 171 }
 172
 173 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, bool bw)
 174 {
 175         return st5(init, goal, ST5STEP, bw);
 176 }
 177
 178 std::vector<RRTNode *> st5(RRTNode *init, RRTNode *goal, float step, bool bw)
 179 {
 180         std::vector<RRTNode *> nodes;
 181         double q0[] = {init->x(), init->y(), init->h()};
 182         double q1[] = {goal->x(), goal->y(), goal->h()};
 183         DubinsPath path;
 184         if (bw)
 185                 dubins_shortest_path(&path, q1, q0, ST5TURNINGRADIUS);
 186         else
 187                 dubins_shortest_path(&path, q0, q1, ST5TURNINGRADIUS);
 188         dubins_path_sample_many(&path, step, cb_st5, &nodes);
 189         if (bw)
 190                 std::reverse(nodes.begin(), nodes.end());
 191         return nodes;
 192 }