public void ChangeSlamStatus(SlamStatusOverall slamStatus, bool wait = false)
{
try
{
if (_SlamChangeStatusLabel != null && ConnectivityState == LinkUpConnectivityState.Connected)
{
if (wait)
{
_SlamChangeStatusLabel.Call(new byte[] { (byte)slamStatus });
}
else
{
_SlamChangeStatusLabel.AsyncCall(new byte[] { (byte)slamStatus });
}
}
}
catch (Exception) { }
}
private static void Program_Fired(LinkUpEventLabel label, byte[] data)
{
if (count == 0)
{
stopWatch = new Stopwatch();
stopWatch.Start();
}
count++;
bytes += data.Length;
if (count % 100 == 0)
{
Console.WriteLine("{0:0.0} events/s\t{1:0.0} KB/s\t{2:0.0} MBit/s", ((double)count) / stopWatch.ElapsedMilliseconds * 1000, ((double)bytes) / stopWatch.ElapsedMilliseconds * 1000 / 1024, ((double)bytes) / stopWatch.ElapsedMilliseconds * 1000 / 1024 / 1024 * 8);
stopWatch.Restart();
count = 0;
bytes = 0;
func.AsyncCall(new byte[2]);
}
//onsole.WriteLine("- EVENT ({0}): {1}", label.Name, data.Length/*string.Join(" ", data.Select(b => string.Format("{0:X2} ", b)))*/);
}
public void UpdateSettings()
{
if (_AccelerometerScaleLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_AccelerometerScaleLabel.Value = _SettingContainer.Settings.ImuSettings.AccelerometerScale;
}
}
catch (Exception) { }
}
if (_GyroscopeScaleLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_GyroscopeScaleLabel.Value = _SettingContainer.Settings.ImuSettings.GyroscopeScale;
}
}
catch (Exception) { }
}
if (_TemperatureScaleLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_TemperatureScaleLabel.Value = _SettingContainer.Settings.ImuSettings.TemperatureScale;
}
}
catch (Exception) { }
}
if (_TemperatureOffsetLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_TemperatureOffsetLabel.Value = _SettingContainer.Settings.ImuSettings.TemperatureOffset;
}
}
catch (Exception) { }
}
if (_RecordRemoteLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_RecordRemoteLabel.Value = _SettingContainer.Settings.ImuSettings.RecordRemote;
}
}
catch (Exception) { }
}
if (_SlamReproducibleExecutionLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_SlamReproducibleExecutionLabel.Value = _SettingContainer.Settings.SlamSettings.ReproducibleExecution;
}
}
catch (Exception) { }
}
if (_EnableVisualInertialLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_EnableVisualInertialLabel.Value = _SettingContainer.Settings.SlamSettings.EnableVisualInertial;
}
}
catch (Exception) { }
}
if (_AccelerometerNoiseLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_AccelerometerNoiseLabel.Value = _SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerNoiseDensity;
}
}
catch (Exception) { }
}
if (_AccelerometerWalkLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_AccelerometerWalkLabel.Value = _SettingContainer.Settings.CalibrationSettings.ImuCalibration.AccelerometerRandomWalk;
}
}
catch (Exception) { }
}
if (_GyroscopeNoiseLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_GyroscopeNoiseLabel.Value = _SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeNoiseDensity;
}
}
catch (Exception) { }
}
if (_GyroscopeWalkLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_GyroscopeWalkLabel.Value = _SettingContainer.Settings.CalibrationSettings.ImuCalibration.GyroscopeRandomWalk;
}
}
catch (Exception) { }
}
if (_TCamImuLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_TCamImuLabel.Value = DoubleArrayArrayToByteArray(_SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.T_Cam_Imu);
}
}
catch (Exception) { }
}
if (_MInvGyroLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_MInvGyroLabel.Value = DoubleArrayArrayToByteArray(_SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.M_Inv_Gyro);
}
}
catch (Exception) { }
}
if (_RAccGyroLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_RAccGyroLabel.Value = DoubleArrayArrayToByteArray(_SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.R_Acc_Gyro);
}
}
catch (Exception) { }
}
if (_MInvAccLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_MInvAccLabel.Value = DoubleArrayArrayToByteArray(_SettingContainer.Settings.CalibrationSettings.ExtrinsicCalibrationSettings.M_Inv_Acc);
}
}
catch (Exception) { }
}
if (_UpdateSettingsLabel != null)
{
try
{
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
_UpdateSettingsLabel.AsyncCall(new byte[] { });
}
}
catch (Exception) { }
}
}
public void ReplayOffline(RawDataReader reader, Action <TimeSpan> updateTime, Action onClose, Func <bool> isPaused, Func <bool> isStopped)
{
_Tasks.Add(Task.Factory.StartNew(() =>
{
ProxyMode = IOProxyMode.Offline;
Stopwatch watch = new Stopwatch();
watch.Start();
int nextTimeUpdate = 1000;
long startTime = -1;
int currentTime = 0;
while (reader.HasNext())
{
while (isPaused())
{
watch.Stop();
Thread.Sleep(500);
watch.Start();
}
if (isStopped())
{
break;
}
Tuple <long, List <Tuple <RawReaderMode, object> > > res = reader.Next();
if (startTime == -1)
{
startTime = res.Item1;
}
ImuEventData imuEventData = null;
CameraEventData cameraEventData = null;
int rawSize = 0;
byte[] rawImage = null;
byte[] rawImu = null;
double exposureTime = 0.0;
foreach (Tuple <RawReaderMode, object> val in res.Item2)
{
if (val.Item1 == RawReaderMode.Imu0)
{
imuEventData = ImuEventData.Parse(res.Item1, (Tuple <double, double, double, double, double, double>)val.Item2, res.Item2.Any(c => c.Item1 == RawReaderMode.Camera0));
rawSize += imuEventData.RawSize;
rawImu = imuEventData.GetRaw(_SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
}
if (val.Item1 == RawReaderMode.Camera0)
{
cameraEventData = CameraEventData.Parse(((Tuple <double, byte[]>)val.Item2).Item2, 0, false, ((Tuple <double, byte[]>)val.Item2).Item1);
rawSize += cameraEventData.RawSize;
rawImage = ((Tuple <double, byte[]>)val.Item2).Item2;
exposureTime = ((Tuple <double, byte[]>)val.Item2).Item1;
}
}
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
if (rawSize > 0)
{
byte[] data = new byte[rawSize];
Array.Copy(rawImu, data, rawImu.Length);
if (rawImage != null)
{
Array.Copy(BitConverter.GetBytes(exposureTime), 0, data, imuEventData.RawSize, sizeof(double));
Array.Copy(rawImage, 0, data, imuEventData.RawSize + sizeof(double), rawImage.Length);
}
_ReplayDataSend.AsyncCall(data);
}
}
else
{
_BackgroundQueue.Add(new Tuple <ImuEventData, CameraEventData>(imuEventData, cameraEventData));
}
currentTime += reader.DeltaTimeMs;
int sleep = (int)(currentTime - watch.ElapsedMilliseconds);
if (sleep > reader.DeltaTimeMs)
{
Thread.Sleep(sleep);
}
if (res.Item1 / 1000 > nextTimeUpdate)
{
nextTimeUpdate += 1000;
updateTime(reader.Length - TimeSpan.FromMilliseconds((res.Item1 - startTime) / (1000 * 1000)));
}
}
reader.Close();
ProxyMode = IOProxyMode.Live;
onClose();
}, TaskCreationOptions.LongRunning));
}