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
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);
804 if(d_connection->pack_message(len, vrpn_Tracker::timestamp, position_m_id, d_sender_id, 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{
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...
virtual void report_changes(void)
vrpn_Button_Filter(const char *, vrpn_Connection *c=NULL)
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:45
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 ...
vrpn_Clipping_Analog_Server(const char *name, vrpn_Connection *c, vrpn_int32 numChannels=vrpn_CHANNEL_MAX)
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 ...
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)
vrpn_float64 d_quat[4]
vrpn_int32 update_rate_id
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_int32 d_sensor
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
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.
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)