public void OnIMUReplayRawDataReceived(object sender, IMUDataEventArgs e)
{
if (replayModeActivated)
{
ProcessImuRawData(e);
}
}
public void UpdateImuDataOnGraph(object sender, IMUDataEventArgs e)
{
oscilloX.AddPointToLine(2, e.timeStampMS / 1000.0, e.accelX);
oscilloY.AddPointToLine(2, e.timeStampMS / 1000.0, e.accelY);
oscilloTheta.AddPointToLine(2, e.timeStampMS / 1000.0, e.gyrZ);
currentTime = e.timeStampMS / 1000.0;
}
public virtual void OnIMU(IMUDataEventArgs dat)
{
var handler = OnIMURawDataFromReplayGeneratedEvent;
if (handler != null)
{
handler(this, new IMUDataEventArgs {
accelX = dat.accelX, accelY = dat.accelY, accelZ = dat.accelZ, gyroX = dat.gyroX, gyroY = dat.gyroY, gyroZ = dat.gyroZ, EmbeddedTimeStampInMs = dat.EmbeddedTimeStampInMs
});
}
}
public void OnIMURawDataReceived(object sender, IMUDataEventArgs e)
{
IMUDataEventArgsLog data = new IMUDataEventArgsLog();
data.accelX = e.accelX;
data.accelY = e.accelY;
data.accelZ = e.accelZ;
data.gyroX = e.gyroX;
data.gyroY = e.gyroY;
data.gyroZ = e.gyroZ;
data.magX = e.magX;
data.magY = e.magY;
data.magZ = e.magZ;
data.EmbeddedTimeStampInMs = e.EmbeddedTimeStampInMs;
data.InstantInMs = DateTime.Now.Subtract(initialDateTime).TotalMilliseconds;
var msg = ZeroFormatterSerializer.Serialize <ZeroFormatterLogging>(data);
Log(msg);
}
private void ProcessImuRawData(IMUDataEventArgs e)
{
//Point3D accelXYZ = new Point3D(e.accelX - offsetAccelX, e.accelY - offsetAccelY, e.accelZ - offsetAccelZ);
Point3D accelXYZ = new Point3D(e.accelX, e.accelY, e.accelZ);
Point3D gyroXYZ = new Point3D(e.gyroX - offsetGyroX, e.gyroY - offsetGyroY, e.gyroZ - offsetGyroZ);
if (calibrationInProgress)
{
accelXCalibrationList.Add(e.accelX);
accelYCalibrationList.Add(e.accelY);
accelZCalibrationList.Add(e.accelZ);
gyroXCalibrationList.Add(e.gyroX);
gyroYCalibrationList.Add(e.gyroY);
gyroZCalibrationList.Add(e.gyroZ);
}
//On envois l'event aux abonnés
OnIMUProcessedData(e.EmbeddedTimeStampInMs, accelXYZ, gyroXYZ);
OnGyroSpeed(robotId, e.gyroZ - offsetGyroZ);
}
public void UpdateImuDataOnGraph(object sender, IMUDataEventArgs e)
{
oscilloX.AddPointToLine(2, e.EmbeddedTimeStampInMs / 1000.0, e.accelX);
oscilloTheta.AddPointToLine(2, e.EmbeddedTimeStampInMs / 1000.0, e.gyroZ);
currentTime = e.EmbeddedTimeStampInMs / 1000.0;
}
private void ReplayLoop()
{
if (!replayLoopInProgress)
{
replayLoopInProgress = true;
if (replayModeActivated)
{
if (isReplayingFileSerieDefined)
{
//Si le nom de la série de fichier à traiter est défini
if (!isReplayingFileOpened)
{
//Si aucun fichier n'est ouvert, on ouvre le courant
if (subFileIndex == 0)
{
filePath = replayFileSerieName + "0_Init.rbt";
}
else
{
filePath = replayFileSerieName + subFileIndex + ".rbt";
}
if (File.Exists(filePath))
{
sr = new StreamReader(filePath);
isReplayingFileOpened = true;
OnFileNameChange(filePath);
if (subFileIndex == 0)
{
initialDateTime = DateTime.Now;
lastDataTimestamp = 0;
}
}
else
{
//On n'a plus de fichier à traiter
isReplayingFileOpened = false;
isReplayingFileSerieDefined = false;
replayModeActivated = false;
}
}
if (isReplayingFileOpened)
{
int successiveDataCounter = 0;
//Récupère l'instant courant de la dernière data du replay
currentTimestamp = (DateTime.Now.Subtract(initialDateTime).TotalMilliseconds + LogDateTimeOffsetInMs);
if (currentTimestamp == null)
{
currentTimestamp = 0;
}
//On le fait tant que l'instant courant des data de Replay est inférieur à l'instant théorique de simulation, on traite les datas
while (lastDataTimestamp <= currentTimestamp * speedFactor && isReplayingFileOpened && successiveDataCounter < 10)
{
var s = sr.ReadLine();
if (s != null)
{
byte[] bytes = Convert.FromBase64String(s);
var deserialization = ZeroFormatterSerializer.Deserialize <ZeroFormatterLogging>(bytes);
switch (deserialization.Type)
{
case ZeroFormatterLoggingType.IMUDataEventArgs:
IMUDataEventArgsLog ImuData = (IMUDataEventArgsLog)deserialization;
lastDataTimestamp = ImuData.InstantInMs;
var e = new IMUDataEventArgs();
e.accelX = ImuData.accelX;
e.accelY = ImuData.accelY;
e.accelZ = ImuData.accelZ;
e.gyroX = ImuData.gyroX;
e.gyroY = ImuData.gyroY;
e.gyroZ = ImuData.gyroZ;
e.magX = ImuData.magX;
e.magY = ImuData.magY;
e.magZ = ImuData.magZ;
e.EmbeddedTimeStampInMs = ImuData.EmbeddedTimeStampInMs;
OnIMU(e);
break;
case ZeroFormatterLoggingType.PolarSpeedEventArgs:
PolarSpeedEventArgsLog robotSpeedData = (PolarSpeedEventArgsLog)deserialization;
lastDataTimestamp = robotSpeedData.InstantInMs;
var eSpeed = new PolarSpeedEventArgs();
eSpeed.RobotId = robotSpeedData.RobotId;
eSpeed.Vtheta = robotSpeedData.Vtheta;
eSpeed.Vx = robotSpeedData.Vx;
eSpeed.Vy = robotSpeedData.Vy;
eSpeed.timeStampMs = robotSpeedData.timeStampMs;
OnSpeedData(eSpeed);
break;
case ZeroFormatterLoggingType.RawLidarArgs:
RawLidarArgsLog currentLidarLog = (RawLidarArgsLog)deserialization;
lastDataTimestamp = currentLidarLog.InstantInMs;
OnLidar(currentLidarLog.RobotId, currentLidarLog.PtList);
break;
default:
break;
}
if (LogDateTimeOffsetInMs == null)
{
LogDateTimeOffsetInMs = lastDataTimestamp;
}
successiveDataCounter++;
}
else
{
//Lecture échouée, on a une fin de fichier
isReplayingFileOpened = false;
sr.Close();
subFileIndex++;
}
//Récupère l'instant courant de la dernière data du replay
currentTimestamp = (double)(DateTime.Now.Subtract(initialDateTime).TotalMilliseconds + LogDateTimeOffsetInMs);
}
}
}
}
replayLoopInProgress = false;
}
}
