void WriterThreadFunction()
{
try
{
using (var rtProtocol = new RTProtocol(RT_LOWEST_SUPPORTED_VERSION_MAJOR, RT_LOWEST_SUPPORTED_VERSION_MINOR))
{
if (!rtProtocol.Connect(IpAddress, udpPort, RT_LOWEST_SUPPORTED_VERSION_MAJOR, RT_LOWEST_SUPPORTED_VERSION_MINOR))
{
throw new WriterThreadException("Error Creating Connection to server" + rtProtocol.GetErrorString());
}
RtProtocolVersion version = new RtProtocolVersion(RTProtocol.Constants.MAJOR_VERSION, RTProtocol.Constants.MINOR_VERSION);
//Upgrade protocol version
for (; version.minor >= RT_LOWEST_SUPPORTED_VERSION_MINOR; --version.minor)
{
lock (syncLock)
{
writerThreadState.rtProtocolVersion.CopyFrom(version);
}
string response;
if (rtProtocol.SetVersion(version.major, version.minor, out response))
{
break;
}
}
if (version.minor < RT_LOWEST_SUPPORTED_VERSION_MINOR)
{
throw new WriterThreadException("Failed to negotiate RT Protocol version with QTM");
}
lock (syncLock)
{
writerThreadState.connectionState = RTConnectionState.Connected;
if (!UpdateSettings(writerThreadState, rtProtocol, componentSelection))
{
throw new WriterThreadException("Failed to update settings: " + rtProtocol.GetErrorString());
}
if (!StartStreaming(writerThreadState, rtProtocol, streamRate, udpPort))
{
throw new WriterThreadException("Failed to start stream: " + rtProtocol.GetErrorString());
}
}
while (true)
{
if (!rtProtocol.IsConnected())
{
throw new WriterThreadException("Connection lost");
}
if (killThread)
{
throw new WriterThreadException("Thread was killed");
}
PacketType packetType;
if (rtProtocol.ReceiveRTPacket(out packetType, false) <= 0)
{
continue;
}
var packet = rtProtocol.GetRTPacket();
if (packet != null)
{
if (packetType == PacketType.PacketData)
{
lock (syncLock)
{
Process(writerThreadState, packet);
}
}
else if (packetType == PacketType.PacketEvent)
{
QTMEvent currentEvent = packet.GetEvent();
switch (currentEvent)
{
case QTMEvent.QTMShuttingDown:
throw new WriterThreadException("Qtm closed connection");
case QTMEvent.RTFromFileStarted:
case QTMEvent.Connected:
case QTMEvent.CaptureStarted:
case QTMEvent.CalibrationStarted:
case QTMEvent.CameraSettingsChanged:
lock (syncLock)
{
// reload settings when we start streaming to get proper settings
if (!UpdateSettings(writerThreadState, rtProtocol, componentSelection))
{
throw new WriterThreadException("Failed to update settings: " + rtProtocol.GetErrorString());
}
if (!StartStreaming(writerThreadState, rtProtocol, streamRate, udpPort))
{
throw new WriterThreadException("Failed to start stream: " + rtProtocol.GetErrorString());
}
}
break;
case QTMEvent.ConnectionClosed:
default: break;
}
}
}
}
}
}
catch (WriterThreadException writerThreadException)
{
lock (syncLock)
{
writerThreadState.errorString = writerThreadException.Message;
writerThreadState.connectionState = RTConnectionState.Disconnected;
}
}
catch (System.Exception e)
{
lock (syncLock)
{
writerThreadState.errorString = "Exception "
+ e.GetType().Name + ": " + e.Message + "\n"
+ e.StackTrace.Replace(" at ", "\n at ");
writerThreadState.connectionState = RTConnectionState.Disconnected;
}
}
}
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);
}
}
private void Timer_Tick(object sender, EventArgs e)
{
// Try and connect
if (!rtProtocol.IsConnected())
{
if (!rtProtocol.Connect(ipAddress))
{
Color.BackColor = System.Drawing.Color.Red;
return;
}
Color.BackColor = System.Drawing.Color.OrangeRed;
}
// Check for available 6DOF data in the stream
if (rtProtocol.Settings6DOF == null)
{
if (!rtProtocol.Get6dSettings())
{
return;
}
Color.BackColor = System.Drawing.Color.Yellow;
if (sixDofBodyNameToUse.Length > 0)
{
for (int bodyIndex = 0; bodyIndex < rtProtocol.Settings6DOF.BodyCount; bodyIndex++)
{
if (string.Equals(rtProtocol.Settings6DOF.Bodies[bodyIndex].Name, sixDofBodyNameToUse, StringComparison.OrdinalIgnoreCase))
{
bodyIndexToUse = bodyIndex;
break;
}
}
}
else
{
if (rtProtocol.Settings6DOF.BodyCount > 0)
{
sixDofBodyNameToUse = rtProtocol.Settings6DOF.Bodies[0].Name;
bodyIndexToUse = 0;
}
}
eulerNames = rtProtocol.Settings6DOF.EulerNames;
// Start streaming 6dof euler residual data at 10Hz frequency
rtProtocol.StreamFrames(StreamRate.RateFrequency, 10, QTMRealTimeSDK.Data.ComponentType.Component6dEulerResidual);
Thread.Sleep(500);
}
// Get RTPacket from stream
PacketType packetType;
rtProtocol.ReceiveRTPacket(out packetType, false);
// Handle data packet
if (packetType == PacketType.PacketData)
{
Color.BackColor = System.Drawing.Color.Green;
var sixDofData = rtProtocol.GetRTPacket().Get6DOFEulerResidualData();
if (sixDofData != null)
{
// Put 6dof data information in the labels
if (sixDofData.Count > bodyIndexToUse)
{
var sixDofBody = sixDofData[bodyIndexToUse];
this.Body.Text = sixDofBodyNameToUse;
this.X.Text = float.IsNaN(sixDofBody.Position.X) ? "X:---" : string.Format("X:{0:F1}", sixDofBody.Position.X);
this.Y.Text = float.IsNaN(sixDofBody.Position.Y) ? "Y:---" : string.Format("Y:{0:F1}", sixDofBody.Position.Y);
this.Z.Text = float.IsNaN(sixDofBody.Position.Z) ? "Z:---" : string.Format("Z:{0:F1}", sixDofBody.Position.Z);
this.Residual.Text = float.IsNaN(sixDofBody.Residual) ? "Residual:---" : string.Format("Residual:{0:F1}", sixDofBody.Residual);
this.First.Text = float.IsNaN(sixDofBody.Rotation.First) ? string.Format("{0}:---", eulerNames.First) : string.Format("{0}:{1:F1}", eulerNames.First, sixDofBody.Rotation.First);
this.Second.Text = float.IsNaN(sixDofBody.Rotation.Second) ? string.Format("{0}:---", eulerNames.Second) : string.Format("{0}:{1:F1}", eulerNames.Second, sixDofBody.Rotation.Second);
this.Third.Text = float.IsNaN(sixDofBody.Rotation.Third) ? string.Format("{0}:---", eulerNames.Third) : string.Format("{0}:{1:F1}", eulerNames.Third, sixDofBody.Rotation.Third);
}
}
}
// Handle event packet
if (packetType == PacketType.PacketEvent)
{
var qtmEvent = rtProtocol.GetRTPacket().GetEvent();
switch (qtmEvent)
{
case QTMEvent.EventConnectionClosed:
case QTMEvent.EventCaptureStopped:
case QTMEvent.EventCalibrationStopped:
case QTMEvent.EventRTFromFileStopped:
case QTMEvent.EventQTMShuttingDown:
// If QTM is shutting down then handle it, disconnect and empty labels
rtProtocol.StreamFramesStop();
rtProtocol.Disconnect();
Color.BackColor = System.Drawing.Color.Red;
this.Body.Text = "Body";
this.X.Text = "X";
this.Y.Text = "Y";
this.Z.Text = "Z";
this.Residual.Text = "Residual";
this.First.Text = eulerNames.First;
this.Second.Text = eulerNames.Second;
this.Third.Text = eulerNames.Third;
break;
}
}
}
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;
}
}
}
void Update()
{
// Rotate
if (bumper)
{
Debug.Log("Rotating holograms...");
mocapGameObject.transform.Rotate(Vector3.up, -rotationSpeed * Time.deltaTime);
}
// Translate
else if (_controller.Touch1PosAndForce.z > 0.1f)
{
Debug.Log("Translating holograms...");
float X = _controller.Touch1PosAndForce.x;
float Y = _controller.Touch1PosAndForce.y;
if (_controller.TriggerValue > 0.2f)
{
Vector3 up = Vector3.Normalize(Vector3.ProjectOnPlane(transform.up, Vector3.forward));
Vector3 force = Vector3.Normalize(Y * up);
mocapGameObject.transform.position += force * Time.deltaTime * translationSpeed;
}
else
{
Vector3 forward = Vector3.Normalize(Vector3.ProjectOnPlane(transform.forward, Vector3.up));
Vector3 right = Vector3.Normalize(Vector3.ProjectOnPlane(transform.right, Vector3.up));
Vector3 force = Vector3.Normalize((X * right) + (Y * forward));
mocapGameObject.transform.position += force * Time.deltaTime * translationSpeed;
}
}
// Do QTM stuff
if (rtProtocol != null && rtProtocol.IsConnected())
{
// Get RTPacket from stream
PacketType packetType;
rtProtocol.ReceiveRTPacket(out packetType, false);
// Handle data packet
if (packetType == PacketType.PacketData)
{
// Get 2D
var twoDData = rtProtocol.GetRTPacket().Get2DMarkerData();
// Print 2D
if (twoDData != null && twoDData.Count > 0)
{
var twoDForCamera0 = twoDData[0];
Debug.LogFormat("Frame:{0:D5} Markers:{1} Status:{2}",
rtProtocol.GetRTPacket().Frame,
twoDForCamera0.MarkerCount,
twoDForCamera0.StatusFlags);
}
// Get 3D
var threeDData = rtProtocol.GetRTPacket().Get3DMarkerResidualData();
// Print 3D
if (print3DToConsole)
{
if (threeDData != null && threeDData.Count > 0)
{
for (int i = 0; i < threeDData.Count; i++)
{
var m = threeDData[i];
if (!Double.IsNaN(m.Position.X))
{
Debug.LogFormat("Frame:{0:D5} Name:{1,16} X:{2,7:F1} Y:{3,7:F1} Z:{4,7:F1} Residual:{5,5:F1}",
rtProtocol.GetRTPacket().Frame, rtProtocol.Settings3D.Labels[i].Name,
m.Position.X, m.Position.Y, m.Position.Z,
m.Residual);
}
else
{
Debug.LogFormat("Frame:{0:D5} Name:{1,20} -----------------------------------",
rtProtocol.GetRTPacket().Frame, rtProtocol.Settings3D.Labels[i].Name);
}
}
}
}
// Render 3D
if (render3D)
{
if (threeDData != null && threeDData.Count > 0)
{
for (int i = 0; i < threeDData.Count; i++)
{
var m = threeDData[i];
if (!Double.IsNaN(m.Position.X))
{
float scale = 0.001f;
Vector3 ballPosition = new Vector3(m.Position.X, m.Position.Z, m.Position.Y) * scale;
balls[i].transform.localPosition = ballPosition;
}
}
}
}
// Get 6D
var sixDData = rtProtocol.GetRTPacket().Get6DOFResidualData();
// Render 3D
if (render6D)
{
if (sixDData != null && sixDData.Count > 0)
{
for (int i = 0; i < sixDData.Count; i++)
{
var b = sixDData[i];
if (!Double.IsNaN(b.Position.X))
{
float scale = 0.001f;
Vector3 cubePosition = new Vector3(b.Position.X, b.Position.Z, b.Position.Y) * scale;
cubes[i].transform.localPosition = cubePosition;
}
}
}
}
}
}
}