From e65b509b1ec729e3b089cb0567900796a8da0140 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 26 May 2020 16:12:33 +0200 Subject: [PATCH] Update drivable ut --- ut/bcar.t.cc | 165 +++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 147 insertions(+), 18 deletions(-) diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc index a0a14dd..f06bd67 100644 --- a/ut/bcar.t.cc +++ b/ut/bcar.t.cc @@ -49,24 +49,6 @@ WVTEST_MAIN("bcar basic geometry") WVPASSEQ_DOUBLE(bc.orradi(), 10.51189802081432, 0.00001); bc.h(M_PI / 2); - // drivable - WVPASS(!bc.drivable(bc.ccl())); - WVPASS(!bc.drivable(bc.ccr())); - BicycleCar bc2; - bc2.x(1); - bc2.y(2); - bc2.h(M_PI / 2); - bc2.mtr(10); - bc2.wb(2); - bc2.w(1); - bc2.l(3); - bc2.he(1.5); - bc2.df(2 + 0.5); - bc2.dr(0.5); - WVPASS(bc.drivable(bc2)); - bc2.y(-2); - WVPASS(bc.drivable(bc2)); - // moving bc.sp(1); bc.st(0); @@ -126,3 +108,150 @@ WVTEST_MAIN("test collide functions") p3.push_back(std::make_tuple(4, 2)); WVPASS(!std::get<0>(collide(p1, p3))); } + +WVTEST_MAIN("drivable") +{ + double tmp_double_1 = 0; + double tmp_double_2 = 0; + BicycleCar g; + // TODO set g.x, g.y to different values + // TODO set g.h to cover all 4 quadrants + BicycleCar n; + n.x(g.x()); + n.y(g.y()); + n.h(g.h()); + WVPASS(g.drivable(n)); // pass the same pose + + n = BicycleCar(g); + n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(g.drivable(n)); // pass right corner case + + n = BicycleCar(g); + n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(g.drivable(n)); // pass left corner case + n.rotate(g.ccl().x(), g.ccl().y(), 0.01); + WVPASS(!g.drivable(n)); // fail left corner case + + n = BicycleCar(g); + n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2))); + n.st(0); + n.next(); + WVPASS(g.drivable(n)); // pass forward corner case + + for (double a = 0; a > -M_PI/2; a -= 0.01) { + n = BicycleCar(g); + n.rotate(g.ccr().x(), g.ccr().y(), a); + WVPASS(g.drivable(n)); // pass drivable border + } + for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) { + // + 0.1 -- compensate for Euclid. dist. check + n = BicycleCar(g); + n.x(n.x() + 0.1*cos(n.h())); + n.y(n.y() + 0.1*sin(n.h())); + n.rotate(n.ccr().x(), n.ccr().y(), a); + WVPASS(g.drivable(n)); // pass near drivable border + } + for (double a = -0.1; a > -M_PI/2; a -= 0.01) { + // = -0.1 -- compensate for near goal + n = BicycleCar(g); + n.x(n.x() - 0.1*cos(n.h())); + n.y(n.y() - 0.1*sin(n.h())); + n.rotate(n.ccr().x(), n.ccr().y(), a); + WVPASS(!g.drivable(n)); // fail near drivable border + } + for (double a = 0; a < M_PI / 2; a += 0.01) { + n = BicycleCar(g); + n.rotate(g.ccl().x(), g.ccl().y(), a); + WVPASS(g.drivable(n)); // pass drivable border + } + for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) { + // - 0.1 -- compensate for Euclid. dist. check + n = BicycleCar(g); + n.x(n.x() + 0.1*cos(n.h())); + n.y(n.y() + 0.1*sin(n.h())); + n.rotate(n.ccl().x(), n.ccl().y(), a); + WVPASS(g.drivable(n)); // pass near drivable border + } + for (double a = 0.1; a < M_PI / 2; a += 0.01) { + // = 0.1 -- compensate for near goal + n = BicycleCar(g); + n.x(n.x() - 0.1*cos(n.h())); + n.y(n.y() - 0.1*sin(n.h())); + n.rotate(n.ccl().x(), n.ccl().y(), a); + WVPASS(!g.drivable(n)); // fail near drivable border + } + + n = BicycleCar(g); + n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2))); + n.sp(n.sp() * -1); + n.st(0); + n.next(); + WVPASS(g.drivable(n)); // pass backward corner case + + n = BicycleCar(g); + n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(g.drivable(n)); // pass right corner case + + n = BicycleCar(g); + n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2); + WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001); + tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2)); + tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)); + WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001); + WVPASS(g.drivable(n)); // pass left corner case + + for (double a = 0; a < M_PI / 2; a += 0.01) { + n = BicycleCar(g); + n.rotate(g.ccr().x(), g.ccr().y(), a); + WVPASS(g.drivable(n)); // pass drivable border + } + for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) { + // - 0.1 -- compensate for Euclid. dist. check + n = BicycleCar(g); + n.x(n.x() - 0.1*cos(n.h())); + n.y(n.y() - 0.1*sin(n.h())); + n.rotate(n.ccr().x(), n.ccr().y(), a); + WVPASS(g.drivable(n)); // pass near drivable border + } + for (double a = 0.1; a < M_PI / 2; a += 0.01) { + // = 0.1 -- compensate for near goal + n = BicycleCar(g); + n.x(n.x() + 0.1*cos(n.h())); + n.y(n.y() + 0.1*sin(n.h())); + n.rotate(n.ccr().x(), n.ccr().y(), a); + WVPASS(!g.drivable(n)); // fail near drivable border + } + for (double a = 0; a > -M_PI/2; a -= 0.01) { + n = BicycleCar(g); + n.rotate(g.ccl().x(), g.ccl().y(), a); + WVPASS(g.drivable(n)); // pass drivable border + } + for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) { + // + 0.1 -- compensate for Euclid. dist. check + n = BicycleCar(g); + n.x(n.x() - 0.1*cos(n.h())); + n.y(n.y() - 0.1*sin(n.h())); + n.rotate(n.ccl().x(), n.ccl().y(), a); + WVPASS(g.drivable(n)); // pass near drivable border + } + for (double a = -0.1; a > -M_PI/2; a -= 0.01) { + // = -0.1 -- compensate for near goal + n = BicycleCar(g); + n.x(n.x() + 0.1*cos(n.h())); + n.y(n.y() + 0.1*sin(n.h())); + n.rotate(n.ccl().x(), n.ccl().y(), a); + WVPASS(!g.drivable(n)); // fail near drivable border + } +} -- 2.39.2