listener.cpp

#ifdef WIN32
#include <windows.h>
#else
#include <csignal>
#endif

/*
 * Define handler functions required to ensure a clean shutdown of the PST Tracker when the
 * application is terminated.
 */

static void Exithandler(int sig);

#ifdef WIN32
BOOL WINAPI ConsoleHandler(DWORD CEvent)
{
    Exithandler(CEvent);
    return TRUE;
}
#endif

/* End of handler functions */

#include <memory>
#include <iostream>
#include <thread>
#include <chrono>

#include "pst-sdk.h"
#include "TrackerExceptions.h"

/*
 * Helper function for clear printing of 4x4 matrices.
 */
static inline void PrintMatrix(const std::array<float, 16>& mat)
{    
    for (int y = 0; y < 4; ++y)
    {
        for (int x = 0; x < 4; ++x)
        {
            std::cout << mat[x + y * 4] << "\t";
        }
        std::cout << "\n";
    }
}

/* Control variable for main loop */
static bool running = true;

/* Number of data points to grab before application termination */
static const uint32_t numberOfSamplesToGrab = 100;

/*
 * Implementation of the PSTech::pstsdk::Listener class to receive tracking data.
 * The OnTrackerData() callback function receives the data as soon as it becomes 
 * available and prints the tracking target pose to the command line.
 */
class MyListener : public PSTech::pstsdk::Listener
{    
    virtual void OnTrackerData(PSTech::pstsdk::TrackerData& td)
    {
        static uint32_t samplesGrabbed = 0;
        if (samplesGrabbed++ >= numberOfSamplesToGrab)
            running = false;

        for (int d = 0; d < td.targetlist.size(); ++d)
        {
            auto& mat = td.targetlist[d].pose;
            std::cout << "Pose for " << td.targetlist[d].name << "\n";
            PrintMatrix(mat);
        }
    }
} listener;

/*
 * Create a shared pointer for storing the PSTech::pstsdk::Tracker object instance.
 * By making use of a shared pointer, proper destruction of the PST Tracker connection
 * is ensured in case the application is shut down.
 */
std::shared_ptr<PSTech::pstsdk::Tracker> pst;

/*
 * Implement the exit handler to shut-down the PST Tracker connection on application termination.
 */
static void Exithandler(int sig)
{
    pst->Shutdown();
    running = false;
}

int main(int argc, char *argv[])
{
    // Register the exit handler with the application
#ifdef WIN32
    SetConsoleCtrlHandler((PHANDLER_ROUTINE)ConsoleHandler, TRUE);
#else
    signal(SIGTERM, Exithandler);
    signal(SIGKILL, Exithandler);
    signal(SIGQUIT, Exithandler);
    signal(SIGINT, Exithandler);
#endif

    // 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.
#ifdef WIN32
        pst = std::make_shared<PSTech::pstsdk::Tracker>();
#else
        // On Linux, specify the type of grabber that needs to be used as the last parameter: 
        // "basler_ace" for PST HD or "basler_dart" for PST Pico
        pst = std::make_shared<PSTech::pstsdk::Tracker>("","config.cfg","models.db",argv[1]);
#endif

        // Check if calibration information is available for all cameras. When this is not the case, provide a warning.
        if (pst->GetUncalibratedCameraUrls(true).size() > 0)
        {
            std::cout << "\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";
            std::cout << "Press enter to continue...\n";
            std::cin.get();
            return 1;
        }

        // Print version number of the tracker server being used.
        std::cout << "Running PST Server version " << pst->GetVersionInfo() << "\n";

        // Register the listener object to the tracker server.
        pst->AddTrackerListener(&listener);
        
        std::cout << "Put the Reference card in front of the PST in order to see tracking results.\n\n";
        
        // Start the tracker server.
        pst->Start();
        
        // Perform a system check to see if the tracker server is running OK and print the result.
        std::cout << "System check: " << (int)pst->Systemcheck() << "\n";
        
        // Set the frame rate to 30 Hz.
        pst->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().
        std::cout << "Frame rate set to " << pst->GetFramerate() << "\n";

        // Main loop, wait for auto-termination.
        while (running)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    catch (PSTech::TrackerException &e)
    {
        // Catch PSTech::TrackerException exceptions and print error messages.
        std::cout << e.full_description() << "\n";

        // Pause command line to see error message.
        std::cout << "Press enter to continue...\n";
        std::cin.get();
    }

    // Make sure that the connection to the PST Tracker is shut down properly.
    pst->Shutdown();

    return 0;
}