PathGen

Autonomous path generation for the Wisconsin Robotics rover. Takes a LiDAR scan of terrain as input and finds a traversable path between GNSS (WGS84 lat/lon) waypoints, avoiding slopes too steep for the rover.

The shortest path from my dorm to Kohl Centre to College Library to Morgridge Hall

Setup

Install dependencies:

pip install numpy scipy laspy[lazrs] pyproj rustworkx folium

Usage

Step 1 — build the graph (once per dataset):

python scripts/build_graph.py
# Enter LAZ file path: data/MDRS-region/lidar-data/mdrs.laz

Step 2 — find a path:

python scripts/find_path.py
# Enter GNSS of start point (lat lon in WGS84 units): 
# Enter number of targets: 
# Enter GNSS of target 1 (lat lon in WGS84 units): 

Step 3 — visualize:

python scripts/visualize.py
# Open artifacts/path_output.html in a browser

Configuration

All parameters are in pathgen/config.py:

Parameter	Default	Description
CELL_SIZE	1.0 m	Grid cell size for point cloud downsampling
K_NEIGHBORS	10	Number of nearest neighbors per node in the graph
MAX_SLOPE_DEG	36°	Maximum traversable slope — steeper edges are dropped
SLOPE_MULTIPLIER	5	How much uphill slope inflates edge cost
Z_SCALE	1.0	Weight of elevation difference in the A* heuristic

Repository layout

scripts/        — runnable pipeline scripts
pathgen/
  config.py         — all parameters and file paths
  pointcloud/       — LiDAR loading and downsampling
  pathplanning/     — graph construction and A*
  utils/            — coordinate conversion, nearest-node lookup
  viz/              — HTML map rendering
data/           — LiDAR datasets (.laz files)
artifacts/      — output files (generated at runtime)

Artifacts

File	Generated by	Contents
nodes.npy	build_graph.py	(N, 3) array of point coordinates [x, y, z] in the LiDAR data's projected CRS
edges.npy	build_graph.py	(M, 3) array of directed edges [src, dst, weight] - one row per direction
meta.json	build_graph.py	EPSG code and source .laz path
path.npy	find_path.py	Array of path segments, each segment a list of node indices into nodes.npy
nearest_nodes.pkl	find_path.py	Start and target node indices with their coordinates, used by the visualizer
path_output.html	visualize.py	Interactive map of the path overlaid on satellite imagery
path_latlon.csv	visualize.py	Path as WGS84 lat, lon coordinates - one row per point. The rover will extract the coordinates it needs to travel to from this file

How it works

The pipeline has three stages:

1. Build graph (scripts/build_graph.py)

Reads a .laz LiDAR point cloud and constructs a navigable graph of the terrain.

• Downsamples the point cloud to at most 5 spatially spread points per 1m² cell, preserving terrain shape while reducing point count
• Builds a K-nearest-neighbor graph (k=10) over the remaining points using a KD-tree, where nearness is defined by euclidean distance
• Computes a directed edge weight for each neighbor pair: weight = distance × (1 + slope_cost), where uphill edges are penalized more than downhill
• Drops any edge whose absolute slope exceeds 36° (too steep for the rover)
• Saves nodes.npy, edges.npy, and meta.json to artifacts/

This step is slow and only needs to be run once per LiDAR dataset. So, this should be run before the competition begins. (a dataset with ~18 million points is processed in ~2 minutes)

2. Find path (scripts/find_path.py)

Loads the prebuilt graph and runs A* between GNSS waypoints.

• Accepts a start point and one or more target waypoints as GNSS coordinates (lat/lon)
• Converts coordinates from WGS84 to the projected CRS of the LiDAR data using the EPSG code stored in meta.json
• Finds the nearest graph node to each waypoint using 2D Euclidean distance (no z, since competition GNSS has no elevation)
• Runs A* across each consecutive pair of waypoints using the rustworkx graph library
• Saves the resulting path to artifacts/path.npy

This step can be re-run with different waypoints without rebuilding the graph (i.e. step1 only needs to be completed once for a given dataset) Note that the path is constructed in the order in which the input targets are provided.

3. Visualize (scripts/visualize.py)

Renders the path as an interactive HTML map overlaid on Esri satellite imagery using folium. Open artifacts/path_output.html in a browser to view it. It also saves the coordinates on the path as a CSV file for the rover to read