static bool UpdateSettings(RTState rtState, RTProtocol rtProtocol, List <ComponentType> componentSelection)
{
if (GetGeneralSettings(rtState, rtProtocol) == false)
{
return(false);
}
rtState.componentsInStream = componentSelection.Select(x =>
{
switch (x)
{
case ComponentType.Component3dResidual: return(Get3DSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
case ComponentType.Component3dNoLabelsResidual: return(Get3DSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
case ComponentType.Component6d: return(Get6DOFSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
case ComponentType.ComponentGazeVector: return(GetGazeVectorSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
case ComponentType.ComponentAnalog: return(GetAnalogSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
case ComponentType.ComponentSkeleton: return(GetSkeletonSettings(rtState, rtProtocol) ? x : ComponentType.ComponentNone);
default: return(ComponentType.ComponentNone);
}
;
})
.Where(x => x != ComponentType.ComponentNone)
.ToList();
return(true);
}
static void Main(string[] args)
{
RTProtocol mRtProtocol = new RTProtocol();
string mIpAddress = "127.0.0.1";
//Example example = new ExampleSkeleton(mRtProtocol, mIpAddress);
//Example example = new Example3D(mRtProtocol, mIpAddress);
//Example example = new ExampleImage(mRtProtocol, mIpAddress);
//Example example = new Example2D(mRtProtocol, mIpAddress);
//Example example = new Example6D(mRtProtocol, mIpAddress);
//Example example = new ExampleGaze(mRtProtocol, mIpAddress);
//Example example = new ExampleTimecode(mRtProtocol, mIpAddress);
Example example = new ExampleUDP(mRtProtocol, mIpAddress);
MainExample mainExample = new MainExample(example, mRtProtocol, mIpAddress);
mainExample.DiscoverQTMServers(4545);
Console.WriteLine("Press key to continue");
Console.ReadKey();
while (true)
{
mainExample.Run();
if (Console.KeyAvailable)
{
if (Console.ReadKey(false).Key == ConsoleKey.Escape)
{
break;
}
}
}
}
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);
}
static void Main(string[] args)
{
Console.WriteLine("Establishing connection...");
RTProtocol rtProtocol = EstablishConnection(qtmName);
if (rtProtocol.IsConnected())
{
Console.WriteLine("Streaming 6DOF (Euler) data to port {0} of {1}...", udpToPort, udpToIP);
Task.Run(() =>
{
while (true)
{
rtProtocol.StreamFrames(
StreamRate.RateFrequency,
20,
ComponentType.Component6dEuler,
udpToPort,
udpToIP);
}
});
}
else
{
Console.WriteLine("Cannot continue without QTM connection and bodies.");
}
Console.WriteLine("Press ESC to quit.");
if (Console.ReadKey(true).Key == ConsoleKey.Escape)
{
return;
}
}
/// <summary>
/// Get list of servers available on network (always add localhost)
/// </summary>
/// <returns><c>true</c>, if discovery packet was sent, <c>false</c> otherwise.</returns>
/// <param name="list">List of discovered servers</param>
public List <DiscoveryResponse> GetServers()
{
// Send discovery packet
using (var protocol = new RTProtocol())
{
List <DiscoveryResponse> list = new List <DiscoveryResponse>();
if (protocol.DiscoverRTServers(replyPort))
{
if (protocol.DiscoveryResponses.Count > 0)
{
//Get list of all servers from protocol
foreach (var discoveryResponse in protocol.DiscoveryResponses)
{
//add them to our list for user to pick from
list.Add(discoveryResponse);
}
}
}
list.Add(new DiscoveryResponse
{
HostName = "Localhost",
IpAddress = "127.0.0.1",
Port = RTProtocol.Constants.STANDARD_BASE_PORT,
InfoText = "",
CameraCount = 0
});
return(list);
}
}
public QTMNetworkConnection()
{
Protocol = new RTProtocol();
Connect(GetRandomPort());
QTMVersionSupported = true;
}
private static void OnProcessExit(object sender, EventArgs e, RTProtocol rtProtocol)
{
if (rtProtocol != null)
{
if (rtProtocol.IsConnected())
{
rtProtocol.StreamFramesStop();
rtProtocol.Disconnect();
}
}
}
static bool GetGeneralSettings(RTState state, RTProtocol mProtocol)
{
bool getStatus = mProtocol.GetGeneralSettings();
if (!getStatus)
{
return(false);
}
state.frequency = mProtocol.GeneralSettings.CaptureFrequency;
return(true);
}
static bool StartStreaming(RTState state, RTProtocol rtProtocol, StreamRate streamRate, short udpPort)
{
if (rtProtocol.StreamFrames(streamRate, -1, state.componentsInStream, udpPort) == false)
{
state.isStreaming = false;
}
else
{
state.isStreaming = true;
}
return(state.isStreaming);
}
static bool Get3DSettings(RTState state, RTProtocol mProtocol)
{
bool getstatus = mProtocol.Get3dSettings();
if (getstatus)
{
state.upAxis = mProtocol.Settings3D.AxisUpwards;
state.coordinateSystemChange = Rotation.GetCoordinateSystemRotation(state.upAxis);
// Save marker settings
state.markers.Clear();
foreach (Settings3DLabel marker in mProtocol.Settings3D.Labels)
{
LabeledMarker newMarker = new LabeledMarker();
newMarker.Name = marker.Name;
newMarker.Position = Vector3.zero;
newMarker.Residual = 0;
newMarker.Color.r = (marker.ColorRGB) & 0xFF;
newMarker.Color.g = (marker.ColorRGB >> 8) & 0xFF;
newMarker.Color.b = (marker.ColorRGB >> 16) & 0xFF;
newMarker.Color /= 255;
newMarker.Color.a = 1F;
state.markers.Add(newMarker);
}
// Save bone settings
if (mProtocol.Settings3D.Bones != null)
{
state.bones.Clear();
foreach (var settingsBone in mProtocol.Settings3D.Bones)
{
Bone bone = new Bone();
bone.From = settingsBone.From;
bone.FromMarker = state.GetMarker(settingsBone.From);
bone.To = settingsBone.To;
bone.ToMarker = state.GetMarker(settingsBone.To);
bone.Color.r = (settingsBone.Color) & 0xFF;
bone.Color.g = (settingsBone.Color >> 8) & 0xFF;
bone.Color.b = (settingsBone.Color >> 16) & 0xFF;
bone.Color /= 255;
bone.Color.a = 1F;
state.bones.Add(bone);
}
}
return(true);
}
return(false);
}
private RTClient()
{
// New instance of protocol, contains a RT packet
mProtocol = new RTProtocol();
mBodies = new List <SixDOFBody>();
mMarkers = new List <LabeledMarker>();
mUnlabeledMarkers = new List <UnlabeledMarker>();
mBones = new List <Bone>();
mGazeVectors = new List <GazeVector>();
mAnalogChannels = new List <AnalogChannel>();
mStreamingStatus = false;
mPacket = RTPacket.ErrorPacket;
}
// Constructor
private RTClient()
{
//New instance of protocol, contains a RT packet
mProtocol = new RTProtocol();
//list of bodies that server streams
mBodies = new List <SixDOFBody>();
//list of markers
mMarkers = new List <LabeledMarker>();
//list of bones
mBones = new List <Bone>();
mStreamingStatus = false;
mPacket = RTPacket.ErrorPacket;
}
private RTClient()
{
// New instance of protocol, contains a RT packet
mProtocol = new RTProtocol();
// we register our function "process" as a callback for when protocol receives real time data packets
// (eventDataCallback is also available to listen to events)
mProtocol.RealTimeDataCallback += Process;
mProtocol.EventDataCallback += Events;
mBodies = new List <SixDOFBody>();
mMarkers = new List <LabeledMarker>();
mUnlabeledMarkers = new List <UnlabeledMarker>();
mBones = new List <Bone>();
mGazeVectors = new List <GazeVector>();
mAnalogChannels = new List <AnalogChannel>();
mSkeletons = new List <Skeleton>();
mStreamingStatus = false;
mPacket = RTPacket.ErrorPacket;
}
static bool GetSkeletonSettings(RTState state, RTProtocol mProtocol)
{
bool getStatus = mProtocol.GetSkeletonSettings();
if (!getStatus)
{
return(false);
}
state.skeletons.Clear();
var skeletonSettings = mProtocol.SkeletonSettings;
foreach (var settingSkeleton in skeletonSettings.Skeletons)
{
Skeleton skeleton = new Skeleton();
skeleton.Name = settingSkeleton.Name;
foreach (var settingSegment in settingSkeleton.Segments)
{
var segment = new Segment();
segment.Name = settingSegment.Name;
segment.Id = settingSegment.Id;
segment.ParentId = settingSegment.ParentId;
if (settingSegment.ParentId == 0)
{
segment.TPosition = settingSegment.Position.QtmRhsToUnityLhs(state.coordinateSystemChange);
segment.TRotation = settingSegment.Rotation.QtmRhsToUnityLhs(state.coordinateSystemChange);
}
else
{
segment.TPosition = settingSegment.Position.QtmRhsToUnityLhs();
segment.TRotation = settingSegment.Rotation.QtmRhsToUnityLhs();
}
skeleton.Segments.Add(segment.Id, segment);
}
state.skeletons.Add(skeleton);
}
return(true);
}
static bool GetGazeVectorSettings(RTState state, RTProtocol mProtocol)
{
bool getStatus = mProtocol.GetGazeVectorSettings();
if (getStatus)
{
state.gazeVectors.Clear();
SettingsGazeVectors settings = mProtocol.GazeVectorSettings;
foreach (var gazeVector in settings.GazeVectors)
{
var newGazeVector = new GazeVector();
newGazeVector.Name = gazeVector.Name;
newGazeVector.Position = Vector3.zero;
newGazeVector.Direction = Vector3.zero;
state.gazeVectors.Add(newGazeVector);
}
return(true);
}
return(false);
}
static bool GetAnalogSettings(RTState state, RTProtocol mProtocol)
{
bool getStatus = mProtocol.GetAnalogSettings();
if (getStatus)
{
state.analogChannels.Clear();
var settings = mProtocol.AnalogSettings;
foreach (var device in settings.Devices)
{
foreach (var channel in device.ChannelInformation)
{
var analogChannel = new AnalogChannel();
analogChannel.Name = channel.Name;
analogChannel.Values = new float[0];
state.analogChannels.Add(analogChannel);
}
}
return(true);
}
return(false);
}
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);
}
}
}
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);
}
public ExampleTimecode(RTProtocol rtProtocol, string ipAddress) : base(rtProtocol, ipAddress)
{
}
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 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 MainExample(Example example, RTProtocol rtProtocol, string ipAddress)
{
mIpAddress = ipAddress;
mExample = example;
mRtProtocol = rtProtocol;
}
public ExampleGaze(RTProtocol rtProtocol, string ipAddress) : base(rtProtocol, ipAddress)
{
}
public Example(RTProtocol rtProtocol, string ipAddress)
{
mRtProtocol = rtProtocol;
mIpAddress = ipAddress;
}
public ExampleSkeleton(RTProtocol rtProtocol, string ipAddress) : base(rtProtocol, ipAddress)
{
}
public override void HandleStreaming()
{
// Check if connection to QTM is possible
if (!mRtProtocol.IsConnected())
{
if (!mRtProtocol.Connect(mIpAddress))
{
Console.WriteLine("QTM: Trying to connect");
Thread.Sleep(1000);
return;
}
Console.WriteLine("QTM: Connected");
}
if (mRtProtocol.GeneralSettings == null)
{
if (!mRtProtocol.GetGeneralSettings())
{
Console.WriteLine("QTM: Trying to get General settings");
Thread.Sleep(500);
return;
}
Console.WriteLine("QTM: General settings available");
Console.WriteLine("Frequency: {0}", mRtProtocol.GeneralSettings.CaptureFrequency);
foreach (var camera in mRtProtocol.GeneralSettings.CameraSettings)
{
Console.WriteLine("{0}", camera.Model);
}
if (!mRtProtocol.GetImageSettings())
{
Console.WriteLine("QTM: Trying to get Image settings");
Thread.Sleep(500);
return;
}
Console.WriteLine("QTM: Image settings available");
Console.WriteLine(mRtProtocol.ImageSettings.Xml);
mRtProtocol.TakeControl("realtimestreamingpassword");
SettingsImage newImageSettings = new SettingsImage();
List <ImageCamera> newImageSettingsCameras = new List <ImageCamera>();
for (int i = 0; i < mRtProtocol.ImageSettings.Cameras.Count; i++)
{
var camera = mRtProtocol.ImageSettings.Cameras[i];
camera.Enabled = true;
camera.Width /= 4;
camera.Height /= 4;
newImageSettingsCameras.Add(camera);
}
newImageSettings.Cameras = newImageSettingsCameras;
string createSettingsError;
var xmlSettings = RTProtocol.CreateSettingsXml(newImageSettings, out createSettingsError);
if (xmlSettings != string.Empty)
{
string response;
mRtProtocol.SendXML(xmlSettings, out response);
Console.WriteLine(response);
}
else
{
Console.WriteLine(createSettingsError);
}
mRtProtocol.StreamAllFrames(QTMRealTimeSDK.Data.ComponentType.ComponentImage);
Console.WriteLine("QTM: Starting to stream Image data");
Thread.Sleep(500);
}
// Get RTPacket from stream
PacketType packetType;
mRtProtocol.ReceiveRTPacket(out packetType, false);
// Handle data packet
if (packetType == PacketType.PacketData)
{
var imageData = mRtProtocol.GetRTPacket().GetImageData();
if (imageData != null && imageData.Count() > 0)
{
foreach (var imageFromCamera in imageData)
{
Console.WriteLine("Frame:{0:D5} Camera Index:{1:D3} Width:{2:D5} Height:{3:D5}",
mRtProtocol.GetRTPacket().Frame, imageFromCamera.CameraID, imageFromCamera.Width, imageFromCamera.Height);
}
}
}
// Handle event packet
if (packetType == PacketType.PacketEvent)
{
// If an event comes from QTM then print it out
var qtmEvent = mRtProtocol.GetRTPacket().GetEvent();
if (qtmEvent == QTMEvent.EventConnectionClosed ||
qtmEvent == QTMEvent.EventRTFromFileStopped)
{
mRtProtocol.ClearSettings();
}
Console.WriteLine("{0}", qtmEvent);
}
}
