Building an obstacle detection pipeline

1. Engineering problem

A mobile robot must know what is in its way. The usual LiDAR chain is: crop to the region of interest, remove the ground, group the remaining points into objects, and report each object’s position and size. Each stage can fail differently, so each needs its own check.

2. When is this needed?

• Adding obstacle avoidance to a mobile robot.
• When the planner reports phantom or missed obstacles.
• Before connecting perception to a costmap.

3. Selection and design decisions

The input is a sensor_msgs/PointCloud2: a header (stamp and frame_id), height and width (an unordered cloud has height 1), a fields array describing the binary layout, point_step, row_step, data and is_dense. Downstream code should read the field layout instead of assuming x, y, z are the first three floats.

4. Advantages and disadvantages

• A height threshold is fast and fails on slopes; plane fitting handles slopes and costs compute.
• Small cluster distance splits objects; large merges neighbours.
• Early downsampling saves compute and loses small objects.

5. Requirements and constraints

• The pipeline meets the sensor rate with margin on the target computer.
• Range, height and cluster thresholds are derived from the smallest object that must be avoided and the robot’s stopping distance.
• Output uses the cloud’s original stamp, not the processing time.

6. Architecture

PointCloud2 ─▶ crop ─▶ ground removal ─▶ clustering ─▶ characterise ─▶ obstacles (frame, stamp, extent)

7. Implementation

1. Define the smallest obstacle and the range from the stopping distance.
2. Crop the cloud to the region of interest and remove the robot’s own body.
3. Remove the ground by height above the plane (or fit a plane for slopes).
4. Cluster with a distance threshold and a minimum size (below).
5. Publish each cluster’s centroid, extent and nearest distance with the cloud’s stamp.
6. Test with objects of known size at known distances.

import math
pts = [(2.0, 0.1, 0.02), (2.1, -0.2, 0.01), (3.0, 0.0, 0.03), (4.0, 1.0, 0.0),            # ground
       (2.0, 1.0, 0.5), (2.1, 1.1, 0.8), (2.05, 1.05, 1.1),                                 # object 1 (person-like)
       (5.0, -1.0, 0.4), (5.1, -1.1, 0.7), (5.2, -0.9, 0.5), (5.15, -1.0, 0.9)]             # object 2
def cluster(points, eps):
    clusters, seen = [], set()
    for i in range(len(points)):
        if i in seen:
            continue
        stack, group = [i], []
        seen.add(i)
        while stack:
            k = stack.pop(); group.append(k)
            for j in range(len(points)):
                if j not in seen and math.dist(points[k], points[j]) <= eps:
                    seen.add(j); stack.append(j)
        clusters.append([points[k] for k in group])
    return clusters
obstacles = [p for p in pts if p[2] > 0.15]                 # drop ground by height above the plane z = 0
for c in cluster(obstacles, 0.5):
    nearest = min(math.hypot(p[0], p[1]) for p in c)
    print(f"cluster of {len(c)} points, nearest {nearest:.2f} m, height {max(p[2] for p in c):.2f} m")

Printed (checked by the site’s tests):

cluster of 3 points, nearest 2.24 m, height 1.10 m
cluster of 4 points, nearest 5.10 m, height 0.90 m

Reading it: the ground points (height below 0.15 m) are discarded, and the remaining points form two clusters: a 3-point object whose nearest point is 2.24 m away and 1.10 m tall, and a 4-point object 5.10 m away. The point positions and thresholds are illustrative and the clustering is a simple connected-components search standing in for a library routine.

8. Real examples

• Indoor AMR: a 3D LiDAR cloud is cropped to 10 m, the floor removed by height, and the clusters turned into costmap obstacles.

9. Failure scenarios

• The floor is not flat (ramps), so ground removal by height keeps floor points as obstacles.
• The robot body is not cropped and appears as a permanent obstacle.
• Processing time stamped instead of the cloud time, so obstacles lag when moving.

10. Troubleshooting

1. Count points after each stage and compare with expectations (see the debugging guide).
2. Visualise each stage’s output in RViz.
3. Test with a known object and measure the reported distance.

11. Validation

• The example prints the lines shown (executed by the site tests).
• Known objects are detected at their true distance within the sensor’s accuracy over the range of interest.
• Pipeline latency is within the budget at the highest sensor rate.

12. Engineering checklist

• Smallest obstacle and range defined
• Own body cropped
• Ground removal validated on slopes
• Cluster parameters tuned
• Frame and stamp correct
• Latency measured
• Tested with known objects

13. Further references

• ROS 2 sensor_msgs/PointCloud2 (Jazzy): PointCloud2.msg, read 2026-11-06.
• Library documentation for your point-cloud tool (for example PCL or Open3D) for the exact filter and clustering functions and parameters; the algorithms here are written out in plain Python so they can be checked.

Related on FixTheRobot