Multi-cell TSP routing, route corridors, and optional headland driving for decomposed fields#127
Closed
u33549 wants to merge 11 commits into
Conversation
Adds three goal fields to ComputeCoveragePath: - generate_headland_swaths: also drive the headland ring area - headland_first: drive the headland before (true) or after the inner coverage - headland_full_coverage: false = single perimeter pass along the headland inner boundary; true = concentric passes spaced by the operation width to sweep the whole band (count = headland_width / operation_width) The headland pass is emitted as HL_SWATH path states and spliced onto the coverage path, starting at the boundary point nearest the route to minimize the connecting jump. discretizeSwathLike now densifies HL_SWATH (F2C's discretizeSwath only splits SWATH), so the headland pass is a dense, followable path with per-pose velocity/direction. A dedicated coverage_server/headland_swaths marker visualizes it distinctly from the planning-field boundary.
Decomposition removes the field headland once before decomposing; the per-sub-cell second headland is dropped so swaths reach the outer field headland instead of leaving a double-wide gap. A new route_headland_width goal field (0 = operation width) sets the corridor carved strictly on the borders that two cells actually share: an edge is carved only when both endpoints and its midpoint lie on a neighbor's border, so edges facing a concave void or the outer boundary keep their single headland. The corridor is built by buffering the shared edge as a line (numerically robust) and differencing it out; sub-cells below a minimum area are dropped so degenerate slivers never reach F2C's unchecked OGR casts. Note: test_server still segfaults on the complex GML field (WIP).
The headland_swaths marker now publishes DELETEALL when a goal drives no headland pass, so a previous goal's loop no longer lingers in RViz. Adds a coverage_server/swath_cells marker (cyan) that draws the exact polygons swaths are generated from, to distinguish carving from swath placement.
Global genRoute interleaved cells and criss-crossed the field. Each cell is now solved alone (paired 1:1 with its carved swath cell), visited in nearest-neighbor order entering from the closer route end (reversing when needed), and bridged along the adjacent travel-cell pair's border graph with whole-set and straight-line fallbacks.
F2C planPath drops connection waypoints when endpoints are close, cutting straight across concave voids. Connections with a detour ratio above 1.3 are now emitted verbatim as HL_SWATH states; near-straight hops keep the single Dubins/RS curve that produces the usual u-turns.
…ilter Cell entry choice now weighs heading mismatch alongside distance, and bridge polylines drop attachment points that double back at the junctions.
Contributor
Author
|
I think its the last pr for rolling after that I will control everything again but my first goal is developing other distros to the same level as rolling. After then we can prepare a guide. |
Member
|
This one's a bit too big, can you break it up into 2-3 PRs? |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Every change is opt-in or behavior-preserving by default, so a request that doesn't
decompose the field (or leaves the new fields unset) plans exactly as before.
route_headland_width)generate_headland_swaths)1. Multi-cell TSP routing
With decomposition, the field becomes several cells and a single global
genRouteboth interleaves swaths across cells and builds F2C's all-pairs pathmatrix, which grows with the square of the swath count and can run out of memory
on large fields.
Instead:
genRoutecall, which still honors the start/end pointexactly.
stitched in nearest-neighbor order. Each cell is entered from whichever end is
cheaper (with a heading-mismatch penalty so the entry doesn't force an
S-maneuver), and the bridge between two cells follows the shared border graph
of that travel-cell pair, so it detours around voids instead of cutting a
straight line through them.
2. Route corridors between adjacent cells
route_headland_width(default0.0= operation width) carves a travelcorridor only on borders shared by two adjacent decomposed cells. Edges that
already have a headland are left untouched — a corridor is only ever added
between two cells, never on the field boundary.
3. Path follows route connections
F2C's path planner drops connections whose endpoints are near a swath end, which
erased the inter-cell / around-void travel from
nav_path. The path generatornow follows those connections (as
HL_SWATHsections) so the detour actuallyappears in the output path.
4. Optional headland driving
Adds goal fields to optionally cover the headland ring ("drive the headland"):
All default false, so headland coverage is off unless requested.
default_generate_headland_swathsis also exposed as a parameter.5. Rejections and dynamic params
cross-cell ordering, so combining them with decomposition is rejected early
with a clear error instead of producing a bad route.
min_turning_radius,linear_curv_change, anddefault_generate_headland_swathsare now dynamic parameters.6. test_server teardown fix
Tearing the server down while the action worker was still running could corrupt
the heap and segfault the test. Teardown now waits for the worker to finish.