static bool Get6DOFSettings(RTState state, RTProtocol mProtocol)
{
// Get settings and information for streamed bodies
bool getstatus = mProtocol.Get6dSettings();
if (getstatus)
{
state.bodies.Clear();
Settings6D settings = mProtocol.Settings6DOF;
foreach (Settings6DOF body in settings.Bodies)
{
SixDOFBody newbody = new SixDOFBody();
newbody.Name = body.Name;
newbody.Color.r = (body.ColorRGB) & 0xFF;
newbody.Color.g = (body.ColorRGB >> 8) & 0xFF;
newbody.Color.b = (body.ColorRGB >> 16) & 0xFF;
newbody.Color /= 255;
newbody.Color.a = 1F;
newbody.Position = Vector3.zero;
newbody.Rotation = Quaternion.identity;
state.bodies.Add(newbody);
}
return(true);
}
return(false);
}
private bool Get6DOFSettings()
{
// Get settings and information for streamed bodies
bool getstatus = mProtocol.Get6dSettings();
if (getstatus)
{
mBodies.Clear();
Settings6D settings = mProtocol.Settings6DOF;
foreach (Settings6DOF body in settings.Bodies)
{
SixDOFBody newbody = new SixDOFBody();
newbody.Name = body.Name;
newbody.Position = Vector3.zero;
newbody.Rotation = Quaternion.identity;
mBodies.Add(newbody);
}
return(true);
}
return(false);
}
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;
}
}
}
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;
}
}
}
private static RTProtocol EstablishConnection(string qtmName)
{
RTProtocol rtProtocol = new RTProtocol();
Console.WriteLine("Discovering QTM servers on the network...");
// We know the name of the computer we want to connect to, find out its IP
DiscoveryResponse qtmToConnect = new DiscoveryResponse();
if (rtProtocol.DiscoverRTServers(qtmDiscoveryPort))
{
var discoveryResponses = rtProtocol.DiscoveryResponses;
if (discoveryResponses.Count != 0)
{
foreach (var discoveryResponse in discoveryResponses)
{
Console.WriteLine(
"Discovered {0,16} {1,16} {2,16} {3,3} cameras",
discoveryResponse.HostName,
discoveryResponse.IpAddress,
discoveryResponse.InfoText,
discoveryResponse.CameraCount
);
if (discoveryResponse.HostName == qtmName)
{
qtmToConnect = discoveryResponse;
}
}
}
else
{
Console.WriteLine("No QTM servers found on available networks!");
}
}
// Only connect if the desired computer name is found
if (qtmToConnect.HostName == qtmName)
{
// Try max. 5 times, then admit failure
for (int i = 0; i < 5; i++)
{
if (rtProtocol.IsConnected())
{
break;
}
if (!rtProtocol.Connect(qtmToConnect.IpAddress))
{
Console.WriteLine("Trying to connect...");
Thread.Sleep(1000);
}
else
{
Console.WriteLine(
"Connected to {0} @ {1}!",
qtmToConnect.HostName,
qtmToConnect.IpAddress
);
}
}
}
else
{
Console.WriteLine("The desired QTM server ({0}) was not found!", qtmName);
}
// Get settings if connected to QTM, otherwise alert user and continue
if (rtProtocol.IsConnected())
{
// 6DOF settings
Console.WriteLine("Getting 6DOF settings...");
// Try max. 5 times, then admit failure
for (int i = 0; i < 5; i++)
{
if (rtProtocol.Get6dSettings())
{
Settings6D settings6D = rtProtocol.Settings6DOF;
int bodyCount = settings6D.BodyCount;
Console.WriteLine("{0} 6DOF bodies found.", bodyCount);
List <Settings6DOF> qtmBodies = settings6D.Bodies;
foreach (Settings6DOF body in qtmBodies)
{
Console.WriteLine("\t Found 6DOF body: {0}", body.Name);
}
break;
}
else
{
Console.WriteLine("Failed to get 6DOF settings!");
}
}
}
else
{
Console.WriteLine("Could not communicate with QTM!");
}
// Disconnect on exit
AppDomain.CurrentDomain.ProcessExit += (sender, e) => OnProcessExit(sender, e, rtProtocol);
return(rtProtocol);
}
void StartStreaming(string ipAddress)
{
Debug.LogFormat("Starting stream from {0}...", ipAddress);
// Init protocol
rtProtocol = new RTProtocol();
if (!rtProtocol.IsConnected())
{
// Check if connection to QTM is possible
if (!rtProtocol.IsConnected())
{
if (!rtProtocol.Connect(ipAddress))
{
Debug.Log("QTM: Trying to connect...");
return;
}
Debug.Log("QTM: Connected!");
}
// Get settings and start stream
if (rtProtocol.GeneralSettings == null)
{
if (!rtProtocol.GetGeneralSettings())
{
Debug.Log("QTM: Trying to get General settings...");
return;
}
Debug.Log("QTM: General settings available.");
// Print camera settings to console
Debug.LogFormat("QTM: Frequency: {0}", rtProtocol.GeneralSettings.CaptureFrequency);
Debug.Log("QTM: Cameras:");
foreach (var camera in rtProtocol.GeneralSettings.CameraSettings)
{
Debug.LogFormat("\t{0}", camera.Model);
}
// Reset mocap gameobject
foreach (Transform child in mocapGameObject.transform)
{
GameObject.Destroy(child.gameObject);
}
// Init components to stream
List <QTMRealTimeSDK.Data.ComponentType> componentsToStream = new List <QTMRealTimeSDK.Data.ComponentType>();
// Start 2D data stream and print to console
if (print2DToConsole)
{
Debug.Log("QTM: Starting to stream 2D data, printing to console.");
componentsToStream.Add(QTMRealTimeSDK.Data.ComponentType.Component2d);
}
// Start 3D data stream...
if (print3DToConsole || render3D)
{
Debug.Log("QTM: Starting to stream 3D data.");
componentsToStream.Add(QTMRealTimeSDK.Data.ComponentType.Component3dResidual);
// ...and print to console...
if (print3DToConsole)
{
Debug.Log("QTM: 3D data stream will print to console.");
}
// ...and/or render as balls
if (render3D)
{
Debug.Log("QTM: 3D data stream will render in world.");
rtProtocol.Get3dSettings();
balls = new GameObject[rtProtocol.Settings3D.Labels.Count];
for (int i = 0; i < balls.Length; i++)
{
balls[i] = GameObject.CreatePrimitive(PrimitiveType.Sphere);
balls[i].transform.parent = mocapGameObject.transform;
balls[i].transform.localScale = new Vector3(0.03f, 0.03f, 0.03f);
}
}
}
// Start 6D data stream and render
if (render6D)
{
rtProtocol.Get6dSettings();
List <QTMRealTimeSDK.Settings.Settings6DOF> qtmBodies = rtProtocol.Settings6DOF.Bodies;
foreach (QTMRealTimeSDK.Settings.Settings6DOF body in qtmBodies)
{
Debug.LogFormat("QTM: Found 6DOF body: {0}", body.Name);
}
cubes = new GameObject[qtmBodies.Count];
for (int i = 0; i < cubes.Length; i++)
{
cubes[i] = GameObject.CreatePrimitive(PrimitiveType.Cube);
cubes[i].transform.parent = mocapGameObject.transform;
cubes[i].transform.localScale = new Vector3(0.1f, 0.1f, 0.1f);
}
Debug.LogFormat("QTM: Starting to stream 6D data, rendering in world.");
componentsToStream.Add(QTMRealTimeSDK.Data.ComponentType.Component6dResidual);
}
rtProtocol.StreamFrames(StreamRate.RateFrequency, streamFrequency, componentsToStream);
}
}
}
