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"); + } }