vehicle_platform/cost.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 "aux.h"
  20 #include "bcar.h"
  21 #include "cost.h"
  22 #include "reeds_shepp.h"
  23 #include "rrtnode.h"
  24
  25 float co1(RRTNode *init, RRTNode *goal)
  26 {
  27         float dx = goal->x() - init->x();
  28         float dy = goal->y() - init->y();
  29         return pow(pow(dx, 2) + pow(dy, 2), 0.5);
  30 }
  31
  32 float cco1(RRTNode *init, RRTNode *goal)
  33 {
  34         return init->ccost() + co1(init, goal);
  35 }
  36
  37 float co2(RRTNode *init, RRTNode *goal)
  38 {
  39         double q0[] = {init->x(), init->y(), init->h()};
  40         double q1[] = {goal->x(), goal->y(), goal->h()};
  41         ReedsSheppStateSpace rsss(10.82); // TODO const param
  42         return static_cast<float>(rsss.distance(q0, q1));
  43 }
  44
  45 float cco2(RRTNode *init, RRTNode *goal)
  46 {
  47         return init->ccost() + co2(init, goal);
  48 }
  49
  50 float co3(RRTNode *init, RRTNode *goal)
  51 {
  52         // The basic values from co1:
  53         // edist is euclidean distance
  54         // dh is difference between headings
  55         // dt is difference between times
  56         //
  57         // The basic values from co2:
  58         // rsdist is Reeds & Shepp distance
  59         // cnt is number of cusps (direction changes)
  60         //
  61         // There is also available:
  62         // node->ccost() -- cumulative cost from root
  63         // node->dcost() -- direct cost to parent
  64         // node->ocost() -- distance to the nearest obstacle
  65         //
  66         float dx = goal->x() - init->x();
  67         float dy = goal->y() - init->y();
  68         float edist = pow(pow(dx, 2) + pow(dy, 2), 0.5);
  69         float dh = std::abs(goal->h() - init->h());
  70         float dt = std::abs(goal->t() - init->t());
  71         if (edist < 0.2 && dh < M_PI / 32)
  72                 return 0;
  73
  74         double q0[] = {init->x(), init->y(), init->h()};
  75         double q1[] = {goal->x(), goal->y(), goal->h()};
  76         ReedsSheppStateSpace rsss(10.82); // TODO const param
  77         float rsdist = static_cast<float>(rsss.distance(q0, q1));
  78         bool lm = false;
  79         bool cm = false;
  80         if (rsss.reedsShepp(q0, q1).length_[0] > 0)
  81                 lm = true;
  82         int cnt = 0; // #cusps
  83         for (int i = 0; i < 5; i++) {
  84                 if (rsss.reedsShepp(q0, q1).length_[i] == 0)
  85                         break;
  86                 if (rsss.reedsShepp(q0, q1).length_[i] > 0)
  87                         cm = true;
  88                 else
  89                         cm = false;
  90                 if (cm != lm)
  91                         cnt++;
  92                 lm = cm;
  93         }
  94         return rsdist + edist * cnt + 1 / (1 + init->ocost());
  95 }
  96
  97 float cco3(RRTNode *init, RRTNode *goal)
  98 {
  99         return init->ccost() + co3(init, goal);
 100 }
 101
 102 float co4(RRTNode *init, RRTNode *goal)
 103 {
 104         float dx = goal->x() - init->x();
 105         float dy = goal->y() - init->y();
 106         float edist = pow(pow(dx, 2) + pow(dy, 2), 0.5);
 107         float heur = BCAR_TURNING_RADIUS * MIN(
 108                 std::abs(goal->h() - init->h()),
 109                 2 * M_PI - abs(goal->h() - init->h())
 110         );
 111         return MAX(edist, heur);
 112 }
 113 float cco4(RRTNode *init, RRTNode *goal)
 114 {
 115         return init->ccost() + co4(init, goal);
 116 }