Beispiel #1
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.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;
            }
        }
Beispiel #2
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);
                                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;
            }
        }