Hmm, Messtaster rundherum die Oberfläche während einer Fahrt abtasten lassen und X/Y-Pos und Messwert durchgehend auszeichnen. Danach generierst du dir einfach den G-Code mit passenden Z-Werten. Dürfte die leichtere Programmieraufgabe sein.