From 67b467fa2b42a6d7c0b924a9109d144fc8087877 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Nov 2026 14:18:08 +0000 Subject: [PATCH 1/2] End of the pass: seen on a pass bent round trees too At the end of a pass with no U-turn and a hard fence, AutoSteer goes off a metre into the headland (EndOfPassWithoutTurn). The check asks whether the tractor is on the line, heading along it, and walks the pass ahead to the fence. Both were measured against the straight pass. A pass bent round Go around obstacles leaves the crop up to 45 degrees off the pass, so the tractor looked like one turning by hand, and the check (made once per headland entry) said no. Walking the straight pass from abreast of the tractor also found crop the bend had already left. On a tester's field (simulator, 8 m front boom) 3R, bent round two trees in the headland, left the crop 36 degrees off the pass; AutoSteer steered on along the bend for 40 m until the fence guard stopped it with the boom over the fence. The pipeline now hands the check the bend it steers (GoAroundPasses.Current): "on the line" is the bend's heading at the tractor, and PassLeavesFieldAhead walks the bend from its point nearest the tractor. Without a bend nothing changes. On that field the check now says yes 1 m past the crop (was no). Tests: EndOfPassWithoutTurnTests (a tree in a wide headland: the bend leaves the crop 35 degrees off and AutoSteer goes off a metre past the line; a bend through a headland strip with crop past it is not the end). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VKEBDFmQpFVsahedjtN2AZ --- CLAUDE.md | 2 +- README.md | 2 +- .../Pipeline/GpsPipelineService.cs | 3 +- .../YouTurn/YouTurnPathingService.cs | 60 +++++++++- .../YouTurn/YouTurnStateMachine.cs | 38 ++++++- .../Pipeline/EndOfPassWithoutTurnTests.cs | 107 +++++++++++++++++- 6 files changed, 200 insertions(+), 12 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 63bd014b..338e5a90 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -594,7 +594,7 @@ turns from the on-screen buttons) with paths built by `YouTurnCreationService`: - A turn tried again with the turn lines moved in (the implement over a hard fence, a landing on a bend, an obstacle in its legs: `CreateTurnPath`'s setback loop, which moves the whole fence in) is seated back from the end of the pass the first try found: walking back from that end, at the moved line's crossing nearest to it (`YouTurnCreationInput.TurnLineEndIndex`, `TurnLineEndOnRetry`; not after a gap crossed). Moved in, the fence's turn line also crossed the pass where the fence bulges in beside it, and the first crossing ahead was taken: on Tapada da Casa an 8 m front boom near the north fence moved the line in 5 m and the turn was made 260 m early, beside the east bulge, landing on the next pass inside its crop and leaving a strip of it unsprayed. Test: `TurnSetbackStaysAtEndTests`. Official since #828; not after a gap crossed is fork only. - The next pass must have room at this end: at least 20 m of it inside the cultivated area back from the end ahead (`IsPathLandable` / `LandingLength`), not just any piece of it inside the field; otherwise "End of field reached - no room on the next pass at this end" (#712). In this fork, when no pass has room but the current pass goes on inside the field past the headland ahead (a corner the headland clips off it, a gap), there is no turn and AutoSteer drives on across it: "No room to turn at this headland - driving on across it" (`GapAhead` / `DrivesOnAcrossGap`, renatorgr/AgOpenWeb#129; upstream left it out until after the v1.0 freeze). The pass must run on at least 10 m inside past the gap (`MinRunAfterGap`): a sliver or spike of the headland line is not more field. - Skip worked passes (the snake order): when the order's next pass has no room at this end, or its turn does not build, the turn goes onto the nearest pass still to do that has room, first on the side the order is going, then on the other side (one turn built per side; a pass at least the skip away before a nearer one; none further than twice the skip's step, else "End of field reached" and the operator decides), never against the side the operator picked with the U-turn arrow (`TurnOntoNearestOpenPass`; AgOpenGPS's `GetNextNotWorkedTrack` only steps outward on the turn's side past worked tracks, so the other side, passes nearer than the skip and the room and build checks are ours); the order is planned again from where it lands. The same nearest pass is tried first when the order's next pass still to do is further than twice the skip's step (`MaxFallbackStep`; the passes in between worked before), from any pass nearer than the order's, either side: the turn went from 17R to 7R along the headland, 80 m, with 16R and 14R open beside the tractor (a tester's field, simulator), and on 21R, with the order's 8R 13 passes over, 15R six over was never looked at and the turn onto 8R did not build, so there was no turn; with none of them taking a turn, the order's pass is turned onto as before. Fork only. With the operator's headland on and the sections off in it (`Tool.IsHeadlandSectionControl`), a pass the snake turns onto (the order's or a nearest one) must also have that room in the crop, inside the headland line (`SnakeLandable`): the turn line is the fence moved in by the U-turn distance, so a pass lying in the headland band had room there and nothing to work, and unpainted it was still to do (a tester's field, simulator: 13L, 12 m of crop, and 15L, none, in a corner of the headland, before 12L). Fork only. An end where no turn was made is remembered (`_failedEnd`) and not tried again on every cycle until the pass, the direction of travel, the nudge, the order, the field or headland, the skip, the U-turn arrow or a turn setting changes. The order is also planned again when the headland line or the outer boundary changes (`SnakeFieldStamp`), and forgotten with what was found about each pass when the line moves (`ForgetPassStatus`: Reference Line Nudge, an end trimmed; the moved line's pass N is another strip, and the old answers turned the snake past clean passes on Tapada da Casa). In the headland the snake looks for its next turn only heading out of the field (no cultivated ground ahead on the pass): a turn that ends in the headland leaves the tractor facing the field on its new pass, which is not the end of that pass. Fork only: a pass worked on the whole (half covered, `IsPassWorked`) is still to do at the end ahead when less than half of its 20 m inside the cultivated area next to that end is covered (`YouTurnPathingService.IsPassOpenAtThisEnd`, asked in `WorkedPasses` when a turn is planned, once per end): on a tester's field a pass 71 % covered, with its last 100 m at that end left, was turned past. Such a pass is turned onto only when the turn built gets to what is left: at least 5 m of unworked crop along the turn's last 20 m and 20 m on past it (`YouTurnPathingService.TurnReachesUnworkedGround`); otherwise the turn is dropped and the pass counts worked at this end (`TurnMissesOpenEnd`). A turn lands abreast of where it leaves the tractor's pass, and the next pass's end can lie beyond that (a slanting headland, a turn seated back from a fence or a tree): on a tester's field the turn from 1L onto 2R, seated back 12.5 m, drove the whole of 2R a second time and its end stayed as it was. -- The end of a pass at a hard outer boundary with no turn planned or driven there ("End of field reached", no turn that fits, U-turns off): as the pivot gets 1 m into the headland from the cultivated area (`HeadlandEntryCheckAfter`), on the line and heading out (the pass leaves the outer boundary within 300 m with no 10 m of field on it first, `YouTurnPathingService.PassLeavesFieldAhead`), AutoSteer goes off with the alarm, "AutoSteer off: end of the pass and no U-turn here. Turn by hand." (`YouTurnStateMachine.EndOfPassWithoutTurn`, step 5b'''' in `GpsPipelineService`; once per entry; fork only). Left on, AutoSteer steered on along the line through the fence (as AgOpenGPS, which only sends geoStop): on Tapada da Casa the simulator drove on 2.8 km. Heading into the field, a headland strip with field past it, or a soft boundary do not count. This stop, the line facing out of the field below and a pass still open at this end measure from the headland line, where the crop ends, not from the turn line the U-turn uses (`TickContext.PassEndLine`, the line the pipeline arms them on: from the turn line a headland more than 10 m wider than the U-turn distance never stopped AutoSteer). +- The end of a pass at a hard outer boundary with no turn planned or driven there ("End of field reached", no turn that fits, U-turns off): as the pivot gets 1 m into the headland from the cultivated area (`HeadlandEntryCheckAfter`), on the line and heading out (the pass leaves the outer boundary within 300 m with no 10 m of field on it first, `YouTurnPathingService.PassLeavesFieldAhead`), AutoSteer goes off with the alarm, "AutoSteer off: end of the pass and no U-turn here. Turn by hand." (`YouTurnStateMachine.EndOfPassWithoutTurn`, step 5b'''' in `GpsPipelineService`; once per entry; fork only). Left on, AutoSteer steered on along the line through the fence (as AgOpenGPS, which only sends geoStop): on Tapada da Casa the simulator drove on 2.8 km. Heading into the field, a headland strip with field past it, or a soft boundary do not count. This stop, the line facing out of the field below and a pass still open at this end measure from the headland line, where the crop ends, not from the turn line the U-turn uses (`TickContext.PassEndLine`, the line the pipeline arms them on: from the turn line a headland more than 10 m wider than the U-turn distance never stopped AutoSteer). On a pass bent round Go around obstacles, "on the line" and the pass walked ahead are the bend's (`GoAroundPasses.Current`): a bend leaves the crop up to 45° off the pass, and measured against the pass the stop took the tractor for one turning by hand (Tapada da Casa, simulator: 3R bent round two trees in the headland left the crop 36° off, and AutoSteer steered on along the bend for 40 m until the fence guard stopped it with the 8 m boom over the fence). - A turn planned for the end the tractor was heading to is dropped when the tractor turns round on the pass (by hand, or the simulator's turn-around), and the turn at the end ahead is planned (TURNED ROUND in `YouTurnStateMachine.Tick`, `_plannedSameWay`; AgOpenGPS drops a created turn once the pivot leaves the turn area without it, `ResetCreatedYouTurn`). Kept, the headland stop and the fence guard below took it for a turn planned at the other end and left AutoSteer on (Tapada da Casa, simulator). Fork only. - Only a turn keeps the implement off a hard outer boundary. With none planned or driven, AutoSteer on, going forward, the tractor inside the fence, an end of the implement (half `ActualToolWidth` plus the offset either side of the tool) heading out across the fence, more than 0.5 m out and further than now within 2 s of travel, takes AutoSteer off with the alarm (`HardFenceGuard`, step 5b'''' in `GpsPipelineService`; fork only). AutoSteer joining a line from a corner with the line moved five passes swung an 8 m front boom 0.9 m over the fence. The 2 s are along the curve steered, not straight ahead: the implement's ends turn with the tractor about its turning centre, from the last cycle's steer angle (the one the line asked for when AutoSteer has just gone on; a cycle with no line steered before it waits for the next). Straight ahead leaves a bend: on a boundary curve's first lap (the boom's end on the fence) it took AutoSteer off at a bend with the tractor on the line (`HardFenceGuardPipelineTests`). Joining a line sideways beside the fence is an S that keeps the implement inside (*Stepping onto a line beside a hard fence*). Driving along the fence a little over it, back in, or below walking pace (`MinLookAhead`) does not count. - Joining a new line in the headland (U-turn settings, `GuidanceConfig.LineEntryMode`: Nearest point, the default and AgOpenGPS's; Ask; Start of the pass; `uturn.entryMode`, `uturn.entryStraight`, saved with the tool profile; fork only): when AutoSteer is engaged, the line moved a pass or another track chosen, with the tractor in the headland (inside the field, outside the headland line), going forward, on an open track, `GpsPipelineService.UpdateLineEntry` asks `YouTurnCreationService.PlanLineEntry` for the way to the start of the pass: where the pass, driven either way, enters the cultivated area nearest the tractor (not behind it, no more than 100° from where it faces, 150 m at most; with none there, the tractor faces out of the field and the way turns it round, if it fits), reached by the shortest Dubins path at the U-turn radius from 3 m ahead (`DubinsPathService.GeneratePaths`) that keeps the pivot in the headland until it is on the pass `LineEntryStraight` (plus a front implement's reach) before the headland line, inside the field, the implement off a hard fence (`ManualTurnClearsHardBoundary`) and clear of turn obstacles (`ClearsTurnObstacles`). Start of the pass drives it like a turn (`FollowLineEntry`, the auto U-turn waits; it ends on the pass a little inside the crop, where the line takes over); Ask offers it (`GpsCycleResult.LineEntryOfferPass`, the scene's `entryOffer` and dashed `entryPath`, the approach sound, `youturn.entryTake` / `youturn.entryKeep`, 30 s) while the nearest point is steered, and plans it again from where the tractor is when taken. No way through says so once (`LineEntryNotice`, a toast) and the nearest point is used. A way or an offer is dropped by AutoSteer off, a pass, nudge or track change, a manual U-turn or reverse. In any mode, AutoSteer joining a line in the headland facing out of the field (the line, driven the way the tractor faces, reaches a hard outer boundary before any crop: `YouTurnStateMachine.LineAheadLeavesField`) with no way driven or offered goes off at once with the alarm, "AutoSteer off: the line ahead runs out of the field. Turn the tractor to face the field." (step 5b'''' in `GpsPipelineService`); it steered the tractor to the fence (Tapada da Casa). Tests: `LineEntryPlannerTests`, `LineEntryPipelineTests`. With the skip mode *Ignore worked tracks*, AutoSteer engaged in the headland joins the nearest pass still to do (not worked: the snake's test) that the way reaches straight, whatever the mode (Ask offers it), a toast naming it (`NearestOpenPassEntry`: the pass under the tractor, then one either side and so on up to 8, of two the shorter way; one the way can't reach is passed over); a lateral move or another track is left alone. Tests: `NearestOpenPassEntryTests`. diff --git a/README.md b/README.md index 72cce743..aec0e026 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ Most of these have been offered to the official project too. When one is merged - **U-turns with no room.** When no proper turn fits, the simple turn is moved back into the field instead of crossing the boundary. Where the headland clips a corner off the pass, AutoSteer drives on across it and says so (a thin spike of the headland line doesn't count). - **U-turn beside a fence that comes in.** With a hard boundary, a turn that had to be moved back to keep the implement off the fence at the end of the pass could be made far too early instead, where the fence comes in beside the pass: it landed on the next pass inside its crop and left a strip of it unsprayed. Now it is moved back from the end of the pass only. - **Skip worked passes.** A pass more than half done but left short of the end the tractor is at (its 20 m next to that end mostly not covered) counts as still to do there, so the turn can go onto it. When the order's next pass is more than four passes over (the ones in between were done before), the turn goes onto the nearest pass still to do instead of along the headland to it, and the order starts again from there. (The turn onto the nearest pass with room is official since #807.) -- **AutoSteer and a hard boundary.** At the end of a pass with no U-turn there (no pass with room at this end, no turn that fits, U-turn off), AutoSteer goes off with the alarm a metre into the headland, so there is the headland to turn by hand; before, it steered on through the fence. And with no U-turn planned, when an end of the implement is heading out across a hard boundary (AutoSteer joining a line from a corner, for example), AutoSteer goes off with the alarm. It looks along the curve the tractor is steering, so on the first lap of a boundary curve it no longer goes off at a bend with the tractor on the line. A U-turn planned for one end is dropped when the tractor turns round on the pass, so these still work at the other end. +- **AutoSteer and a hard boundary.** At the end of a pass with no U-turn there (no pass with room at this end, no turn that fits, U-turn off), AutoSteer goes off with the alarm a metre into the headland, so there is the headland to turn by hand; before, it steered on through the fence. This also works on a pass bent round trees in the headland (Go around), which leaves the crop at an angle. And with no U-turn planned, when an end of the implement is heading out across a hard boundary (AutoSteer joining a line from a corner, for example), AutoSteer goes off with the alarm. It looks along the curve the tractor is steering, so on the first lap of a boundary curve it no longer goes off at a bend with the tractor on the line. A U-turn planned for one end is dropped when the tractor turns round on the pass, so these still work at the other end. - **AutoSteer flickering on and off.** With the steer switch / button set to Button, when the app took AutoSteer off just as it went on (a hard boundary warning, for example), the steer module's answer to the engage turned it on again, and it went on and off for seconds with the alarm. The app now ignores the module's "steering" for 1 s after it takes AutoSteer off itself. - **No line outside a hard boundary.** A pass that lies wholly outside a hard boundary (a lap of a boundary curve outside the fence, say) is no longer shown when the tractor is outside the field with AutoSteer off: the nearest pass inside is shown. Moving the line onto such a pass is refused with a message. - **Moving onto a line beside a hard boundary.** When AutoSteer is turned on, or the line is moved or changed, with the tractor off the new line and the implement near a hard boundary, AutoSteer now drives a smooth S onto the line that keeps the implement's ends inside the fence. A front implement swings out ahead of the tractor as it straightens, so its S is longer (an 8 m front boom 4.5 m ahead, moving one pass at 8 km/h: about 60 m; a rear implement: about 20 m). A tractor already heading steeply towards the line is first turned along it. The S is drawn on the map. When no S fits (the end of the field is too near), AutoSteer goes off with the alarm and says to move over by hand. Checked by replaying a simulator drive, not tried on a tractor yet. diff --git a/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs b/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs index 66943b7c..5a2ccfd5 100644 --- a/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs +++ b/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs @@ -1350,7 +1350,8 @@ private void ProcessCycle(GpsData data) { _headlandEntryArmed = false; if (autoSteerEngaged && !_isReverse && fence.IsPointInside(driftedEasting, driftedNorthing) - && _youTurnStateMachine.EndOfPassWithoutTurn(in tickCtx, _guidanceWorking, _youTurn)) + && _youTurnStateMachine.EndOfPassWithoutTurn(in tickCtx, _guidanceWorking, _youTurn, + _goAround.Current is { GoneRound.Count: > 0 } bend ? bend.Points : null)) { autoSteerEngaged = false; autoSteerDisengaged = true; diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs index c23bc4f4..08300e70 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs @@ -507,10 +507,14 @@ bool RunsOnInside(double t) /// for ? So the tractor, in the headland, is at the end of its pass /// heading out, not heading into the field or across a gap with more field past it. False when /// the fence is not met within , the pass (a curve) ends first, or - /// there is no usable outer boundary. Fork only. + /// there is no usable outer boundary. With (the pass bent round + /// Go around obstacles, the line the tractor follows) that line is walked instead, from its + /// point nearest the tractor: the straight pass beside a bend can still be in the crop where + /// the bend has left it. Fork only. /// public bool PassLeavesFieldAhead(Models.Track.Track track, double abHeading, int pathsAway, - Boundary? boundary, IReadOnlyList? headlandLine, Position position, bool headingSameWay) + Boundary? boundary, IReadOnlyList? headlandLine, Position position, bool headingSameWay, + IReadOnlyList? steeredLine = null) { var outer = boundary?.OuterBoundary; if (outer == null || !outer.IsValid || track.Points.Count < 2) return false; @@ -520,6 +524,12 @@ public bool PassLeavesFieldAhead(Models.Track.Track track, double abHeading, int // The pass's points ahead, a metre apart, from abreast of the tractor. IEnumerable Ahead() { + if (steeredLine is { Count: >= 2 }) + { + foreach (var p in WalkAhead(steeredLine, position, headingSameWay ? abHeading : abHeading + Math.PI, Step, MaxWalkToFence)) + yield return p; + yield break; + } if (track.Points.Count > 2) { var curve = CurveProcessing.CreateOffsetCurve(track.Points, distAway); @@ -574,6 +584,52 @@ IEnumerable Ahead() return false; } + /// + /// Points of every m from its point nearest + /// , the way that runs with (compass + /// radians), up to m or the line's end. + /// + private static IEnumerable WalkAhead(IReadOnlyList line, Position position, double travel, + double step, double maxWalk) + { + int seg = 0; + double bestT = 0, best = double.MaxValue; + for (int i = 0; i < line.Count - 1; i++) + { + double ax = line[i].Easting, ay = line[i].Northing; + double dx = line[i + 1].Easting - ax, dy = line[i + 1].Northing - ay; + double len2 = dx * dx + dy * dy; + if (len2 < 1e-12) continue; + double t = Math.Clamp(((position.Easting - ax) * dx + (position.Northing - ay) * dy) / len2, 0, 1); + double ex = ax + dx * t - position.Easting, ey = ay + dy * t - position.Northing; + double d = ex * ex + ey * ey; + if (d < best) { best = d; seg = i; bestT = t; } + } + double sdx = line[seg + 1].Easting - line[seg].Easting, sdy = line[seg + 1].Northing - line[seg].Northing; + int dir = sdx * Math.Sin(travel) + sdy * Math.Cos(travel) >= 0 ? 1 : -1; + + // Walk from the nearest point, vertex to vertex, putting out a point every step. + double curE = line[seg].Easting + sdx * bestT, curN = line[seg].Northing + sdy * bestT; + int next = dir > 0 ? seg + 1 : seg; + double walked = 0, carry = 0; + while (next >= 0 && next < line.Count && walked < maxWalk) + { + double ne = line[next].Easting, nn = line[next].Northing; + double len = Math.Sqrt((ne - curE) * (ne - curE) + (nn - curN) * (nn - curN)); + double along = step - carry; + while (along <= len && walked < maxWalk) + { + double f = along / len; + walked += step; + yield return new Vec2(curE + (ne - curE) * f, curN + (nn - curN) * f); + along += step; + } + carry = len - (along - step); + curE = ne; curN = nn; + next += dir; + } + } + private static bool InsideFence(double e, double n, Boundary? boundary) => boundary?.OuterBoundary is { IsValid: true } outer && outer.IsPointInside(e, n); diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs index a3fca319..b2709111 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs @@ -783,9 +783,16 @@ public LineEntryPlan PlanLineEntry(Vec3 pivot, Func?> /// it drives on across), on the line, and no U-turn is planned or driven: none could be made /// at this end ("End of field reached", no turn that fits), or U-turns are off. The pipeline /// asks once as the tractor enters the headland and, with a hard outer boundary, takes - /// AutoSteer off, so the operator has the headland to turn by hand. Fork only. + /// AutoSteer off, so the operator has the headland to turn by hand. + /// On a pass bent round Go around obstacles, is the bend the + /// tractor follows: "on the line" and "the pass ahead" are that line's. A bend leaves the + /// crop up to 45 degrees off the pass, and measured against the pass the check took the + /// tractor for one turning by hand: on Tapada da Casa (simulator) 3R, bent round two trees in + /// the headland, left the crop 36 degrees off and AutoSteer steered on along the bend for + /// 40 m, until the fence guard stopped it with the boom over the fence. Fork only. /// - public bool EndOfPassWithoutTurn(in TickContext ctx, GuidanceWorkingState guidance, YouTurnWorkingState turn) + public bool EndOfPassWithoutTurn(in TickContext ctx, GuidanceWorkingState guidance, YouTurnWorkingState turn, + IReadOnlyList? steeredLine = null) { if (turn.IsExecuting || turn.TurnPath != null) return false; var track = ctx.SelectedTrack; @@ -795,11 +802,32 @@ public bool EndOfPassWithoutTurn(in TickContext ctx, GuidanceWorkingState guidan double abHeading = track.Points.Count > 2 ? LocalCurveHeading(track, p) : Math.Atan2(track.Points[1].Easting - track.Points[0].Easting, track.Points[1].Northing - track.Points[0].Northing); - double diff = Math.IEEERemainder(heading - abHeading, 2 * Math.PI); + if (steeredLine is { Count: < 2 }) steeredLine = null; + double lineHeading = steeredLine != null ? NearestSegmentHeading(steeredLine, p.Easting, p.Northing) : abHeading; + double diff = Math.IEEERemainder(heading - lineHeading, 2 * Math.PI); // Off the line's direction (turning by hand) is not the end of a pass. if (Math.Abs(diff) > AlignmentTolerance && Math.PI - Math.Abs(diff) > AlignmentTolerance) return false; - bool sameWay = Math.Abs(diff) < Math.PI / 2; - return _pathing.PassLeavesFieldAhead(track, abHeading, guidance.HowManyPathsAway, ctx.Boundary, ctx.WorkEndLine, p, sameWay); + bool sameWay = Math.Abs(Math.IEEERemainder(heading - abHeading, 2 * Math.PI)) < Math.PI / 2; + return _pathing.PassLeavesFieldAhead(track, abHeading, guidance.HowManyPathsAway, ctx.Boundary, ctx.WorkEndLine, p, sameWay, + steeredLine); + } + + /// The compass heading (radians) of the segment of nearest (e, n). + private static double NearestSegmentHeading(IReadOnlyList line, double e, double n) + { + double best = double.MaxValue, heading = 0; + for (int i = 0; i < line.Count - 1; i++) + { + double ax = line[i].Easting, ay = line[i].Northing; + double dx = line[i + 1].Easting - ax, dy = line[i + 1].Northing - ay; + double len2 = dx * dx + dy * dy; + if (len2 < 1e-12) continue; + double t = Math.Clamp(((e - ax) * dx + (n - ay) * dy) / len2, 0, 1); + double ex = ax + dx * t - e, ey = ay + dy * t - n; + double d = ex * ex + ey * ey; + if (d < best) { best = d; heading = Math.Atan2(dx, dy); } + } + return heading; } /// diff --git a/Tests/AgOpenWeb.Services.Tests/Pipeline/EndOfPassWithoutTurnTests.cs b/Tests/AgOpenWeb.Services.Tests/Pipeline/EndOfPassWithoutTurnTests.cs index 021a4cda..acb2ac1c 100644 --- a/Tests/AgOpenWeb.Services.Tests/Pipeline/EndOfPassWithoutTurnTests.cs +++ b/Tests/AgOpenWeb.Services.Tests/Pipeline/EndOfPassWithoutTurnTests.cs @@ -43,6 +43,7 @@ public class EndOfPassWithoutTurnTests private LocalPlane _plane = null!; private readonly List<(double Northing, GpsCycleResult Result)> _cycles = new(); private double _northing; + private Boundary? _boundary; [SetUp] public void SetUp() @@ -105,16 +106,18 @@ public void TearDown() }; /// A 100 x 100 m field, the headland line at ±40 unless given, AutoSteer on pass 0 of a north AB line, U-turns off. - private void Field(bool hard, List? headlandLine = null) + private void Field(bool hard, List? headlandLine = null, double fieldHalf = 50, params BoundaryPolygon[] obstacles) { var boundary = new Boundary { OuterBoundary = new BoundaryPolygon { - Points = Square(50).Select(p => new BoundaryPoint(p.Easting, p.Northing, 0)).ToList(), + Points = Square(fieldHalf).Select(p => new BoundaryPoint(p.Easting, p.Northing, 0)).ToList(), IsHard = hard, }, }; + boundary.InnerBoundaries.AddRange(obstacles); + _boundary = boundary; _pipeline.SetHasActiveField(true); _pipeline.SetBoundary(boundary); _pipeline.SetHeadlandLine(headlandLine ?? Square(Headland)); @@ -280,4 +283,104 @@ public void A_planned_or_driven_turn_is_not_the_end_of_the_pass() var across = ctx with { CurrentPosition = new Position { Easting = PassEasting, Northing = 41, Heading = 90, Speed = 2 } }; Assert.That(sm.EndOfPassWithoutTurn(in across, guidance, new YouTurnWorkingState()), Is.False, "turning by hand, across the line"); } + /// A tree chosen as Go around: a hard circle of m. + private static BoundaryPolygon Tree(double e, double n, double radius) + { + var tree = new BoundaryPolygon { AtObstacle = ObstacleAction.GoRound, IsHard = true }; + for (int i = 0; i < 24; i++) + { + double a = i * 2 * System.Math.PI / 24; + tree.Points.Add(new BoundaryPoint(e + radius * System.Math.Sin(a), n + radius * System.Math.Cos(a), 0)); + } + return tree; + } + + /// + /// Drive along (the tractor following it, heading along each piece) + /// from northing to , 0.2 m a fix, northwards. + /// + private void DriveAlong(List line, double from, double to) + { + var pts = line.Where(p => p.Northing >= from - 1 && p.Northing <= to + 1).OrderBy(p => p.Northing).ToList(); + for (int i = 0; i < pts.Count - 1; i++) + { + var a = pts[i]; var b = pts[i + 1]; + double len = System.Math.Sqrt((b.Easting - a.Easting) * (b.Easting - a.Easting) + (b.Northing - a.Northing) * (b.Northing - a.Northing)); + if (len < 1e-6) continue; + double heading = System.Math.Atan2(b.Easting - a.Easting, b.Northing - a.Northing) * 180 / System.Math.PI; + for (double t = 0; t < len; t += 0.2) + { + double e = a.Easting + (b.Easting - a.Easting) * t / len; + _northing = a.Northing + (b.Northing - a.Northing) * t / len; + if (_northing < from || _northing > to) continue; + var ll = _plane.ConvertGeoCoordToWgs84(new GeoCoord(_northing, e)); + _gps.UpdateGpsData(new GpsData + { + CurrentPosition = new Position { Latitude = ll.Latitude, Longitude = ll.Longitude, Heading = (heading + 360) % 360, Speed = 2.0 }, + FixQuality = 4, + SatellitesInUse = 14, + Hdop = 0.7, + IsValid = true, + }); + } + } + } + + /// Pass 0 bent round as the pipeline bends it. + private static List BentPass(Boundary boundary) + { + var config = ConfigurationStore.Instance; + var goAround = new GoAroundPasses(config, new AgOpenWeb.Services.Geometry.PolygonOffsetService()); + var track = Models.Track.Track.FromABLine("AB", new Vec3(0, -100, 0), new Vec3(0, 100, 0)); + var pass = new List(); + for (double n = -200; n <= 200; n += 1) pass.Add(new Vec3(PassEasting, n, 0)); + var bend = goAround.For(track, PassEasting, pass, boundary); + Assert.That(bend?.GoneRound, Is.Not.Empty, "the pass goes round the tree"); + return bend!.Points; + } + + // Renato's report of 14 Nov (Tapada da Casa, simulator, 8 m front boom): 3R, bent round two + // trees in the headland, left the crop 36 degrees off the pass. Measured against the pass the + // tractor looked like one turning by hand, the end of the pass was not seen, and AutoSteer + // steered on along the bend for 40 m until the fence guard stopped it with the boom over the fence. + + [Test] + public void On_a_bend_round_a_tree_in_the_headland_AutoSteer_still_goes_off_a_metre_past_the_line() + { + // A wide headland (the line at ±36, the fence at ±80) and a tree in it, its keep-out + // reaching back to 39: the pass runs on 44 m through the headland, as 3R did. + const double line = 36; + Field(hard: true, Square(line), 80, Tree(PassEasting + 0.5, 44, 1.0)); + var bent = BentPass(_boundary!); + + // Where the bend leaves the crop, and how far off the pass it runs there. + int i = bent.FindIndex(p => p.Northing > line); + var a = bent[i - 1]; var b = bent[i + 1]; + double off = System.Math.Abs(System.Math.Atan2(b.Easting - a.Easting, b.Northing - a.Northing)) * 180 / System.Math.PI; + TestContext.Out.WriteLine($"bend leaves the crop at ({bent[i].Easting:F2},{bent[i].Northing:F2}), {off:F0} degrees off the pass"); + Assert.That(off, Is.GreaterThan(25), "the bend leaves the crop well off the pass's direction"); + + DriveAlong(bent, 20, 60); + + var offAt = WentOff(); + Assert.That(offAt, Is.Not.Null, "AutoSteer steered on along the bend towards the fence"); + Assert.That(offAt!.Value.Result.DisengageReason, Does.Contain("end of the pass and no U-turn")); + Assert.That(offAt.Value.Northing, Is.InRange(line + 0.5, line + 1.6), "a metre past the headland line"); + } + + [Test] + public void A_bend_through_the_headland_back_into_the_crop_is_not_the_end() + { + // A tree on the pass in a headland strip across the field (y 10 to 16, the notch of the + // test above), with 24 m of crop past it: the bend dips into the strip and comes back. + var notched = new List + { + new(-40, -40, 0), new(40, -40, 0), new(40, 10, 0), new(-10, 10, 0), + new(-10, 16, 0), new(40, 16, 0), new(40, 40, 0), new(-40, 40, 0), + }; + Field(hard: true, notched, 50, Tree(PassEasting + 0.5, 13, 1.5)); + var bent = BentPass(_boundary!); + DriveAlong(bent, -10, 30); + Assert.That(WentOff(), Is.Null, "the strip is crossed on the bend"); + } } From 49e4a003a83b75d0f1e6f12f00c9929257f151d1 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Nov 2026 14:54:29 +0000 Subject: [PATCH 2/2] Snake: a pass is worked by the map, not by a section on somewhere on it Track.WorkedPaths is set when any section is on anywhere along the pass, and the snake asked it before the job's map. A pass cut by a bay counted as worked once its short piece was done, so its main piece was never turned onto (official #815, the job harness on Baetas: "End of field reached" with 53 % of the field undone). With the map, worked means at least half covered (IsPassWorked); the mark is used only with no map. At the end ahead, a pass still to do on the whole is worked there when the piece a turn there lands on (from that end back until the pass leaves the cultivated area) is at least half covered (IsPieceDoneAtThisEnd): the main piece gets its turn from the other end. A pass with no bay is one piece, so one done only at this end is still turned onto here. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VKEBDFmQpFVsahedjtN2AZ --- CLAUDE.md | 2 +- README.md | 2 +- .../YouTurn/YouTurnPathingService.cs | 74 ++++++++++---- .../YouTurn/YouTurnStateMachine.cs | 50 +++++++--- .../YouTurn/SnakeOpenEndTests.cs | 98 +++++++++++++++++++ 5 files changed, 190 insertions(+), 36 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 63bd014b..16bcef9f 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -593,7 +593,7 @@ turns from the on-screen buttons) with paths built by `YouTurnCreationService`: - The turn is built from the headland crossing **ahead** of the tractor (`FindTrackHeadlandIntersectionAhead`; one behind only past the last crossing). When no real turn fits, the simple fallback (a semicircle) is seated back along the pass until it keeps `UTurnDistanceFromBoundary` from the fence: to the headland crossing and up to 30 m into the field past it, starting ahead of the tractor (`SimpleFallback`, fork only: renatorgr/AgOpenWeb#130, #135). Only on a straight pass: the fallback runs straight from the tractor, so on a curve it is seated nowhere (as official) and built again as the tractor comes closer; seated to fit 190 m away on a boundary curve, it was never joined and the missed turn was made "from here". It is then used only if every point of it stays in the field, outside obstacles a turn may not drive through (`LeavesField`, #698); otherwise no turn, the creation-failed sound and "No U-turn fits inside the boundary at this end - take over manually". A hard boundary refuses a turn that would start under or behind the tractor. - A turn tried again with the turn lines moved in (the implement over a hard fence, a landing on a bend, an obstacle in its legs: `CreateTurnPath`'s setback loop, which moves the whole fence in) is seated back from the end of the pass the first try found: walking back from that end, at the moved line's crossing nearest to it (`YouTurnCreationInput.TurnLineEndIndex`, `TurnLineEndOnRetry`; not after a gap crossed). Moved in, the fence's turn line also crossed the pass where the fence bulges in beside it, and the first crossing ahead was taken: on Tapada da Casa an 8 m front boom near the north fence moved the line in 5 m and the turn was made 260 m early, beside the east bulge, landing on the next pass inside its crop and leaving a strip of it unsprayed. Test: `TurnSetbackStaysAtEndTests`. Official since #828; not after a gap crossed is fork only. - The next pass must have room at this end: at least 20 m of it inside the cultivated area back from the end ahead (`IsPathLandable` / `LandingLength`), not just any piece of it inside the field; otherwise "End of field reached - no room on the next pass at this end" (#712). In this fork, when no pass has room but the current pass goes on inside the field past the headland ahead (a corner the headland clips off it, a gap), there is no turn and AutoSteer drives on across it: "No room to turn at this headland - driving on across it" (`GapAhead` / `DrivesOnAcrossGap`, renatorgr/AgOpenWeb#129; upstream left it out until after the v1.0 freeze). The pass must run on at least 10 m inside past the gap (`MinRunAfterGap`): a sliver or spike of the headland line is not more field. -- Skip worked passes (the snake order): when the order's next pass has no room at this end, or its turn does not build, the turn goes onto the nearest pass still to do that has room, first on the side the order is going, then on the other side (one turn built per side; a pass at least the skip away before a nearer one; none further than twice the skip's step, else "End of field reached" and the operator decides), never against the side the operator picked with the U-turn arrow (`TurnOntoNearestOpenPass`; AgOpenGPS's `GetNextNotWorkedTrack` only steps outward on the turn's side past worked tracks, so the other side, passes nearer than the skip and the room and build checks are ours); the order is planned again from where it lands. The same nearest pass is tried first when the order's next pass still to do is further than twice the skip's step (`MaxFallbackStep`; the passes in between worked before), from any pass nearer than the order's, either side: the turn went from 17R to 7R along the headland, 80 m, with 16R and 14R open beside the tractor (a tester's field, simulator), and on 21R, with the order's 8R 13 passes over, 15R six over was never looked at and the turn onto 8R did not build, so there was no turn; with none of them taking a turn, the order's pass is turned onto as before. Fork only. With the operator's headland on and the sections off in it (`Tool.IsHeadlandSectionControl`), a pass the snake turns onto (the order's or a nearest one) must also have that room in the crop, inside the headland line (`SnakeLandable`): the turn line is the fence moved in by the U-turn distance, so a pass lying in the headland band had room there and nothing to work, and unpainted it was still to do (a tester's field, simulator: 13L, 12 m of crop, and 15L, none, in a corner of the headland, before 12L). Fork only. An end where no turn was made is remembered (`_failedEnd`) and not tried again on every cycle until the pass, the direction of travel, the nudge, the order, the field or headland, the skip, the U-turn arrow or a turn setting changes. The order is also planned again when the headland line or the outer boundary changes (`SnakeFieldStamp`), and forgotten with what was found about each pass when the line moves (`ForgetPassStatus`: Reference Line Nudge, an end trimmed; the moved line's pass N is another strip, and the old answers turned the snake past clean passes on Tapada da Casa). In the headland the snake looks for its next turn only heading out of the field (no cultivated ground ahead on the pass): a turn that ends in the headland leaves the tractor facing the field on its new pass, which is not the end of that pass. Fork only: a pass worked on the whole (half covered, `IsPassWorked`) is still to do at the end ahead when less than half of its 20 m inside the cultivated area next to that end is covered (`YouTurnPathingService.IsPassOpenAtThisEnd`, asked in `WorkedPasses` when a turn is planned, once per end): on a tester's field a pass 71 % covered, with its last 100 m at that end left, was turned past. Such a pass is turned onto only when the turn built gets to what is left: at least 5 m of unworked crop along the turn's last 20 m and 20 m on past it (`YouTurnPathingService.TurnReachesUnworkedGround`); otherwise the turn is dropped and the pass counts worked at this end (`TurnMissesOpenEnd`). A turn lands abreast of where it leaves the tractor's pass, and the next pass's end can lie beyond that (a slanting headland, a turn seated back from a fence or a tree): on a tester's field the turn from 1L onto 2R, seated back 12.5 m, drove the whole of 2R a second time and its end stayed as it was. +- Skip worked passes (the snake order): when the order's next pass has no room at this end, or its turn does not build, the turn goes onto the nearest pass still to do that has room, first on the side the order is going, then on the other side (one turn built per side; a pass at least the skip away before a nearer one; none further than twice the skip's step, else "End of field reached" and the operator decides), never against the side the operator picked with the U-turn arrow (`TurnOntoNearestOpenPass`; AgOpenGPS's `GetNextNotWorkedTrack` only steps outward on the turn's side past worked tracks, so the other side, passes nearer than the skip and the room and build checks are ours); the order is planned again from where it lands. The same nearest pass is tried first when the order's next pass still to do is further than twice the skip's step (`MaxFallbackStep`; the passes in between worked before), from any pass nearer than the order's, either side: the turn went from 17R to 7R along the headland, 80 m, with 16R and 14R open beside the tractor (a tester's field, simulator), and on 21R, with the order's 8R 13 passes over, 15R six over was never looked at and the turn onto 8R did not build, so there was no turn; with none of them taking a turn, the order's pass is turned onto as before. Fork only. With the operator's headland on and the sections off in it (`Tool.IsHeadlandSectionControl`), a pass the snake turns onto (the order's or a nearest one) must also have that room in the crop, inside the headland line (`SnakeLandable`): the turn line is the fence moved in by the U-turn distance, so a pass lying in the headland band had room there and nothing to work, and unpainted it was still to do (a tester's field, simulator: 13L, 12 m of crop, and 15L, none, in a corner of the headland, before 12L). Fork only. An end where no turn was made is remembered (`_failedEnd`) and not tried again on every cycle until the pass, the direction of travel, the nudge, the order, the field or headland, the skip, the U-turn arrow or a turn setting changes. The order is also planned again when the headland line or the outer boundary changes (`SnakeFieldStamp`), and forgotten with what was found about each pass when the line moves (`ForgetPassStatus`: Reference Line Nudge, an end trimmed; the moved line's pass N is another strip, and the old answers turned the snake past clean passes on Tapada da Casa). In the headland the snake looks for its next turn only heading out of the field (no cultivated ground ahead on the pass): a turn that ends in the headland leaves the tractor facing the field on its new pass, which is not the end of that pass. Fork only: a pass worked on the whole (half covered, `IsPassWorked`) is still to do at the end ahead when less than half of its 20 m inside the cultivated area next to that end is covered (`YouTurnPathingService.IsPassOpenAtThisEnd`, asked in `WorkedPasses` when a turn is planned, once per end): on a tester's field a pass 71 % covered, with its last 100 m at that end left, was turned past. Such a pass is turned onto only when the turn built gets to what is left: at least 5 m of unworked crop along the turn's last 20 m and 20 m on past it (`YouTurnPathingService.TurnReachesUnworkedGround`); otherwise the turn is dropped and the pass counts worked at this end (`TurnMissesOpenEnd`). A turn lands abreast of where it leaves the tractor's pass, and the next pass's end can lie beyond that (a slanting headland, a turn seated back from a fence or a tree): on a tester's field the turn from 1L onto 2R, seated back 12.5 m, drove the whole of 2R a second time and its end stayed as it was. Fork only: with the job's map, worked means the map (half covered), never `Track.WorkedPaths`, which a section on anywhere along the pass sets (the mark is used only with no map); and a pass still to do on the whole is worked at the end ahead when the piece a turn there lands on (from that end back until the pass leaves the cultivated area) is at least half covered (`IsPieceDoneAtThisEnd`): a bay cuts the pass and its short piece is done. On Baetas (official #815, the job harness) the short pieces below a bay marked their passes worked and the main pieces were never turned onto, "End of field reached" with 53 % undone; a pass with no bay is one piece, so one done only at this end is still turned onto here. - The end of a pass at a hard outer boundary with no turn planned or driven there ("End of field reached", no turn that fits, U-turns off): as the pivot gets 1 m into the headland from the cultivated area (`HeadlandEntryCheckAfter`), on the line and heading out (the pass leaves the outer boundary within 300 m with no 10 m of field on it first, `YouTurnPathingService.PassLeavesFieldAhead`), AutoSteer goes off with the alarm, "AutoSteer off: end of the pass and no U-turn here. Turn by hand." (`YouTurnStateMachine.EndOfPassWithoutTurn`, step 5b'''' in `GpsPipelineService`; once per entry; fork only). Left on, AutoSteer steered on along the line through the fence (as AgOpenGPS, which only sends geoStop): on Tapada da Casa the simulator drove on 2.8 km. Heading into the field, a headland strip with field past it, or a soft boundary do not count. This stop, the line facing out of the field below and a pass still open at this end measure from the headland line, where the crop ends, not from the turn line the U-turn uses (`TickContext.PassEndLine`, the line the pipeline arms them on: from the turn line a headland more than 10 m wider than the U-turn distance never stopped AutoSteer). - A turn planned for the end the tractor was heading to is dropped when the tractor turns round on the pass (by hand, or the simulator's turn-around), and the turn at the end ahead is planned (TURNED ROUND in `YouTurnStateMachine.Tick`, `_plannedSameWay`; AgOpenGPS drops a created turn once the pivot leaves the turn area without it, `ResetCreatedYouTurn`). Kept, the headland stop and the fence guard below took it for a turn planned at the other end and left AutoSteer on (Tapada da Casa, simulator). Fork only. - Only a turn keeps the implement off a hard outer boundary. With none planned or driven, AutoSteer on, going forward, the tractor inside the fence, an end of the implement (half `ActualToolWidth` plus the offset either side of the tool) heading out across the fence, more than 0.5 m out and further than now within 2 s of travel, takes AutoSteer off with the alarm (`HardFenceGuard`, step 5b'''' in `GpsPipelineService`; fork only). AutoSteer joining a line from a corner with the line moved five passes swung an 8 m front boom 0.9 m over the fence. The 2 s are along the curve steered, not straight ahead: the implement's ends turn with the tractor about its turning centre, from the last cycle's steer angle (the one the line asked for when AutoSteer has just gone on; a cycle with no line steered before it waits for the next). Straight ahead leaves a bend: on a boundary curve's first lap (the boom's end on the fence) it took AutoSteer off at a bend with the tractor on the line (`HardFenceGuardPipelineTests`). Joining a line sideways beside the fence is an S that keeps the implement inside (*Stepping onto a line beside a hard fence*). Driving along the fence a little over it, back in, or below walking pace (`MinLookAhead`) does not count. diff --git a/README.md b/README.md index 72cce743..fdb30c26 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ Most of these have been offered to the official project too. When one is merged - **Headland gaps.** With "Drive across a headland gap up to" set on the U-turn tab, a pass that crosses a narrow tip of the field is driven on with the sections off, and the turn is made at the far end. - **U-turns with no room.** When no proper turn fits, the simple turn is moved back into the field instead of crossing the boundary. Where the headland clips a corner off the pass, AutoSteer drives on across it and says so (a thin spike of the headland line doesn't count). - **U-turn beside a fence that comes in.** With a hard boundary, a turn that had to be moved back to keep the implement off the fence at the end of the pass could be made far too early instead, where the fence comes in beside the pass: it landed on the next pass inside its crop and left a strip of it unsprayed. Now it is moved back from the end of the pass only. -- **Skip worked passes.** A pass more than half done but left short of the end the tractor is at (its 20 m next to that end mostly not covered) counts as still to do there, so the turn can go onto it. When the order's next pass is more than four passes over (the ones in between were done before), the turn goes onto the nearest pass still to do instead of along the headland to it, and the order starts again from there. (The turn onto the nearest pass with room is official since #807.) +- **Skip worked passes.** A pass more than half done but left short of the end the tractor is at (its 20 m next to that end mostly not covered) counts as still to do there, so the turn can go onto it. When the order's next pass is more than four passes over (the ones in between were done before), the turn goes onto the nearest pass still to do instead of along the headland to it, and the order starts again from there. A pass counts as done by what is painted on the map, not by a section having been on somewhere on it: where a bay cuts the passes, doing the short piece no longer marks the whole pass done, and the long piece gets its turn from the other end. (The turn onto the nearest pass with room is official since #807.) - **AutoSteer and a hard boundary.** At the end of a pass with no U-turn there (no pass with room at this end, no turn that fits, U-turn off), AutoSteer goes off with the alarm a metre into the headland, so there is the headland to turn by hand; before, it steered on through the fence. And with no U-turn planned, when an end of the implement is heading out across a hard boundary (AutoSteer joining a line from a corner, for example), AutoSteer goes off with the alarm. It looks along the curve the tractor is steering, so on the first lap of a boundary curve it no longer goes off at a bend with the tractor on the line. A U-turn planned for one end is dropped when the tractor turns round on the pass, so these still work at the other end. - **AutoSteer flickering on and off.** With the steer switch / button set to Button, when the app took AutoSteer off just as it went on (a hard boundary warning, for example), the steer module's answer to the engage turned it on again, and it went on and off for seconds with the alarm. The app now ignores the module's "steering" for 1 s after it takes AutoSteer off itself. - **No line outside a hard boundary.** A pass that lies wholly outside a hard boundary (a lap of a boundary curve outside the fence, say) is no longer shown when the tractor is outside the field with AutoSteer off: the nearest pass inside is shown. Moving the line onto such a pass is refused with a message. diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs index c23bc4f4..6f08edfe 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnPathingService.cs @@ -747,11 +747,15 @@ public void AdvanceSnakeSequence(YouTurnWorkingState turn, int pass, Func - /// Is pass worked? It is when sections were on while the tractor was - /// on it (), or when the job's coverage covers at - /// least half of it inside the cultivated area, as the map shows it: a pass worked before the - /// field was reopened, or while guidance was on another pass. A U-turn painted across the - /// end of a pass leaves the pass unworked. + /// Is pass worked? With the job's map, when it covers at least half of + /// the pass inside the cultivated area: a pass worked before the field was reopened, or while + /// guidance was on another pass, counts, and a U-turn painted across the end of a pass leaves + /// it unworked. With no map, when sections were on while the tractor was on it + /// (). With a map that mark is not asked: it is set + /// by a section on anywhere along the pass, so a pass split by a bay into a short piece and the + /// main one counted worked once its short piece was done, and the main piece was never turned + /// onto (official #815, the job harness on Baetas: "End of field reached" with 53 % of the + /// field undone). Fork only. /// public bool IsPassWorked( Models.Track.Track track, @@ -761,8 +765,7 @@ public bool IsPassWorked( IReadOnlyList? headlandLine, Func? isPointCovered) { - if (track.IsPathWorked(pass)) return true; - if (isPointCovered == null || track.Points.Count < 2) return false; + if (isPointCovered == null || track.Points.Count < 2) return track.IsPathWorked(pass); double widthMinusOverlap = _configStore.ActualToolWidth - _configStore.Tool.Overlap; double distAway = PassGeometry.Offset(track, pass, widthMinusOverlap, nudgeOffset); @@ -875,11 +878,40 @@ public bool IsPassOpenAtThisEnd(Models.Track.Track track, double abHeading, int { if (isPointCovered == null || track.Points.Count < 2) return false; var stretch = EndStretch(track, abHeading, pathsAway, nudgeOffset, boundary, headlandLine, position, headingSameWay, headlandAhead); - if (stretch.Count == 0) return false; + return stretch.Count > 0 && CoveredCount(stretch, isPointCovered) * 2 < stretch.Count; + } + + /// + /// The longest piece of a pass walks, m. + /// + private const double MaxPieceLength = 2000.0; + + /// + /// Is pass , still to do on the whole (), + /// done at the end the tractor is heading for? It is when the piece of it a turn here lands on, + /// from that end back the way the tractor came until the pass leaves the cultivated area, is at + /// least half covered on the job's map: a bay or a headland band cuts the pass, and its piece on + /// this side of it is worked. A turn onto it here would drive that piece again and stop at the + /// band; the rest is done from the far end. A pass with no band across it is one piece, so the + /// whole pass is judged, as does. False with no map, or with no piece + /// found near this end. Fork only (official #815). + /// + public bool IsPieceDoneAtThisEnd(Models.Track.Track track, double abHeading, int pathsAway, double nudgeOffset, + Boundary? boundary, IReadOnlyList? headlandLine, Position position, bool headingSameWay, double headlandAhead, + Func? isPointCovered) + { + if (isPointCovered == null || track.Points.Count < 2) return false; + var piece = EndStretch(track, abHeading, pathsAway, nudgeOffset, boundary, headlandLine, position, headingSameWay, + headlandAhead, MaxPieceLength, step: 2.0); + return piece.Count > 0 && CoveredCount(piece, isPointCovered) * 2 >= piece.Count; + } + + private static int CoveredCount(List points, Func isPointCovered) + { int covered = 0; - foreach (var p in stretch) + foreach (var p in points) if (isPointCovered(p.Easting, p.Northing)) covered++; - return covered * 2 < stretch.Count; + return covered; } /// @@ -937,16 +969,18 @@ public bool TurnReachesUnworkedGround(IReadOnlyList turnPath, Boundary? bo } /// - /// Points every metre along the first of pass - /// inside the cultivated area back from the end the tractor is - /// heading for: from where the pass leaves the area at that end (found from the point abreast - /// of the end, forward to the edge when that point is inside, back to the area when it is - /// past the pass's end, as ), back the way the tractor came. + /// Points every m along the first m + /// ( unless given) of pass inside the + /// cultivated area back from the end the tractor is heading for: from where the pass leaves the + /// area at that end (found from the point abreast of the end, forward to the edge when that + /// point is inside, back to the area when it is past the pass's end, as + /// ), back the way the tractor came, as far as the pass stays inside. /// private List EndStretch(Models.Track.Track track, double abHeading, int pathsAway, double nudgeOffset, - Boundary? boundary, IReadOnlyList? headlandLine, Position position, bool headingSameWay, double headlandAhead) + Boundary? boundary, IReadOnlyList? headlandLine, Position position, bool headingSameWay, double headlandAhead, + double length = OpenEndStretch, double step = 1.0) { - const double Step = 1.0; + double Step = step; const double Search = 300.0; var points = new List(); if (headlandLine is not { Count: >= 3 } && boundary?.OuterBoundary is not { IsValid: true }) return points; @@ -997,12 +1031,12 @@ double Dist(int a, int b) if (!found) return points; } double sinceLast = Step; - for (double length = 0; InRange(at) && length <= OpenEndStretch && InsideAt(at); at -= fwd) + for (double back = 0; InRange(at) && back <= length && InsideAt(at); at -= fwd) { if (sinceLast >= Step) { points.Add(new Vec2(curve[at].Easting, curve[at].Northing)); sinceLast = 0; } if (!InRange(at - fwd)) break; double d = Dist(at, at - fwd); - length += d; + back += d; sinceLast += d; } return points; @@ -1033,7 +1067,7 @@ double Dist(int a, int b) if (InsideAtT(tAt - s * d)) { tAt -= s * d; found = true; break; } if (!found) return points; } - for (double d = Step / 2; d <= OpenEndStretch; d += Step) + for (double d = Step / 2; d <= length; d += Step) { double t = tAt - s * d; if (!InsideAtT(t)) break; diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs index a3fca319..21d54fa5 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs @@ -86,6 +86,11 @@ public sealed class YouTurnStateMachine private readonly Dictionary _openAtEnd = new(); private (int Pass, bool SameWay) _openAtEndKey; + // Of the passes still to do on the whole, the ones whose piece at the end ahead is done + // (YouTurnPathingService.IsPieceDoneAtThisEnd: a bay cuts the pass), decided once per end and + // cleared with _openAtEnd. Fork only. + private readonly Dictionary _doneAtEnd = new(); + // Which way the last turn stepped across the field (+1 to a higher pass, -1 lower) and on // which track (by name: a nudge or edit makes a new Track). A normal turn keeps stepping that // way, as AgOpenGPS does by flipping isTurnLeft after each turn, so the work goes on across @@ -962,6 +967,7 @@ public void ForgetTrack(YouTurnWorkingState turn) turn.AltSign = 0; _coveredPasses.Clear(); _openAtEnd.Clear(); + _doneAtEnd.Clear(); _failedEnd = null; } @@ -977,13 +983,14 @@ public void ForgetPassStatus(YouTurnWorkingState turn) turn.SnakeIndex = -1; _coveredPasses.Clear(); _openAtEnd.Clear(); + _doneAtEnd.Clear(); _failedEnd = null; } /// - /// Is pass worked: marked so, or at least half covered on the job's map - /// inside the cultivated area (, the snake's - /// test). For AutoSteer choosing the pass to join (fork only). + /// Is pass worked: at least half covered on the job's map inside the + /// cultivated area, else (no map) marked so (, + /// the snake's test). For AutoSteer choosing the pass to join (fork only). /// public bool IsPassWorked(Models.Track.Track track, int pass, double nudgeOffset, Boundary? boundary, IReadOnlyList? headlandLine, Func? isPointCovered) => @@ -1375,7 +1382,7 @@ private bool TurnMissesOpenEnd(in TickContext ctx, Models.Track.Track track, int YouTurnEffects effects, string? messageBefore) { if (turn.TurnPath is not { } path || !_openAtEnd.TryGetValue(pass, out bool open) || !open) return false; - if (!track.IsPathWorked(pass) && !(_coveredPasses.TryGetValue(pass, out bool covered) && covered)) return false; + if (!(_coveredPasses.TryGetValue(pass, out bool covered) && covered)) return false; if (_pathing.TurnReachesUnworkedGround(path, ctx.Boundary, ctx.WorkEndLine, ctx.IsPointCovered)) return false; _logger.LogDebug("[YouTurn] Snake pass {Pass} is left to do only beyond where a turn here lands on it - worked at this end", pass); _openAtEnd[pass] = false; @@ -1430,13 +1437,15 @@ internal static long SnakeFieldStamp(Boundary? boundary, IReadOnlyList? he private static long Cm(double metres) => (long)Math.Round(metres * 100); /// - /// The worked-pass test for the snake: marked worked, or covered on the job's map + /// The worked-pass test for the snake: covered on the job's map, else marked worked /// (). The map test is remembered for the /// plan in hand (see ); the tractor's own pass, the only one /// being covered as it drives, is never the next pass of the plan. With /// (the track's heading, radians) for a turn at the end ahead, a /// pass worked on the whole but still to do at that end is not worked - /// (; fork only). + /// (), and a pass still to do on the + /// whole whose piece at that end is done (a bay cuts it) is worked there + /// (; fork only). /// private Func WorkedPasses(in TickContext ctx, Models.Track.Track track, GuidanceWorkingState guidance, YouTurnWorkingState turn, double? atEndOf) @@ -1452,6 +1461,7 @@ private Func WorkedPasses(in TickContext ctx, Models.Track.Track trac { _coveredPasses.Clear(); _openAtEnd.Clear(); + _doneAtEnd.Clear(); _coveredPassesKey = (plan, track, nudge, boundary, headlandLine); } int current = guidance.HowManyPathsAway; @@ -1459,6 +1469,7 @@ private Func WorkedPasses(in TickContext ctx, Models.Track.Track trac if (_openAtEndKey != (current, sameWay)) { _openAtEnd.Clear(); + _doneAtEnd.Clear(); _openAtEndKey = (current, sameWay); } var position = ctx.CurrentPosition; @@ -1466,16 +1477,26 @@ private Func WorkedPasses(in TickContext ctx, Models.Track.Track trac var workEndLine = ctx.WorkEndLine; return pass => { - bool worked = track.IsPathWorked(pass); - if (!worked && !_coveredPasses.TryGetValue(pass, out worked)) + // No map: the mark set while sections were on on the pass. + if (isPointCovered == null) return track.IsPathWorked(pass); + if (!_coveredPasses.TryGetValue(pass, out bool worked)) _coveredPasses[pass] = worked = _pathing.IsPassWorked(track, pass, nudge, boundary, headlandLine, isPointCovered); - if (!worked || atEndOf is not { } abHeading || pass == current || isPointCovered == null) return worked; - // Worked on the whole but left to do at this end: open here, so a turn can be made - // onto it (Tapada da Casa: 27R, its last 100 m at the end ahead, was turned past). - if (!_openAtEnd.TryGetValue(pass, out bool open)) - _openAtEnd[pass] = open = _pathing.IsPassOpenAtThisEnd(track, abHeading, pass, nudge, boundary, workEndLine, + if (atEndOf is not { } abHeading || pass == current) return worked; + if (worked) + { + // Worked on the whole but left to do at this end: open here, so a turn can be made + // onto it (Tapada da Casa: 27R, its last 100 m at the end ahead, was turned past). + if (!_openAtEnd.TryGetValue(pass, out bool open)) + _openAtEnd[pass] = open = _pathing.IsPassOpenAtThisEnd(track, abHeading, pass, nudge, boundary, workEndLine, + position, sameWay, headlandAhead, isPointCovered); + return !open; + } + // Still to do on the whole, but its piece on this side of a bay is done: a turn onto it + // here drives that piece again and stops at the bay (official #815, Baetas). + if (!_doneAtEnd.TryGetValue(pass, out bool done)) + _doneAtEnd[pass] = done = _pathing.IsPieceDoneAtThisEnd(track, abHeading, pass, nudge, boundary, workEndLine, position, sameWay, headlandAhead, isPointCovered); - return !open; + return done; }; } @@ -1946,6 +1967,7 @@ private void CompleteTurn( WorkedPasses(in ctx, ctx.SelectedTrack, guidance, turn, atEndOf: null)); _coveredPasses.Clear(); // decided again for the next turn _openAtEnd.Clear(); + _doneAtEnd.Clear(); } } else if (turn.PlannedNextPath is { } planned) diff --git a/Tests/AgOpenWeb.Services.Tests/YouTurn/SnakeOpenEndTests.cs b/Tests/AgOpenWeb.Services.Tests/YouTurn/SnakeOpenEndTests.cs index 7d42bab9..f23e73b7 100644 --- a/Tests/AgOpenWeb.Services.Tests/YouTurn/SnakeOpenEndTests.cs +++ b/Tests/AgOpenWeb.Services.Tests/YouTurn/SnakeOpenEndTests.cs @@ -244,4 +244,102 @@ public void The_stretch_at_this_end_is_measured_from_where_the_pass_leaves_the_f headingSameWay: false, headlandAhead: 15, null), Is.False, "no map"); }); } + + // Official #815 (the job harness on Baetas): a bay cuts the passes, and a section on anywhere on + // a pass marked it worked (Track.WorkedPaths), so once its short piece was done its main piece + // was never turned onto. With the map the snake asks the map, and at each end the piece of the + // pass a turn there lands on. + + /// + /// The cultivated area with a bay in from the east fence between n = -18 and -10, to x = 12: the + /// passes from 3 (x = 14) east are cut into a 22 m piece in the south and a 50 m piece north of it. + /// + private static List AreaWithABay() => new PolygonOffsetService().CalculatePointHeadings(new List + { new(-40, -40), new(40, -40), new(40, -18), new(12, -18), new(12, -10), new(40, -10), new(40, 40), new(-40, 40) }); + + private (YouTurnEffects Effects, YouTurnWorkingState Turn) OnPass6(List area, Func covered, + bool north, double northing) + { + var ctx = new YouTurnStateMachine.TickContext( + new Position { Easting = PassX(6), Northing = northing, Heading = north ? 0 : 180, Speed = 2.78 }, + _track, _boundary, area, + UTurnSkipRows: 1, IsSkipWorkedMode: true, + HeadlandCalculatedWidth: 10.0, HeadlandDistance: 10.0, + IsPointCovered: covered); + var guidance = new GuidanceWorkingState { HowManyPathsAway = 6, IsHeadingSameWay = north }; + var turn = new YouTurnWorkingState { IsEnabled = true }; + var effects = _sm.Tick(ctx, guidance, turn); + return (effects, turn); + } + + [Test] + public void A_pass_cut_by_a_bay_with_only_its_short_piece_done_gets_the_turn_at_the_other_end() + { + var area = AreaWithABay(); + var covered = AllCoveredBut(n => n > -18); // pass 4: only its short piece south of the bay + for (int k = -10; k <= 9; k++) _track.MarkPathWorked(k); // sections were on on every pass + Assert.That(_pathing.IsPassWorked(_track, 4, 0, _boundary, area, covered), Is.False, + "the scenario: 22 m of pass 4's 72 m in the field are covered"); + + var (effects, turn) = OnPass6(area, covered, north: true, northing: 25); + + Assert.Multiple(() => + { + Assert.That(turn.TurnPath, Is.Not.Null, $"a turn, not \"{effects.StatusMessage}\""); + Assert.That(turn.ReturnPassTargetPath, Is.EqualTo(4), "onto pass 4's main piece, at its north end"); + }); + } + + [Test] + public void A_pass_cut_by_a_bay_is_not_turned_onto_where_its_piece_is_done() + { + // The same field at the south end: a turn onto pass 4 there drives its 22 m piece again and + // stops at the bay. Its main piece is turned onto from the north. + var area = AreaWithABay(); + var covered = AllCoveredBut(n => n > -18); + var position = new Position { Easting = PassX(6), Northing = -25, Heading = 180 }; + Assert.That(_pathing.IsPieceDoneAtThisEnd(_track, 0, 4, 0, _boundary, area, position, + headingSameWay: false, headlandAhead: 15, covered), Is.True, "the scenario: the piece at the south end is done"); + + var (effects, turn) = OnPass6(area, covered, north: false, northing: -25); + + Assert.Multiple(() => + { + Assert.That(turn.TurnPath, Is.Null, $"no turn onto pass {turn.ReturnPassTargetPath}"); + Assert.That(effects.StatusMessage, Is.EqualTo("Field complete — all tracks worked")); + }); + } + + [Test] + public void A_pass_with_no_bay_done_only_at_this_end_still_gets_the_turn_here() + { + // No bay: pass 4 is one piece, done only in its south 20 m (sections on there, so marked + // worked). A turn onto it at the south end drives on over the rest. + var covered = AllCoveredBut(n => n > -20); + for (int k = -10; k <= 9; k++) _track.MarkPathWorked(k); + var position = new Position { Easting = PassX(6), Northing = -25, Heading = 180 }; + Assert.That(_pathing.IsPieceDoneAtThisEnd(_track, 0, 4, 0, _boundary, _square, position, + headingSameWay: false, headlandAhead: 15, covered), Is.False, "the piece at this end is the whole pass, 25 % done"); + + var (effects, turn) = DriveSouthOnPass6(covered); + + Assert.Multiple(() => + { + Assert.That(turn.TurnPath, Is.Not.Null, $"a turn, not \"{effects.StatusMessage}\""); + Assert.That(turn.ReturnPassTargetPath, Is.EqualTo(4)); + }); + } + + [Test] + public void With_no_map_the_mark_says_which_passes_are_worked() + { + _track.MarkPathWorked(4); + Assert.Multiple(() => + { + Assert.That(_pathing.IsPassWorked(_track, 4, 0, _boundary, _square, null), Is.True); + Assert.That(_pathing.IsPassWorked(_track, 5, 0, _boundary, _square, null), Is.False); + Assert.That(_pathing.IsPassWorked(_track, 4, 0, _boundary, _square, (e, n) => false), Is.False, + "with a map, the map: nothing is covered"); + }); + } }