| PST SDK 6.0.0.0-272350a

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 <iostream>
#include <thread>
#include <chrono>
#include <atomic>
#include "pstsdk_cpp.h"
#include "TrackerExceptions.h"
#include "PstStringIoStream.h"
/*
* Helper function for clear printing of 4x4 matrices.
*/
static inline void PrintMatrix(const PSTech::Utils::PstArray<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 std::atomic<bool> running(true);
/* Flag for signaling tracker restart */
static std::atomic<bool> restart(false);
/* 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.
*/
class MyListener : public PSTech::pstsdk::Listener
{
/*
* The OnTrackerData() callback function receives the data as soon as it
* becomes available and prints the tracking target pose to the command line.
*/
virtual void OnTrackerData(const 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);
}
}
/*
* The OnTrackerMode() callback function receives tracking status information
* as soon as the tracking mode changes and prints the tracking mode to the
* command line.
*/
virtual void OnTrackerMode(const PSTech::pstsdk::TrackerMode mode)
{
switch(mode)
{
case PSTech::pstsdk::TrackerMode::TRACKING:
std::cout << "Tracker tracking" << std::endl;
break;
case PSTech::pstsdk::TrackerMode::LOWPOWER:
std::cout << "Tracker paused" << std::endl;
break;
case PSTech::pstsdk::TrackerMode::DISCONNECT:
std::cout << "Tracker disconnected" << std::endl;
break;
case PSTech::pstsdk::TrackerMode::RECONNECT:
std::cout << "Tracker reconnected" << std::endl;
restart = true; // Tracker reconnects in paused state. Attempt restart.
break;
default:
std::cout << "Mode " << static_cast<int>(mode) << std::endl;
break;
}
}
} listener;
/*
* Implement the exit handler to shut-down the PST Tracker connection on application termination.
*/
static void Exithandler(int sig)
{
PSTech::pstsdk::Tracker::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
PSTech::pstsdk::Tracker pst;
#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
PSTech::pstsdk::Tracker pst("","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)
{
// If tracker has reconnected, restart it.
if(restart)
{
pst.Start();
restart = false;
}
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.
PSTech::pstsdk::Tracker::Shutdown();
return 0;
}