ut/rrts.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "rrts.h"
   5
   6 WVTEST_MAIN("RRT node basic tests")
   7 {
   8         RRTNode n1;
   9         RRTNode n2;
  10 }
  11
  12 WVTEST_MAIN("RRT* basic tests")
  13 {
  14         RRTS rrts;
  15         rrts.goals().push_back(BicycleCar());
  16         rrts.goals().back().x(10);
  17         rrts.goals().back().y(10);
  18         rrts.goals().back().h(0);
  19         WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001);
  20         WVPASSEQ(rrts.nodes().size(), 1);
  21         rrts.next();
  22         WVPASSLT(1, rrts.nodes().size());
  23         WVPASSEQ(rrts.samples().size(), 1);
  24         WVPASSEQ(rrts.goals().size(), 1);
  25         rrts.set_sample(0, 10, 0, 10, 0, 2 * M_PI);
  26         Obstacle o;
  27         o.poly().push_back(std::make_tuple(5, 5));
  28         o.poly().push_back(std::make_tuple(6, 5));
  29         o.poly().push_back(std::make_tuple(6, 6));
  30         o.poly().push_back(std::make_tuple(5, 6));
  31         o.poly().push_back(std::make_tuple(5, 5));
  32         rrts.obstacles().push_back(o);
  33         while (rrts.next()) {}
  34         WVPASS(rrts.path().size() > 0);
  35         WVPASS(
  36                 rrts.nodes().size() > 0
  37                 && rrts.path().size() > 0
  38                 && &rrts.nodes().front() == rrts.path().front()
  39         );
  40         WVPASS(
  41                 rrts.goals().size() > 0
  42                 && rrts.path().size() > 0
  43                 && &rrts.goals().front() == rrts.path().back()
  44         );
  45 }
  46
  47 WVTEST_MAIN("goal found zone")
  48 {
  49         class P : public RRTS {
  50                 public:
  51                         bool goal_found(RRTNode &f) {
  52                                 return RRTS::goal_found(f);
  53                         }
  54         };
  55         double tmp_double_1 = 0;
  56         double tmp_double_2 = 0;
  57         P p;
  58         p.goals().push_back(BicycleCar());
  59         BicycleCar &g = p.goals().front();
  60         // TODO set g.x, g.y to different values
  61         // TODO set g.h to cover all 4 quadrants
  62         RRTNode n;
  63         n.x(g.x());
  64         n.y(g.y());
  65         n.h(g.h());
  66         WVPASS(p.goal_found(n)); // pass the same pose
  67
  68         n = RRTNode(g);
  69         n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
  70         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
  71         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
  72         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
  73         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
  74         WVPASS(p.goal_found(n)); // pass right corner case
  75         n.rotate(g.ccr().x(), g.ccr().y(), -0.01);
  76         WVPASS(!p.goal_found(n)); // fail right corner case
  77
  78         n = RRTNode(g);
  79         n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
  80         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
  81         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
  82         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
  83         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
  84         WVPASS(p.goal_found(n)); // pass left corner case
  85         n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
  86         WVPASS(!p.goal_found(n)); // fail left corner case
  87
  88         n = RRTNode(g);
  89         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
  90         n.st(0);
  91         n.next();
  92         WVPASS(p.goal_found(n)); // pass forward corner case
  93         n.sp(0.01);
  94         n.next();
  95         WVPASS(!p.goal_found(n)); // fail forward corner case
  96
  97         for (double a = 0; a > -M_PI/2; a -= 0.01) {
  98                 n = RRTNode(g);
  99                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 100                 WVPASS(p.goal_found(n)); // pass drivable border
 101         }
 102         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 103                 // + 0.1 -- compensate for Euclid. dist. check
 104                 n = RRTNode(g);
 105                 n.x(n.x() + 0.1*cos(n.h()));
 106                 n.y(n.y() + 0.1*sin(n.h()));
 107                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 108                 WVPASS(p.goal_found(n)); // pass near drivable border
 109         }
 110         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 111                 n = RRTNode(g);
 112                 n.x(n.x() - 0.1*cos(n.h()));
 113                 n.y(n.y() - 0.1*sin(n.h()));
 114                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 115                 WVPASS(!p.goal_found(n)); // fail near drivable border
 116         }
 117         for (double a = 0; a < M_PI / 2; a += 0.01) {
 118                 n = RRTNode(g);
 119                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 120                 WVPASS(p.goal_found(n)); // pass drivable border
 121         }
 122         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 123                 // - 0.1 -- compensate for Euclid. dist. check
 124                 n = RRTNode(g);
 125                 n.x(n.x() + 0.1*cos(n.h()));
 126                 n.y(n.y() + 0.1*sin(n.h()));
 127                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 128                 WVPASS(p.goal_found(n)); // pass near drivable border
 129         }
 130         for (double a = 0; a < M_PI / 2; a += 0.01) {
 131                 n = RRTNode(g);
 132                 n.x(n.x() - 0.1*cos(n.h()));
 133                 n.y(n.y() - 0.1*sin(n.h()));
 134                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 135                 WVPASS(!p.goal_found(n)); // fail near drivable border
 136         }
 137
 138         n = RRTNode(g);
 139         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 140         n.sp(n.sp() * -1);
 141         n.st(0);
 142         n.next();
 143         WVPASS(p.goal_found(n)); // pass backward corner case
 144         n.sp(-0.01);
 145         n.next();
 146         WVPASS(!p.goal_found(n)); // fail backward corner case
 147
 148         n = RRTNode(g);
 149         n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
 150         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 151         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 152         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 153         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 154         WVPASS(p.goal_found(n)); // pass right corner case
 155         n.rotate(g.ccr().x(), g.ccr().y(), 0.01);
 156         WVPASS(!p.goal_found(n)); // fail right corner case
 157
 158         n = RRTNode(g);
 159         n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
 160         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 161         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 162         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 163         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 164         WVPASS(p.goal_found(n)); // pass left corner case
 165         n.rotate(g.ccl().x(), g.ccl().y(), -0.01);
 166         WVPASS(!p.goal_found(n)); // fail left corner case
 167
 168         for (double a = 0; a < M_PI / 2; a += 0.01) {
 169                 n = RRTNode(g);
 170                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 171                 WVPASS(p.goal_found(n)); // pass drivable border
 172         }
 173         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 174                 // - 0.1 -- compensate for Euclid. dist. check
 175                 n = RRTNode(g);
 176                 n.x(n.x() - 0.1*cos(n.h()));
 177                 n.y(n.y() - 0.1*sin(n.h()));
 178                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 179                 WVPASS(p.goal_found(n)); // pass near drivable border
 180         }
 181         for (double a = 0; a < M_PI / 2; a += 0.01) {
 182                 n = RRTNode(g);
 183                 n.x(n.x() + 0.1*cos(n.h()));
 184                 n.y(n.y() + 0.1*sin(n.h()));
 185                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 186                 WVPASS(!p.goal_found(n)); // fail near drivable border
 187         }
 188         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 189                 n = RRTNode(g);
 190                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 191                 WVPASS(p.goal_found(n)); // pass drivable border
 192         }
 193         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 194                 // + 0.1 -- compensate for Euclid. dist. check
 195                 n = RRTNode(g);
 196                 n.x(n.x() - 0.1*cos(n.h()));
 197                 n.y(n.y() - 0.1*sin(n.h()));
 198                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 199                 WVPASS(p.goal_found(n)); // pass near drivable border
 200         }
 201         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 202                 n = RRTNode(g);
 203                 n.x(n.x() + 0.1*cos(n.h()));
 204                 n.y(n.y() + 0.1*sin(n.h()));
 205                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 206                 WVPASS(!p.goal_found(n)); // fail near drivable border
 207         }
 208 }