diff --git a/Shared/AgOpenWeb.Models/Pipeline/YouTurnSnapshot.cs b/Shared/AgOpenWeb.Models/Pipeline/YouTurnSnapshot.cs index cd6dd731..24283c64 100644 --- a/Shared/AgOpenWeb.Models/Pipeline/YouTurnSnapshot.cs +++ b/Shared/AgOpenWeb.Models/Pipeline/YouTurnSnapshot.cs @@ -39,6 +39,7 @@ public record YouTurnSnapshot // Turn path public IReadOnlyList? TurnPath { get; init; } public int PathIndex { get; init; } + public Vec3? ArcStart { get; init; } // Direction public bool IsTurnLeft { get; init; } diff --git a/Shared/AgOpenWeb.Models/Pipeline/YouTurnWorkingState.cs b/Shared/AgOpenWeb.Models/Pipeline/YouTurnWorkingState.cs index 30f0b605..6539b795 100644 --- a/Shared/AgOpenWeb.Models/Pipeline/YouTurnWorkingState.cs +++ b/Shared/AgOpenWeb.Models/Pipeline/YouTurnWorkingState.cs @@ -37,6 +37,9 @@ public class YouTurnWorkingState // Turn path public List? TurnPath { get; set; } public int PathIndex { get; set; } + // Where TurnPath leaves the current pass (the arc's first point); null when the path has + // no entry leg. The approach alarm and the too-close rule measure against it. + public Vec3? ArcStart { get; set; } // Direction public bool IsTurnLeft { get; set; } @@ -106,6 +109,7 @@ public void Reset() IsTriggered = false; IsExecuting = false; TurnPath = null; + ArcStart = null; PathIndex = 0; PreviousDistToTurnEnd = double.MaxValue; DistanceToHeadland = double.MaxValue; @@ -128,6 +132,7 @@ public void CompleteTurn() IsExecuting = false; IsTriggered = false; TurnPath = null; + ArcStart = null; LastTurnWasLeft = IsTurnLeft; HasCompletedFirstTurn = true; YouTurnCounter = 0; diff --git a/Shared/AgOpenWeb.Models/State/YouTurnState.cs b/Shared/AgOpenWeb.Models/State/YouTurnState.cs index c6f77f57..ca03f3ac 100644 --- a/Shared/AgOpenWeb.Models/State/YouTurnState.cs +++ b/Shared/AgOpenWeb.Models/State/YouTurnState.cs @@ -63,6 +63,14 @@ public int PathIndex set => SetProperty(ref _pathIndex, value); } + // Where TurnPath leaves the current pass (the arc's first point); null without an entry leg. + private Vec3? _arcStart; + public Vec3? ArcStart + { + get => _arcStart; + set => SetProperty(ref _arcStart, value); + } + // Direction private bool _isTurnLeft; public bool IsTurnLeft @@ -225,6 +233,7 @@ public void Reset() IsTriggered = false; IsExecuting = false; TurnPath = null; + ArcStart = null; PathIndex = 0; DistanceToHeadland = double.MaxValue; DistanceToTrigger = 0; @@ -246,6 +255,7 @@ public void CompleteTurn() IsExecuting = false; IsTriggered = false; TurnPath = null; + ArcStart = null; LastTurnWasLeft = IsTurnLeft; HasCompletedFirstTurn = true; YouTurnCounter = 0; diff --git a/Shared/AgOpenWeb.Models/YouTurn/YouTurnCreationOutput.cs b/Shared/AgOpenWeb.Models/YouTurn/YouTurnCreationOutput.cs index 6af2d818..d7dcf50a 100644 --- a/Shared/AgOpenWeb.Models/YouTurn/YouTurnCreationOutput.cs +++ b/Shared/AgOpenWeb.Models/YouTurn/YouTurnCreationOutput.cs @@ -69,5 +69,11 @@ public class YouTurnCreationOutput /// Closest point on turn line where turn ends. /// public Vec3 OutClosestTurnPoint { get; set; } + + /// + /// Where the path leaves the current pass: the first point of the arc, before the + /// entry leg was prepended. Null when the path has no entry leg. + /// + public Vec3? ArcStart { get; set; } } } diff --git a/Shared/AgOpenWeb.RemoteServer/wwwroot/app.js b/Shared/AgOpenWeb.RemoteServer/wwwroot/app.js index 72cc12e1..53435d32 100644 --- a/Shared/AgOpenWeb.RemoteServer/wwwroot/app.js +++ b/Shared/AgOpenWeb.RemoteServer/wwwroot/app.js @@ -1995,8 +1995,15 @@ document.getElementById('ln-fieldtools').addEventListener('pointerdown', e => { const anyOpen = ['fieldtools', 'offsetfix'].some(id => document.getElementById(id).classList.contains('open')); if (anyOpen) lnCloseAll(); else lnOpen('fieldtools', 'ln-fieldtools'); }); +// Field Tools work on the open field; without one, say so instead of asking (#305). +function requireField() { + if (scene && scene.hasField) return true; + showToast(tr('Open a field first')); + return false; +} document.getElementById('ft-deleteapplied').addEventListener('pointerdown', e => { e.stopPropagation(); + if (!requireField()) return; showConfirm(tr('Delete Applied Area'), tr('Delete all applied area coverage? This cannot be undone.'), () => transport.send('field.deleteApplied')); @@ -2032,7 +2039,7 @@ document.getElementById('rp-resume').addEventListener('pointerdown', e => { e.st document.getElementById('rp-reverse').addEventListener('pointerdown', e => { e.stopPropagation(); transport.send('recpath.reverse'); }); // Boundary recording menu. document.getElementById('ft-boundary').addEventListener('pointerdown', e => { - e.stopPropagation(); transport.send('boundary.refresh'); lnOpen('boundarymenu', 'ln-fieldtools', renderBoundaryMenu); + e.stopPropagation(); if (!requireField()) return; transport.send('boundary.refresh'); lnOpen('boundarymenu', 'ln-fieldtools', renderBoundaryMenu); }); document.getElementById('bm-back').addEventListener('pointerdown', e => { e.stopPropagation(); lnOpen('fieldtools', 'ln-fieldtools'); }); @@ -2042,7 +2049,7 @@ document.getElementById('bm-back').addEventListener('pointerdown', e => { e.stop let fbTab = 'tracks'; // active FB tab let fbSel = -1; // selected track index in the Tracks tab document.getElementById('ft-fieldbuilder').addEventListener('pointerdown', e => { - e.stopPropagation(); fbTab = 'tracks'; fbSel = -1; lnOpen('fieldbuilder', 'ln-fieldtools', renderFieldBuilder); + e.stopPropagation(); if (!requireField()) return; fbTab = 'tracks'; fbSel = -1; lnOpen('fieldbuilder', 'ln-fieldtools', renderFieldBuilder); }); document.getElementById('fieldbuilder').addEventListener('pointerdown', e => e.stopPropagation()); // panel taps don't pan the map (NO_SCRIM) document.querySelector('#fieldbuilder .fb-back').addEventListener('pointerdown', e => { e.stopPropagation(); lnOpen('fieldtools', 'ln-fieldtools'); }); diff --git a/Shared/AgOpenWeb.RemoteServer/wwwroot/i18n/en.json b/Shared/AgOpenWeb.RemoteServer/wwwroot/i18n/en.json index 17a4f601..56fab3ec 100644 --- a/Shared/AgOpenWeb.RemoteServer/wwwroot/i18n/en.json +++ b/Shared/AgOpenWeb.RemoteServer/wwwroot/i18n/en.json @@ -542,6 +542,7 @@ "On-map track / headland / tram editor": "On-map track / headland / tram editor", "On-Screen Buttons": "On-Screen Buttons", "On-screen Kbd": "On-screen Kbd", + "Open a field first": "Open a field first", "Open Field Only": "Open Field Only", "Open simulator": "Open simulator", "Operator": "Operator", diff --git a/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs b/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs index 365b3ff4..96e54081 100644 --- a/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs +++ b/Shared/AgOpenWeb.Services/Pipeline/GpsPipelineService.cs @@ -1418,6 +1418,7 @@ private void ProcessCycle(GpsData data) IsExecuting = src.IsExecuting, TurnPath = src.TurnPath, PathIndex = src.PathIndex, + ArcStart = src.ArcStart, IsTurnLeft = src.IsTurnLeft, LastTurnWasLeft = src.LastTurnWasLeft, DistanceToHeadland = src.DistanceToHeadland, diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.Orchestration.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.Orchestration.cs index b2ea5190..843d00b6 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.Orchestration.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.Orchestration.cs @@ -53,6 +53,10 @@ public readonly record struct TurnPathResult(List? Path, bool UsedFallback /// auto-engaged — the operator should be warned to take over. /// public bool ClearanceBlocked { get; init; } + + /// Where the path leaves the current pass (see ); + /// null for a fallback path, which has no entry leg. + public Vec3? ArcStart { get; init; } } /// @@ -225,7 +229,7 @@ public TurnPathResult CreateTurnPath( _logger.LogDebug("[YouTurn] Path created with {Count} points, net={Net:F0}° cumulative={Cum:F0}° (setback {S:F2}m)", path.Count, netHeadingChange * 180 / Math.PI, totalHeadingChange * 180 / Math.PI, extraSetback); - return new TurnPathResult(path, UsedFallback: false); + return new TurnPathResult(path, UsedFallback: false) { ArcStart = output.ArcStart }; } // Creation failed outright. diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.cs index 008b4ef6..c93e956d 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnCreationService.cs @@ -69,6 +69,7 @@ public YouTurnCreationService( private int semiCircleIndex = -1; private bool isOutOfBounds = false; private bool isOutSameCurve = false; + private Vec3? arcStart; private int youTurnPhase = 0; /// @@ -98,6 +99,7 @@ public YouTurnCreationOutput CreateTurn(YouTurnCreationInput input) youTurnPhase = 0; isOutOfBounds = false; isOutSameCurve = false; + arcStart = null; // Lateral span of the arc = the pass spacing. NO ToolOffset compensation: // AgOpenGPS shifts the arc by ±2*ToolOffset because its steering applies the @@ -136,6 +138,7 @@ public YouTurnCreationOutput CreateTurn(YouTurnCreationInput input) output.DistancePivotToTurnLine = Distance(ytList[0], input.PivotPosition); output.InClosestTurnPoint = inClosestTurnPt.ClosePt; output.OutClosestTurnPoint = outClosestTurnPt.ClosePt; + output.ArcStart = arcStart; } else { @@ -1288,6 +1291,7 @@ private bool AddCurveSequenceLines(YouTurnCreationInput input) bool sameWay = input.IsHeadingSameWay; // Entry leg: walk the current pass inward from the arc start for legLength METRES. + arcStart = ytList[0]; WalkLegByDistance(input.GuidancePoints, ytList[0], inClosestTurnPt.CurveIndex, sameWay ? -1 : 1, legLength, insertFront: true); diff --git a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs index 1ecde8a2..a5c7c044 100644 --- a/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs +++ b/Shared/AgOpenWeb.Services/YouTurn/YouTurnStateMachine.cs @@ -57,8 +57,11 @@ public sealed class YouTurnStateMachine // ~20 degrees. Wider tolerances risk creating turns while mid-turn when heading swings. private static readonly double AlignmentTolerance = Math.PI / 9; - // Trigger when the tractor is within this far of the pre-computed turn start point. - private const double TriggerProximityMeters = 2.0; + // A tractor this close to the planned path, heading along it, is on the path and the + // turn starts (#306). The entry leg is the current pass itself, so the tractor joins it + // wherever it is when the turn is armed; an exact rendezvous with the path's first point + // was never needed. + private const double OnPathLateralMeters = 1.5; // Completion thresholds. // ClosestApproachThreshold: if the tractor was this close to the end @@ -217,14 +220,23 @@ public YouTurnEffects Tick(in TickContext ctx, GuidanceWorkingState guidance, Yo HandleNormalCreation(in ctx, track, abHeading, currentPosition, headingRadians, guidance, turn, effects); } // ── TURN TRIGGER ──────────────────────────────────────────────── - else if (turn.TurnPath != null && turn.TurnPath.Count > 2 && !turn.IsTriggered && !turn.IsExecuting) + // The planned path begins with an entry leg that IS the current pass, so the tractor + // is on the path as soon as it is on that leg and takes the turn from wherever it + // joins (#306). The old rule, within 2 m of the path's first point, never fired when + // the turn was armed late and the leg already began behind the tractor; the path then + // stayed on screen while the tractor drove into the headland with no warning. + if (turn.TurnPath != null && turn.TurnPath.Count > 2 && !turn.IsTriggered && !turn.IsExecuting) { var turnStart = turn.TurnPath[0]; double distToTurnStart = Math.Sqrt( (currentPosition.Easting - turnStart.Easting) * (currentPosition.Easting - turnStart.Easting) + (currentPosition.Northing - turnStart.Northing) * (currentPosition.Northing - turnStart.Northing)); + var arcStart = turn.ArcStart ?? turnStart; + double distToArcStart = Math.Sqrt( + (currentPosition.Easting - arcStart.Easting) * (currentPosition.Easting - arcStart.Easting) + + (currentPosition.Northing - arcStart.Northing) * (currentPosition.Northing - arcStart.Northing)); - // Publish the pivot→trigger distance for the UI countdown widget as ARC LENGTH + // Publish the pivot→path-start distance for the UI countdown widget as ARC LENGTH // ALONG THE TRACK, not straight-line. On a curve that wraps a field end, the turn // start can be far to the east while the tractor drives west around the wrap toward // it — a Euclidean distance would COUNT UP as it drives away in a straight line, when @@ -234,32 +246,63 @@ public YouTurnEffects Tick(in TickContext ctx, GuidanceWorkingState guidance, Yo ? ArcLengthAlongTrack(track.Points, currentPosition, turnStart) : distToTurnStart; - // Alarm once as the tractor comes within 20 m of the turn. AgOpenGPS tests a - // 18–20 m band, which a fast approach steps straight over between fixes — the - // alarm then sounded on roughly every other turn (#150). - if (distToTurnStart <= 20.0 && !_approachAlarmed) + // Alarm once as the tractor comes within 20 m of the arc, where the path leaves + // the pass. AgOpenGPS tests a 18–20 m band, which a fast approach steps straight + // over between fixes — the alarm then sounded on roughly every other turn (#150). + if (distToArcStart <= 20.0 && !_approachAlarmed) { _approachAlarmed = true; effects.ApproachAlarmSound = true; } - // Trigger on physical proximity (straight-line): the tractor must actually reach the - // turn start, regardless of how the arc-length display reads. - if (distToTurnStart <= TriggerProximityMeters) + if (IsOnPath(turn.TurnPath, currentPosition, headingRadians)) { turn.IsTriggered = true; turn.IsExecuting = true; effects.StatusMessage = "YouTurn triggered!"; - _logger.LogDebug("[YouTurn] Triggered at {Dist:F2}m from turn start", distToTurnStart); + _logger.LogDebug("[YouTurn] Triggered on the path, {Dist:F1}m from the arc", distToArcStart); + } + else if (isAlignedWithABLine && IsHeadingAlong(turn.TurnPath, headingRadians) + && IsBehind(arcStart, currentPosition, headingRadians)) + { + // The arc already begins behind the tractor, so this path cannot be joined. + // Turn from here instead, the way a manual turn does; when there is no room + // for that either, say so, because the operator has to turn by hand now. + var fromHere = _creation.CreateManualArcPath( + currentPosition, abHeading, turn.IsTurnLeft, + ctx.Boundary, guidance, ctx.UTurnSkipRows, turn.NextTrack); + if (fromHere.Count > 2) + { + turn.TurnPath = fromHere; + turn.ArcStart = null; + turn.YouTurnCounter = 0; + turn.IsTriggered = true; + turn.IsExecuting = true; + effects.StatusMessage = "YouTurn started from here: the planned arc was already behind the tractor"; + _logger.LogDebug("[YouTurn] Arc start {Dist:F1}m behind the tractor - turning from here ({Count} points)", + distToArcStart, fromHere.Count); + } + else + { + _logger.LogDebug("[YouTurn] Arc start {Dist:F1}m behind the tractor and no room to turn from here - turn missed", + distToArcStart); + DiscardPlannedTurn(turn); + effects.TurnCreationFailedSound = true; + effects.StatusMessage = "U-turn missed: too close to the headland to turn. Take over manually."; + } + effects.SyncTurnPathToMap = true; + effects.SyncNextTrackToMap = true; + } + else if (isInHeadlandZone) + { + // Drove into the headland without ever joining the path: the turn is missed. + _logger.LogDebug("[YouTurn] Entered the headland without joining the path - turn missed"); + DiscardPlannedTurn(turn); + effects.TurnCreationFailedSound = true; + effects.StatusMessage = "U-turn missed: entered the headland off the turn path. Take over manually."; + effects.SyncTurnPathToMap = true; + effects.SyncNextTrackToMap = true; } - } - // ── TURN RESET (drove past turn start into headland without triggering) ─ - else if (turn.TurnPath != null && !turn.IsTriggered && isInHeadlandZone) - { - _logger.LogDebug("[YouTurn] Entered headland without triggering - resetting turn"); - DiscardPlannedTurn(turn); - effects.SyncTurnPathToMap = true; - effects.SyncNextTrackToMap = true; } // ── TURN COMPLETION ───────────────────────────────────────────── @@ -625,6 +668,57 @@ public static void DiscardPlannedTurn(YouTurnWorkingState turn) // ── Private helpers ───────────────────────────────────────────────── + /// + /// True when the tractor is on the path: within of its + /// nearest segment and heading along that segment (within ). + /// The heading test keeps the exit leg, which runs the other way on the next pass, and an + /// omega's crossing legs from counting. No allocation; the path is a few hundred points. + /// + internal static bool IsOnPath(List path, Position position, double headingRadians) + { + double px = position.Easting, py = position.Northing; + double bestD2 = double.MaxValue; + int best = -1; + for (int i = 0; i < path.Count - 1; i++) + { + double ax = path[i].Easting, ay = path[i].Northing; + double dx = path[i + 1].Easting - ax, dy = path[i + 1].Northing - ay; + double len2 = dx * dx + dy * dy; + if (len2 < 1e-9) continue; + double t = ((px - ax) * dx + (py - ay) * dy) / len2; + if (t < 0) t = 0; else if (t > 1) t = 1; + double ex = ax + t * dx - px, ey = ay + t * dy - py; + double d2 = ex * ex + ey * ey; + if (d2 < bestD2) { bestD2 = d2; best = i; } + } + if (best < 0 || bestD2 > OnPathLateralMeters * OnPathLateralMeters) return false; + + double segHeading = Math.Atan2(path[best + 1].Easting - path[best].Easting, + path[best + 1].Northing - path[best].Northing); + double diff = headingRadians - segHeading; + while (diff > Math.PI) diff -= 2 * Math.PI; + while (diff < -Math.PI) diff += 2 * Math.PI; + return Math.Abs(diff) < AlignmentTolerance; + } + + /// True when the tractor drives the way the path sets off (its first segment), within 90°. + /// A tractor going back down the pass is not a candidate for a turn planned at the far end. + internal static bool IsHeadingAlong(List path, double headingRadians) + { + double segHeading = Math.Atan2(path[1].Easting - path[0].Easting, path[1].Northing - path[0].Northing); + double diff = headingRadians - segHeading; + while (diff > Math.PI) diff -= 2 * Math.PI; + while (diff < -Math.PI) diff += 2 * Math.PI; + return Math.Abs(diff) < Math.PI / 2; + } + + /// True when lies behind the tractor along its heading. + internal static bool IsBehind(Vec3 point, Position position, double headingRadians) + { + double dx = point.Easting - position.Easting, dy = point.Northing - position.Northing; + return dx * Math.Sin(headingRadians) + dy * Math.Cos(headingRadians) < 0; + } + private void HandleSnakeCreation( in TickContext ctx, Models.Track.Track track, @@ -842,6 +936,7 @@ private void CreatePathAndSync( _approachAlarmed = false; turn.TurnPath = result.Path; + turn.ArcStart = result.ArcStart; turn.YouTurnCounter = 0; if (!result.UsedFallback && effects.StatusMessage == null) effects.StatusMessage = $"YouTurn path created ({result.Path.Count} points)"; diff --git a/Shared/AgOpenWeb.ViewModels/MainViewModel.ApplyResults.cs b/Shared/AgOpenWeb.ViewModels/MainViewModel.ApplyResults.cs index 68477e87..691dd88e 100644 --- a/Shared/AgOpenWeb.ViewModels/MainViewModel.ApplyResults.cs +++ b/Shared/AgOpenWeb.ViewModels/MainViewModel.ApplyResults.cs @@ -135,6 +135,7 @@ public void ApplyGpsCycleResult(GpsCycleResult result) // only on turn start/end) — see TMP-001 resolution. sy.TurnPath = yt.TurnPath is List tp ? tp : yt.TurnPath?.ToList(); sy.PathIndex = yt.PathIndex; + sy.ArcStart = yt.ArcStart; sy.IsTurnLeft = yt.IsTurnLeft; sy.LastTurnWasLeft = yt.LastTurnWasLeft; sy.DistanceToHeadland = yt.DistanceToHeadland; diff --git a/Shared/AgOpenWeb.ViewModels/MainViewModel.Commands.Track.cs b/Shared/AgOpenWeb.ViewModels/MainViewModel.Commands.Track.cs index 1ad59c03..93ea1afe 100644 --- a/Shared/AgOpenWeb.ViewModels/MainViewModel.Commands.Track.cs +++ b/Shared/AgOpenWeb.ViewModels/MainViewModel.Commands.Track.cs @@ -52,6 +52,12 @@ private void DeleteContourFile() public void DeleteAppliedAreaConfirmed() { + if (!IsFieldOpen) + { + ReportFailure("Open a field first"); + return; + } + _coverageMapService.ClearAll(); // AgOpenGPS "delete all contours and sections": the contour strips go too, and the diff --git a/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnDistanceToTriggerTests.cs b/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnDistanceToTriggerTests.cs index a65c914d..cee60ea0 100644 --- a/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnDistanceToTriggerTests.cs +++ b/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnDistanceToTriggerTests.cs @@ -50,7 +50,7 @@ public void Tick_with_armed_path_far_from_trigger_publishes_actual_distance() { Assert.That(turn.DistanceToTrigger, Is.EqualTo(10.0).Within(0.01), "Tick must publish the Euclidean distance from current pivot to TurnPath[0]"); - Assert.That(turn.IsTriggered, Is.False, "10 m is far past the 2 m trigger threshold"); + Assert.That(turn.IsTriggered, Is.False, "10 m short of the path, the tractor is not on it yet"); Assert.That(turn.IsExecuting, Is.False); }); } @@ -76,7 +76,7 @@ public void Tick_with_tractor_at_trigger_point_publishes_near_zero_distance() { Assert.That(turn.DistanceToTrigger, Is.EqualTo(0.0).Within(0.01), "At the trigger point, the published distance must be ~0"); - Assert.That(turn.IsTriggered, Is.True, "Within the proximity threshold, the trigger must fire"); + Assert.That(turn.IsTriggered, Is.True, "On the path and heading along it, the trigger must fire"); Assert.That(turn.IsExecuting, Is.True); }); } @@ -294,13 +294,15 @@ private static YouTurnStateMachine.TickContext BuildTickContext( /// (start of the arc); the rest of the geometry is irrelevant for the /// distance-to-trigger publication path. /// + // A short path heading north from the start point, along the AB line the tractor drives + // (#306: the trigger needs the tractor ON the path and heading along it). private static List BuildTurnPath(double turnStartEasting, double turnStartNorthing) { return new List { new(turnStartEasting, turnStartNorthing, 0), - new(turnStartEasting + 1, turnStartNorthing + 1, 0), - new(turnStartEasting + 2, turnStartNorthing + 2, 0), + new(turnStartEasting, turnStartNorthing + 1, 0), + new(turnStartEasting, turnStartNorthing + 2, 0), }; } } diff --git a/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnTriggerOnPathTests.cs b/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnTriggerOnPathTests.cs new file mode 100644 index 00000000..d5c256ae --- /dev/null +++ b/Tests/AgOpenWeb.Services.Tests/YouTurn/YouTurnTriggerOnPathTests.cs @@ -0,0 +1,261 @@ +// AgOpenWeb +// Copyright (C) 2024-2026 AgOpenWeb Contributors +// +// Licensed under GNU GPL v3. See LICENSE.md. + +using System; +using System.Collections.Generic; +using System.Linq; + +using AgOpenWeb.Models; +using AgOpenWeb.Models.Base; +using AgOpenWeb.Models.Configuration; +using AgOpenWeb.Models.Pipeline; +using AgOpenWeb.Models.State; +using AgOpenWeb.Services.Geometry; +using AgOpenWeb.Services.YouTurn; + +using Microsoft.Extensions.Logging.Abstractions; +using NSubstitute; + +namespace AgOpenWeb.Services.Tests.YouTurn; + +/// +/// #306: an automatic turn starts when the tractor is ON the planned path, heading along +/// it, wherever it joins. The planned path's entry leg is the current pass itself, so a turn +/// armed late (its first point already behind the tractor) is taken all the same. A path the +/// tractor cannot join any more is replanned from where it is, or dropped with a warning; +/// it never stays on screen while the tractor drives into the headland. +/// +[TestFixture] +public class YouTurnTriggerOnPathTests +{ + // ── The rule, on hand-built paths ──────────────────────────────────── + + [Test] + public void OnTheEntryLeg_HeadingAlongIt_TheTurnStarts() + { + var sm = BuildStateMachine(); + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0) }; + + // 8 m along a leg that began at (0,0): the old 2 m rendezvous with the first point is long gone. + var effects = sm.Tick(Ctx(0, 8, headingDeg: 0), new GuidanceWorkingState(), turn); + + Assert.Multiple(() => + { + Assert.That(turn.IsTriggered, Is.True); + Assert.That(turn.IsExecuting, Is.True); + Assert.That(effects.StatusMessage, Is.EqualTo("YouTurn triggered!")); + }); + } + + [Test] + public void BesideThePath_NoTrigger() + { + var sm = BuildStateMachine(); + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0) }; + + sm.Tick(Ctx(3, 8, headingDeg: 0), new GuidanceWorkingState(), turn); + + Assert.That(turn.IsTriggered, Is.False, "3 m off the line is not on the path"); + Assert.That(turn.TurnPath, Is.Not.Null, "still armed: the tractor may come back onto the line"); + } + + [Test] + public void OnTheLineButHeadingTheOtherWay_NoTrigger() + { + var sm = BuildStateMachine(); + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0) }; + + sm.Tick(Ctx(0, 8, headingDeg: 180), new GuidanceWorkingState(), turn); + + Assert.That(turn.IsTriggered, Is.False, "the exit leg of a turn runs the other way; heading must match"); + } + + [Test] + public void ShortOfThePath_NoTriggerYet_AndTheCountdownIsPublished() + { + var sm = BuildStateMachine(); + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0) }; + + sm.Tick(Ctx(0, -12, headingDeg: 0), new GuidanceWorkingState(), turn); + + Assert.That(turn.IsTriggered, Is.False); + Assert.That(turn.DistanceToTrigger, Is.EqualTo(12.0).Within(0.01)); + } + + [Test] + public void TheApproachAlarm_MeasuresToTheArc_NotToTheLegStart() + { + var sm = BuildStateMachine(); + // Leg from (0,0), arc at (0,20). 18 m before the LEG start is 38 m from the arc: no alarm. + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0) }; + + var far = sm.Tick(Ctx(0, -18, headingDeg: 0), new GuidanceWorkingState(), turn); + Assert.That(far.ApproachAlarmSound, Is.False, "38 m from the arc"); + + var near = sm.Tick(Ctx(0, 2, headingDeg: 0), new GuidanceWorkingState(), turn); + Assert.That(near.ApproachAlarmSound, Is.True, "18 m from the arc"); + } + + [Test] + public void InTheHeadlandOffThePath_TheTurnIsDroppedWithAWarning() + { + var sm = BuildStateMachine(); + // A path on another line entirely; the tractor is in the headland (between the ±45 + // turn line and the ±50 fence) and never joined it. + var turn = new YouTurnWorkingState { TurnPath = LegThenArc(easting: 20), ArcStart = new Vec3(20, 20, 0) }; + + var effects = sm.Tick(Ctx(0, 47, headingDeg: 0), new GuidanceWorkingState(), turn); + + Assert.Multiple(() => + { + Assert.That(turn.TurnPath, Is.Null, "a missed turn must not stay on screen"); + Assert.That(turn.IsTriggered, Is.False); + Assert.That(effects.TurnCreationFailedSound, Is.True, "the operator must hear that the turn is missed"); + Assert.That(effects.StatusMessage, Does.Contain("missed")); + Assert.That(effects.SyncTurnPathToMap, Is.True); + }); + } + + [Test] + public void ArcAlreadyBehindTheTractor_TurnsFromHereInstead() + { + var sm = BuildStateMachine(); + // The planned arc left the line at (0,20) and the tractor, 2 m off that line, is at + // y = 30: it cannot join the path any more, but there is room to turn from here. + var turn = new YouTurnWorkingState + { + TurnPath = LegThenArc(), ArcStart = new Vec3(0, 20, 0), IsTurnLeft = false, + }; + var planned = turn.TurnPath; + + var effects = sm.Tick(Ctx(2, 30, headingDeg: 0), new GuidanceWorkingState(), turn); + + Assert.Multiple(() => + { + Assert.That(turn.IsExecuting, Is.True, "replanned from the tractor and started"); + Assert.That(turn.TurnPath, Is.Not.SameAs(planned)); + Assert.That(turn.TurnPath!.Count, Is.GreaterThan(2)); + Assert.That(effects.StatusMessage, Does.Contain("from here")); + }); + } + + // ── Through the real planner: renatorgr's turn (#306) ──────────────── + + /// + /// The turn is armed 15 m before the turn line, which puts the planned path's first + /// point behind the tractor (16 m entry leg plus the arc's own setback). The tractor + /// is on the entry leg, so the turn must start before it reaches the headland. + /// + [Test] + public void ArmedLate_OnTheLeg_TheTurnStartsBeforeTheHeadland() + { + var (sm, ctxAt) = BuildPlannerBench(); + var guidance = new GuidanceWorkingState(); + var turn = new YouTurnWorkingState { IsEnabled = true }; + + double y = 30; // turn line at y = 45 + bool pathWasBehind = false; + for (int i = 0; i < 40 && !turn.IsExecuting; i++, y += 0.5) + { + turn.YouTurnCounter++; + sm.Tick(ctxAt(0, y, 0), guidance, turn); + if (turn.TurnPath != null && turn.TurnPath[0].Northing < y) pathWasBehind = true; + Assert.That(turn.CurrentZone, Is.EqualTo(TractorZone.InCultivatedArea), + $"reached the headland at y={y:F1} without the turn starting"); + } + + Assert.That(turn.IsExecuting, Is.True, "the turn must start while the tractor is still on the pass"); + Assert.That(pathWasBehind, Is.True, "the scenario: the path's first point was behind the tractor when armed"); + } + + // ── Fixtures ───────────────────────────────────────────────────────── + + /// A 20 m leg north from (easting, 0), then a quarter arc of 8 m radius to the east. + private static List LegThenArc(double easting = 0) + { + var path = new List(); + for (double n = 0; n <= 20; n += 1) path.Add(new Vec3(easting, n, 0)); + for (int k = 1; k <= 9; k++) + { + double a = k * Math.PI / 18; // 10° steps + path.Add(new Vec3(easting + 8 - 8 * Math.Cos(a), 20 + 8 * Math.Sin(a), a)); + } + return path; + } + + private static YouTurnStateMachine BuildStateMachine() + { + ConfigurationStore.SetInstance(new ConfigurationStore()); + var c = ConfigurationStore.Instance; + c.Vehicle.Wheelbase = 2.5; + c.Vehicle.MaxSteerAngle = 35; + c.NumSections = 1; + c.Tool.SetSectionWidth(0, 600); + c.Guidance.UTurnRadius = 8.0; + var polygonOffset = Substitute.For(); + var creation = new YouTurnCreationService(NullLogger.Instance, polygonOffset, c); + var pathing = new YouTurnPathingService(NullLogger.Instance, c); + return new YouTurnStateMachine(creation, pathing, NullLogger.Instance, c); + } + + /// 100 m square fence at ±50, turn line at ±45, AB line north along x = 0. + private static YouTurnStateMachine.TickContext Ctx(double e, double n, double headingDeg) + { + var outer = new BoundaryPolygon + { + Points = new List { new(-50, -50), new(50, -50), new(50, 50), new(-50, 50) } + .Select(p => new BoundaryPoint(p.Easting, p.Northing, 0)).ToList(), + }; + return new YouTurnStateMachine.TickContext( + new Position { Easting = e, Northing = n, Heading = headingDeg, Speed = 2.5 }, + Models.Track.Track.FromABLine("AB-test", new Vec3(0, -100, 0), new Vec3(0, 100, 0)), + new Boundary { OuterBoundary = outer }, + new List { new(-45, -45, 0), new(45, -45, 0), new(45, 45, 0), new(-45, 45, 0) }, + UTurnSkipRows: 0, IsSkipWorkedMode: false, + HeadlandCalculatedWidth: 5.0, HeadlandDistance: 5.0); + } + + /// + /// The real planner on a 120 m square field: fence at ±60, turn line 15 m inside it, + /// 6 m tool, 8 m radius, 16 m legs; an AB line north through the middle. + /// + private static (YouTurnStateMachine, Func) BuildPlannerBench() + { + ConfigurationStore.SetInstance(new ConfigurationStore()); + var c = ConfigurationStore.Instance; + c.Vehicle.Wheelbase = 2.5; + c.Vehicle.MaxSteerAngle = 35; + c.Tool.Overlap = 0; + c.NumSections = 1; + c.Tool.SetSectionWidth(0, 600); + c.Guidance.UTurnRadius = 8.0; + c.Guidance.UTurnExtension = 16.0; + c.Guidance.UTurnDistanceFromBoundary = 2.0; + c.Guidance.UTurnStyle = 0; + + var offset = new PolygonOffsetService(); + var creation = new YouTurnCreationService(NullLogger.Instance, offset, c); + var pathing = new YouTurnPathingService(NullLogger.Instance, c); + var sm = new YouTurnStateMachine(creation, pathing, NullLogger.Instance, c); + + var outer = new List { new(-60, -60), new(60, -60), new(60, 60), new(-60, 60) }; + var boundary = new Boundary + { + OuterBoundary = new BoundaryPolygon + { + Points = outer.Select(p => new BoundaryPoint(p.Easting, p.Northing, 0)).ToList(), + }, + }; + var turnLine = offset.CalculatePointHeadings(offset.CreateInwardOffset(outer, 15.0)!); + var track = Models.Track.Track.FromABLine("AB", new Vec3(0, -100, 0), new Vec3(0, 100, 0)); + + YouTurnStateMachine.TickContext CtxAt(double e, double n, double headingDeg) => new( + new Position { Easting = e, Northing = n, Heading = headingDeg, Speed = 2.78 }, + track, boundary, turnLine, + UTurnSkipRows: 0, IsSkipWorkedMode: false, + HeadlandCalculatedWidth: 15.0, HeadlandDistance: 15.0); + return (sm, CtxAt); + } +}