Merge pull request #3560 from Courseplay/corner

Sharp corner improvements

config/CourseGeneratorSettingsSetup.xml

@@ -26,7 +26,7 @@
 	<SettingSubTitle title="headland" isVisible="hasHeadlandsSelected">	
 		<Setting classType="AIParameterBooleanSetting" name="sharpenCorners" defaultBool="false"/>
 		<Setting classType="AIParameterSettingList" name="headlandsWithRoundCorners" min="0" max="50" default="1"/>
-		<Setting classType="AIParameterSettingList" name="turningRadius" min="5" max="15" setDefault="setDefaultTurningRadius"/>
+		<Setting classType="AIParameterSettingList" name="turningRadius" min="5" max="12" setDefault="setDefaultTurningRadius"/>
 		<Setting classType="AIParameterBooleanSetting" name="headlandClockwise" defaultBool="true">
 			<Texts>
 				<Text>counterclockwise</Text>

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

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())

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)

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)

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())

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)

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