listener.py

"""Listener example of the PST SDK
 
This example shows how to implement the PST SDK using the Tracker class
and how to receive data by implementing the Listener class. The example
initializes the PST Tracker and grabs 100 data points.
 
In order to be able to run this example, the PST Tracker has to be initialized first.
This can be done by starting the PST-Server and the PST-Client application and making
sure the calibration files have been downloaded and a tracking target is available.
The tracking target can be the default Reference target or a newly trained or imported
target. For more information, please see the Initialization section of the PST SDK manual
or check the PST Manual.
 
Copyright PS-Tech B.V. All Rights Reserved.
"""
import context
import time
import sys
import pstech.pstsdk.tracker as pst
import pstech.pstsdk.errors as psterrors
 
# Control variable for main loop
running = True
 
# Number of data points to grab before application termination
max_samples = 100
 
# Global number of samples
samples = 0
 
# Flag for signaling tracker restart
restart = False
 
"""Helper function for clear printing of 4x4 matrices."""
def print_matrix(matrix):
    idx = 0
    for y in range(4):
        for x in range(4):
            print(str(matrix[x + y * 4]), end="\t")
        print("")
    print("\n")
 
"""Implementation of the pst.Listener class to receive tracking data and mode changes."""
class MyListener(pst.Listener):
 
    """Implementation of a tracker data callback function
 
    Implementation of a tracker data callback function. The on_tracker_data
    function receives the data as soon as it becomes available and prints the 
    tracking target pose to the command line.
 
    Args:
        tracker_data: Object containing tracking information retrieved from tracker
        status_message: Status message reported by the tracker.
 
    See Also:
        pstech.pstdk.trackerdata.TrackerData
        pstech.pstsdk.errors.EStatusMessage
    """
    def on_tracker_data(self, tracker_data, status_message):
        global samples
        global running
 
        if samples >= max_samples:
            running = False
 
        if len(tracker_data.targetlist) > 0:
            for target_pose in tracker_data.targetlist:
                print("Pose for " + target_pose.name)
                print_matrix(target_pose.pose)
 
        samples += 1
 
    """Implementation of a tracker mode callback function
 
    Implementation of a tracker mode callback function. The on_tracker_mode
    function receives any mode update as soon as it become available and prints it.
 
    Args:
        mode: Current mode reported by the tracker.
 
    See Also:
        pstech.pstsdk.tracker.ETrackerMode
    """
    def on_tracker_mode(self, mode):
        global restart
        if mode == pst.ETrackerMode.MODE_TRACKING:
            print("Tracker tracking")
        elif mode == pst.ETrackerMode.MODE_LOWPOWER:
            print("Tracker paused")
        elif mode == pst.ETrackerMode.MODE_DISCONNECT:
            print("Tracker disconnected")
        elif mode == pst.ETrackerMode.MODE_RECONNECT:
            print("Tracker reconnected")
            restart = True
        else:
            print("Mode " + str(mode))
 
"""Helper function to register the exit handler with the application"""
def register_exit_handler():
    if sys.platform.startswith("linux"):
        import signal
        signal.signal(signal.SIGTERM, exit_handler)
        signal.signal(signal.SIGHUP, exit_handler)
        signal.signal(signal.SIGQUIT, exit_handler)
        signal.signal(signal.SIGINT, exit_handler)
    elif sys.platform.startswith("win"):
        import win32api
        win32api.SetConsoleCtrlHandler(exit_handler, True)
 
"""Implement the exit handler to shut-down the PST Tracker connection on application termination."""
def exit_handler(*args):
    global running
    pst.Tracker.shutdown()
    running = False
    return True
 
def main():
    if(len(sys.argv) < 2):
        print("\nConfiguration Error: A camera configuration file needs to be specified. This file can be found in the Redist folder of your installation. "
                "See the documentation of the Python bindings for more information.")
        exit(0)
 
    # Register exit_handler for proper shutdown
    register_exit_handler()
 
    try:
        # Use Context Manager to prevent improper Tracker shutdown on errors.
        # Create an instance of the Tracker object using the default configuration path and file names.
        with pst.Tracker("", "","", sys.argv[1]) as tracker:
 
            # Check if calibration information is available for all cameras. When this is not the case, provide a warning and exit.
            if(len(tracker.get_uncalibrated_camera_urls()) > 0):
                print("\nNo calibration information could be found in the configuration directory.\n"
                    "Please use the PST Server and PST Client application to initialize the PST Tracker and create/import a tracking target.\n"
                    "More information can be found in the Initialization section of the PST SDK manual and the PST Manual.\n"
                    )
                sys.exit()
 
            # Print version number of the tracker server being used.
            print("Running PST Server version " + tracker.get_version_info())
 
            # Create listener with callback functions for data and/or mode updates.
            listener = MyListener()
            
            # Register the listener object to the tracker server.
            tracker.add_tracker_listener(listener)
 
            print("Put the Reference card in front of the PST in order to see tracking results.\n")
 
            # Start the tracker server.
            tracker.start()
 
            # Perform a system check to see if the tracker server is running OK and print the result.
            print("System check: " + str(tracker.system_check()))
 
            # Set the frame rate to 30 Hz.
            tracker.set_framerate(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 Tracker.set_framerate().
            print("Frame rate set to: " + str(tracker.get_framerate()))
 
            # Main loop, wait for auto-termination.
            while running:
                # If tracker has reconnected, restart it.
                global restart
                if restart:
                    tracker.start()
                    restart = False
 
                time.sleep(0.1)
 
    except psterrors.TrackerError as err:
        # Catch TrackerError and print error messages.
        print(err.message)
 
if __name__ == "__main__":
    main()