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 "reeds_shepp.h"
  20 #include "steer.h"
  21
  22 #define ST2WHEELBASE 2.450
  23 #define ST2TURNINGRADI 10.82
  24 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
  25
  26 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
  27 {
  28         float angl;
  29         float new_x;
  30         float new_y;
  31         float new_h;
  32         std::vector<RRTNode *> nodes;
  33         int i;
  34         for (i = 1; i < 2; i++) { // TODO
  35                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  36                 new_x = init->x() + i * 1 * cos(angl); // TODO
  37                 new_y = init->y() + i * 1 * sin(angl); // TODO
  38                 new_h = goal->h();
  39                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
  40         }
  41         return nodes;
  42 }
  43
  44 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
  45 {
  46         float speed = 1;
  47         float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
  48         float sa = angl - init->h(); // steering angle
  49         float co = cos(angl - init->h());
  50         float si = sin(angl - init->h());
  51         if (co < 0) {
  52                 speed = -speed;
  53                 if (si > 0)
  54                         sa = -sa;
  55         } else {
  56                 if (si < 0)
  57                         sa = -sa;
  58         }
  59         if (sa > ST2MAXSTEERING)
  60                 sa = ST2MAXSTEERING;
  61         if (sa < -ST2MAXSTEERING)
  62                 sa = -ST2MAXSTEERING;
  63         float new_h = init->h() + (speed / ST2WHEELBASE) * tan(sa);
  64         float new_x = init->x() + speed * cos(new_h);
  65         float new_y = init->y() + speed * sin(new_h);
  66         std::vector<RRTNode *> nodes;
  67         nodes.push_back(new RRTNode(new_x, new_y, new_h));
  68         return nodes;
  69 }
  70
  71 int cb_st3(double q[3], void *user_data)
  72 {
  73         std::vector<RRTNode *> *nodes = (std::vector<RRTNode *> *) user_data;
  74         nodes->push_back(new RRTNode(q[0], q[1], q[2]));
  75         return 0;
  76 }
  77
  78 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
  79 {
  80         std::vector<RRTNode *> nodes;
  81         double q0[] = {init->x(), init->y(), init->h()};
  82         double q1[] = {goal->x(), goal->y(), goal->h()};
  83         ReedsSheppStateSpace rsss(10.82); // TODO const param
  84         rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
  85         return nodes;
  86 }