Demo: remove robot body from lidar scans using dynamic bounding box

Lidar sensors often capture a few measurements from the robot’s own body. Self-measurements from the robot body appear static relative to the environment and confuse mapping algorithms. Dynamic self-reflections from moving parts (e.g., manipulator arms) are particularly problematic. peci1/robot_body_filter addresses this by filtering scan points against the URDF model, but only works with ROS 1. ROS 2 requires a different solution.

The approach presented here uses a dynamic axis-aligned bounding box (AABB). An AABB is the smallest rectangular box that fully encloses a given set of points with its edges aligned to the coordinate axes. It does not rotate with the object it encloses. This makes containment checks very efficient: a scan point is inside the box only if all its coordinates fall between the AABB’s minimum and maximum values. Though less precise than mesh-based filtering, its simplicity reduces processing overhead.

In case of the L4XZ autonomy stack the axis aligned bounding box is computed at runtime. The following pseudo code illustrates the algorithm:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16

// Compute 3D positions of all robot extremities using forward kinematics.
position_list ← fk_solve_head(head.pan_angle, head.tilt_angle)
for (leg : robot.legs)
  position_list ← position_list + fk_solve(leg.coxa_angle, leg.femur_angle, leg.tibia_angle)

// Determine AABB corners from min/max coordinates across all positions.
aabb_min ← ( ∞,  ∞,  ∞)
aabb_max ← (-∞, -∞, -∞)
for (p : position_list)
  aabb_min ← min(aabb_min, p);
  aabb_max ← max(aabb_max, p);

// Expand by a safety tolerance to fully enclose the robot body.
TOLERANCE ← (x_tol, y_tol, z_tol)
aabb_min  ← aabb_min - TOLERANCE / 2.
aabb_size ← (aabb_max - aabb_min) + TOLERANCE

The resulting bounding box is published as a ROS 2 message:

1
2
3

std_msgs/Header header
geometry_msgs/Point aabb_min
geometry_msgs/Vector3 aabb_size

A custom Rviz2 plugin is used to display the AABB as a transparent rectangular cuboid around the robot:

Size and position of the bounding box dynamically change to always fully envelope the robot, regardless of changes to its mechanical configuration.

A robot body filter ROS 2 node subscribes to both the lidar point cloud and the bounding box topic. For each incoming point cloud the bounding box is first transformed from the robot’s base frame (base_link) into the lidar sensor frame (lidar_link) using tf2. Then each point is tested against the transformed bounding box. Only points outside the bounding box are kept. All points falling inside the box – i.e. originating from the robot body – are discarded. A new point cloud is constructed from the remaining points and published for downstream consumers such as the mapping algorithm.

The result: no more echoes from robot body parts. The robot body has been filtered out from the lidar scan.