Row perception

Docs index · Navigation · Control

Inside a row the useful structure is a corridor, not a map. The trunks either side are the only thing that constrains the machine and the lidar sees them directly, so kraken_orchard measures the aisle it is standing in rather than looking itself up in a map.

Following that needs no estimate of where the robot is in the world, only what is beside it — which is why it survives the localisation drift that a costmap search does not. It will acquire a row it is not yet lined up with, but only from close to — about four metres before the first trees, measured below. It measures the row it is in; it does not choose which row that is. That is mission planning.

Available only on the O3DE side. The headless model has no ranging sensor.

1. From cloud to trunks

cloud_filter reduces a PointCloud2 in stages, and reports what survived each one so a bad fit can be blamed on the right stage — received, above_ground, voxels, clusters, trunks, and for the clusters that were not trunks, which test rejected them: sparse, wide, shallow.

  1. Ground removal on a 0.5 m grid, keeping returns in a height band (0.40–1.80 m by default). Two thirds of the cloud is ground.
  2. Range gate, then voxel downsample at 0.10 m.
  3. Euclidean clustering at 0.35 m.
  4. Cluster acceptance by width and extent: too wide is canopy or a wall, too thin is noise.

The defaults describe a collider-based lidar. The tuning actually shipped in config/row_follower.yaml describes the RGL one, and the difference matters: RGL raycasts visual geometry, so what comes back is the canopy surface rather than a bare trunk. A cluster is then a whole tree, 1.3–1.5 m across, and max_trunk_width goes to 1.80 m to match.

2. From cloud to costmap

cloud_filter above is for fitting a row. The costmap needs something else, and feeding it trunk clusters would be a safety bug: an obstacle layer that only sees trees cannot see a person, a bin, or a parked vehicle.

costmap_cloud is therefore a plain pass-through on height and range. It keeps everything in the band and removes only what the obstacle layer would have rejected anyway. Measured on the orchard, five frames of ~21k points:

  share of the cloud
ground, below min_obstacle_height 77.3%
canopy, above max_obstacle_height 8.5% (reaching 11.5 m)
in the band the layer can mark 14.2%

Raw range reaches 96.5 m against an obstacle_max_range of 20. So 86% of every frame was being transported, transformed and then discarded. Filtering first takes 23040 points and 360 KB per frame down to 3005 and 47 KB — 87% removed — and the costmap still marks (341215 lethal cells in the global grid).

Points are judged in base_link, where the layer’s height limits apply, but published in the frame they arrived in so the costmap’s own transform is unchanged. Whole points are copied rather than x/y/z, so intensity and ring survive for anything else subscribing.

Trap. A failed tf lookup publishes nothing rather than passing the cloud through. The silent version of that failure is the ground back in the costmap.

Not a grass filter. Grass standing above min_obstacle_height is in the band and is kept, correctly — height alone cannot tell it from a trunk. It was worth checking whether that matters, and measured, it does not: of the columns that mark within the 20 m the layer uses, 3.0% are entirely below 0.60 m, and none at all within 5 m. The 16.5% that looks like grass across all ranges is long-range grazing, median range 24.2 m, past what marks. Do not raise min_obstacle_height to “fix grass” without re-measuring.

Points between 20 and 25 m are kept deliberately. They never mark, but they do clear, via raytrace_max_range.

Measured rates

With the simulator holding real time — check it, or the numbers are the host’s and not the sensor’s:

topic rate median gap worst gap
pc (raw) 11.92 Hz 80.0 ms 170.0 ms
pc_costmap (filtered) 11.92 Hz 80.0 ms 170.0 ms

So the lidar is an 80 ms sensor that occasionally skips one frame, and the filter costs nothing in rate — the two are identical message for message. The IMU is 50 Hz, not the 100 Hz quoted elsewhere.

RViz was reported as showing periodic errors on the lidar topic. It could not be reproduced: ninety seconds idle and sixty driving, with both the raw and the filtered cloud displayed, produced no warning, transform complaint or dropped message at all. Several things changed underneath it in the meantime — the ground truth frame, the filtered topic, the QoS — so it may simply be gone. If it returns, the text matters: RViz reports per-display status in the Displays panel that never reaches the terminal.

3. From trunks to a corridor

fitRow splits the surviving centres by sign of their lateral coordinate, fits a line to each side, and reports the corridor between them: width, lateral_offset, row_heading, and how many trunks each side contributed. The Kraken’s lidar is bolted on without rotation, so the offset and heading read directly as the robot’s error against the row.

A fit is refused, with a reason, unless it has enough trunks per side (min_trunks), enough longitudinal span (min_span), a small enough residual (max_residual) and two sides that agree on heading (max_heading_difference). Only three or four trees per side stay unoccluded, so the thresholds are looser than a trunk-based fit would need, and the residual allowance is nearly double because canopy centroids scatter about the row line far more than trunk centres do.

The result goes out as kraken_interfaces/msg/RowEstimate, and the centreline as a nav_msgs/Path so Nav2’s controller can drive it through FollowPath and keep its own footprint collision checking. This replaces the planner inside a row, not the controller — what drives the path is still ArcTracker.

The detected trunks are republished on orchard/trunks for RViz, because a fit that fails is almost always a filtering problem two stages earlier.

4. Running it

ros2 launch kraken_orchard row_follower.launch.py namespace:=kraken1

It publishes orchard/row, orchard/centreline, and orchard/trunks — the detected trunks as a cloud, which is what to look at when a fit is refused.

It subscribes to the raw cloud, not the filtered one the costmap uses: it does its own ground removal and needs the returns below min_obstacle_height to find the ground to remove.

orchard.launch.py now starts it as well, so the standalone launch file above is only for running it against a stack that is already up. It had no launch file at all until the trunk extent needed measuring, and until then the row perception described here had never run in the stack it describes — every number below post-dates that.

It publishes an estimate for every cloud, valid or not, before it checks validity. No estimates at all therefore means the node is not running; it never means there was no row.

5. What it actually does

Measured on the O3DE orchard, steered from the map pose so perception never saw the pose it was scored against.

Holding a row — 41 m down row one at 1.0 m/s, 370 estimates, 85% of them a fit. Lateral offset error median 0.05 m, worst 0.15 m; heading error median 1.2°, worst 4.9°. Nothing valid and gross: 0 of 313.

Entering one — approaching the mouth 20° off and 2.5 m to the side, driven in on the surveyed centreline:

Distance before the first trees Trunks in view Fits
7–5 m 0.7 0%
5–3 m 2.5 21%
3–1 m 3.8 71%
level with them 5.2 96%
1–5 m in 6.1 100%

First fit four metres out, reporting the row 2.1° off when the truth was 4.3°. From 32° off and 7 m to the side it still acquires, a metre later. Acquisition wants proximity, not alignment.

What it cannot do is see a row from the headland. Out there the tree line comes back as a single cluster wider than max_trunk_width — the canopy fills the gaps between trunks and single-linkage clustering chains the whole line into a wall — so nothing is valid more than about five metres ahead of the first trees. A machine cannot use this to decide which row it is approaching. Widening that means telling trunk from canopy at 10 m, which is a capability rather than a threshold.

The measured width is biased low: 3.18 m against a true 3.50 m pitch, because the visible canopy is the near face of each tree, so each side’s line sits about 0.16 m toward the sensor. Corridor geometry is right; absolute width is not.

6. Tests

colcon test --packages-select kraken_orchard covers the filter and the fit on synthetic clouds; neither needs a simulator.