groundvehicle freezes if there is a change in z-value between waypoints

[han16nah]

The problem
Surveys using platforms of type groundvehicle freeze/hang up as soon as there is a change in z-value between waypoints.

How to reproduce
Create a survey using a platform of type groundvehicle and add multiple legs which has different z-coordinates. Below is an example (modified version of data/surveys/demo/mls_wheat_demo.xml):

<?xml version="1.0" encoding="UTF-8"?>
<document>
    <survey name="mls_arbaro_wheat" scene="data/scenes/demo/arbaro_wheat_field.xml#arbaro_wheat" platform="data/platforms.xml#tractor" scanner="data/scanners_tls.xml#riegl_vz400">
		
		<leg>
			<platformSettings  x="-10" y="-10" z="0" movePerSec_m="3"/>
			<scannerSettings  active="true" pulseFreq_hz="100000" scanAngle_deg="20" scanFreq_hz="50" headRotatePerSec_deg="0.00" headRotateStart_deg="0.00" headRotateStop_deg="0.00" trajectoryTimeInterval_s="0.05"/>
		</leg>
		
		<leg>
			<platformSettings  x="10" y="-10" z="0" movePerSec_m="3"/>
			<scannerSettings  active="true" pulseFreq_hz="100000" scanAngle_deg="20" scanFreq_hz="50" headRotatePerSec_deg="0.00" headRotateStart_deg="0.00" headRotateStop_deg="0.00" trajectoryTimeInterval_s="0.05"/>
		</leg>
		
		<leg>
			<platformSettings  x="10" y="10" z="1" movePerSec_m="3"/>
			<scannerSettings  active="true" pulseFreq_hz="100000" scanAngle_deg="20" scanFreq_hz="50" headRotatePerSec_deg="0.00" headRotateStart_deg="0.00" headRotateStop_deg="0.00" trajectoryTimeInterval_s="0.05"/>
		</leg>
		
		<leg>
			<platformSettings  x="-10" y="10" z="1" movePerSec_m="3"/>
			<scannerSettings  active="true" pulseFreq_hz="100000" scanAngle_deg="20" scanFreq_hz="50" headRotatePerSec_deg="0.00" headRotateStart_deg="0.00" headRotateStop_deg="0.00" trajectoryTimeInterval_s="0.05"/>
		</leg>
		
		<leg>
			<platformSettings  x="-10" y="-10" z="0" movePerSec_m="3"/>
			<scannerSettings  active="false" pulseFreq_hz="100000" scanAngle_deg="20" scanFreq_hz="50" headRotatePerSec_deg="0.00" headRotateStart_deg="0.00" headRotateStop_deg="0.00" trajectoryTimeInterval_s="0.05"/>
		</leg>
    </survey>
</document>

Expected behavior

The vehicle follows the route along the trajectory upwards (as long as the slope is not too high, maybe a threshold needs to be introduced here).

Actual behavior
The survey freezes without any warning or error message.

Your environment (please complete the following information):

• OS: Windows 11
• HELIOS++/pyhelios version: v2.0.2

Can you identify the location in the source code where the problem exists? (Optional)

I guess this should be fixed in the GroundVehiclePlatform.cpp.

[han16nah]

Addition: This error also occurs if we set onGround="true" and using a ground surface with z-variation (e.g., a digital terrain model from GeoTIFF).