diff --git a/config/CourseGeneratorSettingsSetup.xml b/config/CourseGeneratorSettingsSetup.xml index fc72644fa..613e229f3 100644 --- a/config/CourseGeneratorSettingsSetup.xml +++ b/config/CourseGeneratorSettingsSetup.xml @@ -26,7 +26,7 @@ - + counterclockwise diff --git a/scripts/courseGenerator/CourseGenerator.lua b/scripts/courseGenerator/CourseGenerator.lua index 915793a13..6f8411d4a 100644 --- a/scripts/courseGenerator/CourseGenerator.lua +++ b/scripts/courseGenerator/CourseGenerator.lua @@ -21,7 +21,7 @@ CourseGenerator.cRowWaypointDistance = 7 -- Maximum cross track error we tolerate when a vehicle follows a path. This is used to -- find corners which the vehicle can't make due to its turning radius, without deviating more than -- cMaxCrossTrackError meters from the vertex in the corner. -CourseGenerator.cMaxCrossTrackError = 0.5 +CourseGenerator.cMaxCrossTrackError = 0.2 -- Maximum cross track error when generating rows parallel to a non-straight field edge. The row will end when -- the cross track error is bigger than this limit CourseGenerator.cMaxCrossTrackErrorForCurvedRows = 0.15 diff --git a/scripts/courseGenerator/CourseGeneratorInterface.lua b/scripts/courseGenerator/CourseGeneratorInterface.lua index e702a14ba..57edb774c 100644 --- a/scripts/courseGenerator/CourseGeneratorInterface.lua +++ b/scripts/courseGenerator/CourseGeneratorInterface.lua @@ -39,7 +39,7 @@ function CourseGeneratorInterface.generate(fieldPolygon, context:setBaselineEdge(startPosition.x, -startPosition.z) context:setFieldMargin(settings.fieldMargin:getValue()) context:setUseBaselineEdge(settings.useBaseLineEdge:getValue()) - context:setFieldCornerRadius(settings.turningRadius:getValue()) + context:setFieldCornerRadius(7) --using a default, that is used during testing context:setHeadlandFirst(settings.startOnHeadland:getValue()) context:setHeadlandClockwise(settings.headlandClockwise:getValue()) context:setHeadlandOverlap(settings.headlandOverlapPercent:getValue()) diff --git a/scripts/courseGenerator/FieldworkCourse.lua b/scripts/courseGenerator/FieldworkCourse.lua index 5192b7834..e30f003e4 100644 --- a/scripts/courseGenerator/FieldworkCourse.lua +++ b/scripts/courseGenerator/FieldworkCourse.lua @@ -399,6 +399,7 @@ function FieldworkCourse:_setContext(context) self.logger:debug('sharpening field boundary corners') self.boundary:ensureMinimumRadius(self.context.fieldCornerRadius, true) end + CourseGenerator.addDebugPolyline(self.boundary, {0, 1, 1, 0.3}) end function FieldworkCourse:_removeHeadland(n) diff --git a/scripts/courseGenerator/test/PolygonTest.lua b/scripts/courseGenerator/test/PolygonTest.lua index ceb97d93f..1addadc41 100644 --- a/scripts/courseGenerator/test/PolygonTest.lua +++ b/scripts/courseGenerator/test/PolygonTest.lua @@ -418,7 +418,7 @@ function testPolygon() p = Polygon({ Vector(-20, 0), Vector(-15, -3), Vector(-10, -4), Vector(-5, -4), Vector(0, 0), Vector(5, 0), Vector(10, 0), Vector(15, 0) }) - lu.assertAlmostEquals(p:getSmallestRadiusWithinDistance(4, 15, 20), 3.02) + lu.assertAlmostEquals(p:getSmallestRadiusWithinDistance(4, 15, 20), 1.61) p = Polygon({ Vector(0, 0), Vector(0, 5), Vector(5, 5), Vector(5, 0) }) lu.assertEquals(p:moveForward(1, 6), 3) diff --git a/scripts/courseGenerator/test/VertexTest.lua b/scripts/courseGenerator/test/VertexTest.lua index 3fbc9807f..b9d5e7bfd 100644 --- a/scripts/courseGenerator/test/VertexTest.lua +++ b/scripts/courseGenerator/test/VertexTest.lua @@ -35,20 +35,32 @@ function testVertex() v:getExitEdge():assertAlmostEquals(CourseGenerator.LineSegment(0, 0, 1, 0)) lu.assertEquals(v:getSignedRadius(), math.huge) + v = Vertex(0, 0) + v:calculateProperties(Vertex(-1, 0), Vertex(0, 1)) + lu.assertAlmostEquals(v:getEntryHeading(), 0) + lu.assertAlmostEquals(v:getExitHeading(), math.pi / 2) + + v:getEntryEdge():assertAlmostEquals(CourseGenerator.LineSegment(-1, 0, 0, 0)) + v:getExitEdge():assertAlmostEquals(CourseGenerator.LineSegment(0, 0, 0, 1)) + lu.assertAlmostEquals(v:getSignedRadius(), 1) + v:calculateProperties(Vertex(-1, 0), Vertex(0, -1)) + lu.assertAlmostEquals(v:getSignedRadius(), -1) + v = Vertex(0, 0) v:calculateProperties(Vertex(-1, 0), Vertex(1, 1)) lu.assertAlmostEquals(v:getEntryHeading(), 0) lu.assertAlmostEquals(v:getExitHeading(), math.pi / 4) + v:getEntryEdge():assertAlmostEquals(CourseGenerator.LineSegment(-1, 0, 0, 0)) v:getExitEdge():assertAlmostEquals(CourseGenerator.LineSegment(0, 0, 1, 1)) - lu.assertAlmostEquals(v:getSignedRadius(), 1.84) + lu.assertAlmostEquals(v:getSignedRadius(), 3.41) v:calculateProperties(Vertex(-1, 0), Vertex(1, -1)) - lu.assertAlmostEquals(v:getSignedRadius(), -1.84) + lu.assertAlmostEquals(v:getSignedRadius(), -3.41) v = Vertex(0, 0) v:calculateProperties(Vertex(-5, 0), Vertex(5, 5)) - lu.assertAlmostEquals(v:getSignedRadius(), 9.23) + lu.assertAlmostEquals(v:getSignedRadius(), 17.07) v:calculateProperties(Vertex(-5, 0), Vertex(0, 5)) - lu.assertAlmostEquals(v:getSignedRadius(), 2.5 * math.sqrt(2)) + lu.assertAlmostEquals(v:getSignedRadius(), 5) end os.exit(lu.LuaUnit.run()) \ No newline at end of file diff --git a/scripts/geometry/Polyline.lua b/scripts/geometry/Polyline.lua index 0aacf4595..0292892d5 100644 --- a/scripts/geometry/Polyline.lua +++ b/scripts/geometry/Polyline.lua @@ -487,8 +487,9 @@ function Polyline:ensureMinimumRadius(r, makeCorners) currentIx = nextIx nextIx = currentIx + 1 local xte = self:at(currentIx):getXte(r) + local radius = self:at(currentIx):getRadius() if xte > CourseGenerator.cMaxCrossTrackError then - self.logger:debug('ensureMinimumRadius (%s): found a corner at %d, r: %.1f, xte: %.1f', debugId, currentIx, r, xte) + self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f', debugId, currentIx, radius, r, xte) -- looks like we can't make this turn without deviating too much from the course, local entry = CourseGenerator.Slider(self, currentIx, 0) local exit = CourseGenerator.Slider(self, currentIx, 0) diff --git a/scripts/geometry/Vertex.lua b/scripts/geometry/Vertex.lua index d610120b0..7bbfd0c39 100644 --- a/scripts/geometry/Vertex.lua +++ b/scripts/geometry/Vertex.lua @@ -51,13 +51,6 @@ function Vertex:getExitHeading() return self.exitHeading end ---- The radius at this vertex, calculated from the direction of the entry/exit edges as unit vectors. ---- Positive values are left turns, negative values right turns ----@return number radius -function Vertex:getUnitRadius() - return self.unitRadius -end - --- The radius at this vertex, calculated from the direction of the entry/exit edges and the length of --- the exit edge. This is the radius a vehicle would need to drive to reach the next waypoint. --- Positive values are left turns, negative values right turns @@ -122,9 +115,9 @@ function Vertex:calculateProperties(entry, exit) end if self.entryHeading and self.exitHeading then self.dA = CpMathUtil.getDeltaAngle(self.entryHeading, self.exitHeading) - -- This is the radius of the unit circle written between - -- entryEdge and exitEdge, which are tangents of the circle - self.unitRadius = 1 / (2 * math.sin(self.dA / 2)) + -- This is the radius of a circle written between + -- entryEdge and exitEdge, which are tangents of the circle, touching them 1 unit away from the vertex + self.unitRadius = 1 / (math.tan(self.dA / 2)) self.curvature = 1 / self.unitRadius self.xte = math.abs(1 / math.cos(self.dA / 2)) - 1 end