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 #define ST2WHEELBASE 2.450
  26 #define ST2TURNINGRADI 10.82
  27 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
  28 #define ST4COUNT 10
  29 #define ST4KP 1
  30 #define ST4KI 0
  31 #define ST4KD 0
  32
  33 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
  34 {
  35         float angl;
  36         float new_x;
  37         float new_y;
  38         float new_h;
  39         std::vector<RRTNode *> nodes;
  40         int i;
  41         for (i = 1; i < ST1COUNT + 1; i++) {
  42                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  43                 new_x = init->x() + i * ST1STEP * cos(angl);
  44                 new_y = init->y() + i * ST1STEP * sin(angl);
  45                 new_h = goal->h();
  46                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  47         }
  48         return nodes;
  49 }
  50
  51 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
  52 {
  53         float speed = 1;
  54         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  55         float sa = angl - init->h(); // steering angle
  56         float co = cos(angl - init->h());
  57         float si = sin(angl - init->h());
  58         if (co < 0) {
  59                 speed = -speed;
  60                 if (si > 0)
  61                         sa = -sa;
  62         } else {
  63                 if (si < 0)
  64                         sa = -sa;
  65         }
  66         if (sa > ST2MAXSTEERING)
  67                 sa = ST2MAXSTEERING;
  68         if (sa < -ST2MAXSTEERING)
  69                 sa = -ST2MAXSTEERING;
  70         float new_h = init->h() + (speed / ST2WHEELBASE) * tan(sa);
  71         float new_x = init->x() + speed * cos(new_h);
  72         float new_y = init->y() + speed * sin(new_h);
  73         std::vector<RRTNode *> nodes;
  74         nodes.push_back(new RRTNode(new_x, new_y, new_h));
  75         return nodes;
  76 }
  77
  78 int cb_st3(double q[3], void *user_data)
  79 {
  80         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
  81         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
  82         return 0;
  83 }
  84
  85 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
  86 {
  87         std::vector<RRTNode *> nodes;
  88         double q0[] = {init->x(), init->y(), init->h()};
  89         double q1[] = {goal->x(), goal->y(), goal->h()};
  90         ReedsSheppStateSpace rsss(10.82); // TODO const param
  91         rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
  92         return nodes;
  93 }
  94
  95 std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
  96 {
  97         std::vector<RRTNode *> nodes;
  98         BicycleCar bc(init->x(), init->y(), init->h());
  99
 100         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
 101         float co = cos(angl - init->h());
 102         //float si = sin(angl - init->h());
 103
 104         float speed = 1; // desired
 105         float gx = goal->x();
 106         float gy = goal->y();
 107         if (co < 0)
 108                 speed = -speed;
 109         float cerr = 0;
 110         float lerr = 0;
 111         float rx = 0; // recomputed goal x
 112         float ry = 0; // recomputed goal y
 113         float r = 0;
 114         int i = 0;
 115         for (i = 0; i < ST4COUNT; i++) {
 116                 // speed controller
 117                 cerr = speed - bc.speed();
 118                 bc.speed(ST4KP * cerr + ST4KI * lerr);
 119                 lerr += cerr;
 120                 // steer controller
 121                 rx = (gx - bc.x()) * cos(-bc.h()) -
 122                         (gy - bc.y()) * sin(-bc.h());
 123                 ry = (gx - bc.x()) * sin(-bc.h()) -
 124                         (gy - bc.y()) * cos(-bc.h());
 125                 if (ry != 0) {
 126                         r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
 127                         bc.steer(1 / r);
 128                 }
 129                 // next iteration
 130                 bc.next();
 131                 nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
 132         }
 133         return nodes;
 134 }