public void PolarSpeedProcessIntoPosition(object sender, PolarSpeedEventArgs e)
{
Theta += (float)e.Vtheta / Fech;
posX += (float)e.Vx / Fech * (float)Math.Cos(Theta);
posY += (float)e.Vx / Fech * (float)Math.Sin(Theta);
Console.WriteLine("posX : " + posX + " posY : " + posY + " Theta : " + Theta);
}
public void UpdateSpeedPolarOdometryOnInterface(object sender, PolarSpeedEventArgs e)
{
oscilloX.AddPointToLine(1, e.timeStampMs / 1000.0, e.Vx);
oscilloTheta.AddPointToLine(1, e.timeStampMs / 1000.0, e.Vtheta);
currentTime = e.timeStampMs / 1000.0;
asservSpeedDisplay.UpdatePolarOdometrySpeed(e.Vx, e.Vtheta);
}
public virtual void OnPolarOdometrySpeedFromRobot(PolarSpeedEventArgs e)
{
var handler = OnPolarOdometrySpeedFromRobotEvent;
if (handler != null)
{
handler(this, e);
}
}
public virtual void OnSpeedData(PolarSpeedEventArgs dat)
{
var handler = OnSpeedPolarOdometryFromReplayEvent;
if (handler != null)
{
handler(this, new PolarSpeedEventArgs {
Vx = dat.Vx, Vy = dat.Vy, Vtheta = dat.Vtheta, RobotId = dat.RobotId, timeStampMs = dat.timeStampMs
});
}
}
public void OnPolarSpeedDataReceived(object sender, PolarSpeedEventArgs e)
{
PolarSpeedEventArgsLog data = new PolarSpeedEventArgsLog();
data.Vx = e.Vx;
data.Vy = e.Vy;
data.Vtheta = e.Vtheta;
data.RobotId = e.RobotId;
data.timeStampMs = e.timeStampMs;
data.InstantInMs = DateTime.Now.Subtract(initialDateTime).TotalMilliseconds;
var msg = ZeroFormatterSerializer.Serialize <ZeroFormatterLogging>(data);
Log(msg);
}
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.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;
}
}
public virtual void OnSpeedData(PolarSpeedEventArgs dat)
{
OnSpeedPolarOdometryFromReplayEvent?.Invoke(this, new PolarSpeedEventArgs {
Vx = dat.Vx, Vy = dat.Vy, Vtheta = dat.Vtheta, RobotId = dat.RobotId, timeStampMs = dat.timeStampMs
});
}
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);
initialDateTime = DateTime.Now;
}
else
{
//On n'a plus de fichier à traiter
isReplayingFileOpened = false;
isReplayingFileSerieDefined = false;
replayModeActivated = false;
}
}
if (isReplayingFileOpened)
{
//Si un fichier est déjà ouvert
do
{
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;
newReplayInstant = 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;
newReplayInstant = 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;
newReplayInstant = currentLidarLog.InstantInMs;
OnLidar(currentLidarLog.RobotId, currentLidarLog.PtList);
break;
default:
break;
}
if (LogDateTimeOffsetInMs == null)
{
LogDateTimeOffsetInMs = newReplayInstant;
}
}
else
{
//Lecture échouée, on a une fin de fichier
isReplayingFileOpened = false;
sr.Close();
subFileIndex++;
}
}
//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 (newReplayInstant / speedFactor < DateTime.Now.Subtract(initialDateTime).TotalMilliseconds + LogDateTimeOffsetInMs && isReplayingFileOpened);
}
}
}
replayLoopInProgress = false;
}
}
