vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_PhaseSpace.C
Go to the documentation of this file.
1/*
2 PhaseSpace, Inc 2017
3 vrpn_Tracker_PhaseSpace.C
4
5 ChangeLog
6 =================
7 20170802
8 * fixed a bug where timestamp's microsecond field was same as seconds in Windows.
9
10 20170629
11 * added support non-fatal errors (i.e. license warnings)
12 * added server version query for newer cores that support it.
13 * changed indentation of some debugging output.
14
15 20170201
16 * removed some extraneous comments
17
18 20151008
19 * slave mode now uses cfg specs to assign sensors to markers and rigids.
20 Unassigned devices will still be assigned arbitrarily.
21
22 20150818
23 * Update to libowl2 API for X2e device support, removed support for Impulse
24 * removed stylus and button support
25 * removed maximum rigid and marker limitations
26 * vrpn.cfg: x,y, and z parameters for point sensors changed to single pos
27 parameter
28 * vrpn.cfg: k1,k2,k3,k4 parameters for rigid sensors changed to single init
29 parameter
30
31 */
33
34#include <errno.h>
35#include <math.h>
36
37#include <limits>
38#include <string>
39#include <sstream>
40#include <algorithm>
41#include <map>
42#include <vector>
43
44#define MM_TO_METERS (0.001)
45#define MSGBUFSIZE 1024
46
47#ifdef VRPN_INCLUDE_PHASESPACE
48
49//
50struct SensorInfo {
51 vrpn_int32 sensor_id; // vrpn sensor
52
53 int type; // owl tracker type
54 uint32_t tracker; // tracker_id
55 uint32_t id; // marker_id
56 std::string opt;
57
58 size_t search_hint;
59
60 SensorInfo()
61 {
62 type = -1;
63 tracker = 0;
64 id = 0;
65 opt = "";
66 search_hint = std::numeric_limits<uint32_t>::max();
67 }
68};
69
70
71//
72typedef std::vector<SensorInfo> Sensors;
73
74
75//
76bool operator<(const SensorInfo& a, const SensorInfo& b)
77{
78 if(a.type < b.type) return true;
79 if(a.type > b.type) return false;
80 return a.id < b.id;
81}
82
83//
84class vrpn_Tracker_PhaseSpace::SensorManager
85{
86protected:
87 Sensors _v;
88
89public:
90 void add(const SensorInfo& s)
91 {
92 _v.push_back(s);
93 std::sort(_v.begin(), _v.end());
94 }
95
96 const SensorInfo* get_by_sensor(int sensor_id)
97 {
98 for(size_t i = 0; i < _v.size(); i++)
99 if(_v[i].sensor_id == sensor_id) return &_v[i];
100 return NULL;
101 }
102
103 Sensors::iterator begin()
104 {
105 return _v.begin();
106 }
107
108 Sensors::iterator end()
109 {
110 return _v.end();
111 }
112};
113
114
115// ctor
117 : vrpn_Tracker(name ,c),
118 vrpn_Button_Filter(name, c),
120 debug(false),
121 drop_frames(false),
122 smgr(NULL)
123{
124 // TODO fix
125 //num_buttons = vrpn_BUTTON_MAX_BUTTONS;
126 num_buttons = 0;
127
128 smgr = new SensorManager();
129
130 if(d_connection) {
131 // Register a handler for the update change callback
133 fprintf(stderr,"vrpn_Tracker: Can't register workspace handler\n");
134 }
135}
136
137
138// dtor
140{
141 context.done();
142 context.close();
143}
144
145
146//
148{
149 printf("connecting to OWL server at %s...\n", device.c_str());
150
151 std::string init_options = "event.markers=1 event.rigids=1";
152 if(options.length()) init_options += " " + options;
153
154 if(drop_frames) printf("drop_frames enabled\n");
155
156 // connect to Impulse server
157 if(context.open(device) <= 0)
158 {
159 fprintf(stderr, "owl connect error: %s\n", context.lastError().c_str());
160 return false;
161 }
162
163 if(debug)
164 {
165 std::string coreversion = context.property<std::string>("coreversion");
166 printf("[debug] server version: %s\n", coreversion.length() ? coreversion.c_str() : "unknown");
167
168 std::string apiversion = context.property<std::string>("apiversion");
169
170 printf("[debug] API version: %s\n", apiversion.length() ? apiversion.c_str() : "unknown");
171 }
172
173 if(debug) printf("[debug] initialization parameters: %s\n", init_options.c_str());
174 if(context.initialize(init_options) <= 0)
175 {
176 fprintf(stderr, "owl init error: %s\n", context.lastError().c_str());
177 return false;
178 }
179
180 if(options.find("timebase=") == std::string::npos) context.timeBase(1, 1000000);
181 if(options.find("scale=") == std::string::npos) context.scale(MM_TO_METERS);
182
183 if(context.lastError().length())
184 {
185 fprintf(stderr, "owl error: %s\n", context.lastError().c_str());
186 return false;
187 }
188
189 int slave = context.property<int>("slave");
190
191 if(slave) printf("slave mode enabled.\n");
192 else if(!create_trackers()) return false;
193
194 printf("owl initialized\n");
195 return true;
196}
197
198
199//
201{
202 if(debug) printf("[debug] creating trackers...\n");
203
204 // create trackers
205 int nr = 0;
206 int nm = 0;
207 std::vector<uint32_t> ti;
208
209 // create rigid trackers
210 for(Sensors::iterator s = smgr->begin(); s != smgr->end(); s++)
211 {
212 if(s->type != OWL::Type::RIGID) continue;
213 nr++;
214 if(std::find(ti.begin(), ti.end(), s->tracker) != ti.end())
215 {
216 fprintf(stderr, "rigid tracker %d already defined\n", s->tracker);
217 continue;
218 }
219
220 printf("creating rigid tracker %d\n", s->tracker);
221 std::string type = "rigid";
222 std::stringstream name; name << type << nr;
223 if(debug) printf("[debug] type=%s id=%d name=%s options=\'%s\'\n", type.c_str(), s->tracker, name.str().c_str(), s->opt.c_str());
224
225 if(!context.createTracker(s->tracker, type, name.str(), s->opt))
226 {
227 fprintf(stderr, "tracker creation error: %s\n", context.lastError().c_str());
228 return false;
229 }
230 ti.push_back(s->tracker);
231 }
232
233 // create point trackers
234 for(Sensors::iterator s = smgr->begin(); s != smgr->end(); s++)
235 {
236 if(s->type != OWL::Type::MARKER) continue;
237 nm++;
238 if(std::find(ti.begin(), ti.end(), s->tracker) == ti.end())
239 {
240 printf("creating point tracker %d\n", s->tracker);
241 std::string type = "point";
242 std::stringstream name; name << type << nm;
243 ti.push_back(s->tracker);
244 context.createTracker(s->tracker, type, name.str());
245 }
246 if(!context.assignMarker(s->tracker, s->id, "", s->opt))
247 {
248 fprintf(stderr, "marker assignment error: %s\n", context.lastError().c_str());
249 return false;
250 }
251 if(debug) printf("[debug] id=%d tracker=%d options=\'%s\'\n", s->id, s->tracker, s->opt.c_str());
252 }
253
254 if(context.lastError().length())
255 {
256 fprintf(stderr, "tracker creation error: %s\n", context.lastError().c_str());
257 return false;
258 }
259
260 return true;
261}
262
263
264// This function should be called each time through the main loop
265// of the server code. It polls for data from the OWL server and
266// sends them if available.
268{
269 get_report();
271 return;
272}
273
274
275//
276std::string trim(char* line, int len)
277{
278 // cut off leading whitespace
279 int s, e;
280 for(s = 0; isspace(line[s]) && s < len; s++);
281 // cut off comments and trailing whitespace
282 e = s;
283 for(int i = s; line[i] && i < len; i++)
284 {
285 if(line[i] == '#') break;
286 if(!isspace(line[i])) e = i+1;
287 }
288 return std::string(line+s, e-s);
289}
290
291
292//
293bool read_int(const char* str, int &i)
294{
295 if(!str) return false;
296 errno = 0;
297 char* endptr = NULL;
298 i = strtol(str, &endptr, 10);
299 if(*endptr || errno) {
300 fprintf(stderr, "Error, expected an integer but got token: \"%s\"\n", str);
301 return false;
302 }
303 return true;
304}
305
306
307//
308bool read_uint(const char* str, uint32_t &i)
309{
310 if(!str) return false;
311 errno = 0;
312 char* endptr = NULL;
313 i = strtoul(str, &endptr, 10);
314 if(*endptr || errno) {
315 fprintf(stderr, "Error, expected an unsigned integer but got token: \"%s\"\n", str);
316 return false;
317 }
318 return true;
319}
320
321
322//
323bool read_bool(const char* str, bool &b)
324{
325 if(!str) return false;
326 std::string s(str);
327 if(s == "true") {
328 b = true;
329 return true;
330 } else if(s == "false") {
331 b = false;
332 return true;
333 }
334 int i = 0;
335 bool ret = read_int(str, i);
336 b = i ? true : false;
337 return ret;
338}
339
340
341//
342bool read_float(const char* str, float &f)
343{
344 if(!str) return false;
345 errno = 0;
346 char* endptr = NULL;
347 f = (float) strtod(str, &endptr);
348 if(*endptr || errno) {
349 fprintf(stderr, "Error, expected a float but got token: \"%s\"\n", str);
350 return false;
351 }
352 return true;
353}
354
355
356// TODO: Replace all of this junk with <regex> once it is standard in gcc
357class ConfigParser
358{
359protected:
360 struct Spec {
361 std::string key;
362 std::string type;
363 void* dest;
364 bool required;
365 };
366
367 std::map <std::string, std::string> keyvals;
368 std::stringstream _error;
369
370public:
371
372 std::string error()
373 {
374 return _error.str();
375 }
376
377 bool contains(const std::string &key)
378 {
379 return keyvals.find(key) != keyvals.end();
380 }
381
382 std::string join()
383 {
384 std::stringstream s;
385 for(std::map<std::string, std::string>::iterator i = keyvals.begin(); i != keyvals.end(); i++)
386 s << (i==keyvals.begin()?"":" ") << i->first << "=" << i->second;
387 return s.str();
388 }
389
390 //
391 bool parse(SensorInfo &si)
392 {
393 _error.str("");
394
395 std::string spec_type;
396 float x=std::numeric_limits<float>::quiet_NaN();
397 float y=std::numeric_limits<float>::quiet_NaN();
398 float z=std::numeric_limits<float>::quiet_NaN();
399
400 Spec spec_marker[] = {
401 { "led", "uint32_t", &si.id, true },
402 { "tracker", "uint32_t", &si.tracker, false },
403 { "x", "float", &x, false },
404 { "y", "float", &y, false },
405 { "z", "float", &z, false },
406 { "", "", NULL, false } // sentinel
407 };
408
409 Spec spec_rigid[] = {
410 { "tracker", "uint32_t", &si.tracker, true },
411 { "", "", NULL, false } // sentinel
412 };
413
414 if(pop_int("sensor", si.sensor_id))
415 {
416 _error << "required key 'sensor' not found";
417 return false;
418 }
419
420 if(pop("type", spec_type))
421 {
422 _error << "required key 'type' not found";
423 return false;
424 }
425
426 Spec* spec = NULL;
427 if(spec_type == "rigid" || spec_type == "rigid_body")
428 {
429 si.type = OWL::Type::RIGID;
430 spec = spec_rigid;
431 }
432 else if(spec_type == "point")
433 {
434 si.type = OWL::Type::MARKER;
435 spec = spec_marker;
436 }
437 else {
438 _error << "unknown sensor type: " << spec_type;
439 return false;
440 }
441
442 for(int i = 0; spec[i].dest; i++)
443 {
444 int ret = 0;
445 if(spec[i].type == "string")
446 ret = pop(spec[i].key, *((std::string*) spec[i].dest));
447 else if(spec[i].type == "uint32_t")
448 ret = pop_uint(spec[i].key, *((uint32_t*) spec[i].dest));
449 else if(spec[i].type == "float")
450 ret = pop_float(spec[i].key, *((float*) spec[i].dest));
451 else
452 {
453 _error << "unknown spec type: " << spec[i].type;
454 return false;
455 }
456 if(ret == 1)
457 {
458 if(spec[i].required)
459 {
460 _error << "required key not found: " << spec[i].key;
461 return false;
462 }
463 }
464 else if(ret)
465 {
466 _error << "error reading value for key \'" << spec[i].key << "'";
467 return false;
468 }
469 }
470
471 if(si.type == OWL::Type::RIGID)
472 si.id = si.tracker;
473
474 //special case (legacy)
475 if(!isnan(x) || !isnan(y) || !isnan(z))
476 {
477 if(isnan(x) || isnan(y) || isnan(z))
478 {
479 _error << "x,y,z keys must all be specified if any are specified.";
480 return false;
481 }
482 if(contains("pos"))
483 {
484 _error << "pos and x,y,z keys are mutually exclusive.";
485 return false;
486 }
487 std::stringstream pos; pos << x << ',' << y << ',' << z;
488 keyvals["pos"] = pos.str();
489 }
490
491 si.opt = join();
492 return true;
493 }
494
495 // returns: 0 on success, 1 on key error
496 int pop(std::string key, std::string &val)
497 {
498 if(keyvals.find(key) == keyvals.end())
499 return 1;
500 val = keyvals[key];
501 keyvals.erase(key);
502 return 0;
503 }
504
505 // returns: 0 on success, 1 on key error, 2 on parse error
506 int pop_int(std::string key, int &n)
507 {
508 std::string v;
509 if(pop(key, v)) return 1;
510 return read_int(v.c_str(), n) ? 0 : 2;
511 }
512
513 // returns: 0 on success, 1 on key error, 2 on parse error
514 int pop_uint(std::string key, uint32_t &n)
515 {
516 std::string v;
517 if(pop(key, v)) return 1;
518 return read_uint(v.c_str(), n) ? 0 : 2;
519 }
520
521 // returns: 0 on success, 1 on key error, 2 on parse error
522 int pop_float(std::string key, float &n)
523 {
524 std::string v;
525 if(pop(key, v)) return 1;
526 return read_float(v.c_str(), n) ? 0 : 2;
527 }
528
529 // returns: 0 on success, 1 on key error, 2 on parse error
530 int pop_bool(std::string key, bool &n)
531 {
532 std::string v;
533 if(pop(key, v)) return 1;
534 return read_bool(v.c_str(), n) ? 0 : 2;
535 }
536
537 // return index of error, or -1 on success
538 int parse_kv(std::string str)
539 {
540 keyvals.clear();
541 _error.str("");
542
543 std::vector<char> current_key;
544 std::vector<char> current_val;
545
546 int i = 0;
547 while(str[i])
548 {
549 current_key.clear();
550 current_val.clear();
551 while(isspace(str[i])) i++;
552 while(str[i]) {
553 if(str[i] == '"') {
554 _error << "key names are not allowed to contain quotes or be contained in quotes.";
555 return i;
556 } else if(isspace(str[i])) {
557 _error << "unexpected whitespace.";
558 return i;
559 } else if(str[i] == '=') {
560 i++;
561 break;
562 } else current_key.push_back(str[i++]);
563 }
564 if(!current_key.size()) {
565 _error << "empty key name.";
566 return i;
567 }
568 bool quoted = false;
569 if(str[i] == '"') {
570 quoted = true;
571 i++;
572 }
573 while(str[i]) {
574 if(str[i] == '"') {
575 if(quoted) {
576 quoted = false;
577 i++;
578 break;
579 } else {
580 _error << "misplaced quotes.";
581 return i;
582 }
583 } else if(str[i] == '=') {
584 _error << "unexpected '=' char in value token.";
585 return i;
586 } else if(!quoted && isspace(str[i])) break;
587 else current_val.push_back(str[i++]);
588 }
589 if(quoted) {
590 _error << "unterminated quotes.";
591 return i;
592 }
593 if(str[i] && !isspace(str[i])) {
594 _error << "expected whitespace after value token.";
595 return i;
596 }
597 if(!current_val.size()) {
598 _error << "empty value string.";
599 return i;
600 }
601 std::string key = std::string(current_key.data(), current_key.size());
602 std::string val = std::string(current_val.data(), current_val.size());
603
604 if(keyvals.find(key) != keyvals.end())
605 {
606 _error << "duplicate key encountered: '" << key << "'";
607 return i;
608 }
609 keyvals[key] = val;
610 }
611
612 return -1;
613 }
614};
615
616
617//
619{
620 const int BUFSIZE = 1024;
621 char line[BUFSIZE];
622 bool inTag = false;
623
624 std::string ln;
625 bool eof = false;
626
627 while(!(eof = (fgets(line, BUFSIZE, file) == NULL)))
628 {
629 ConfigParser parser;
630 ln = trim(line, BUFSIZE);
631
632 // skip contentless lines
633 if(!ln.length()) continue;
634
635 // skip lines unless we're nested in <owl> tags
636 if(ln == "<owl>") {
637 if (inTag) {
638 fprintf (stderr, "Error, nested <owl> tag encountered. Aborting...\n");
639 return false;
640 } else {
641 inTag = true;
642 continue;
643 }
644 } else if (ln == "</owl>") {
645 if (inTag) {
646 if(debug)
647 {
648 printf("[debug] parsed config file:\n");
649 printf("[debug] %s\n", options.c_str());
650 for(Sensors::const_iterator s = smgr->begin(); s != smgr->end(); s++)
651 fprintf(stdout, "[debug] sensor=%d type=%d tracker=%d options=\'%s\'\n", s->sensor_id, s->type, s->tracker, s->opt.c_str());
652 }
653 // closing tag encountered, exit.
654 return true;
655 } else {
656 fprintf (stderr, "Error, </owl> tag without <owl> tag. Aborting...\n");
657 return false;
658 }
659 }
660
661 // parse line for key-value pairs
662 if(inTag) {
663 int e = parser.parse_kv(ln);
664 if(e >= 0) {
665 fprintf(stderr, "Error at character %d.\n", e);
666 break;
667 }
668 }
669
670 if(parser.contains("sensor")) {
671 // line is a sensor specification
672 SensorInfo info;
673 if(parser.parse(info))
674 {
675 // check duplicates
676 if(smgr->get_by_sensor(info.sensor_id))
677 {
678 fprintf(stderr, "duplicate sensor defined: %d\n", info.sensor_id);
679 break;
680 }
681 smgr->add(info);
682 }
683 else
684 {
685 fprintf(stderr, "%s\n", parser.error().c_str());
686 break;
687 }
688 } else {
689 // line is an option specification
690 if(parser.pop("device", device) == 2)
691 {
692 fprintf(stderr, "error reading value for key 'device'\n");
693 break;
694 }
695 if(parser.pop_bool("drop_frames", drop_frames) == 2)
696 {
697 fprintf(stderr, "error reading value for key 'drop_frames'\n");
698 break;
699 }
700 if(parser.pop_bool("debug", debug) == 2)
701 {
702 fprintf(stderr, "error reading value for key 'debug'\n");
703 break;
704 }
705
706
707 std::string new_options = parser.join();
708 if(new_options.length()) options += (options.length()?" ":"") + parser.join();
709 }
710 }
711
712 if(eof) fprintf(stderr, "Unexpected end of file.\n");
713 else fprintf(stderr, "Unable to parse line: \"%s\"\n", ln.c_str());
714 return false;
715}
716
717
718//
720{
721 if(!context.isOpen()) return false;
722
723 context.streaming(enable ? 1 : 0);
724 printf("streaming: %d\n", enable);
725
726 if(context.lastError().length())
727 {
728 fprintf(stderr, "owl error: %s\n", context.lastError().c_str());
729 return false;
730 }
731
732 return true;
733}
734
735
736//
737void vrpn_Tracker_PhaseSpace::set_pose(const OWL::Rigid &r)
738{
739 //set the position
740 pos[0] = r.pose[0];
741 pos[1] = r.pose[1];
742 pos[2] = r.pose[2];
743
744 //set the orientation quaternion
745 //OWL has the scale factor first, whereas VRPN has it last.
746 d_quat[0] = r.pose[4];
747 d_quat[1] = r.pose[5];
748 d_quat[2] = r.pose[6];
749 d_quat[3] = r.pose[3];
750}
751
752//
753void vrpn_Tracker_PhaseSpace::report_marker(vrpn_int32 sensor, const OWL::Marker &m)
754{
755 d_sensor = sensor;
756
757 if(debug)
758 {
759 int tr = context.markerInfo(m.id).tracker_id;
760 printf("[debug] sensor=%d type=point tracker=%d led=%d x=%f y=%f z=%f cond=%f\n", d_sensor, tr, m.id, m.x, m.y, m.z, m.cond);
761 }
762
763 if(m.cond <= 0) return;
764
765 pos[0] = m.x;
766 pos[1] = m.y;
767 pos[2] = m.z;
768
769 //raw positions have no rotation
770 d_quat[0] = 0;
771 d_quat[1] = 0;
772 d_quat[2] = 0;
773 d_quat[3] = 1;
774
775 send_report();
776}
777
778
779//
780void vrpn_Tracker_PhaseSpace::report_rigid(vrpn_int32 sensor, const OWL::Rigid& r, bool is_stylus)
781{
782 d_sensor = sensor;
783
784 if(debug)
785 {
786 printf("[debug] sensor=%d type=rigid tracker=%d x=%f y=%f z=%f w=%f a=%f b=%f c=%f cond=%f\n", d_sensor, r.id, r.pose[0], r.pose[1], r.pose[2], r.pose[3], r.pose[4], r.pose[5], r.pose[6], r.cond);
787 }
788
789 if(r.cond <= 0) return;
790
791 // set the position/orientation
792 set_pose(r);
793 send_report();
794}
795
796
797//
799{
800 if(d_connection)
801 {
802 char msgbuf[MSGBUFSIZE];
803 int len = vrpn_Tracker::encode_to(msgbuf);
806 fprintf(stderr,"PhaseSpace: cannot write message: tossing\n");
807 }
808 }
809}
810
811
812//
813void vrpn_Tracker_PhaseSpace::report_button(vrpn_int32 sensor, int value)
814{
815 if(d_sensor < 0 || d_sensor >= vrpn_BUTTON_MAX_BUTTONS)
816 {
817 fprintf(stderr, "error: sensor %d exceeds max button count\n", sensor);
818 return;
819 }
820
821 d_sensor = sensor;
822 buttons[d_sensor] = value;
823
824 if(debug)
825 {
826 printf("[debug] button %d 0x%x\n", d_sensor, value);
827 }
828}
829
830
831//
832void vrpn_Tracker_PhaseSpace::report_button_analog(vrpn_int32 sensor, int value)
833{
834 d_sensor = sensor;
836
837 if(debug)
838 {
839 printf("[debug] analog button %d 0x%x\n", d_sensor, value);
840 }
841}
842
843
844//
845template<class A>
846const A* find(int id, size_t& hint, std::vector<A> &data)
847{
848 if(hint >= data.size() || id != data[hint].id)
849 {
850 for(size_t i = 0; i < data.size(); i++)
851 {
852 const A &d = data[i];
853 if(d.id == id)
854 {
855 hint = i;
856 return &d;
857 }
858 }
859 return NULL;
860 }
861 return &data[hint];
862}
863
864//
866{
867 if(!context.isOpen()) return 0;
868
869 int maxiter = 1;
870
871 // set limit on looping
872 if(drop_frames) maxiter = 10000;
873
874 // read new data
875 const OWL::Event *event = NULL;
876
877 {
878 const OWL::Event *e = NULL;
879 int count = 0;
880 while(context.isOpen() && context.property<int>("initialized") && (e = context.nextEvent()))
881 {
882 if(e->type_id() == OWL::Type::FRAME) event = e;
883 else if(e->type_id() == OWL::Type::ERROR)
884 {
885 std::string err;
886 e->get(err);
887 fprintf(stderr, "owl error event: %s\n", err.c_str());
888 if(e->name() == "fatal")
889 {
890 fprintf(stderr, "stopping...\n");
891 context.done();
892 context.close();
893 return -1;
894 }
895 }
896 else if(e->type_id() == OWL::Type::BYTE)
897 {
898 std::string s; e->get(s);
899 printf("%s: %s\n", e->name(), s.c_str());
900
901 if(strcmp(e->name(), "done") == 0)
902 return 0;
903 }
904 if(maxiter && ++count >= maxiter) break;
905 }
906 }
907
908 if(!event) return 0;
909
910 // set timestamp
911#if WIN32 // msvc 2008
912 vrpn_Tracker::timestamp.tv_sec = (long) event->time() / 1000000;
913 vrpn_Tracker::timestamp.tv_usec = (long) event->time() % 1000000;
914#else
915 lldiv_t divresult = lldiv(event->time(), 1000000);
916 vrpn_Tracker::timestamp.tv_sec = divresult.quot;
917 vrpn_Tracker::timestamp.tv_usec = divresult.rem;
918#endif
919
920 if(debug)
921 {
922 printf("[debug] time=%ld.%06ld\n", vrpn_Tracker::timestamp.tv_sec, vrpn_Tracker::timestamp.tv_usec);
923 }
924
925 OWL::Markers markers;
926 OWL::Rigids rigids;
927
928 for(const OWL::Event *e = event->begin(); e != event->end(); e++)
929 {
930 if(e->type_id() == OWL::Type::MARKER)
931 e->get(markers);
932 else if(e->type_id() == OWL::Type::RIGID)
933 e->get(rigids);
934 }
935
936 int slave = context.property<int>("slave");
937 std::vector<const OWL::Marker*> reported_markers;
938 std::vector<const OWL::Rigid*> reported_rigids;
939 int sensor = 0;
940
941 for(Sensors::iterator s = smgr->begin(); s != smgr->end(); s++)
942 {
943 switch(s->type)
944 {
945 case OWL::Type::MARKER:
946 {
947 const OWL::Marker *m = find<OWL::Marker>(s->id, s->search_hint, markers);
948 if(m) {
949 report_marker(s->sensor_id, *m);
950 if(slave) reported_markers.push_back(m);
951 }
952 }
953 break;
954 case OWL::Type::RIGID:
955 {
956 const OWL::Rigid *r = find<OWL::Rigid>(s->id, s->search_hint, rigids);
957 if(r) {
958 report_rigid(s->sensor_id, *r);
959 if(slave) reported_rigids.push_back(r);
960 }
961 }
962 break;
963 default:
964 break;
965 }
966
967 // TODO implement styluses
968
969 if(s->sensor_id > sensor) sensor = s->sensor_id;
970 }
971
972 if(slave)
973 {
974 // in slave mode, client doesn't necessarily know what the incoming data will be.
975 // Report the ones not specified in the cfg file
976 if(sensor < 1000) sensor = 1000; // start arbitrarily at 1000
977 for(OWL::Markers::iterator m = markers.begin(); m != markers.end(); m++)
978 if(std::find(reported_markers.begin(), reported_markers.end(), &*m) == reported_markers.end())
979 report_marker(sensor++, *m);
980 for(OWL::Rigids::iterator r = rigids.begin(); r != rigids.end(); r++)
981 if(std::find(reported_rigids.begin(), reported_rigids.end(), &*r) == reported_rigids.end())
982 report_rigid(sensor++, *r);
983 }
984
985 // finalize button and analog reports
988
989 if(context.lastError().length())
990 {
991 printf("owl error: %s\n", context.lastError().c_str());
992 return -1;
993 }
994
995 if(!context.property<int>("initialized"))
996 return -1;
997
998 return (markers.size() || rigids.size()) ? 1 : 0;
999}
1000
1001
1002//
1004{
1006 if(thistracker->debug) {
1007 printf("[debug] vrpn_Tracker_PhaseSpace::handle_update_rate_request\n");
1008 }
1009 vrpn_float64 update_rate = 0;
1010 vrpn_unbuffer(&p.buffer,&update_rate);
1011 thistracker->context.frequency((float) update_rate);
1012 return 0;
1013}
1014
1015#endif // VRPN_INCLUDE_PHASESPACE
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
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...
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
virtual void report_changes(void)
Definition: vrpn_Button.C:383
vrpn_int32 num_buttons
Definition: vrpn_Button.h:48
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
Analog server that can scale and clip its range to -1..1.
Definition: vrpn_Analog.h:130
int setChannelValue(int channel, double value)
This method should be used to set the value of a channel. It will be scaled and clipped as described ...
Definition: vrpn_Analog.C:252
Generic connection class not specific to the transport mechanism.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
virtual void mainloop()
For this server, the user must normally call report() or report_changes() directly....
void set_pose(const OWL::Rigid &r)
vrpn_Tracker_PhaseSpace(const char *name, vrpn_Connection *c)
void report_marker(vrpn_int32 sensor, const OWL::Marker &m)
void report_rigid(vrpn_int32 sensor, const OWL::Rigid &r, bool is_stylus=false)
void report_button(vrpn_int32 sensor, int value)
void report_button_analog(vrpn_int32 sensor, int value)
static int VRPN_CALLBACK handle_update_rate_request(void *userdata, vrpn_HANDLERPARAM p)
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_int32 update_rate_id
Definition: vrpn_Tracker.h:89
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
#define MM_TO_METERS
const int vrpn_BUTTON_MAX_BUTTONS
Definition: vrpn_Button.h:13
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:321
std::vector< SensorInfo > Sensors
bool read_int(const char *str, int &i)
bool read_float(const char *str, float &f)
#define MSGBUFSIZE
bool read_uint(const char *str, uint32_t &i)
bool read_bool(const char *str, bool &b)
std::string trim(char *line, int len)
const A * find(int id, size_t &hint, std::vector< A > &data)
bool operator<(const SensorInfo &a, const SensorInfo &b)