public void HandleStreaming() { // Check if there is a connection with QTM if (!rtProtocol.IsConnected()) { // If not connected, establish a connection if (!rtProtocol.Connect(ipAddress)) { Console.WriteLine("QTM: Trying to connect"); Thread.Sleep(1000); return; } Console.WriteLine("QTM: Connected"); // Take control of QTM and load the desired file and start the realtime stream if (rtProtocol.TakeControl(password)) { Console.WriteLine("QTM: Took control of QTM using specified password in Options/Real-Time Output."); rtProtocol.LoadFile(filename); rtProtocol.StartCapture(true); } else { Console.WriteLine("QTM: Failed to take control of QTM using specified password in Options/Real-Time Output."); } } // Check for available 6DOF rigid body data in the stream if (rtProtocol.Settings6DOF == null) { if (!rtProtocol.Get6dSettings()) { Console.WriteLine("QTM: Trying to get 6DOF settings"); Thread.Sleep(500); return; } Console.WriteLine("QTM: 6DOF data available"); // If 6DOF was not streaming tell QTM to give the data as fast as possible rtProtocol.StreamAllFrames(QTMRealTimeSDK.Data.ComponentType.Component6dEulerResidual); Console.WriteLine("QTM: Starting to stream 6DOF data"); Thread.Sleep(500); } // Get RTPacket from stream PacketType packetType; rtProtocol.ReceiveRTPacket(out packetType, false); // Handle 6DOF rigid body data if (packetType == PacketType.PacketData) { var sixDofData = rtProtocol.GetRTPacket().Get6DOFEulerResidualData(); if (sixDofData != null) { // Print out the available 6DOF data. for (int body = 0; body < sixDofData.Count; body++) { var sixDofBody = sixDofData[body]; var bodySetting = rtProtocol.Settings6DOF.Bodies[body]; Console.WriteLine("Frame:{0:D5} Body:{1,20} X:{2,7:F1} Y:{3,7:F1} Z:{4,7:F1} First Angle:{5,7:F1} Second Angle:{6,7:F1} Third Angle:{7,7:F1} Residual:{8,7:F1}", rtProtocol.GetRTPacket().Frame, bodySetting.Name, sixDofBody.Position.X, sixDofBody.Position.Y, sixDofBody.Position.Z, sixDofBody.Rotation.First, sixDofBody.Rotation.Second, sixDofBody.Rotation.Third, sixDofBody.Residual); } } } // Handle event packet if (packetType == PacketType.PacketEvent) { // If an event comes from QTM then print it out var qtmEvent = rtProtocol.GetRTPacket().GetEvent(); Console.WriteLine("{0}", qtmEvent); } }
static void Main(string[] args) { //CreateConfigFile(); //Constants var config = new Configuration(); try { config = Configuration.LoadFromFile("config.cfg"); Console.WriteLine("Settings loaded from config.cfg"); } catch { CreateConfigFile(); Console.WriteLine("No Configuration file found, new file created: config.cfg"); Environment.Exit(0); } var section = config["General"]; string dateToday = DateTime.Today.Year.ToString() + DateTime.Today.Month.ToString() + DateTime.Today.Day.ToString(); string filenameCSV = section["Filename Prefix"].StringValue + dateToday + "_" + (int)DateTime.Now.TimeOfDay.TotalSeconds + ".csv"; string filenameReadable = section["Filename Prefix"].StringValue + dateToday + "_" + (int)DateTime.Now.TimeOfDay.TotalSeconds + ".txt"; bool logReadable = section["Log Readable"].BoolValue; bool logCsv = section["Log CSV"].BoolValue; bool verbose = section["Verbose"].BoolValue; bool verboseCsv = section["Verbose CSV"].BoolValue; string ipAddress = section["ipAdress"].StringValue; int flipDeg = section["Flip deg"].IntValue; int jumpDist = section["Jump mm"].IntValue; //Dictionary<string, sixDofBody> bodys = new Dictionary<string, sixDofBody>(); List <sixDofBody> ListOfBodies = new List <sixDofBody>(); int totalNumberofFrames = 0; DateTime lastBlip = new DateTime(); lastBlip = DateTime.Now; int frameNumber = 0; RTProtocol mRtProtocol = new RTProtocol(); while (true) { if (!mRtProtocol.IsConnected()) { if (!mRtProtocol.Connect(ipAddress)) { Console.WriteLine("QTM: Trying to connect"); Thread.Sleep(1000); } } else { Console.WriteLine("QTM: Connected"); break; } } if (mRtProtocol.Settings6DOF == null) { if (!mRtProtocol.Get6dSettings()) { Console.WriteLine("QTM: Trying to get 6DOF settings"); Thread.Sleep(500); } Console.WriteLine("QTM: 6DOF settings available"); //List<ComponentType> componentsToStream = new List<ComponentType> //{ // ComponentType.Component6dEulerResidual, // ComponentType.ComponentTimecode //}; mRtProtocol.StreamAllFrames(ComponentType.Component6dEulerResidual); Console.WriteLine("QTM: Starting to stream 6DOF data"); Thread.Sleep(500); } string fileName = DateTime.Today.Year + DateTime.Today.Month + DateTime.Today.Day + "_" + (int)DateTime.Now.TimeOfDay.TotalSeconds + ".csv"; PacketType packetType; List <Q6DOFEuler> previousFrame6dData = new List <Q6DOFEuler>(); while (previousFrame6dData.Count == 0) { mRtProtocol.ReceiveRTPacket(out packetType, false); if (packetType == PacketType.PacketData) { previousFrame6dData = mRtProtocol.GetRTPacket().Get6DOFEulerResidualData(); foreach (var body in mRtProtocol.Settings6DOF.Bodies) { var mbody = new sixDofBody(); mbody.Name = body.Name; mbody.lastSeen = mRtProtocol.GetRTPacket().Frame; mbody.numberOfFrames = 1; mbody.biggestGap = 0; ListOfBodies.Add(mbody); } var duplicates = ListOfBodies.GroupBy(x => x.Name).Where(x => x.Count() > 1).ToList(); //.Select(x => new { Name = x.Key, objs = x.ToList() }); foreach (var dup in duplicates) { Console.WriteLine("Warning multiple bodies with the same name: " + dup.Key); Console.WriteLine("This will effect gap length calculations"); foreach (var bod in dup) { } } } } var csvHeader = "Frame;Body;Type;Magnitude;X;Y;Z\n"; using (var writer = File.AppendText(filenameCSV)) { writer.Write(csvHeader); } while (true) { mRtProtocol.ReceiveRTPacket(out packetType, false); if (packetType == PacketType.PacketData) { StringBuilder writeBufferReadable = new StringBuilder(); StringBuilder writeBufferCSV = new StringBuilder(); var frame6dData = mRtProtocol.GetRTPacket().Get6DOFEulerResidualData(); var packet = mRtProtocol.GetRTPacket(); frameNumber = packet.Frame; if (frame6dData != null) { for (int body = 0; body < frame6dData.Count; body++) { var sixDofBody = frame6dData[body]; var prevSexDofBody = previousFrame6dData[body]; var bodySetting = mRtProtocol.Settings6DOF.Bodies[body]; totalNumberofFrames++; //if (float.IsNaN(sixDofBody.Residual)) //{ // bodys[bodySetting.Name].numberOfFrames++; // if ((frameNumber - bodys[bodySetting.Name].lastSeen)> bodys[bodySetting.Name].biggestGap) // { // bodys[bodySetting.Name].biggestGap = (frameNumber - bodys[bodySetting.Name].lastSeen); // } // bodys[bodySetting.Name].lastSeen = frameNumber; //} if (float.IsNaN(sixDofBody.Residual) && float.IsNaN(prevSexDofBody.Residual)) { } else if (!float.IsNaN(sixDofBody.Residual) && float.IsNaN(prevSexDofBody.Residual)) { writeBufferReadable.AppendFormat("{0} Body: {1} appeard with coordinates {2:F2} {3:F2} {4:F2}", frameNumber, bodySetting.Name, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); writeBufferCSV.AppendFormat("{0};{1};A;NaN;{2:F2};{3:F2};{4:F2}", frameNumber, bodySetting.Name, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); } else if (float.IsNaN(sixDofBody.Residual) && !float.IsNaN(prevSexDofBody.Residual)) { writeBufferReadable.AppendFormat("{0} Body: {1} disappeared with coordinates {2:F2} {3:F2} {4:F2}", frameNumber, bodySetting.Name, prevSexDofBody.Position.X / 1000, prevSexDofBody.Position.Y / 1000, prevSexDofBody.Position.Z / 1000); writeBufferCSV.AppendFormat("{0};{1};D;NaN;{2:F2};{3:F2};{4:F2}", frameNumber, bodySetting.Name, prevSexDofBody.Position.X / 1000, prevSexDofBody.Position.Y / 1000, prevSexDofBody.Position.Z / 1000); } else if (!float.IsNaN(sixDofBody.Residual) && !float.IsNaN(prevSexDofBody.Residual)) { var movementX = sixDofBody.Position.X - prevSexDofBody.Position.X; var movementY = sixDofBody.Position.Y - prevSexDofBody.Position.Y; var movementZ = sixDofBody.Position.Z - prevSexDofBody.Position.Z; var movementABS = Math.Sqrt(Math.Pow(movementX, 2) + Math.Pow(movementY, 2) + Math.Pow(movementZ, 2)); var angMovement1 = Math.Abs(sixDofBody.Rotation.First - prevSexDofBody.Rotation.First); var angMovement2 = Math.Abs(sixDofBody.Rotation.Second - prevSexDofBody.Rotation.Second); var angMovement3 = Math.Abs(sixDofBody.Rotation.Third - prevSexDofBody.Rotation.Third); var angMoveMax = Math.Max(angMovement1, Math.Max(angMovement2, angMovement3)); if (movementABS > jumpDist) { writeBufferReadable.AppendFormat("{0} Body: {1} jumped {2:F}mm at {3:F2} {4:F2} {5:F2}", frameNumber, bodySetting.Name, movementABS, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); writeBufferCSV.AppendFormat("{0};{1};J;{2:F2};{3:F2};{4:F2};{5:F2}", frameNumber, bodySetting.Name, movementABS, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); } if (angMoveMax > flipDeg) { writeBufferReadable.AppendFormat("{0} Body: {1} flipped {2:F0} deg at {2:F2} {3:F2} {4:F2}", frameNumber, bodySetting.Name, angMoveMax, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); writeBufferCSV.AppendFormat("{0};{1};F;{2:F2};{3:F2};{4:F2};{5:F2}", frameNumber, bodySetting.Name, movementABS, sixDofBody.Position.X / 1000, sixDofBody.Position.Y / 1000, sixDofBody.Position.Z / 1000); } } if (writeBufferCSV.Length > 0) { if (logCsv) { using (var writer = File.AppendText(filenameCSV)) { writer.WriteLine(writeBufferCSV); } } if (logReadable) { using (var writer = File.AppendText(filenameReadable)) { writer.WriteLine(writeBufferReadable); } } if (verbose) { Console.WriteLine(writeBufferReadable); } if (verboseCsv) { Console.WriteLine(writeBufferCSV); } writeBufferReadable.Clear(); writeBufferCSV.Clear(); } } previousFrame6dData = frame6dData; } } if (Console.KeyAvailable) { if (Console.ReadKey(false).Key == ConsoleKey.Escape) { break; } } if (lastBlip.AddSeconds(1) < DateTime.Now) { Console.WriteLine("Logging, current frame " + frameNumber); lastBlip = DateTime.Now; } } }