From 6908191504e6971a75fd5508b98d326c1e3fd806 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 28 Apr 2020 09:02:40 +0200 Subject: [PATCH] Compensate for near goal area --- ut/rrts.t.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/ut/rrts.t.cc b/ut/rrts.t.cc index 261c617..038f1c9 100644 --- a/ut/rrts.t.cc +++ b/ut/rrts.t.cc @@ -107,7 +107,8 @@ WVTEST_MAIN("goal found zone") n.rotate(n.ccr().x(), n.ccr().y(), a); WVPASS(p.goal_found(n)); // pass near drivable border } - for (double a = 0; a > -M_PI/2; a -= 0.01) { + for (double a = -0.1; a > -M_PI/2; a -= 0.01) { + // = -0.1 -- compensate for near goal n = RRTNode(g); n.x(n.x() - 0.1*cos(n.h())); n.y(n.y() - 0.1*sin(n.h())); @@ -127,7 +128,8 @@ WVTEST_MAIN("goal found zone") n.rotate(n.ccl().x(), n.ccl().y(), a); WVPASS(p.goal_found(n)); // pass near drivable border } - for (double a = 0; a < M_PI / 2; a += 0.01) { + for (double a = 0.1; a < M_PI / 2; a += 0.01) { + // = 0.1 -- compensate for near goal n = RRTNode(g); n.x(n.x() - 0.1*cos(n.h())); n.y(n.y() - 0.1*sin(n.h())); @@ -178,7 +180,8 @@ WVTEST_MAIN("goal found zone") n.rotate(n.ccr().x(), n.ccr().y(), a); WVPASS(p.goal_found(n)); // pass near drivable border } - for (double a = 0; a < M_PI / 2; a += 0.01) { + for (double a = 0.1; a < M_PI / 2; a += 0.01) { + // = 0.1 -- compensate for near goal n = RRTNode(g); n.x(n.x() + 0.1*cos(n.h())); n.y(n.y() + 0.1*sin(n.h())); @@ -198,7 +201,8 @@ WVTEST_MAIN("goal found zone") n.rotate(n.ccl().x(), n.ccl().y(), a); WVPASS(p.goal_found(n)); // pass near drivable border } - for (double a = 0; a > -M_PI/2; a -= 0.01) { + for (double a = -0.1; a > -M_PI/2; a -= 0.01) { + // = -0.1 -- compensate for near goal n = RRTNode(g); n.x(n.x() + 0.1*cos(n.h())); n.y(n.y() + 0.1*sin(n.h())); -- 2.39.2