// 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;
}
/// <summary>
/// Create connection to server
///</summary>
/// <param name="serverAddr">Adress to server</param>
/// <param name="serverPortUDP">port to use if UDP socket is desired, set to 0 for automatic port selection</param>
/// <param name="majorVersion">Major protocol version to use, default is latest</param>
/// <param name="minorVersion">Minor protocol version to use, default is latest</param>
/// <param name="port">base port for QTM server, default is 22222</param>
/// <returns>true if connection was successful, otherwise false</returns>
public bool Connect(string serverAddr, short serverPortUDP = -1,
int majorVersion = Constants.MAJOR_VERSION, int minorVersion = Constants.MINOR_VERSION,
int port = Constants.STANDARD_BASE_PORT)
{
Disconnect();
mMajorVersion = majorVersion;
mMinorVersion = minorVersion;
if (mMajorVersion > 1 || mMinorVersion > 7)
{
// Allow for 1.8 and above versions
}
else
{
mErrorString = "Version of 1.8 or less of the protocol can not be used by the c# sdk";
return false;
}
PacketType packetType;
port += 1;
mPacket = new RTPacket(majorVersion, minorVersion);
if (mNetwork.Connect(serverAddr, port))
{
if (serverPortUDP >= 0)
{
mUDPport = (ushort)serverPortUDP;
if (mNetwork.CreateUDPSocket(ref mUDPport, false) == false)
{
mErrorString = String.Format("Error creating UDP socket: {0}", mNetwork.GetErrorString());
Disconnect();
return false;
}
}
//Get connection response from server
if (ReceiveRTPacket(out packetType) > 0)
{
if (packetType == PacketType.PacketError)
{
//Error from QTM
mErrorString = mPacket.GetErrorString();
Disconnect();
return false;
}
if (packetType == PacketType.PacketCommand)
{
string response = mPacket.GetCommandString();
if (response == "QTM RT Interface connected")
{
if (SetVersion(mMajorVersion, mMinorVersion))
{
string expectedResponse = String.Format("Version set to {0}.{1}", mMajorVersion, mMinorVersion);
response = mPacket.GetCommandString();
if (response == expectedResponse)
{
return true;
}
else
{
mErrorString = "Unexpected response from server";
Disconnect();
return false;
}
}
else
{
//Error setting version
mErrorString = "Error setting version of protocol";
Disconnect();
return false;
}
}
else
{
//missing QTM response
mErrorString = "Missing response from QTM Server";
Disconnect();
return false;
}
}
}
else
{
//Error receiving packet.
mErrorString = String.Format("Error Recieveing packet: {0}", mNetwork.GetErrorString());
Disconnect();
return false;
}
}
else
{
if (mNetwork.GetError() == SocketError.ConnectionRefused)
{
mErrorString = "Connection refused, Check if QTM is running on target machine";
Disconnect();
}
else
{
mErrorString = String.Format("Error connecting TCP socket: {0}", mNetwork.GetErrorString());
Disconnect();
}
return false;
}
return false;
}
/// <summary>
/// Default constructor
///</summary>
public RTProtocol(int majorVersion = Constants.MAJOR_VERSION, int minorVersion = Constants.MINOR_VERSION)
{
mMajorVersion = majorVersion;
mMinorVersion = minorVersion;
mPacket = new RTPacket(mMinorVersion, mMajorVersion);
mErrorString = "";
mNetwork = new RTNetwork();
mBroadcastSocketCreated = false;
mDiscoveryResponses = new HashSet<DiscoveryResponse>();
}
///</summary>
/// Get current frame from server
///</summary>
/// <param name="streamAll">boolean if all component types should be streamed</param>
/// <param name="packet">packet with data returned from server</param>
/// <param name="components">list of specific component types to stream, ignored if streamAll is set to true</param>
/// <returns>true if command was sent successfully and response was a datapacket with frame</returns>
public bool GetCurrentFrame(out RTPacket packet, bool streamAll, List<ComponentType> components = null)
{
bool status;
if (components != null)
{
status = GetCurrentFrame(streamAll, components);
}
else
{
status = GetCurrentFrame(streamAll);
}
if (status)
{
packet = mPacket;
return true;
}
else
{
packet = RTPacket.ErrorPacket;
return false;
}
}
// processor of realtime data
// Function is called every time protocol receives a datapacket from server
public void Process(RTPacket packet)
{
mPacket = packet;
List<Q6DOF> bodyData = packet.Get6DOFData();
List<Q3D> markerData = packet.Get3DMarkerData();
if (bodyData != null)
{
for (int i = 0; i < bodyData.Count; i++)
{
Vector3 position = new Vector3(bodyData[i].Position.X, bodyData[i].Position.Y, bodyData[i].Position.Z);
//Set rotation and position to work with unity
position /= 1000;
mBodies[i].Position = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
mBodies[i].Position.z *= -1;
mBodies[i].Rotation = mCoordinateSystemChange * QuaternionHelper.FromMatrix(bodyData[i].Matrix);
mBodies[i].Rotation.z *= -1;
mBodies[i].Rotation.w *= -1;
mBodies[i].Rotation *= QuaternionHelper.RotationZ(Mathf.PI * .5f);
mBodies[i].Rotation *= QuaternionHelper.RotationX(-Mathf.PI * .5f);
}
}
//Get marker data that is labeled and update values
if (markerData != null)
{
for (int i = 0; i < markerData.Count; i++)
{
Q3D marker = markerData[i];
Vector3 position = new Vector3(marker.Position.X, marker.Position.Y, marker.Position.Z);
position /= 1000;
mMarkers[i].Position = QuaternionHelper.Rotate(mCoordinateSystemChange, position);
mMarkers[i].Position.z *= -1;
}
}
}
// called every time a event is broad casted from QTM server.
public void Events(RTPacket packet)
{
QTMEvent currentEvent = packet.GetEvent();
Debug.Log("Event occurred! : " + currentEvent);
if (currentEvent == QTMEvent.EventRTFromFileStarted)
{
// reload settings when we start streaming to get proper settings
Debug.Log("Reloading Settings");
Get3DSettings();
Get6DOFSettings();
}
}
