DTrackSDK  v2.9.0
example_listening_multicast.cpp
1 /* DTrackSDK: C++ example
2  *
3  * C++ example using DTrackSDK for pure multicast listening
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 without DTrack2/DTrack3 remote commands: just collects frames from multicast address
32  * - please start measurement manually e.g. in DTrack frontend application
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 != 3 )
55  {
56  std::cout << "Usage: example_listening_multicast <multicast ip> <data port>" << std::endl;
57  return -1;
58  }
59 
60  std::istringstream portstream( argv[ 2 ] );
61  unsigned short port;
62  portstream >> port; // data port
63 
64  if ( portstream.fail() )
65  {
66  std::cout << "invalid port '" << argv[ 2 ] << "'" << std::endl;
67  return -2;
68  }
69 
70  // initialization:
71 
72  dt = new DTrackSDK( argv[ 1 ], port );
73 
74  if ( ! dt->isDataInterfaceValid() )
75  {
76  std::cout << "DTrackSDK init error" << std::endl;
77  return -3;
78  }
79  std::cout << "listening at multicast ip " << argv[ 1 ] << ", local data port " << dt->getDataPort() << std::endl;
80 
81 // dt->setDataTimeoutUS( 3000000 ); // NOTE: change here timeout for receiving tracking data, if necessary
82 // dt->setDataBufferSize( 100000 ); // NOTE: change here buffer size for receiving tracking data, if necessary
83 
84  // measurement:
85 
86  int count = 0;
87  while ( count++ < 1000 ) // collect 1000 frames
88  {
89  if ( dt->receive() )
90  {
91  output_to_console();
92  }
93  else
94  {
95  data_error_to_console();
96  }
97  }
98 
99  delete dt; // clean up
100  return 0;
101 }
102 
103 
107 static void output_to_console()
108 {
109  std::cout.precision( 3 );
110  std::cout.setf( std::ios::fixed, std::ios::floatfield );
111 
112  std::cout << std::endl << "frame " << dt->getFrameCounter() << " ts " << dt->getTimeStamp()
113  << " nbod " << dt->getNumBody() << " nfly " << dt->getNumFlyStick()
114  << " nmea " << dt->getNumMeaTool() << " nmearef " << dt->getNumMeaRef()
115  << " nhand " << dt->getNumHand() << " nmar " << dt->getNumMarker()
116  << " nhuman " << dt->getNumHuman() << " ninertial " << dt->getNumInertial()
117  << std::endl;
118 
119  // Standard bodies:
120  for ( int i = 0; i < dt->getNumBody(); i++ )
121  {
122  const DTrackBody* body = dt->getBody( i );
123  if ( body == NULL )
124  {
125  std::cout << "DTrackSDK fatal error: invalid body id " << i << std::endl;
126  break;
127  }
128 
129  if ( ! body->isTracked() )
130  {
131  std::cout << "bod " << body->id << " not tracked" << std::endl;
132  }
133  else
134  {
135  std::cout << "bod " << body->id << " qu " << body->quality
136  << " loc " << body->loc[ 0 ] << " " << body->loc[ 1 ] << " " << body->loc[ 2 ]
137  << " rot " << body->rot[ 0 ] << " " << body->rot[ 1 ] << " " << body->rot[ 2 ]
138  << " " << body->rot[ 3 ] << " " << body->rot[ 4 ] << " " << body->rot[ 5 ]
139  << " " << body->rot[ 6 ] << " " << body->rot[ 7 ] << " " << body->rot[ 8 ]
140  << std::endl;
141 
142  DTrackQuaternion quat = body->getQuaternion();
143  std::cout << "bod " << body->id << " quatw " << quat.w
144  << " quatxyz " << quat.x << " " << quat.y << " " << quat.z << std::endl;
145  }
146  }
147 
148  // A.R.T. Flysticks:
149  for ( int i = 0; i < dt->getNumFlyStick(); i++ )
150  {
151  const DTrackFlyStick* flystick = dt->getFlyStick( i );
152  if ( flystick == NULL )
153  {
154  std::cout << "DTrackSDK fatal error: invalid Flystick id " << i << std::endl;
155  break;
156  }
157 
158  if ( ! flystick->isTracked() )
159  {
160  std::cout << "fly " << flystick->id << " not tracked" << std::endl;
161  }
162  else
163  {
164  std::cout << "flystick " << flystick->id << " qu " << flystick->quality
165  << " loc " << flystick->loc[ 0 ] << " " << flystick->loc[ 1 ] << " " << flystick->loc[ 2 ]
166  << " rot " << flystick->rot[ 0 ] << " " << flystick->rot[ 1 ] << " " << flystick->rot[ 2 ]
167  << " " << flystick->rot[ 3 ] << " " << flystick->rot[ 4 ] << " " << flystick->rot[ 5 ]
168  << " " << flystick->rot[ 6 ] << " " << flystick->rot[ 7 ] << " " << flystick->rot[ 8 ]
169  << std::endl;
170  }
171 
172  std::cout << " btn";
173  for ( int j = 0; j < flystick->num_button; j++ )
174  {
175  std::cout << " " << flystick->button[ j ];
176  }
177  std::cout << " joy";
178  for ( int j = 0; j < flystick->num_joystick; j++ )
179  {
180  std::cout << " " << flystick->joystick[ j ];
181  }
182  std::cout << std::endl;
183  }
184 
185  // Measurement tools:
186  for ( int i = 0; i < dt->getNumMeaTool(); i++ )
187  {
188  const DTrackMeaTool* meatool = dt->getMeaTool( i );
189  if ( meatool == NULL )
190  {
191  std::cout << "DTrackSDK fatal error: invalid Measurement tool id " << i << std::endl;
192  break;
193  }
194 
195  if ( ! meatool->isTracked() )
196  {
197  std::cout << "mea " << meatool->id << " not tracked" << std::endl;
198  }
199  else
200  {
201  std::cout << "mea " << meatool->id << " qu " << meatool->quality
202  << " loc " << meatool->loc[ 0 ] << " " << meatool->loc[ 1 ] << " " << meatool->loc[ 2 ]
203  << " rot " << meatool->rot[ 0 ] << " " << meatool->rot[ 1 ] << " " << meatool->rot[ 2 ]
204  << " " << meatool->rot[ 3 ] << " " << meatool->rot[ 4 ] << " " << meatool->rot[ 5 ]
205  << " " << meatool->rot[ 6 ] << " " << meatool->rot[ 7 ] << " " << meatool->rot[ 8 ]
206  << std::endl;
207  }
208 
209  if ( meatool->tipradius > 0.0 )
210  {
211  std::cout << " radius " << meatool->tipradius << std::endl;
212  }
213 
214  if ( meatool->num_button > 0 )
215  {
216  std::cout << " btn";
217  for ( int j = 0; j < meatool->num_button; j++ )
218  {
219  std::cout << " " << meatool->button[ j ];
220  }
221  std::cout << std::endl;
222  }
223  }
224 
225  // Measurement references:
226  for ( int i = 0; i < dt->getNumMeaRef(); i++ )
227  {
228  const DTrackMeaRef* mearef = dt->getMeaRef( i );
229  if ( mearef == NULL )
230  {
231  std::cout << "DTrackSDK fatal error: invalid Measurement reference id " << i << std::endl;
232  break;
233  }
234 
235  if ( ! mearef->isTracked() )
236  {
237  std::cout << "mearef " << mearef->id << " not tracked" << std::endl;
238  }
239  else
240  {
241  std::cout << "mearef " << mearef->id << " qu " << mearef->quality
242  << " loc " << mearef->loc[ 0 ] << " " << mearef->loc[ 1 ] << " " << mearef->loc[ 2 ]
243  << " rot " << mearef->rot[ 0 ] << " " << mearef->rot[ 1 ] << " " << mearef->rot[ 2 ]
244  << " " << mearef->rot[ 3 ] << " " << mearef->rot[ 4 ] << " " << mearef->rot[ 5 ]
245  << " " << mearef->rot[ 6 ] << " " << mearef->rot[ 7 ] << " " << mearef->rot[ 8 ]
246  << std::endl;
247  }
248  }
249 
250  // Single markers:
251  for ( int i = 0; i < dt->getNumMarker(); i++ )
252  {
253  const DTrackMarker* marker = dt->getMarker( i );
254  if ( marker == NULL )
255  {
256  std::cout << "DTrackSDK fatal error: invalid marker index " << i << std::endl;
257  break;
258  }
259 
260  std::cout << "mar " << marker->id << " qu " << marker->quality
261  << " loc " << marker->loc[ 0 ] << " " << marker->loc[ 1 ] << " " << marker->loc[ 2 ]
262  << std::endl;
263  }
264 
265  // A.R.T. FINGERTRACKING hands:
266  for ( int i = 0; i < dt->getNumHand(); i++ )
267  {
268  const DTrackHand* hand = dt->getHand( i );
269  if ( hand == NULL )
270  {
271  std::cout << "DTrackSDK fatal error: invalid FINGERTRACKING id " << i << std::endl;
272  break;
273  }
274 
275  if ( ! hand->isTracked() )
276  {
277  std::cout << "hand " << hand->id << " not tracked" << std::endl;
278  }
279  else
280  {
281  std::cout << "hand " << hand->id << " qu " << hand->quality
282  << " lr " << ( ( hand->lr == 0 ) ? "left" : "right") << " nf " << hand->nfinger
283  << " loc " << hand->loc[ 0 ] << " " << hand->loc[ 1 ] << " " << hand->loc[ 2 ]
284  << " rot " << hand->rot[ 0 ] << " " << hand->rot[ 1 ] << " " << hand->rot[ 2 ]
285  << " " << hand->rot[ 3 ] << " " << hand->rot[ 4 ] << " " << hand->rot[ 5 ]
286  << " " << hand->rot[ 6 ] << " " << hand->rot[ 7 ] << " " << hand->rot[ 8 ]
287  << std::endl;
288 
289  for ( int j = 0; j < hand->nfinger; j++ )
290  {
291  std::cout << " fi " << j
292  << " loc " << hand->finger[ j ].loc[ 0 ] << " " << hand->finger[ j ].loc[ 1 ] << " " << hand->finger[ j ].loc[ 2 ]
293  << " rot " << hand->finger[ j ].rot[ 0 ] << " " << hand->finger[ j ].rot[ 1 ] << " " << hand->finger[ j ].rot[ 2 ]
294  << " " << hand->finger[ j ].rot[ 3 ] << " " << hand->finger[ j ].rot[ 4 ] << " " << hand->finger[ j ].rot[ 5 ]
295  << " " << hand->finger[ j ].rot[ 6 ] << " " << hand->finger[ j ].rot[ 7 ] << " " << hand->finger[ j ].rot[ 8 ]
296  << std::endl;
297  std::cout << " fi " << j
298  << " tip " << hand->finger[ j ].radiustip
299  << " pha " << hand->finger[ j ].lengthphalanx[ 0 ] << " " << hand->finger[ j ].lengthphalanx[ 1 ]
300  << " " << hand->finger[ j ].lengthphalanx[ 2 ]
301  << " ang " << hand->finger[ j ].anglephalanx[ 0 ] << " " << hand->finger[ j ].anglephalanx[ 1 ]
302  << std::endl;
303  }
304  }
305  }
306 
307  // A.R.T human models:
308  if ( dt->getNumHuman() < 1 )
309  {
310  std::cout << "no human model data" << std::endl;
311  }
312 
313  for ( int i = 0; i < dt->getNumHuman(); i++ )
314  {
315  const DTrackHuman* human = dt->getHuman( i );
316  if ( human == NULL )
317  {
318  std::cout << "DTrackSDK fatal error: invalid human model id " << i << std::endl;
319  break;
320  }
321 
322  if ( ! human->isTracked() )
323  {
324  std::cout << "human " << human->id << " not tracked" << std::endl;
325  }
326  else
327  {
328  std::cout << "human " << human->id << " num joints " << human->num_joints << std::endl;
329  for ( int j = 0; j < human->num_joints; j++ )
330  {
331  if ( ! human->joint[ j ].isTracked() )
332  {
333  std::cout << "joint " << human->joint[ j ].id << " not tracked" << std::endl;
334  }
335  else
336  {
337  std::cout << "joint " << human->joint[ j ].id << " qu " << human->joint[j].quality
338  << " loc " << human->joint[ j ].loc[ 0 ] << " " << human->joint[j].loc[ 1 ] << " " << human->joint[ j ].loc[ 2 ]
339  << " rot " << human->joint[ j ].rot[ 0 ] << " " << human->joint[j].rot[ 1 ] << " " << human->joint[ j ].rot[ 2 ]
340  << " " << human->joint[ j ].rot[ 3 ] << " " << human->joint[j].rot[ 4 ] << " " << human->joint[ j ].rot[ 5 ]
341  << " " << human->joint[ j ].rot[ 6 ] << " " << human->joint[j].rot[ 7 ] << " " << human->joint[ j ].rot[ 8 ]
342  << std::endl;
343  }
344  }
345  }
346  std::cout << std::endl;
347  }
348 
349  // Hybrid bodies:
350  if ( dt->getNumInertial() < 1 )
351  {
352  std::cout << "no inertial body data" << std::endl;
353  }
354 
355  for ( int i = 0; i < dt->getNumInertial(); i++ )
356  {
357  const DTrackInertial* inertial = dt->getInertial( i );
358  if ( inertial == NULL )
359  {
360  std::cout << "DTrackSDK fatal error: invalid hybrid body id " << i << std::endl;
361  break;
362  }
363 
364  std::cout << " inertial body " << inertial->id << " st " << inertial->st << " error " << inertial->error << std::endl;
365  if ( inertial->isTracked() )
366  {
367  std::cout << " loc " << inertial->loc[ 0 ] << " " << inertial->loc[ 1 ] << " " << inertial->loc[ 2 ]
368  << " rot " << inertial->rot[ 0 ] << " " << inertial->rot[ 1 ] << " " << inertial->rot[ 2 ]
369  << " " << inertial->rot[ 3 ] << " " << inertial->rot[ 4 ] << " " << inertial->rot[ 5 ]
370  << " " << inertial->rot[ 6 ] << " " << inertial->rot[ 7 ] << " " << inertial->rot[ 8 ]
371  << std::endl;
372  }
373  }
374 }
375 
376 
382 static bool data_error_to_console()
383 {
385  {
386  std::cout << "--- timeout while waiting for tracking data" << std::endl;
387  return false;
388  }
389 
390  if ( dt->getLastDataError() == DTrackSDK::ERR_NET )
391  {
392  std::cout << "--- error while receiving tracking data" << std::endl;
393  return false;
394  }
395 
396  if ( dt->getLastDataError() == DTrackSDK::ERR_PARSE )
397  {
398  std::cout << "--- error while parsing tracking data" << std::endl;
399  return false;
400  }
401 
402  return true;
403 }
404 
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)
double loc[3]
Location (in [mm])
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).
Hybrid (optical-inertial) body data (6DOF).
double error
Drift error estimate (only during inertial tracking, in [deg])
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])
int st
State of hybrid body (0: not tracked, 1: inertial tracking, 2: optical tracking, 3: inertial and opti...
bool isTracked() const
Returns if body is currently tracked.
double tipradius
Radius of tip (in [mm]) if applicable.
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.
int id
ID number (starting with 0)
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.
const DTrackInertial * getInertial(int id) const
Get hybrid (optical-inertial) data.
Errors getLastDataError() const
Get last error at receiving tracking data (data transmission).
Definition: DTrackSDK.cpp:549
int getNumInertial() const
Get number of calibrated hybrid (optical-inertial) bodies.
int button[DTRACKSDK_FLYSTICK_MAX_BUTTON]
Button state (1 pressed, 0 not pressed): 0 front, 1..n-1 right to left.
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.
double rot[9]
Rotation matrix (column-wise)
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.
bool isTracked() const
Returns if body is currently tracked.
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.