Skip to main content
The Innex1 Rover’s perception system uses depth camera point clouds and Open3D processing to detect rocks, craters, and other hazards in the lunar environment.

​
Hazard Detection Node

Location: src/lunabot_perception/lunabot_perception/hazard_detection.py The HazardDetectionNode processes depth camera data to identify obstacles for the navigation costmap.

​
Architecture

class HazardDetectionNode(Node):
    def __init__(self):
        super().__init__("hazard_detection_node")
        
        self.publisher = self.create_publisher(
            PointCloud2, 
            "/hazards/front", 
            10
        )
        
        self.subscription = self.create_subscription(
            PointCloud2,
            "/camera_front/points",
            self.listener_callback,
            10
        )
Topics:
  • Input: /camera_front/points (PointCloud2 from depth camera)
  • Output: /hazards/front (filtered obstacles)

​
Processing Pipeline

The hazard detection algorithm applies a multi-stage filtering pipeline:
1

Extract Valid Points

extracted_points_generator = point_cloud2.read_points(
    msg, field_names=("x", "y", "z"), skip_nans=True
)
points_list = list(extracted_points_generator)
extracted_points = np.array(
    [[p[0], p[1], p[2]] for p in points_list], 
    dtype=np.float64
)
Filter out NaN and infinite values:
valid_mask = np.all(np.isfinite(extracted_points), axis=1)
extracted_points = extracted_points[valid_mask]
2

Voxel Downsampling

Reduce point density for efficient processing:
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(extracted_points)

pcd_downsampled = pcd.voxel_down_sample(voxel_size=0.05)
5 cm voxel grid balances detail with computation speed.
3

Statistical Outlier Removal

Remove sensor noise and spurious points:
pcd_cleaned, outlier_indices = pcd_downsampled.remove_statistical_outlier(
    nb_neighbors=20,
    std_ratio=2.0
)
Points are removed if their distance to the 20 nearest neighbors exceeds 2 standard deviations from the mean.
4

Ground Plane Segmentation

Use RANSAC to fit a plane model to the lunar surface:
plane_model, inliers = pcd_cleaned.segment_plane(
    distance_threshold=0.05,
    ransac_n=3,
    num_iterations=1000
)
Parameters:
  • distance_threshold: 5 cm - points within this distance are considered ground
  • ransac_n: 3 points to fit plane
  • num_iterations: 1000 iterations for robust fitting
5

Extract Obstacles

All non-ground points are obstacles:
pcd_obstacles = pcd_cleaned.select_by_index(inliers, invert=True)
6

Publish Obstacle Cloud

points = np.array(pcd_obstacles.points)
pc2_message = point_cloud2.create_cloud_xyz32(msg.header, points)
self.publisher.publish(pc2_message)

​
Algorithm Parameters

​
Voxel Size

voxel_size=0.05  # 5 cm grid
Trade-offs:
  • Smaller (e.g., 0.02): Higher resolution, slower processing
  • Larger (e.g., 0.10): Faster but may miss small rocks

​
Outlier Removal

nb_neighbors=20
std_ratio=2.0
ParameterEffectTypical Range
nb_neighborsMore neighbors = smoother filtering10-30
std_ratioLower = more aggressive noise removal1.0-3.0
std_ratio=2.0 follows the 95% confidence interval in normal distribution, balancing noise removal with feature preservation.

​
Plane Segmentation

distance_threshold=0.05  # 5 cm tolerance
ransac_n=3
num_iterations=1000
RANSAC configuration:
  • distance_threshold: Accommodates regolith surface roughness
  • num_iterations: Ensures convergence even with 50% outliers

​
Integration with Navigation

The /hazards/front topic is consumed by Nav2’s obstacle layer: 
# From nav2_params.yaml
obstacle_layer:
  observation_sources: hazards_front front_points_clear
  
  hazards_front:
    topic: /hazards/front
    data_type: "PointCloud2"
    marking: true
    clearing: false
    max_obstacle_height: 0.50
    min_obstacle_height: 0.10
    obstacle_max_range: 3.0
See the Navigation component documentation for full configuration details.

​
Height Filtering

max_obstacle_height: 0.50  # Ignore points above 50 cm (e.g., overhead structures)
min_obstacle_height: 0.10  # Ignore dust and small debris below 10 cm
This filters the obstacle cloud to relevant hazard heights before costmap insertion.

​
Error Handling

​
Empty Point Clouds

if len(extracted_points) == 0:
    return
Skip processing if the depth camera returns no valid points.

​
Post-Filtering Validation

if len(extracted_points) == 0:
    self.get_logger().warn("No valid points after filtering infinities")
    return
Logs a warning if all points are NaN/infinite.
If you frequently see “No valid points” warnings, check:
  1. Depth camera exposure and gain settings
  2. Lighting conditions (depth cameras struggle in direct sunlight)
  3. Sensor calibration and mounting stability

​
Visualization

Visualize obstacle detection in RViz: 
Topic: /camera_front/points
Style: Points
Size: 0.01
Color: White

​
Performance Metrics

​
Processing Time

Typical pipeline latency on a 640x480 depth image:
StageDuration
Point extraction~5 ms
Voxel downsampling~10 ms
Outlier removal~15 ms
Plane segmentation~20 ms
Total~50 ms
This achieves ~20 Hz processing rate, sufficient for real-time navigation.

​
Point Cloud Size

  • Input: ~300,000 points (640x480 depth image)
  • After downsampling: ~8,000 points (5 cm voxels)
  • After outlier removal: ~7,000 points
  • Obstacles (output): ~500-2000 points (depends on environment)

​
Running Hazard Detection

1

Launch the perception node

ros2 run lunabot_perception hazard_detection
2

Verify input point cloud

ros2 topic hz /camera_front/points
Should show depth camera frame rate (typically 30 Hz).
3

Monitor obstacle detections

ros2 topic echo /hazards/front --no-arr
4

Visualize in RViz

ros2 launch lunabot_perception perception_viz.launch.py

​
Tuning for Different Environments

​
Rocky Terrain

Increase plane distance threshold to accommodate uneven ground: 
distance_threshold=0.08  # 8 cm tolerance

​
Dusty Conditions

More aggressive outlier removal to filter sensor noise: 
std_ratio=1.5  # Stricter threshold

​
High-Speed Navigation

Reduce computation time with larger voxels: 
voxel_size=0.10  # 10 cm grid
The current configuration is optimized for the NASA Lunabotics arena with regolith simulant and distributed rock obstacles.

​
Future Enhancements

Potential improvements to the perception pipeline:
  1. Crater detection - Identify concave surface features
  2. Traversability analysis - Classify terrain slope and roughness
  3. Multi-camera fusion - Combine front and side depth cameras
  4. Semantic segmentation - Distinguish rock types and excavatable regolith
  5. Temporal filtering - Track persistent obstacles across frames

Build docs developers (and LLMs) love

Get started for freeTalk to us