SampleClientML.cs

The following sample console application will be included in NatNet SDK 1.11.

<source lang="cs"> //============================================================================= // Copyright © 2016 NaturalPoint, Inc. All Rights Reserved. // // This software is provided by the copyright holders and contributors "as is" and // any express or implied warranties, including, but not limited to, the implied // warranties of merchantability and fitness for a particular purpose are disclaimed. // In no event shall NaturalPoint, Inc. or contributors be liable for any direct, // indirect, incidental, special, exemplary, or consequential damages // (including, but not limited to, procurement of substitute goods or services; // loss of use, data, or profits; or business interruption) however caused // and on any theory of liability, whether in contract, strict liability, // or tort (including negligence or otherwise) arising in any way out of // the use of this software, even if advised of the possibility of such damage. //=============================================================================

using System; using System.Collections; using System.Collections.Generic;

using NatNetML;

/* SampleClientML.cs

* 
* This program is a sample console application which uses the managed NatNet assembly (NatNetML.dll) for receiving NatNet data
* from a tracking server (e.g. Motive) and outputting them in every 200 mocap frames. This is provided mainly for demonstration purposes,
* and thus, the program is designed at its simpliest approach. The program connects to a server application at a localhost IP address
* (127.0.0.1) using Multicast connection protocol.
*  
* You may integrate this program into your applications if needed. This program is not designed to take account for latency/frame build up
* when tracking a high number of assets. For more robust and comprehensive use of the NatNet assembly, refer to the provided WinFormSample project.
* 
*  Note: The NatNet .NET assembly is derived from the native NatNetLib.dll library, so make sure the NatNetLib.dll is linked to your application
*        along with the NatNetML.dll file.  
* 
*  List of Output Data:
*  ====================
*      - Markers Data : Prints out total number of markers reconstructed in the scene.
*      - Rigid Body Data : Prints out position and orientation data
*      - Skeleton Data : Prints out only the position of the hip segment
*      - Force Plate Data : Prints out only the first subsample data per each mocap frame
* 
*/

namespace SampleClientML {

   public class SampleClientML
   {

       /*  [NatNet] Network connection configuration    */
       private static NatNetML.NatNetClientML m_NatNet;    // The client instance
       private static string strLocalIP = "127.0.0.1";     // Local IP address (string)
       private static string strServerIP = "127.0.0.1";    // Server IP address (string)
       private static int iConnectionType = 0;             // Connection Type (int): 0 = Multicast, 1 = Unicast

       /*  List for saving each of datadescriptors */
       private static List<NatNetML.DataDescriptor> m_DataDescriptor = new List<NatNetML.DataDescriptor>(); 

       /*  Lists and Hashtables for saving data descriptions   */
       private static Hashtable htSkelRBs = new Hashtable();
       private static List<RigidBody> mRigidBodies = new List<RigidBody>();
       private static List<Skeleton> mSkeletons = new List<Skeleton>();
       private static List<ForcePlate> mForcePlates = new List<ForcePlate>();

       /*  boolean value for detecting change in asset */
       private static bool assetChanged = false;

       static void Main()
       {
           Console.WriteLine("SampleClientML managed client application starting...\n");
           /*  [NatNet] Initialize client object and connect to the server  */
           connectToServer();                          // Initialize a NatNetClient object and connect to a server.

           Console.WriteLine("============================ SERVER DESCRIPTOR ================================\n");
           /*  [NatNet] Confirming Server Connection. Instantiate the server descriptor object and obtain the server description. */
           bool connectionConfirmed = fetchServerDescriptor();    // To confirm connection, request server description data

           if (connectionConfirmed)                         // Once the connection is confirmed.
           {
               Console.WriteLine("============================= DATA DESCRIPTOR =================================\n");
               Console.WriteLine("Now Fetching the Data Descriptor.\n");
               fetchDataDescriptor();                  //Fetch and parse data descriptor

               Console.WriteLine("============================= FRAME OF DATA ===================================\n");
               Console.WriteLine("Now Fetching the Frame Data\n");

               /*  [NatNet] Assigning a event handler function for fetching frame data each time a frame is received   */
               m_NatNet.OnFrameReady += new NatNetML.FrameReadyEventHandler(fetchFrameData);
              
               Console.WriteLine("Success: Data Port Connected \n");

               Console.WriteLine("======================== STREAMING IN (PRESS ESC TO EXIT) =====================\n");
           }

           while (!(Console.KeyAvailable && Console.ReadKey().Key == ConsoleKey.Escape))
           {
               // Continuously listening for Frame data
               // Enter ESC to exit

               // Exception handler for updated assets list.
               if (assetChanged == true)
               {
                   Console.WriteLine("\n===============================================================================\n");
                   Console.WriteLine("Change in the list of the assets. Refetching the descriptions");

                   /*  Clear out existing lists */
                   m_DataDescriptor.Clear();
                   htSkelRBs.Clear();
                   mRigidBodies.Clear();
                   mSkeletons.Clear();
                   mForcePlates.Clear();

                   /* [NatNet] Re-fetch the updated list of descriptors  */
                   fetchDataDescriptor();
                   Console.WriteLine("===============================================================================\n");
                   assetChanged = false;
               }
           }
           /*  [NatNet] Disabling data handling function   */
           m_NatNet.OnFrameReady -= fetchFrameData;

           /*  Clearing Saved Descriptions */
           mRigidBodies.Clear();
           mSkeletons.Clear();
           htSkelRBs.Clear();
           mForcePlates.Clear();
           m_NatNet.Uninitialize();
       }

       /// <summary>
       /// [NatNet] parseFrameData will be called when a frame of Mocap
       /// data has is received from the server application.
       ///
       /// Note: This callback is on the network service thread, so it is
       /// important to return from this function quickly as possible 
       /// to prevent incoming frames of data from buffering up on the
       /// network socket.
       ///
       /// Note: "data" is a reference structure to the current frame of data.
       /// NatNet re-uses this same instance for each incoming frame, so it should
       /// not be kept (the values contained in "data" will become replaced after
       /// this callback function has exited).
       /// </summary>
       /// <param name="data">The actual frame of mocap data</param>
       /// <param name="client">The NatNet client instance</param>
       static void fetchFrameData(NatNetML.FrameOfMocapData data, NatNetML.NatNetClientML client)
       {

           /*  Exception handler for cases where assets are added or removed.
               Data description is re-obtained in the main function so that contents
               in the frame handler is kept minimal. */
           if (( data.bTrackingModelsChanged == true || data.nRigidBodies != mRigidBodies.Count || data.nSkeletons != mSkeletons.Count || data.nForcePlates != mForcePlates.Count))
           {
               assetChanged = true;
           }

           /*  Processing and ouputting frame data every 200th frame.
               This conditional statement is included in order to simplify the program output */
           if(data.iFrame % 200 == 0)
           {
               if (data.bRecording == false)
                   Console.WriteLine("Frame #{0} Received:", data.iFrame);
               else if (data.bRecording == true)
                   Console.WriteLine("[Recording] Frame #{0} Received:", data.iFrame);

               processFrameData(data);
           }
       }

       static void processFrameData(NatNetML.FrameOfMocapData data)
       {
           /*  Parsing Rigid Body Frame Data   */
           for (int i = 0; i < mRigidBodies.Count; i++)
           {
               int rbID = mRigidBodies[i].ID;              // Fetching rigid body IDs from the saved descriptions

               for (int j = 0; j < data.nRigidBodies; j++)
               {
                   if(rbID == data.RigidBodies[j].ID)      // When rigid body ID of the descriptions matches rigid body ID of the frame data.
                   {
                       NatNetML.RigidBody rb = mRigidBodies[i];                // Saved rigid body descriptions
                       NatNetML.RigidBodyData rbData = data.RigidBodies[j];    // Received rigid body descriptions

                       if (rbData.Tracked == true)
                       {
                           Console.WriteLine("\tRigidBody ({0}):", rb.Name);
                           Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);

                           // Rigid Body Euler Orientation
                           float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw };
                           float[] eulers = new float[3];

                           eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
                           double xrot = RadiansToDegrees(eulers[0]);
                           double yrot = RadiansToDegrees(eulers[1]);
                           double zrot = RadiansToDegrees(eulers[2]);

                           Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
                       }
                       else
                       {
                           Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
                       }
                   }
               }
           }

           /* Parsing Skeleton Frame Data  */
           for (int i = 0; i < mSkeletons.Count; i++)      // Fetching skeleton IDs from the saved descriptions
           {
               int sklID = mSkeletons[i].ID;

               for (int j = 0; j < data.nSkeletons; j++)
               {
                   if (sklID == data.Skeletons[j].ID)      // When skeleton ID of the description matches skeleton ID of the frame data.
                   {
                       NatNetML.Skeleton skl = mSkeletons[i];              // Saved skeleton descriptions
                       NatNetML.SkeletonData sklData = data.Skeletons[j];  // Received skeleton frame data

                       Console.WriteLine("\tSkeleton ({0}):", skl.Name);
                       Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);

                       /*  Now, for each of the skeleton segments  */
                       for (int k = 0; k < sklData.nRigidBodies; k++)
                       {
                           NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];

                           /*  Decoding skeleton bone ID   */
                           int skeletonID = HighWord(boneData.ID);
                           int rigidBodyID = LowWord(boneData.ID);
                           int uniqueID = skeletonID * 1000 + rigidBodyID;
                           int key = uniqueID.GetHashCode();

                           NatNetML.RigidBody bone = (RigidBody)htSkelRBs[key];   //Fetching saved skeleton bone descriptions

                           //Outputting only the hip segment data for the purpose of this sample.
                           if (k == 0)
                               Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
                       }
                   }
               }
           }
           
           /*  Parsing Force Plate Frame Data  */
           for (int i = 0; i < mForcePlates.Count; i++)
           {
               int fpID = mForcePlates[i].ID;                  // Fetching force plate IDs from the saved descriptions

               for (int j = 0; j < data.nForcePlates; j++)
               {
                   if (fpID == data.ForcePlates[j].ID)         // When force plate ID of the descriptions matches force plate ID of the frame data.
                   {
                       NatNetML.ForcePlate fp = mForcePlates[i];                // Saved force plate descriptions
                       NatNetML.ForcePlateData fpData = data.ForcePlates[i];    // Received forceplate frame data

                       Console.WriteLine("\tForce Plate ({0}):", fp.Serial);

                       // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
                       for (int k = 0; k < fpData.nChannels; k++)
                       {
                           Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
                       }
                   }
               }
           }
           Console.WriteLine("\n");
       }

       static void connectToServer()
       {
           /*  [NatNet] Instantiate the client object  */
           m_NatNet = new NatNetML.NatNetClientML(iConnectionType);

           /*  [NatNet] Checking verions of the NatNet SDK library  */
           int[] verNatNet = new int[4];           // Saving NatNet SDK version number
           verNatNet = m_NatNet.NatNetVersion();
           Console.WriteLine("NatNet SDK Version: {0}.{1}.{2}.{3}", verNatNet[0], verNatNet[1], verNatNet[2], verNatNet[3]);

           /*  [NatNet] Connecting to the Server    */
           Console.WriteLine("\nConnecting...\n\tLocal IP address: {0}\n\tServer IP Address: {1}\n\n", strLocalIP, strServerIP);
           m_NatNet.Initialize(strLocalIP, strServerIP);
       }

       static bool fetchServerDescriptor()
       {
           NatNetML.ServerDescription m_ServerDescriptor = new NatNetML.ServerDescription();
           int errorCode = m_NatNet.GetServerDescription(m_ServerDescriptor);

           if (errorCode == 0)
           {
               Console.WriteLine("Success: Connected to the server\n");
               parseSeverDescriptor(m_ServerDescriptor);
               return true;
           }
           else
           {
               Console.WriteLine("Error: Failed to connect. Check the connection settings.");
               Console.WriteLine("Program terminated (Enter ESC to exit)");
               return false;
           }
       }

       static void parseSeverDescriptor(NatNetML.ServerDescription server)
       {
           Console.WriteLine("Server Info:");
           Console.WriteLine("\tHost: {0}", server.HostComputerName);
           Console.WriteLine("\tApplication Name: {0}", server.HostApp);
           Console.WriteLine("\tApplication Version: {0}.{1}.{2}.{3}", server.HostAppVersion[0], server.HostAppVersion[1], server.HostAppVersion[2], server.HostAppVersion[3]);
           Console.WriteLine("\tNatNet Version: {0}.{1}.{2}.{3}\n", server.NatNetVersion[0], server.NatNetVersion[1], server.NatNetVersion[2], server.NatNetVersion[3]);
       }

       static void fetchDataDescriptor()
       {
           /*  [NatNet] Fetch Data Descriptions. Instantiate objects for saving data descriptions and frame data    */        
           bool result = m_NatNet.GetDataDescriptions(out m_DataDescriptor);
           if (result)
           {
               Console.WriteLine("Success: Data Descriptions obtained from the server.");
               parseDataDescriptor(m_DataDescriptor);
           }
           else
           {
               Console.WriteLine("Error: Could not get the Data Descriptions");
           }
           Console.WriteLine("\n");
       }

       static void parseDataDescriptor(List<NatNetML.DataDescriptor> description)
       {
           //  [NatNet] Request a description of the Active Model List from the server. 
           //  This sample will list only names of the data sets, but you can access 
           int numDataSet = description.Count;
           Console.WriteLine("Total {0} data sets in the capture:", numDataSet);

           for (int i = 0; i < numDataSet; ++i)
           {
               int dataSetType = description[i].type;
               // Parse Data Descriptions for each data sets and save them in the delcared lists and hashtables for later uses.
               switch (dataSetType)
               {
                   case ((int) NatNetML.DataDescriptorType.eMarkerSetData):
                       NatNetML.MarkerSet mkset = (NatNetML.MarkerSet)description[i];
                       Console.WriteLine("\tMarkerSet ({0})", mkset.Name);
                       break;

                   case ((int) NatNetML.DataDescriptorType.eRigidbodyData):
                       NatNetML.RigidBody rb = (NatNetML.RigidBody)description[i];
                       Console.WriteLine("\tRigidBody ({0})", rb.Name);

                       // Saving Rigid Body Descriptions
                       mRigidBodies.Add(rb);
                       break;

                   case ((int) NatNetML.DataDescriptorType.eSkeletonData):
                       NatNetML.Skeleton skl = (NatNetML.Skeleton)description[i];
                       Console.WriteLine("\tSkeleton ({0}), Bones:", skl.Name);

                       //Saving Skeleton Descriptions
                       mSkeletons.Add(skl);

                       // Saving Individual Bone Descriptions
                       for (int j = 0; j < skl.nRigidBodies; j++)
                       {

                           Console.WriteLine("\t\t{0}. {1}", j + 1, skl.RigidBodies[j].Name);
                           int uniqueID = skl.ID * 1000 + skl.RigidBodies[j].ID;
                           int key = uniqueID.GetHashCode();
                           htSkelRBs.Add(key, skl.RigidBodies[j]); //Saving the bone segments onto the hashtable
                       }
                       break;

                   case ((int) NatNetML.DataDescriptorType.eForcePlateData):
                       NatNetML.ForcePlate fp = (NatNetML.ForcePlate)description[i];
                       Console.WriteLine("\tForcePlate ({0})", fp.Serial);

                       // Saving Force Plate Channel Names
                       mForcePlates.Add(fp);
              
                       for (int j = 0; j < fp.ChannelCount; j++)
                       {
                           Console.WriteLine("\t\tChannel {0}: {1}", j + 1, fp.ChannelNames[j]);
                       }
                       break;

                   default:
                       // When a Data Set does not match any of the descriptions provided by the SDK.
                       Console.WriteLine("\tError: Invalid Data Set");
                       break;
               }
           }
       }

       static double RadiansToDegrees(double dRads)
       {
           return dRads * (180.0f / Math.PI);
       }
       
       static int LowWord(int number)
       {
           return number & 0xFFFF;
       }

       static int HighWord(int number)
       {
           return ((number >> 16) & 0xFFFF);
       }
   } // End. ManagedClient class

} // End. NatNetML Namespace