DTrackSDK  v2.9.0
example_dtrack1.cpp
1 /* DTrackSDK: C++ example
2  *
3  * C++ example using DTrackSDK to control DTrack1 PC
4  *
5  * Copyright 2005-2021, Advanced Realtime Tracking GmbH & Co. KG
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * 3. Neither the name of copyright holder nor the names of its contributors
16  * may be used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *
30  * Purpose:
31  * - example with DTrack1 remote commands:
32  * starts measurement, collects frames and stops measurement again
33  * - for DTrackSDK v2.6.0 (or newer)
34  */
35 
36 #include "DTrackSDK.hpp"
37 
38 #include <iostream>
39 #include <sstream>
40 
41 // global DTrackSDK
42 static DTrackSDK* dt = NULL;
43 
44 // prototypes
45 static void output_to_console();
46 static bool data_error_to_console();
47 
48 
52 int main( int argc, char** argv )
53 {
54  if ( argc != 4 )
55  {
56  std::cout << "Usage: example_dtrack1 <server host> <server port> <data port>" << std::endl;
57  return -1;
58  }
59 
60  std::istringstream portstream( argv[ 2 ] );
61  unsigned short rport;
62  portstream >> rport; // remote port
63 
64  if ( portstream.fail() || rport == 0 )
65  {
66  std::cout << "invalid remote port '" << argv[ 2 ] << "'" << std::endl;
67  return -2;
68  }
69 
70  portstream.clear();
71  portstream.str( argv[ 3 ] );
72  unsigned short port;
73  portstream >> port; // data port
74 
75  if ( portstream.fail() )
76  {
77  std::cout << "invalid data port '" << argv[ 3 ] << "'" << std::endl;
78  return -2;
79  }
80 
81  // initialization:
82 
83  dt = new DTrackSDK( (const char *)argv[ 1 ], rport, port );
84 
85  if ( ! dt->isDataInterfaceValid() )
86  {
87  std::cout << "DTrackSDK init error" << std::endl;
88  return -3;
89  }
90  std::cout << "connected to ATC '" << argv[ 1 ] << "', listening at local data port " << dt->getDataPort() << std::endl;
91 
92 // dt->setDataTimeoutUS( 3000000 ); // NOTE: change here timeout for receiving tracking data, if necessary
93 // dt->setDataBufferSize( 100000 ); // NOTE: change here buffer size for receiving tracking data, if necessary
94 
95  // measurement:
96 
97  if ( ! dt->startMeasurement() ) // start measurement
98  {
99  std::cout << "Measurement start failed!" << std::endl;
100  data_error_to_console();
101  delete dt;
102  return -4;
103  }
104 
105  int count = 0;
106  while ( count++ < 1000 ) // collect 1000 frames
107  {
108  if ( dt->receive() )
109  {
110  output_to_console();
111  }
112  else
113  {
114  data_error_to_console();
115  }
116  }
117 
118  dt->stopMeasurement(); // stop measurement
119 
120  delete dt; // clean up
121  return 0;
122 }
123 
124 
128 static void output_to_console()
129 {
130  std::cout.precision( 3 );
131  std::cout.setf( std::ios::fixed, std::ios::floatfield );
132 
133  std::cout << std::endl << "frame " << dt->getFrameCounter() << " ts " << dt->getTimeStamp()
134  << " nbod " << dt->getNumBody() << " nfly " << dt->getNumFlyStick()
135  << " nmea " << dt->getNumMeaTool() << " nmearef " << dt->getNumMeaRef()
136  << " nhand " << dt->getNumHand() << " nmar " << dt->getNumMarker()
137  << " nhuman " << dt->getNumHuman()
138  << std::endl;
139 
140  // Standard bodies:
141  for ( int i = 0; i < dt->getNumBody(); i++ )
142  {
143  const DTrackBody* body = dt->getBody( i );
144  if ( body == NULL )
145  {
146  std::cout << "DTrackSDK fatal error: invalid body id " << i << std::endl;
147  break;
148  }
149 
150  if ( ! body->isTracked() )
151  {
152  std::cout << "bod " << body->id << " not tracked" << std::endl;
153  }
154  else
155  {
156  std::cout << "bod " << body->id << " qu " << body->quality
157  << " loc " << body->loc[ 0 ] << " " << body->loc[ 1 ] << " " << body->loc[ 2 ]
158  << " rot " << body->rot[ 0 ] << " " << body->rot[ 1 ] << " " << body->rot[ 2 ]
159  << " " << body->rot[ 3 ] << " " << body->rot[ 4 ] << " " << body->rot[ 5 ]
160  << " " << body->rot[ 6 ] << " " << body->rot[ 7 ] << " " << body->rot[ 8 ]
161  << std::endl;
162 
163  DTrackQuaternion quat = body->getQuaternion();
164  std::cout << "bod " << body->id << " quatw " << quat.w
165  << " quatxyz " << quat.x << " " << quat.y << " " << quat.z << std::endl;
166  }
167  }
168 
169  // A.R.T. Flysticks:
170  for ( int i = 0; i < dt->getNumFlyStick(); i++ )
171  {
172  const DTrackFlyStick* flystick = dt->getFlyStick( i );
173  if ( flystick == NULL )
174  {
175  std::cout << "DTrackSDK fatal error: invalid Flystick id " << i << std::endl;
176  break;
177  }
178 
179  if ( ! flystick->isTracked() )
180  {
181  std::cout << "fly " << flystick->id << " not tracked" << std::endl;
182  }
183  else
184  {
185  std::cout << "flystick " << flystick->id << " qu " << flystick->quality
186  << " loc " << flystick->loc[ 0 ] << " " << flystick->loc[ 1 ] << " " << flystick->loc[ 2 ]
187  << " rot " << flystick->rot[ 0 ] << " " << flystick->rot[ 1 ] << " " << flystick->rot[ 2 ]
188  << " " << flystick->rot[ 3 ] << " " << flystick->rot[ 4 ] << " " << flystick->rot[ 5 ]
189  << " " << flystick->rot[ 6 ] << " " << flystick->rot[ 7 ] << " " << flystick->rot[ 8 ]
190  << std::endl;
191  }
192 
193  std::cout << " btn";
194  for ( int j = 0; j < flystick->num_button; j++ )
195  {
196  std::cout << " " << flystick->button[ j ];
197  }
198  std::cout << " joy";
199  for ( int j = 0; j < flystick->num_joystick; j++ )
200  {
201  std::cout << " " << flystick->joystick[ j ];
202  }
203  std::cout << std::endl;
204  }
205 
206  // Measurement tools:
207  for ( int i = 0; i < dt->getNumMeaTool(); i++ )
208  {
209  const DTrackMeaTool* meatool = dt->getMeaTool( i );
210  if ( meatool == NULL )
211  {
212  std::cout << "DTrackSDK fatal error: invalid Measurement tool id " << i << std::endl;
213  break;
214  }
215 
216  if ( ! meatool->isTracked() )
217  {
218  std::cout << "mea " << meatool->id << " not tracked" << std::endl;
219  }
220  else
221  {
222  std::cout << "mea " << meatool->id << " qu " << meatool->quality
223  << " loc " << meatool->loc[ 0 ] << " " << meatool->loc[ 1 ] << " " << meatool->loc[ 2 ]
224  << " rot " << meatool->rot[ 0 ] << " " << meatool->rot[ 1 ] << " " << meatool->rot[ 2 ]
225  << " " << meatool->rot[ 3 ] << " " << meatool->rot[ 4 ] << " " << meatool->rot[ 5 ]
226  << " " << meatool->rot[ 6 ] << " " << meatool->rot[ 7 ] << " " << meatool->rot[ 8 ]
227  << std::endl;
228  }
229 
230  if ( meatool->num_button > 0 )
231  {
232  std::cout << " btn";
233  for ( int j = 0; j < meatool->num_button; j++ )
234  {
235  std::cout << " " << meatool->button[ j ];
236  }
237  std::cout << std::endl;
238  }
239  }
240 
241  // Measurement references:
242  for ( int i = 0; i < dt->getNumMeaRef(); i++ )
243  {
244  const DTrackMeaRef* mearef = dt->getMeaRef( i );
245  if ( mearef == NULL )
246  {
247  std::cout << "DTrackSDK fatal error: invalid Measurement reference id " << i << std::endl;
248  break;
249  }
250 
251  if ( ! mearef->isTracked() )
252  {
253  std::cout << "mearef " << mearef->id << " not tracked" << std::endl;
254  }
255  else
256  {
257  std::cout << "mearef " << mearef->id << " qu " << mearef->quality
258  << " loc " << mearef->loc[ 0 ] << " " << mearef->loc[ 1 ] << " " << mearef->loc[ 2 ]
259  << " rot " << mearef->rot[ 0 ] << " " << mearef->rot[ 1 ] << " " << mearef->rot[ 2 ]
260  << " " << mearef->rot[ 3 ] << " " << mearef->rot[ 4 ] << " " << mearef->rot[ 5 ]
261  << " " << mearef->rot[ 6 ] << " " << mearef->rot[ 7 ] << " " << mearef->rot[ 8 ]
262  << std::endl;
263  }
264  }
265 
266  // Single markers:
267  for ( int i = 0; i < dt->getNumMarker(); i++ )
268  {
269  const DTrackMarker* marker = dt->getMarker( i );
270  if ( marker == NULL )
271  {
272  std::cout << "DTrackSDK fatal error: invalid marker index " << i << std::endl;
273  break;
274  }
275 
276  std::cout << "mar " << marker->id << " qu " << marker->quality
277  << " loc " << marker->loc[ 0 ] << " " << marker->loc[ 1 ] << " " << marker->loc[ 2 ]
278  << std::endl;
279  }
280 
281  // A.R.T. FINGERTRACKING hands:
282  for ( int i = 0; i < dt->getNumHand(); i++ )
283  {
284  const DTrackHand* hand = dt->getHand( i );
285  if ( hand == NULL )
286  {
287  std::cout << "DTrackSDK fatal error: invalid FINGERTRACKING id " << i << std::endl;
288  break;
289  }
290 
291  if ( ! hand->isTracked() )
292  {
293  std::cout << "hand " << hand->id << " not tracked" << std::endl;
294  }
295  else
296  {
297  std::cout << "hand " << hand->id << " qu " << hand->quality
298  << " lr " << ( ( hand->lr == 0 ) ? "left" : "right") << " nf " << hand->nfinger
299  << " loc " << hand->loc[ 0 ] << " " << hand->loc[ 1 ] << " " << hand->loc[ 2 ]
300  << " rot " << hand->rot[ 0 ] << " " << hand->rot[ 1 ] << " " << hand->rot[ 2 ]
301  << " " << hand->rot[ 3 ] << " " << hand->rot[ 4 ] << " " << hand->rot[ 5 ]
302  << " " << hand->rot[ 6 ] << " " << hand->rot[ 7 ] << " " << hand->rot[ 8 ]
303  << std::endl;
304 
305  for ( int j = 0; j < hand->nfinger; j++ )
306  {
307  std::cout << " fi " << j
308  << " loc " << hand->finger[ j ].loc[ 0 ] << " " << hand->finger[ j ].loc[ 1 ] << " " << hand->finger[ j ].loc[ 2 ]
309  << " rot " << hand->finger[ j ].rot[ 0 ] << " " << hand->finger[ j ].rot[ 1 ] << " " << hand->finger[ j ].rot[ 2 ]
310  << " " << hand->finger[ j ].rot[ 3 ] << " " << hand->finger[ j ].rot[ 4 ] << " " << hand->finger[ j ].rot[ 5 ]
311  << " " << hand->finger[ j ].rot[ 6 ] << " " << hand->finger[ j ].rot[ 7 ] << " " << hand->finger[ j ].rot[ 8 ]
312  << std::endl;
313  std::cout << " fi " << j
314  << " tip " << hand->finger[ j ].radiustip
315  << " pha " << hand->finger[ j ].lengthphalanx[ 0 ] << " " << hand->finger[ j ].lengthphalanx[ 1 ]
316  << " " << hand->finger[ j ].lengthphalanx[ 2 ]
317  << " ang " << hand->finger[ j ].anglephalanx[ 0 ] << " " << hand->finger[ j ].anglephalanx[ 1 ]
318  << std::endl;
319  }
320  }
321  }
322 
323  // A.R.T human models:
324  if ( dt->getNumHuman() < 1 )
325  {
326  std::cout << "no human model data" << std::endl;
327  }
328 
329  for ( int i = 0; i < dt->getNumHuman(); i++ )
330  {
331  const DTrackHuman* human = dt->getHuman( i );
332  if ( human == NULL )
333  {
334  std::cout << "DTrackSDK fatal error: invalid human model id " << i << std::endl;
335  break;
336  }
337 
338  if ( ! human->isTracked() )
339  {
340  std::cout << "human " << human->id << " not tracked" << std::endl;
341  }
342  else
343  {
344  std::cout << "human " << human->id << " num joints " << human->num_joints << std::endl;
345  for ( int j = 0; j < human->num_joints; j++ )
346  {
347  if ( ! human->joint[ j ].isTracked() )
348  {
349  std::cout << "joint " << human->joint[ j ].id << " not tracked" << std::endl;
350  }
351  else
352  {
353  std::cout << "joint " << human->joint[ j ].id << " qu " << human->joint[j].quality
354  << " loc " << human->joint[ j ].loc[ 0 ] << " " << human->joint[j].loc[ 1 ] << " " << human->joint[ j ].loc[ 2 ]
355  << " rot " << human->joint[ j ].rot[ 0 ] << " " << human->joint[j].rot[ 1 ] << " " << human->joint[ j ].rot[ 2 ]
356  << " " << human->joint[ j ].rot[ 3 ] << " " << human->joint[j].rot[ 4 ] << " " << human->joint[ j ].rot[ 5 ]
357  << " " << human->joint[ j ].rot[ 6 ] << " " << human->joint[j].rot[ 7 ] << " " << human->joint[ j ].rot[ 8 ]
358  << std::endl;
359  }
360  }
361  }
362  std::cout << std::endl;
363  }
364 }
365 
366 
372 static bool data_error_to_console()
373 {
375  {
376  std::cout << "--- timeout while waiting for tracking data" << std::endl;
377  return false;
378  }
379 
380  if ( dt->getLastDataError() == DTrackSDK::ERR_NET )
381  {
382  std::cout << "--- error while receiving tracking data" << std::endl;
383  return false;
384  }
385 
386  if ( dt->getLastDataError() == DTrackSDK::ERR_PARSE )
387  {
388  std::cout << "--- error while parsing tracking data" << std::endl;
389  return false;
390  }
391 
392  return true;
393 }
394 
double getTimeStamp() const
Get timestamp since midnight.
int getNumFlyStick() const
Get number of calibrated Flysticks.
double loc[3]
Location (in [mm])
const DTrackFlyStick * getFlyStick(int id) const
Get Flystick data.
const DTrackMeaTool * getMeaTool(int id) const
Get Measurement Tool data.
int getNumBody() const
Get number of calibrated standard bodies (as far as known).
bool receive()
Receive and process one tracking data packet.
Definition: DTrackSDK.cpp:441
double rot[9]
Rotation matrix (column-wise)
unsigned short getDataPort() const
Get UDP data port where tracking data is received.
Definition: DTrackSDK.cpp:321
int lr
Left (0) or right (1) hand.
double loc[3]
Location (in [mm])
double loc[3]
Location (in [mm])
Single marker data (3DOF).
double x
Quaternion component x.
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
int getNumHand() const
Get number of calibrated A.R.T. FINGERTRACKING hands (as far as known).
A.R.T. Flystick data (6DOF + buttons).
bool isTracked() const
Returns if joint is currently tracked.
double quality
Quality of joint (0.0 <= qu <= 1.0, no tracking if -1.0)
Measurement Tool data (6DOF + buttons).
A.R.T. FINGERTRACKING hand data (6DOF + fingers).
const DTrackBody * getBody(int id) const
Get standard body data.
bool isTracked() const
Returns if Measurement Tool reference is currently tracked.
struct DTrackSDK_Datatypes::DTrackHuman::DTrackJoint joint[DTRACKSDK_HUMAN_MAX_JOINTS]
Joint data.
double y
Quaternion component y.
int nfinger
Number of fingers (maximum 5)
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
double rot[9]
Rotation matrix of outermost phalanx (column-wise)
double loc[3]
Location of tip (in [mm])
int id
ID number (starting with 0)
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
const DTrackMeaRef * getMeaRef(int id) const
Get Measurement Tool reference data.
DTrack SDK main class derived from DTrackParser.
Definition: DTrackSDK.hpp:77
int id
ID number (starting with 1)
const DTrackMarker * getMarker(int index) const
Get single marker data.
int getNumMeaRef() const
Get number of calibrated Measurement Tool references.
double anglephalanx[2]
Angle between phalanxes (order: outermost, innermost; in [deg])
bool isTracked() const
Returns if body is currently tracked.
double rot[9]
Rotation matrix (column-wise)
int id
ID number of joint (starting with 0)
double lengthphalanx[3]
Length of phalanxes (order: outermost, middle, innermost; in [mm])
int getNumMarker() const
Get number of tracked single markers.
double loc[3]
Location of joint (in [mm])
unsigned int getFrameCounter() const
Get frame counter.
double rot[9]
Rotation matrix (column-wise)
int id
ID number (starting with 0)
const DTrackHuman * getHuman(int id) const
Get ART-Human model data.
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
double loc[3]
Location of back of the hand (in [mm])
const DTrackHand * getHand(int id) const
Get A.R.T. FINGERTRACKING hand data.
double quality
Quality (0.0 <= qu <= 1.0)
int num_joystick
Number of joystick values.
Timeout occured.
Definition: DTrackSDK.hpp:93
int id
ID number (starting with 0)
int id
ID number (starting with 0)
Error while parsing command.
Definition: DTrackSDK.hpp:95
bool isDataInterfaceValid() const
Returns if UDP socket is open to receive tracking data on local machine.
Definition: DTrackSDK.cpp:310
double rot[9]
Rotation matrix of joint (column-wise) in relation to room coordinate system.
Errors getLastDataError() const
Get last error at receiving tracking data (data transmission).
Definition: DTrackSDK.cpp:549
int button[DTRACKSDK_FLYSTICK_MAX_BUTTON]
Button state (1 pressed, 0 not pressed): 0 front, 1..n-1 right to left.
bool stopMeasurement()
Stop measurement.
Definition: DTrackSDK.cpp:615
bool startMeasurement()
Start measurement.
Definition: DTrackSDK.cpp:595
bool isTracked() const
Returns if human model is currently tracked.
double rot[9]
Rotation matrix of back of the hand (column-wise)
Measurement Tool reference data (6DOF).
double w
Quaternion component w.
double z
Quaternion component z.
bool isTracked() const
Returns if Measurement Tool is currently tracked.
double rot[9]
Rotation matrix (column-wise)
struct DTrackSDK_Datatypes::DTrackHand::DTrackFinger finger[DTRACKSDK_HAND_MAX_FINGER]
Finger data (order: thumb, index finger, middle finger, ...)
DTrackQuaternion getQuaternion() const
Returns rotation as quaternion.
bool isTracked() const
Returns if Flystick is currently tracked.
int button[DTRACKSDK_MEATOOL_MAX_BUTTON]
Button state (1 pressed, 0 not pressed): 0 point measurement state.
int getNumHuman() const
Get number of calibrated ART-Human models.
bool isTracked() const
Returns if hand is currently tracked.
Network error.
Definition: DTrackSDK.hpp:94
double loc[3]
Location (in [mm])
Standard body data (6DOF).
double loc[3]
Location (in [mm])
double joystick[DTRACKSDK_FLYSTICK_MAX_JOYSTICK]
Joystick value (-1.0 <= joystick <= 1.0); 0 horizontal, 1 vertical.
int id
ID number (starting with 0)
int id
ID number of human model (starting with 0)
ART-Human model (joints (6DOF) including optional Fingertracking).
int getNumMeaTool() const
Get number of calibrated Measurement Tools.