listener/listener.cs

// Copyright PS-Tech B.V. All Rights Reserved.
 
using System;
using System.Runtime.InteropServices;
using System.Threading;
using PSTech.Pstsdk;
 
namespace PSTech.Listener
{
    public class Listener
    {
        //Registering control handler function.
        [DllImport("Kernel32")]
        private static extern bool SetConsoleCtrlHandler(ConsoleEventHandler handler, bool add);
 
        private delegate bool ConsoleEventHandler(CtrlType sig);
        static ConsoleEventHandler ConsoleCtrlEventHandler;
 
        public enum CtrlType
        {
            CtrlCEvent = 0,
            CtrlBreakEvent = 1,
            CtrlCloseEvent = 2,
            CtrlLogOffEvent = 5,
            CtrlShutdownEvent = 6,
        }
 
        private static bool ConsoleHandler(CtrlType sig)
        {
            Exithandler((int)sig);
            return true;
        }
 
        public static void PrintMatrix(float[] matrix)
        {
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    Console.Write(matrix[j + i * 4] + "\t");
                }
                Console.WriteLine();
            }
        }
 
        // Control variable for main loop.
        static bool Running = true;
 
        // Flag for signaling tracker restart.
        static bool Restart = false;
 
        // Number of data points to grab before application termination
        static int NumberOfSamplesToGrab = 100;
        static int SamplesGrabbed = 0;
 
        class MyTrackerListener : TrackerListener
        {
            public override void OnTrackerData(TrackerData data, ErrorStatus status)
            {
                if (SamplesGrabbed++ >= NumberOfSamplesToGrab)
                {
                    Running = false;
                }
 
                for (int d = 0; d < data.TargetPoseList.Length; ++d)
                {
                    var mat = data.TargetPoseList[d].PoseMatrix;
                    Console.Write("Pose for " + data.TargetPoseList[d].Target.Name + "\n");
                    PrintMatrix(mat);
                }
            }
 
            public override void OnTrackerMode(ETrackerMode mode)
            {
                switch (mode)
                {
                    case ETrackerMode.ModeTracking:
                        Console.WriteLine("Tracker tracking");
                        break;
                    case ETrackerMode.ModeLowpower:
                        Console.WriteLine("Tracker paused");
                        break;
                    case ETrackerMode.ModeDisconnect:
                        Console.WriteLine("Tracker disconnected");
                        break;
                    case ETrackerMode.ModeReconnect:
                        Console.WriteLine("Tracker reconnected");
                        Restart = true; // Tracker reconnects in paused state. Attempt restart.
                        break;
                    default:
                        Console.WriteLine("Mode " + (int) mode);
                        break;
                }
            }
        }
 
        static void Exithandler(int sig)
        {
            Tracker.Shutdown();
            Running = false;
        }
 
        static int Main(string[] args)
        {
            // Register the exit handler with the application
            ConsoleCtrlEventHandler += new ConsoleEventHandler(ConsoleHandler);
            SetConsoleCtrlHandler(ConsoleCtrlEventHandler, true);
 
            // Implement error handling of PSTech.TrackerException exceptions to prevent
            // improper PST Tracker shutdown on errors.
            try
            {
                // Create an instance of the Tracker object using the default configuration path and file names.
                Tracker tracker = new Tracker();
 
                // Check if calibration information is available for all cameras. When this is not the case, provide a warning.
                if (tracker.GetUncalibratedCameraUrls().Length > 0)
                {
                    Console.Write("\nNo calibration information could be found in the configuration directory. " +
                                 "Please use the PST Server and PST Client application to initialize the PST Tracker and create/import a tracking target. " +
                                 "More information can be found in the Initialization section of the PST SDK manual and the PST Manual.\n\n");
                    Console.Write("Press enter to continue...\n");
                    Console.Read();
                    return 1;
                }
 
                // Print version number of the tracker server being used.
                Console.Write("Running PST Server version " + tracker.GetVersionInfo() + "\n");
 
                //Create tracker listener with callback functions for data and/or mode updates.
                TrackerListener listener = new MyTrackerListener();
 
                // Register the TrackerListener object to the tracker server.
                tracker.AddTrackerListener(ref listener);
 
                Console.Write("Put the Reference card in front of the PST in order to see tracking results.\n\n");
 
                // Start the tracker server.
                tracker.StartTracker();
 
                // Perform a system check to see if the tracker server is running OK and print the result.
                Console.Write("System check: " + tracker.Systemcheck() + "\n");
 
                // Set the frame rate to 30 Hz.
                tracker.SetFramerate(30);
 
                // Print the new frame rate to see if it was set correctly. Note that for PST HD and Pico
                // trackers the frame rate actually being set can differ from the value provided to SetFramerate().
                Console.Write("Frame rate set to " + tracker.GetFramerate() + "\n");
 
                // Main loop, wait for auto-termination.
                while (Running)
                {
                    // If tracker has reconnected, restart it.
                    if (Restart)
                    {
                        tracker.StartTracker();
                        Restart = false;
                    }
 
                    Thread.Sleep(100); ;
                }
            }
            catch (Exception e)
            {
                // Catch PSTech.TrackerException exceptions and print error messages.
                Console.Write(e + "\n");
                // Pause command line to see error message.
                Console.Write("Press enter to continue...\n");
                Console.Read();
            }
            finally
            {
                // Make sure that the connection to the PST Tracker is shut down properly.
                Tracker.Shutdown();
            }
 
            return 0;
        }
    }
}