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;
}
}
}
