vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_MotionNode.C
Go to the documentation of this file.
1 /*
2 Copyright (C) Motion Workshop 2014. Public domain.
3 
4 Permission is hereby granted, free of charge, to any person or organization
5 obtaining a copy of the software and accompanying documentation covered by
6 this license (the "Software") to use, reproduce, display, distribute,
7 execute, and transmit the Software, and to prepare derivative works of the
8 Software, and to permit third-parties to whom the Software is furnished to
9 do so, all subject to the following:
10 
11 The copyright notices in the Software and this entire statement, including
12 the above license grant, this restriction and the following disclaimer,
13 must be included in all copies of the Software, in whole or in part, and
14 all derivative works of the Software, unless such copies or derivative
15 works are solely in the form of machine-executable object code generated by
16 a source language processor.
17 
18 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
21 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
22 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
23 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
24 DEALINGS IN THE SOFTWARE.
25 */
27 #ifdef VRPN_USE_MOTIONNODE
28 
29 // Option to print out more information for run-time testing.
30 //#define TRACKER_MOTIONNODE_TEST 1
31 
32 //
33 // Use the binary Motion C API library provided by Motion Workshop. The library
34 // is loaded dynamically at run-time and is not required to compile this
35 // tracker The C API is available in your software installation or directly
36 // from Motion Workshop at:
37 //
38 // http://www.motionnode.com/sdk.html#capi
39 //
40 // The API is not required to compile this tracker class. The binary
41 // library is required at run-time to connect to the MotionNode data
42 // streams.
43 //
44 //#include <MotionNodeCAPI.h>
45 
46 // For convenience, the functions we will use are included directly in this
47 // file. No external compile time dependencies.
48 #if !defined(MNCAPI_PREVIEW_SIZE)
49 
50 #if defined(_WIN32)
51 # define MNCAPI_IMPORT_API __declspec(dllimport)
52 # define MNCAPI_CALL_API __cdecl
53 #else
54 # define MNCAPI_IMPORT_API
55 # define MNCAPI_CALL_API
56 #endif // defined(_WIN32)
57 
58 extern "C" {
59 
60 #define MNCAPI_PREVIEW_SIZE (14)
61 
65 };
66 
71 };
72 
73 }
74 
75 #endif // !defined(MNCAPI_PREVIEW_SIZE)
76 
77 #include <string>
78 #include <vector>
79 
80 #if defined(_WIN32)
81 # include <windows.h>
82 # define MOTION_C_API_LIBRARY "MotionCAPI.dll"
83 #else
84 # include <dlfcn.h>
85 # include <stdlib.h>
86 # if defined(__APPLE__)
87 # define MOTION_C_API_LIBRARY "libMotionCAPI.dylib"
88 # else
89 # define MOTION_C_API_LIBRARY "libMotionCAPI.so"
90 # endif // __APPLE__
91 # define GetProcAddress dlsym
92 #endif // _WIN32
93 
94 typedef int (MNCAPI_CALL_API *MNCAPI_OPEN_HOST_FN)(enum mncapi_stream_t, const char *, int);
95 typedef int (MNCAPI_CALL_API *MNCAPI_SAMPLE_FN)(int, float *, int);
96 typedef void (MNCAPI_CALL_API *MNCAPI_CLOSE_FN)(int);
97 
98 
108 class Sampler {
109 public:
110  typedef std::vector<float> data_type;
111 
112  Sampler(const std::string &address, unsigned port);
113  ~Sampler();
114 
115  bool get_data_block(data_type &data, const unsigned &num_sensor);
116 
117 private:
118  int m_handle;
119 #if defined(_WIN32)
120  HMODULE m_library_handle;
121 #else
122  void *m_library_handle;
123 #endif // WIN32
124  MNCAPI_SAMPLE_FN m_mncapi_sample;
125 }; // class Sampler
126 
127 typedef Sampler sampler_type;
128 
129 
131  vrpn_Connection *c,
132  unsigned num_sensor,
133  const char *address,
134  unsigned port)
135  : vrpn_Tracker(name, c), m_num_sensor(num_sensor), m_handle(NULL)
136 {
137 #if defined(TRACKER_MOTIONNODE_TEST)
138  printf("vrpn_Tracker_MotionNode {\n");
139 #endif // TRACKER_MOTIONNODE_TEST
140 
142 
143  {
144  std::string remote_address = "127.0.0.1";
145  unsigned remote_port = 32079;
146  if (NULL != address) {
147  remote_address = address;
148  }
149  if (port > 0) {
150  remote_port = port;
151  }
152 
153  sampler_type *sampler = new sampler_type(address, port);
154 
155  // Attempt to read a single sample from
156  // the stream.
157  for (int i=0; i<4; i++) {
158  sampler_type::data_type data;
159  if (sampler->get_data_block(data, m_num_sensor) && !data.empty()) {
160  m_handle = sampler;
161  sampler = NULL;
162  break;
163  }
164  }
165 
166  if (NULL == m_handle) {
167  fprintf(stderr, "MotionNode driver failed to start sampling, device not currently reading\n");
168  }
169 #if defined(TRACKER_MOTIONNODE_TEST)
170  else {
171  printf(
172  "Connected to Motion data service at \"%s:%d\"\n",
173  remote_address.c_str(), remote_port);
174  }
175 #endif // TRACKER_MOTIONNODE_TEST
176 
177  // Clean up resources now if the sampler was never
178  // copied into this object.
179  if ((NULL == m_handle) && (NULL != sampler)) {
180  delete sampler;
181  }
182  }
183 
184  if (NULL != m_handle) {
186  } else {
188  }
189 
190 #if defined(TRACKER_MOTIONNODE_TEST)
191  printf("} vrpn_Tracker_MotionNode, status=%d\n", vrpn_Tracker::status);
192 #endif // TRACKER_MOTIONNODE_TEST
193 }
194 
196 {
197 #if defined(TRACKER_MOTIONNODE_TEST)
198  printf("~vrpn_Tracker_MotionNode() {\n");
199 #endif // TRACKER_MOTIONNODE_TEST
200 
201  if (NULL != m_handle) {
202  sampler_type *sampler = reinterpret_cast<sampler_type *>(m_handle);
203  delete sampler;
204 
205  m_handle = NULL;
206  }
207 
208 #if defined(TRACKER_MOTIONNODE_TEST)
209  printf("} ~vrpn_Tracker_MotionNode()\n");
210 #endif // TRACKER_MOTIONNODE_TEST
211 }
212 
214 {
215  // Call the generic server mainloop, since we are a server
216  server_mainloop();
217 
218  get_report();
219 }
220 
221 void vrpn_Tracker_MotionNode::get_report()
222 {
223  if (NULL != m_handle) {
224  sampler_type *sampler = reinterpret_cast<sampler_type *>(m_handle);
225 
226  sampler_type::data_type data;
227  if (sampler->get_data_block(data, m_num_sensor) && !data.empty()) {
228 
229  for (std::size_t i=0; i<m_num_sensor; i++) {
230  const std::size_t index = i * MNCAPI_PREVIEW_SIZE;
231  if (index + 4 < data.size()) {
232 
234  vrpn_Tracker::d_quat[0] = data[index + 1];
235  vrpn_Tracker::d_quat[1] = data[index + 2];
236  vrpn_Tracker::d_quat[2] = data[index + 3];
237  vrpn_Tracker::d_quat[3] = data[index + 0];
238 
239  send_report();
240  }
241  }
242 
243  }
244  }
245 
246 }
247 
248 void vrpn_Tracker_MotionNode::send_report()
249 {
250  // Send the message on the connection
251  if (NULL != vrpn_Tracker::d_connection) {
252  char buffer[1024];
253  int len = encode_to(buffer);
255  fprintf(stderr, "MotionNode: cannot write message: tossing\n");
256  }
257  }
258 }
259 
260 
261 Sampler::Sampler(const std::string &address, unsigned port)
262  : m_handle(0), m_library_handle(NULL), m_mncapi_sample(NULL)
263 {
264  // Load the dynamic library. This will use the search paths
265  // and fail if the library can not be found.
266 #if defined(_WIN32)
267 
268  HMODULE library_handle = LoadLibrary(MOTION_C_API_LIBRARY);
269  if (NULL == library_handle) {
270  library_handle = LoadLibrary("C:/Program Files/Motion/tools/plugin/capi/" MOTION_C_API_LIBRARY);
271  }
272 
273 #else
274 
275  // Manually search for the shared library in likely locations.
276  const char *SearchPaths[3] = {
279 #if defined(__APPLE__)
280  "/Applications/Motion.app/Contents/Resources/tools/plugin/capi" MOTION_C_API_LIBRARY
281 #else
282  "/opt/Motion/tools/plugin/capi" MOTION_C_API_LIBRARY
283 #endif // defined(__APPLE__)
284  };
285 
286  void *library_handle = NULL;
287  for (int i=0; i<3; i++) {
288  library_handle = dlopen(SearchPaths[i], RTLD_LAZY);
289  if (NULL != library_handle) {
290 #if defined(TRACKER_MOTIONNODE_TEST)
291  printf("Loaded Motion C API library, \"%s\"\n", SearchPaths[i]);
292 #endif // TRACKER_MOTIONNODE_TEST
293  break;
294  }
295  }
296 
297 #endif // defined(_WIN32)
298 
299  if (NULL != library_handle) {
300  m_library_handle = library_handle;
301 
302  // Bind the open function and connect to the data service.
303  MNCAPI_OPEN_HOST_FN mncapi_open_host =
304  reinterpret_cast<MNCAPI_OPEN_HOST_FN>(GetProcAddress(m_library_handle, "mncapi_open_host"));
305  if (NULL != mncapi_open_host) {
306  // Connect and get the integer handle back.
307  int handle = mncapi_open_host(MNCAPI_PREVIEW, address.c_str(), static_cast<int>(port));
308  if (handle > 0) {
309  m_handle = handle;
310 
311  // Bind the sampling function that we will reuse many times in the get_data_block method.
312  MNCAPI_SAMPLE_FN mncapi_sample =
313  reinterpret_cast<MNCAPI_SAMPLE_FN>(GetProcAddress(m_library_handle, "mncapi_sample"));
314  if (NULL != mncapi_sample) {
315  m_mncapi_sample = mncapi_sample;
316  } else {
317  fprintf(
318  stderr,
319  "failed to bind function \"mncapi_sample\" from Motion C API library\n");
320  }
321 
322  } else {
323  fprintf(
324  stderr,
325  "failed to connect to Motion data service at \"%s:%d\"\n",
326  address.c_str(), port);
327  }
328 
329  } else {
330  fprintf(
331  stderr,
332  "failed to bind function \"mncapi_open_host\" from Motion C API library\n");
333  }
334  } else {
335  fprintf(
336  stderr,
337  "failed to find Motion C API library, \"" MOTION_C_API_LIBRARY "\"\n");
338  }
339 }
340 
341 Sampler::~Sampler()
342 {
343  // Clean up resource. Unload the dynamic library after closing the active connection.
344  if (NULL != m_library_handle) {
345  if (m_handle > 0) {
346  MNCAPI_CLOSE_FN mncapi_close =
347  reinterpret_cast<MNCAPI_CLOSE_FN>(GetProcAddress(m_library_handle, "mncapi_close"));
348  if (NULL != mncapi_close) {
349  mncapi_close(m_handle);
350  m_handle = 0;
351  } else {
352  fprintf(
353  stderr,
354  "failed to bind function \"mncapi_close\" from Motion C API library\n");
355  }
356  }
357 
358  m_mncapi_sample = NULL;
359 
360 #if defined(_WIN32)
361  if (FreeLibrary(m_library_handle)) {
362 #else
363  if (0 == dlclose(m_library_handle)) {
364 #endif // defined(_WIN32)
365  } else {
366  fprintf(stderr, "failed to unload Motion C API library\n");
367  }
368 
369  m_library_handle = NULL;
370  }
371 }
372 
373 bool Sampler::get_data_block(data_type &data, const unsigned &num_sensor)
374 {
375  bool result = false;
376 
377  if ((m_handle > 0) && (NULL != m_mncapi_sample) && (num_sensor > 0)) {
378  // Accept up to num_sensor results. We may not receive that many.
379  data.resize(MNCAPI_PREVIEW_SIZE * num_sensor);
380 
381  int sample_result = m_mncapi_sample(m_handle, &data[0], data.size());
382  if (sample_result > MNCAPI_FAILURE) {
383  result = true;
384  }
385  }
386 
387  return result;
388 }
389 
390 #endif // VRPN_USE_MOTIONNODE
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_Tracker_MotionNode(const char *name, vrpn_Connection *c, unsigned num_sensor, const char *address, unsigned port)
int register_server_handlers(void)
Definition: vrpn_Tracker.C:318
#define MNCAPI_CALL_API
#define GetProcAddress
Generic connection class not specific to the transport mechanism.
int(MNCAPI_CALL_API * MNCAPI_OPEN_HOST_FN)(enum mncapi_stream_t, const char *, int)
#define MOTION_C_API_LIBRARY
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
void(MNCAPI_CALL_API * MNCAPI_CLOSE_FN)(int)
vrpn_Connection * d_connection
Connection that this object talks to.
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
Sampler sampler_type
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
#define MNCAPI_PREVIEW_SIZE
vrpn_int32 d_sender_id
Sender ID registered with the connection.
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
int(MNCAPI_CALL_API * MNCAPI_SAMPLE_FN)(int, float *, int)
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80