From 185916c9e3e34c02210cea030b547bd334e5b2fd Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Nov 2026 11:16:02 +0000 Subject: [PATCH] Front boom: back on the line after a Go-around bend; bend radius it can follow Two faults with a front-mounted implement round a Go-around obstacle (a tester's field, simulator, 8 m front boom 4.5 m ahead of the pivot): 1. After each bend the steered line stayed 0.40 m beside the pass for about 100 m. FrontToolLine.Steered moves each point of the line by the bend measured around it, and an open pass has a single point at each end of a long straight stretch: the shift worked out at the end of the bend was carried by the long segment to the far end. Open passes now get points every metre near the ends of a stretch longer than the window (closed passes stay one for one, so PassUnfit still lines up). 2. The Go-around bend was built at the setting's radius (5 m), tighter than the pivot can follow while holding a front implement on it: the implement runs outside a bend of radius R by sqrt(R^2 + h^2) - R, so the boom left the bend by 1.35 m. With a front-fixed implement the radius is now never under sqrt(minR^2 + h^2) (6.46 m for this rig). Tests: FrontToolLineTests (a bend, then a 200 m stretch: back on the pass within a few metres; a closed pass keeps its points), GoAroundPassesTests (the radius floor for a front implement, none for a rear one). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01VKEBDFmQpFVsahedjtN2AZ --- CLAUDE.md | 7 ++- .../AgOpenWeb.Services/Track/FrontToolLine.cs | 48 ++++++++++++++++- .../Track/GoAroundPasses.cs | 15 +++++- .../Track/FrontToolLineTests.cs | 53 +++++++++++++++++++ .../Track/GoAroundPassesTests.cs | 27 ++++++++++ 5 files changed, 144 insertions(+), 6 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 55624aae2..6bc789ae9 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -588,7 +588,7 @@ turns from the on-screen buttons) with paths built by `YouTurnCreationService`: - While a turn is driven (`IsExecuting`) the pass shown is the one it lands on (`GpsPipelineService.TurnTargetPass`): the highlighted line, the tool's band, the guidelines and the pass label, as AgOpenGPS, which moves to the next pass when the turn starts (`CYouTurn.YouTurnTrigger`). The pass worked (`HowManyPathsAway`: the worked-pass mark, the snake order, a turn backed out of, #588) still moves on in `CompleteTurn`. - A turn that loops (the next pass closer than twice the radius) is moved in to stay inside the turn line, as AgOpenGPS; when the headland is narrower than the loop is deep it leaves its pass inside the headland line, still working, and the status says "Headland too narrow for this U-turn" with how far before the line (`TurnStartInsideHeadlandLine`). - Obstacles (inner boundaries) chosen as *U-turn* (`AtObstacle == Turn`) get a turn line of their own, the obstacle offset outward by `UTurnDistanceFromBoundary` (`YouTurnCreationService.Obstacles.cs`), like AgOpenGPS with Drive thru = No, a hard one by the implement's reach (half its width, the sections' total `ActualToolWidth`, not the stored `Tool.Width`, plus its offset, `HardReach`) more, so the pass itself keeps the implement that distance from a wall or a ditch: the turn-area test returns -1 outside the outer turn line, 0 in the workable area, k inside obstacle turn line k. A turn at an obstacle has no simple fallback and is never "turned from here" when missed; a hard obstacle keeps the implement outside. *Drive through* obstacles are driven through. A planned turn is planned again when the obstacles change (`YouTurnCreationService.ObstacleStamp`), a manual turn that would cross a U-turn obstacle is refused, and with AutoSteer on, a U-turn obstacle within 10 m plus 2 s of travel ahead with no turn planned before it takes AutoSteer off with the alarm (`ObstacleAheadWithoutTurn`: a turn lost, missed or aborted there, or U-turn off); an obstacle changed under a turn being driven (another choice, Hard, added, moved), with the rest of the turn or the 20 m after it running into it, does the same (`ObstacleChangedUnderTurn`: a turn being driven is not planned again). A turn's 20 m of exit keep the tractor outside every turn line and the implement the safety margin off every hard fence (`ClearsTurnObstacles`). The generator seats its arc and slides it to the turn line by a test that also blocks the room round every Go around obstacle (`IsPointBlockedForTurn`), and an arc that slides on without touching the line (seated inside a bend, sliding along the bend's heading) is seated further back (`MoveTurnInsideTurnLine`'s bound), so a turn at an obstacle just past a tree the pass goes round is made before the tree, not 127 m away. A turn lands on a straight stretch of the next pass: one whose last point is off the straight pass, on a bend round a Go around obstacle, is seated further back (`LandsOnBend`, 4 m steps, 30 m at most), as is one whose legs or exit meet an obstacle; in a turn a Go around obstacle gets a ditch's room (`GoAroundRoom`: half the track plus the larger of the Go around margin and the U-turn distance), the implement the U-turn distance off a hard one along the path and the safety margin while the rig settles on the next pass (8 m plus twice a trailed hitch). New obstacles are Hard (`Boundary.AddNewObstacle`); a field whose obstacles had no choice in its file (AgOpenGPS, or saved before) says once on opening how many now get a U-turn. -- Obstacles chosen as *Go around* (`AtObstacle == GoRound`; a new circle's default) bend the pass round them (`ObstacleDetour`: the obstacle grown by half the implement plus the Safety margin, the offset limited to 45° and rounded to the Curve radius, both settings on the U-turn tab and in the tool profile). One `GoAroundPasses` singleton serves the pipeline (steering, display, light bar) and the U-turn planner (the current pass walked to the turn, the next pass the exit lands on), so a turn is never planned from a line the tractor does not drive; it pins the side of each obstacle while the tractor is beside it and over on that side, so a nudge or snap never rebuilds the bend across the tree (the pin is let go when the tractor gets to the other side, as a U-turn next to the tree does). An obstacle with no room on the allowed side is promoted to a U-turn obstacle for that pass (toast "No room to go around Inner N on this pass: U-turn before it"); a turn keeps the tractor out of every Go around obstacle and the implement off a hard one; and with AutoSteer on, a Go around obstacle within 10 m plus 2 s straight ahead while the tractor is more than 1.5 m off the steered line takes AutoSteer off with the alarm. The margin is never under 0.3 m on a 6 m bend and more on a tighter one (`GoAroundPasses.MinMarginFor`), and the radius never under the tractor's minimum turning radius (`GoAroundPasses.Radius`). A bend never moves the line more than two implement widths (never under 20 m) off the pass (`ObstacleDetour.MaxOffset`): a tree, or a row of trees, needing more is a U-turn obstacle for that pass. Design: `Plans/OBSTACLE_GO_AROUND_PLAN.md`; scenarios: `Tests/AgOpenWeb.Services.Tests/YouTurn/ObstacleScenarioTests.cs` (`GoAroundLegs`). +- Obstacles chosen as *Go around* (`AtObstacle == GoRound`; a new circle's default) bend the pass round them (`ObstacleDetour`: the obstacle grown by half the implement plus the Safety margin, the offset limited to 45° and rounded to the Curve radius, both settings on the U-turn tab and in the tool profile). One `GoAroundPasses` singleton serves the pipeline (steering, display, light bar) and the U-turn planner (the current pass walked to the turn, the next pass the exit lands on), so a turn is never planned from a line the tractor does not drive; it pins the side of each obstacle while the tractor is beside it and over on that side, so a nudge or snap never rebuilds the bend across the tree (the pin is let go when the tractor gets to the other side, as a U-turn next to the tree does). An obstacle with no room on the allowed side is promoted to a U-turn obstacle for that pass (toast "No room to go around Inner N on this pass: U-turn before it"); a turn keeps the tractor out of every Go around obstacle and the implement off a hard one; and with AutoSteer on, a Go around obstacle within 10 m plus 2 s straight ahead while the tractor is more than 1.5 m off the steered line takes AutoSteer off with the alarm. The margin is never under 0.3 m on a 6 m bend and more on a tighter one (`GoAroundPasses.MinMarginFor`), and the radius never under the tractor's minimum turning radius (`GoAroundPasses.Radius`), with a front implement h ahead never under √(r² + h²), the bend its pivot can keep the implement on (fork only: 5 m bends took a 4.5 m-ahead boom 1.35 m off the pass on a tester's field). A bend never moves the line more than two implement widths (never under 20 m) off the pass (`ObstacleDetour.MaxOffset`): a tree, or a row of trees, needing more is a U-turn obstacle for that pass. Design: `Plans/OBSTACLE_GO_AROUND_PLAN.md`; scenarios: `Tests/AgOpenWeb.Services.Tests/YouTurn/ObstacleScenarioTests.cs` (`GoAroundLegs`). - A pass that leaves the turn area and comes back within `UTurnCrossGap` metres (U-turn settings, "Drive across a headland gap up to"; 0 = off), across the headland only (inside the boundary, outside the headland line and every obstacle not driven through, and never through an obstacle's turn area, which is turned at as without the setting), is not turned at the gap: `GapCrossingEnd` in `YouTurnCreationService` walks on past it in both crossing searches, the turn is planned at the far end and the pass is driven through the gap with the sections off by headland section control (a narrow tip of the field, a notch in its edge). The next pass is taken from abreast of the gap's end (`gapResumeIndex`), since abreast of the tractor it can be outside the field. - 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. @@ -609,7 +609,10 @@ Key parameters: ## A front implement round a bend AutoSteer steers the pivot on the line; an implement fixed `h` ahead of it (`ToolMount.FrontFixed`, `RigGeometry.AttachX`: the vehicle's front hitch plus the tool's working point from it) runs outside a bend of radius R by √(R² + h²) − R. `FrontToolLine.Steered` moves each point of -the line steered (after the Go around bend, the same points one for one, so `PassUnfit` still lines up) towards the +the line steered (after the Go around bend; a closed pass's points one for one, so `PassUnfit` still lines up; an +open pass first gets a point every metre near both ends of each long straight stretch, `WithPointsNearStretchEnds`: +a pass bent round Go around trees is straight between its bends, and moved one for one the stretch after a bend ran +slanted, the tractor and the boom 0.40 m beside the pass for 100 m on a tester's field) towards the bend's centre by R − √(R² − h²), the pivot's bend never tighter than `Vehicle.MinTurningRadius`; R is measured over `max(h, 3 m)` either side of the point half way to the implement, headings over 2 m chords (a loop's ends overlap a little where it starts). Cached per pass in `GpsPipelineService.FrontToolSteered`; straight lines and AB lines are diff --git a/Shared/AgOpenWeb.Services/Track/FrontToolLine.cs b/Shared/AgOpenWeb.Services/Track/FrontToolLine.cs index 4b610c85c..d4d45fef4 100644 --- a/Shared/AgOpenWeb.Services/Track/FrontToolLine.cs +++ b/Shared/AgOpenWeb.Services/Track/FrontToolLine.cs @@ -4,6 +4,7 @@ using System; using System.Collections.Generic; +using System.Linq; using AgOpenWeb.Models.Base; namespace AgOpenWeb.Services.Track; @@ -33,7 +34,9 @@ public static class FrontToolLine /// /// The pass moved in round its bends for an implement metres ahead /// of the pivot; null when it would move less than anywhere (straight). - /// The points are the pass's, one for one. + /// A closed pass's points are the pass's, one for one (PassUnfit lines up with them); an open + /// pass gets points every near the ends of a long straight stretch + /// (). /// /// The pass steered, in its own order. /// The pass is a loop. @@ -41,8 +44,9 @@ public static class FrontToolLine /// The tightest the tractor turns, m. public static List? Steered(IReadOnlyList pass, bool closed, double reach, double minRadius) { + if (pass.Count < 3 || reach <= 0.05) return null; + if (!closed) pass = WithPointsNearStretchEnds(pass, Math.Max(reach, MinWindow) + reach / 2 + Chord + StretchStep); int n = pass.Count; - if (n < 3 || reach <= 0.05) return null; // Along the pass, and its heading on each stretch. var cum = new double[n]; @@ -105,6 +109,46 @@ Vec2 PointAt(double s) } } + /// The spacing of the points added near the ends of a long straight stretch, m. + public const double StretchStep = 1.0; + + /// + /// The pass with a point every along the first and last + /// metres of every stretch longer than the span. A pass bent round Go + /// around obstacles is straight between its bends, one stretch often hundreds of metres long: + /// the point that starts it moved with the bend and the far one did not, so the line steered + /// ran slanted along the whole stretch (on a tester's field, simulator, the tractor and its + /// front boom 0.40 m beside the pass for 100 m after the last tree, to the end of the pass). + /// The span is as far as a bend moves the line past its end. + /// + internal static IReadOnlyList WithPointsNearStretchEnds(IReadOnlyList pass, double span) + { + List? result = null; + for (int i = 0; i < pass.Count - 1; i++) + { + var a = pass[i]; + var b = pass[i + 1]; + double len = GeometryMath.Distance(a, b); + if (len <= Math.Max(span, 2 * StretchStep)) + { + result?.Add(a); + continue; + } + result ??= new List(pass.Take(i)); + result.Add(a); + for (double d = StretchStep; d < len - StretchStep / 2; d += StretchStep) + { + if (d > span && d < len - span) d = Math.Floor((len - span) / StretchStep) * StretchStep; + if (d <= 0 || d >= len - StretchStep / 2) continue; + double t = d / len; + result.Add(new Vec3(a.Easting + (b.Easting - a.Easting) * t, a.Northing + (b.Northing - a.Northing) * t, a.Heading)); + } + } + if (result == null) return pass; + result.Add(pass[^1]); + return result; + } + private static double AngleDiff(double a, double b) { double d = a - b; diff --git a/Shared/AgOpenWeb.Services/Track/GoAroundPasses.cs b/Shared/AgOpenWeb.Services/Track/GoAroundPasses.cs index 6f745dcba..4b78946a8 100644 --- a/Shared/AgOpenWeb.Services/Track/GoAroundPasses.cs +++ b/Shared/AgOpenWeb.Services/Track/GoAroundPasses.cs @@ -105,14 +105,25 @@ public static double MinMarginFor(double radius) => /// /// The radius the bends are built with: the Curve radius setting, but never under the /// tractor's own minimum turning radius (wheelbase over the tangent of the steer angle), - /// since the follower cannot hold a bend the tractor cannot steer. + /// since the follower cannot hold a bend the tractor cannot steer. With a front-mounted + /// implement, never under √(r² + h²) for a minimum turning radius r and the implement h ahead + /// of the pivot: the pivot steers a bend of √(R² − h²) to keep the implement on one of R + /// (), and on a tighter bend the implement runs outside it (fork + /// only: on a tester's field, simulator, a 5 m bend with a 6 m boom 4.5 m ahead and a 4.6 m + /// turning radius took the boom 1.35 m off the pass and onto the next tree). /// public double Radius { get { double setting = _config.Guidance.GoAroundRadius, min = _config.Vehicle.MinTurningRadius; - return double.IsFinite(min) && min > 0 ? Math.Max(setting, min) : setting; + if (!double.IsFinite(min) || min <= 0) min = 0; + if (Models.Tool.RigGeometry.Mount(_config.Tool) == Models.Tool.ToolMount.FrontFixed) + { + double h = Math.Abs(Models.Tool.RigGeometry.AttachX(_config.Vehicle, _config.Tool)); + if (double.IsFinite(h)) min = Math.Sqrt(min * min + h * h); + } + return Math.Max(setting, min); } } diff --git a/Tests/AgOpenWeb.Services.Tests/Track/FrontToolLineTests.cs b/Tests/AgOpenWeb.Services.Tests/Track/FrontToolLineTests.cs index 47b0b1fa0..ef322e289 100644 --- a/Tests/AgOpenWeb.Services.Tests/Track/FrontToolLineTests.cs +++ b/Tests/AgOpenWeb.Services.Tests/Track/FrontToolLineTests.cs @@ -90,6 +90,59 @@ public void Only_the_bend_moves_and_it_starts_as_the_implement_reaches_it() }); } + // The line's easting at a northing, between its points (a line running north). + private static double EastingAt(IReadOnlyList line, double northing) + { + for (int i = 0; i + 1 < line.Count; i++) + { + var a = line[i]; + var b = line[i + 1]; + if ((a.Northing - northing) * (b.Northing - northing) > 0 || Math.Abs(b.Northing - a.Northing) < 1e-9) continue; + double t = (northing - a.Northing) / (b.Northing - a.Northing); + return a.Easting + (b.Easting - a.Easting) * t; + } + throw new ArgumentOutOfRangeException(nameof(northing)); + } + + [Test] + public void After_a_bend_the_line_is_back_on_a_long_straight_stretch_within_a_few_metres() + { + // As a pass bent round a Go around tree: north on points a metre apart, a sideways bump of + // 3 m over 30 m, then one straight stretch of 200 m, two points. The stretch's first point + // moved with the bend and its far one did not: steered one for one, the line ran slanted + // the whole 200 m (0.40 m beside the pass 100 m on, on a tester's field). + var pts = new List(); + for (int i = 0; i < 50; i++) pts.Add(new Vec3(0, i, 0)); + for (int i = 0; i <= 30; i++) pts.Add(new Vec3(3 * (1 - Math.Cos(2 * Math.PI * i / 30)) / 2, 50 + i, 0)); + pts.Add(new Vec3(0, 280, 0)); + + var line = FrontToolLine.Steered(pts, closed: false, Reach, MinRadius)!; + Assert.That(line, Is.Not.Null); + Assert.Multiple(() => + { + Assert.That(Math.Abs(EastingAt(line, 65)), Is.GreaterThan(0.05), "the bend itself moves"); + Assert.That(Math.Abs(EastingAt(line, 95)), Is.LessThan(0.01), "15 m past the bend, back on the pass"); + Assert.That(Math.Abs(EastingAt(line, 180)), Is.LessThan(1e-9), "and on it along the stretch"); + Assert.That(line[^1].Northing, Is.EqualTo(280), "to the pass's end"); + }); + } + + [Test] + public void A_closed_pass_keeps_its_points_one_for_one() + { + // PassUnfit marks a closed pass's points by index. + var square = new List(); + for (int i = 0; i < 4; i++) + { + double a = Math.PI / 2 * i; + square.Add(new Vec3(50 * Math.Sin(a), 50 * Math.Cos(a), 0)); + } + var circle = Circle(20); + Assert.That(FrontToolLine.Steered(circle, closed: true, Reach, MinRadius)!.Count, Is.EqualTo(circle.Count)); + var steered = FrontToolLine.Steered(square, closed: true, Reach, MinRadius); + Assert.That(steered == null || steered.Count == square.Count); + } + [Test] public void Never_tighter_than_the_tractor_turns() { diff --git a/Tests/AgOpenWeb.Services.Tests/Track/GoAroundPassesTests.cs b/Tests/AgOpenWeb.Services.Tests/Track/GoAroundPassesTests.cs index bd8e6c924..fc0d47a01 100644 --- a/Tests/AgOpenWeb.Services.Tests/Track/GoAroundPassesTests.cs +++ b/Tests/AgOpenWeb.Services.Tests/Track/GoAroundPassesTests.cs @@ -67,6 +67,33 @@ public void The_bend_radius_never_goes_under_the_tractors_minimum_turning_radius Assert.That(passes.Radius, Is.EqualTo(10)); } + [Test] + public void With_a_front_implement_the_bend_is_one_its_pivot_can_keep_the_implement_on() + { + // The pivot steers √(R² − h²) to keep an implement h ahead on a bend of R (FrontToolLine): + // the bend is never under √(r² + h²), r the tractor's turning radius. + var (passes, _, _) = Setup(); + var c = ConfigurationStore.Instance; + c.Vehicle.Wheelbase = 3; + c.Vehicle.MaxSteerAngle = 35; // 4.28 m + c.Vehicle.FrontHitchLength = 4.5; + c.Tool.WorkingPointFromHitch = 0; + c.Tool.IsToolFrontFixed = true; + c.Tool.IsToolRearFixed = false; + c.Tool.IsToolTrailing = false; + c.Tool.IsToolTBT = false; + double r = 3 / System.Math.Tan(35 * System.Math.PI / 180); + c.Guidance.GoAroundRadius = 5; + Assert.That(passes.Radius, Is.EqualTo(System.Math.Sqrt(r * r + 4.5 * 4.5)).Within(0.01)); + c.Guidance.GoAroundRadius = 10; + Assert.That(passes.Radius, Is.EqualTo(10)); + + c.Tool.IsToolFrontFixed = false; + c.Tool.IsToolRearFixed = true; + c.Guidance.GoAroundRadius = 5; + Assert.That(passes.Radius, Is.EqualTo(5), "a rear implement: the tractor's own radius only"); + } + [Test] public void The_least_margin_grows_as_the_bend_tightens() {