DTrackSDK  v2.9.0
example_tactile_flystick.cpp
1 /* DTrackSDK in C++: example_tactile_flystick.cpp
2  *
3  * C++ example using Flystick to control a tactile FINGERTRACKING device.
4  *
5  * Copyright (c) 2020-2023 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 or without DTrack2/DTRACK3 remote commands
32  * - in communicating mode: starts measurement, collects some frames and stops measurement again
33  * - in listening mode: please start measurement manually e.g. in DTrack frontend application
34  * - uses a Flystick to control an ART tactile FINGERTRACKING device
35  * - for DTrackSDK v2.8.0 (or newer)
36  *
37  * NOTE: link with 'winmm.lib' if compiling under Windows
38  * NOTE: link with '-lrt' if compiling under older Linux (only for glibc versions before 2.17)
39  */
40 
41 #include <cmath>
42 
43 #include "DTrackSDK.hpp"
44 
45 #include <iostream>
46 #include <sstream>
47 
48 // usually the following should work; otherwise define OS_* manually:
49 #if defined( _WIN32 ) || defined( WIN32 ) || defined( _WIN64 )
50  #define OS_WIN // for MS Windows
51 #else
52  #define OS_UNIX // for Unix (Linux)
53 #endif
54 
55 #if defined( OS_WIN )
56  #include <windows.h>
57 #endif
58 #if defined( OS_UNIX )
59  #include <time.h>
60 #endif
61 
62 // global DTrackSDK
63 static DTrackSDK* dt = NULL;
64 
65 static const int NUMBER_OF_FINGERS = 3; // for 3 fingers
66 static std::vector< double > strength( NUMBER_OF_FINGERS, 0.0 );
67 
68 static const unsigned int REPEAT_PERIOD = 1000; // period (in milliseconds) to repeat tactile command
69 static unsigned int lastTimeMs;
70 
71 // prototypes
72 static bool doTactile( int flystickId, int handId );
73 static bool data_error_to_console();
74 static void messages_to_console();
75 static unsigned int getTimeMs();
76 
77 
86 int main( int argc, char** argv )
87 {
88  if ( argc != 4 )
89  {
90  std::cout << "Usage: example_tactile_flystick [<server host/ip>:]<data port> <Flystick id> <hand id>" << std::endl;
91  return -1;
92  }
93 
94  std::istringstream is( argv[ 2 ] );
95  int flystickId;
96  is >> flystickId; // Flystick id, range 0 ..
97 
98  if ( is.fail() || ( flystickId < 0 ) )
99  {
100  std::cout << "invalid Flystick ID '" << argv[ 2 ] << "'" << std::endl;
101  return -2;
102  }
103 
104  is.clear();
105  is.str( argv[ 3 ] );
106  int handId;
107  is >> handId; // hand id, range 0 ..
108 
109  if ( is.fail() || ( handId < 0 ) )
110  {
111  std::cout << "invalid hand id '" << argv[ 3 ] << "'" << std::endl;
112  return -2;
113  }
114 
115  // initialization:
116 
117  dt = new DTrackSDK( (const char *)argv[ 1 ] );
118 
119  if ( ! dt->isDataInterfaceValid() )
120  {
121  std::cout << "DTrackSDK init error" << std::endl;
122  delete dt;
123  return -3;
124  }
125  std::cout << "connected to ATC '" << argv[ 1 ] << "', listening at local data port " << dt->getDataPort() << std::endl;
126 
127 // dt->setCommandTimeoutUS( 30000000 ); // NOTE: change here timeout for exchanging commands, if necessary
128 // dt->setDataTimeoutUS( 3000000 ); // NOTE: change here timeout for receiving tracking data, if necessary
129 // dt->setDataBufferSize( 100000 ); // NOTE: change here buffer size for receiving tracking data, if necessary
130 
131  if ( dt->isCommandInterfaceValid() ) // ensure full access for DTrack2/DTRACK3 commands, if in communicating mode
132  {
133  if ( ! dt->isCommandInterfaceFullAccess() )
134  {
135  std::cout << "Full access to ATC required!" << std::endl; // maybe DTrack2/3 frontend is still connected to ATC
136  data_error_to_console();
137  messages_to_console();
138  delete dt;
139  return -10;
140  }
141  }
142 
143  lastTimeMs = getTimeMs();
144 
145  // measurement:
146 
147  if ( dt->isCommandInterfaceValid() )
148  {
149  if ( ! dt->startMeasurement() ) // start measurement
150  {
151  std::cout << "Measurement start failed!" << std::endl;
152  data_error_to_console();
153  messages_to_console();
154  delete dt;
155  return -4;
156  }
157  }
158 
159  int count = 0;
160  while ( true ) // collect frames
161  {
162  count++;
163 
164  if ( dt->receive() )
165  {
166  if ( flystickId >= dt->getNumFlyStick() || handId >= dt->getNumHand() )
167  {
168  std::cout << "Flystick ID or Hand ID doesn't exist!" << std::endl;
169  break;
170  }
171 
172  if ( ! doTactile( flystickId, handId ) )
173  break;
174  }
175  else
176  {
177  data_error_to_console();
178  if ( dt->isCommandInterfaceValid() ) messages_to_console();
179  }
180 
181  if ( ( count % 100 == 1 ) && dt->isCommandInterfaceValid() )
182  messages_to_console();
183  }
184 
185  dt->tactileHandOff( handId, NUMBER_OF_FINGERS );
186 
187  if ( dt->isCommandInterfaceValid() )
188  {
189  dt->stopMeasurement(); // stop measurement
190  messages_to_console();
191  }
192 
193  delete dt; // clean up
194  return 0;
195 }
196 
197 
205 static bool doTactile( int flystickId, int handId )
206 {
207  const DTrackFlyStick* fly = dt->getFlyStick( flystickId );
208  if ( fly == NULL )
209  {
210  std::cout << "DTrackSDK fatal error: invalid Flystick id " << flystickId << std::endl;
211  return false;
212  }
213 
214  if ( fly->button[ 0 ] != 0 ) // stop program if trigger button is pressed
215  return false;
216 
217  // get new feedback strengths:
218 
219  std::vector< double > newStrength( NUMBER_OF_FINGERS, 0.0 );
220 
221  for ( int i = 0; i < NUMBER_OF_FINGERS; i++ )
222  {
223  if ( fly->button[ i + 1 ] != 0 ) // fixed strength if pressing upper buttons
224  newStrength[ i ] = 0.5;
225  }
226 
227  double joy = fly->joystick[ 0 ];
228  if ( joy > 0.0 ) // variable strength if using joystick
229  {
230  newStrength[ 0 ] = joy;
231  }
232  else if ( joy < 0.0 )
233  {
234  newStrength[ 2 ] = -joy;
235  }
236 
237  joy = fly->joystick[ 1 ];
238  if ( joy > 0.0 )
239  {
240  newStrength[ 1 ] = joy;
241  }
242 
243  // check if sending of tactile command is necessary:
244 
245  bool dosend = false;
246  for ( int i = 0; i < NUMBER_OF_FINGERS; i++ )
247  {
248  if ( fabs( newStrength[ i ] - strength[ i ] ) >= 0.01 )
249  {
250  strength[ i ] = newStrength[ i ];
251  dosend = true;
252  }
253  }
254 
255  unsigned int timeMs = getTimeMs();
256  if ( timeMs - lastTimeMs >= REPEAT_PERIOD ) // repeat tactile command
257  dosend = true;
258 
259  // send tactile command:
260 
261  if ( dosend )
262  {
263  dt->tactileHand( handId, strength );
264 
265  lastTimeMs = timeMs;
266  }
267 
268  return true;
269 }
270 
271 
277 static bool data_error_to_console()
278 {
279  bool ret = true;
280 
281  if ( dt->getLastDataError() != DTrackSDK::ERR_NONE )
282  {
284  {
285  std::cout << "--- timeout while waiting for tracking data" << std::endl;
286  }
287  else if ( dt->getLastDataError() == DTrackSDK::ERR_NET )
288  {
289  std::cout << "--- error while receiving tracking data" << std::endl;
290  }
291  else if ( dt->getLastDataError() == DTrackSDK::ERR_PARSE )
292  {
293  std::cout << "--- error while parsing tracking data" << std::endl;
294  }
295 
296  ret = false;
297  }
298 
300  {
302  {
303  std::cout << "--- timeout while waiting for Controller command" << std::endl;
304  }
305  else if ( dt->getLastServerError() == DTrackSDK::ERR_NET )
306  {
307  std::cout << "--- error while receiving Controller command" << std::endl;
308  }
309  else if ( dt->getLastServerError() == DTrackSDK::ERR_PARSE )
310  {
311  std::cout << "--- error while parsing Controller command" << std::endl;
312  }
313 
314  ret = false;
315  }
316 
317  return ret;
318 }
319 
320 
324 static void messages_to_console()
325 {
326  while ( dt->getMessage() )
327  {
328  std::cout << "ATC message: \"" << dt->getMessageStatus() << "\" \"" << dt->getMessageMsg() << "\"" << std::endl;
329  }
330 }
331 
332 
338 static unsigned int getTimeMs()
339 {
340 #if defined( OS_WIN )
341  return (unsigned int )timeGetTime();
342 #endif
343 #if defined( OS_UNIX )
344  struct timespec ts;
345 
346  clock_gettime( CLOCK_MONOTONIC, &ts );
347 
348  return (unsigned int )ts.tv_sec * 1000u + (unsigned int )ts.tv_nsec / 1000000u;
349 #endif
350 }
351 
int getNumFlyStick() const
Get number of calibrated Flysticks.
const DTrackFlyStick * getFlyStick(int id) const
Get Flystick data.
bool receive()
Receive and process one tracking data packet.
Definition: DTrackSDK.cpp:441
unsigned short getDataPort() const
Get UDP data port where tracking data is received.
Definition: DTrackSDK.cpp:321
bool isCommandInterfaceFullAccess()
Returns if TCP connection has full access for DTrack2/DTRACK3 commands.
Definition: DTrackSDK.cpp:341
int getNumHand() const
Get number of calibrated A.R.T. FINGERTRACKING hands (as far as known).
A.R.T. Flystick data (6DOF + buttons).
DTrack SDK main class derived from DTrackParser.
Definition: DTrackSDK.hpp:77
std::string getMessageStatus() const
Get status of last DTrack2/DTRACK3 event message.
Definition: DTrackSDK.cpp:949
Errors getLastServerError() const
Get last error at exchanging commands with Controller (command transmission).
Definition: DTrackSDK.cpp:558
bool isCommandInterfaceValid() const
Returns if TCP connection for DTrack2/DTRACK3 commands is active.
Definition: DTrackSDK.cpp:330
std::string getMessageMsg() const
Get message text of last DTrack2/DTRACK3 event message.
Definition: DTrackSDK.cpp:958
Timeout occured.
Definition: DTrackSDK.hpp:93
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
bool getMessage()
Get DTrack2/DTRACK3 event message from the Controller.
Definition: DTrackSDK.cpp:864
Errors getLastDataError() const
Get last error at receiving tracking data (data transmission).
Definition: DTrackSDK.cpp:549
bool tactileHandOff(int handId, int numFinger)
Send tactile FINGERTRACKING command to turn off tactile feedback on all fingers of a specific hand...
Definition: DTrackSDK.cpp:1012
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
Network error.
Definition: DTrackSDK.hpp:94
double joystick[DTRACKSDK_FLYSTICK_MAX_JOYSTICK]
Joystick value (-1.0 <= joystick <= 1.0); 0 horizontal, 1 vertical.
bool tactileHand(int handId, const std::vector< double > &strength)
Send tactile FINGERTRACKING command to set tactile feedback on all fingers of a specific hand...
Definition: DTrackSDK.cpp:987