/// <summary>
/// Collects the laser scanner data from the simulator.
/// </summary>
/// <returns>Returns the collected laser scanner data.</returns>
private double[,] GetLaserScannerData()
{
double[,] laserScannerData;
// reading the laser scanner stream
simx_error s = VREPWrapper.simxReadStringStream(_clientId, R.MeasuredData0, ref _signalValuePtr, ref _signalLength, simx_opmode.streaming);
if (s != simx_error.noerror)
{
return(new double[0, 0]);
}
//Debug.WriteLine(s);
// Debug.WriteLine(String.Format("test: {0:X8} {1:D} {2:X8}", _signalValuePtr, _signalLength, _signalValuePtr+_signalLength));
float[] f = new float[685 * 3];
if (_signalLength / sizeof(float) >= f.GetLength(0))
{
//we managed to get the laserdatas from Vrep
laserScannerData = new double[3, f.GetLength(0) / 3];
// todo read the latest stream (this is not the latest)
int i;
unsafe
{
float *pp = (float *)_signalValuePtr.ToPointer();
//Debug.WriteLine("pp: " + *pp);
for (i = 0; i < f.GetLength(0); i++)
{
f[i] = *pp++; // pointer to float array
}
}
// reshaping the 1D [3*685] data to 2D [3, 685] > x, y, z coordinates
for (i = 0; i < f.GetLength(0); i++)
{
if (!(Math.Abs(f[i]) < 0.000001))
{
laserScannerData[i % 3, i / 3] = f[i];
}
}
return(laserScannerData);
}
// we couldnt get the laserdata, so we return an empty array
laserScannerData = new double[0, 0];
return(laserScannerData);
}
public VREPException(simx_error Error, string Message = "")
: base(String.Format("{0}: {1}", Enum.GetName(typeof(simx_error), Error), Message))
{
}
public VREPException(simx_error Error, string Message = "")
: base(String.Format("{0}: {1}", Enum.GetName(typeof(simx_error), Error), Message))
{
}
