| PST SDK 6.0.0.0-272350a

reference.py
1 """Reference example of the PST SDK
2
3 This example shows how to use the PST SDK to adjust the PST Tracker reference system.
4 The reference system defines the Cartesian coordinate system in which tracking results
5 are reported. The example shows how to set the reference system by supplying a 4x4
6 homogeneous transformation matrix. It also shows how to check if the reference system
7 was set successfully.
8
9 Copyright PS-Tech B.V. All Rights Reserved.
10 """
11 import context
12 import time
13 import sys
14 import pstech.pstsdk.tracker as pst
15 import pstech.pstsdk.errors as psterrors
16
17 #global variables for example
18 running = True
19 max_samples = 1000
20 samples = 0
21
22 """Helper function for printing """
23 def print_matrix(matrix):
24 for y in range(4):
25 for x in range(4):
26 print(str(matrix[x + y * 4]), end="\t")
27 print("")
28 print("\n")
29
30 # Control variable for main loop
31 running = True
32
33 # Number of data points to grab before application termination
34 max_samples = 100
35
36 # Global number of samples
37 samples = 0
38
39 """Implementation of the pst.Listener class to receive tracking data and mode changes."""
40 class MyListener(pst.Listener):
41
42 """Implementation of a tracker data callback function
43
44 Implementation of a tracker data callback function. The on_tracker_data
45 function receives the data as soon as it becomes available.
46
47 Args:
48 tracker_data: Object containing tracking information retrieved from tracker
49 status_message: Status message reported by the tracker.
50
51 See Also:
52 pstech.pstdk.trackerdata.TrackerData
53 pstech.pstsdk.errors.EStatusMessage
54 """
55 def on_tracker_data(self, tracker_data, status_message):
56 global samples
57 global running
58
59 if samples >= max_samples:
60 running = False
61
62 samples += 1
63 # Do something here with the received data
64
65 """Helper function to register the exit handler with the application"""
66 def register_exit_handler():
67 if sys.platform.startswith("linux"):
68 import signal
69 signal.signal(signal.SIGTERM, exit_handler)
70 signal.signal(signal.SIGHUP, exit_handler)
71 signal.signal(signal.SIGQUIT, exit_handler)
72 signal.signal(signal.SIGINT, exit_handler)
73 elif sys.platform.startswith("win"):
74 import win32api
75 win32api.SetConsoleCtrlHandler(exit_handler, True)
76
77 """Implement the exit handler to shut-down the PST Tracker connection on application termination."""
78 def exit_handler(*args):
79 global running
80 pst.Tracker.shutdown()
81 running = False
82 return True
83
84 def main():
85 if(len(sys.argv) < 2):
86 print("\nConfiguration Error: A camera configuration file needs to be specified. This file can be found in the Redist folder of your installation. "
87 "See the documentation of the Python bindings for more information.")
88 exit(0)
89
90 # Register exit_handler for proper shutdown
91 register_exit_handler()
92
93 try:
94 # Use Context Manager to prevent improper Tracker shutdown on errors.
95 # Create an instance of the Tracker object using the default configuration path and file names.
96 with pst.Tracker("", "","", sys.argv[1]) as tracker:
97 # Print version number of the tracker server being used.
98 print("Running PST Server version " + tracker.get_version_info())
99
100 # Create listener with callback functions for data and/or mode updates.
101 listener = MyListener()
102
103 # Register the listener object to the tracker server.
104 tracker.add_tracker_listener(listener)
105
106 # Start the tracker server.
107 tracker.start()
108
109 # Perform a system check to see if the tracker server is running OK and print the result.
110 print("System check: " + str(tracker.system_check()))
111
112 # Set the frame rate to 30 Hz.
113 tracker.set_framerate(30)
114
115 # Print the new frame rate to see if it was set correctly. Note that for PST HD and Pico
116 # trackers the frame rate actually being set can differ from the value provided to Tracker.set_framerate().
117 print("Frame rate set to: " + str(tracker.get_framerate()))
118 print("***************************\n")
119
120 # Get the transformation matrix for the current reference system.
121 print("Current reference system transformation matrix:")
122 print_matrix(tracker.get_reference())
123
124 # Define new reference system transformation matrix rotating the reference system by
125 # 90 degrees around the X-axis and 180 degrees around the Y-axis. Then translate the
126 # origin of the reference system by 0.1 m in the X direction, -0.5 m in the Y direction
127 # and 0.5 m in the Z direction.
128 reference = [-1.0, 0.0, 0.0, 0.1,
129 0.0, 0.0, 1.0,-0.5,
130 0.0, 1.0, 0.0, 0.5,
131 0.0, 0.0, 0.0, 1.0]
132
133 tracker.set_reference(reference)
134 print("New reference system transformation matrix:")
135 print_matrix(tracker.get_reference())
136
137
138 # Trying to set the reference using a non-orthonormal transformation matrix (this should fail).
139 non_orthonormal_reference = [-1.0, 1.0, 0.0, 0.1,
140 0.0, 0.0,-1.0,-0.5,
141 0.0,-1.0, 0.0, 0.5,
142 0.0, 0.0, 0.0, 1.0
143 ]
144 try:
145 tracker.set_reference(non_orthonormal_reference)
146 print("Reference input incorrectly applied!")
147 except psterrors.TrackerError as err:
148 # This should fail because the supplied matrix is non-orthonormal
149 print("Reference input correctly ignored: %s" % err.message)
150 print("New reference system after applying non-orthonormal transformation:")
151 print_matrix(tracker.get_reference())
152
153 # Adjust the reference system by applying a relative transformation. The new reference system will have
154 # the X-axis pointing outward from the tracker and the Y-axis parallel to the tracker front.
155 relative_reference = [ 0.0,-1.0, 0.0, 0.5,
156 1.0, 0.0, 0.0, 0.4,
157 0.0, 0.0, 1.0, 0.0,
158 0.0, 0.0, 0.0, 1.0
159 ]
160 tracker.set_reference(relative_reference, True)
161 print("New reference system after applying relative transformation:")
162 print_matrix(tracker.get_reference())
163 print("***************************\n")
164
165 # Reset reference system to default (origin at 1 m from center of the PST Tracker,
166 # Z-axis pointing outward from the PST Tracker).
167 tracker.set_default_reference()
168 print("Reset default reference system:")
169 print_matrix(tracker.get_reference())
170
171 while running:
172 time.sleep(0.1)
173
174 except psterrors.TrackerError as err:
175 # Catch TrackerError and print error messages.
176 print(err.message)
177
178 if __name__ == "__main__":
179 main()