listener.c

 
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#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 "pstsdk_c.h"
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <signal.h>
 
/*
 * Helper function for clear printing of 4x4 matrices.
 */
static inline void PrintMatrix(float mat[16])
{
    for (int y = 0; y < 4; ++y)
    {
        for (int x = 0; x < 4; ++x)
        {
            printf("%f \t", mat[x + y * 4]);
        }
        printf("\n");
    }
}
 
/* Control variable for main loop */
static sig_atomic_t running = 1;
 
/* Flag for signaling tracker restart */
static sig_atomic_t restart = 0;
 
/* Number of data points to grab before application termination */
static const uint32_t numberOfSamplesToGrab = 100;
 
 /*
  * Implementation of a tracker data callback function.
  * The OnTrackerData() callback function receives the data as soon as it becomes
  * available and prints the tracking target pose to the command line.
  */
void OnTrackerData(const PstTrackerData* tracker_data, EPstErrorStatus status)
{
    static uint32_t samplesGrabbed = 0;
    if (samplesGrabbed++ >= numberOfSamplesToGrab)
    {
        running = 0;
    }
 
    for (int d = 0; d < tracker_data->number_of_targets; ++d)
    {
        float* mat = tracker_data->targetlist[d].pose_matrix;
        printf("Pose for %s \n", tracker_data->targetlist[d].target.name);
        PrintMatrix(mat);
    }
}
 
 /*
  * Implementation of a tracker mode callback function.
  * The OnTrackerMode() callback function receives tracking status information
  * as soon as the tracking mode changes and prints the tracking mode to the
  * command line.
  */
void OnTrackerMode(EPstTrackerMode mode)
{
    switch (mode)
    {
    case PST_MODE_TRACKING:
        printf("Tracker tracking\n");
        break;
    case PST_MODE_LOWPOWER:
        printf("Tracker paused\n");
        break;
    case PST_MODE_DISCONNECT:
        printf("Tracker disconnected\n");
        break;
    case PST_MODE_RECONNECT:
        printf("Tracker reconnected\n");
        restart = 1; // Tracker reconnects in paused state. Attempt restart.
        break;
    default:
        printf("Mode: %d\n", (int)mode);
        break;
    }
}
 
/*
 * Implement the exit handler to shut-down the PST Tracker connection on application termination.
 */
static void Exithandler(int sig)
{
    pst_sdk_shutdown();
    running = 0;
}
 
// Print the last error message.
void PrintLastErrorMessage()
{
    char* last_error_message = NULL;
    EPstErrorStatus error_status = pst_alloc_and_get_last_error_message(&last_error_message);
    if (error_status != PST_ERROR_STATUS_OK)
    {
        last_error_message = "Failed to allocate memory error.";
    }
    printf("last error message: %s \n", last_error_message);
    pst_free(last_error_message);
}
 
// Check error status and shutdown tracker upon error.
void CheckErrorCode(EPstErrorStatus status)
{
    if (status != PST_ERROR_STATUS_OK)
    {
        PrintLastErrorMessage();
        pst_sdk_shutdown();
        exit(status);
    }
}
 
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
 
    PstTracker ctracker;
 
#ifdef WIN32
    // Create an instance of the Tracker object using the default configuration path and file names.
    CheckErrorCode(pst_tracker_init(&ctracker));
#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
    CheckErrorCode(pst_tracker_init4(&ctracker, "", "config.cfg", "models.db", argv[1]));
#endif
 
    // Make sure to initialize the PstCameraURLs with ::pst_camera_urls_init before using pst_tracker_get_uncalibrated_camera_urls function.
    PstCameraURLs urls;
    pst_camera_urls_init(&urls);
    
    CheckErrorCode(pst_tracker_get_uncalibrated_camera_urls(&ctracker, true, &urls));
    // Check if calibration information is available for all cameras. When this is not the case, provide a warning.
    if (urls.number_of_urls > 0)
    {
        printf("\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");
        printf("Press enter to continue...\n");
        getchar();
        pst_camera_urls_destroy(&urls);
        return 1;
    }
 
    // Make sure to destroy PstCameraURLs after using it.
    pst_camera_urls_destroy(&urls);
 
    char* version_string;
    CheckErrorCode(pst_tracker_alloc_and_get_version_info(&ctracker, &version_string));
 
    // Print version number of the tracker server being used.
    printf("Running PST Server version %s \n", version_string);
    pst_free(version_string);
 
    // Register the OnTrackerData and the OnTrackerMode callback functions to the tracker server.
    CheckErrorCode(pst_tracker_add_tracker_callbacks(&ctracker, OnTrackerData, OnTrackerMode));
 
    printf("Put the Reference card in front of the PST in order to see tracking results.\n\n");
 
    // Start the tracker server.
    CheckErrorCode(pst_tracker_start(&ctracker));
 
    // Perform a system check to see if the tracker server is running OK and print the result.
    printf("System check: %i \n", (int)pst_tracker_system_check(&ctracker));
 
    // Set the frame rate to 30 Hz.
    CheckErrorCode(pst_tracker_set_framerate(&ctracker, 30.0));
 
    // 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().
    double fps;
    CheckErrorCode(pst_tracker_get_framerate(&ctracker, &fps));
    printf("Frame rate set to %f \n", fps);
 
    // Main loop, wait for auto-termination.
    while (running == 1)
    {
        // If tracker has reconnected, restart it.
        if (restart == 1)
        {
            CheckErrorCode(pst_tracker_start(&ctracker));
            restart = false;
        }
#ifdef WIN32
        Sleep(100);
#else
        usleep(100000);
#endif
    }
 
    // Unregister the OnTrackerData and the OnTrackerMode callback functions from the tracker server.
    CheckErrorCode(pst_tracker_remove_tracker_callbacks(&ctracker, OnTrackerData, OnTrackerMode));
 
    // Make sure that the connection to the PST Tracker is shut down properly.
    pst_tracker_destroy(&ctracker);
    return 0;
}