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