44#define MM_TO_METERS (0.001)
45#define MSGBUFSIZE 1024
47#ifdef VRPN_INCLUDE_PHASESPACE
66 search_hint = std::numeric_limits<uint32_t>::max();
76bool operator<(
const SensorInfo& a,
const SensorInfo& b)
78 if(a.type < b.type)
return true;
79 if(a.type > b.type)
return false;
84class vrpn_Tracker_PhaseSpace::SensorManager
90 void add(
const SensorInfo& s)
93 std::sort(_v.begin(), _v.end());
96 const SensorInfo* get_by_sensor(
int sensor_id)
98 for(
size_t i = 0; i < _v.size(); i++)
99 if(_v[i].sensor_id == sensor_id)
return &_v[i];
103 Sensors::iterator begin()
108 Sensors::iterator end()
128 smgr =
new SensorManager();
133 fprintf(stderr,
"vrpn_Tracker: Can't register workspace handler\n");
149 printf(
"connecting to OWL server at %s...\n",
device.c_str());
151 std::string init_options =
"event.markers=1 event.rigids=1";
159 fprintf(stderr,
"owl connect error: %s\n",
context.lastError().c_str());
165 std::string coreversion =
context.property<std::string>(
"coreversion");
166 printf(
"[debug] server version: %s\n", coreversion.length() ? coreversion.c_str() :
"unknown");
168 std::string apiversion =
context.property<std::string>(
"apiversion");
170 printf(
"[debug] API version: %s\n", apiversion.length() ? apiversion.c_str() :
"unknown");
173 if(
debug) printf(
"[debug] initialization parameters: %s\n", init_options.c_str());
174 if(
context.initialize(init_options) <= 0)
176 fprintf(stderr,
"owl init error: %s\n",
context.lastError().c_str());
180 if(
options.find(
"timebase=") == std::string::npos)
context.timeBase(1, 1000000);
183 if(
context.lastError().length())
185 fprintf(stderr,
"owl error: %s\n",
context.lastError().c_str());
189 int slave =
context.property<
int>(
"slave");
191 if(slave) printf(
"slave mode enabled.\n");
194 printf(
"owl initialized\n");
202 if(
debug) printf(
"[debug] creating trackers...\n");
207 std::vector<uint32_t> ti;
210 for(Sensors::iterator s =
smgr->begin(); s !=
smgr->end(); s++)
212 if(s->type != OWL::Type::RIGID)
continue;
214 if(std::find(ti.begin(), ti.end(), s->tracker) != ti.end())
216 fprintf(stderr,
"rigid tracker %d already defined\n", s->tracker);
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());
225 if(!
context.createTracker(s->tracker,
type, name.str(), s->opt))
227 fprintf(stderr,
"tracker creation error: %s\n",
context.lastError().c_str());
230 ti.push_back(s->tracker);
234 for(Sensors::iterator s =
smgr->begin(); s !=
smgr->end(); s++)
236 if(s->type != OWL::Type::MARKER)
continue;
238 if(std::find(ti.begin(), ti.end(), s->tracker) == ti.end())
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());
246 if(!
context.assignMarker(s->tracker, s->id,
"", s->opt))
248 fprintf(stderr,
"marker assignment error: %s\n",
context.lastError().c_str());
251 if(
debug) printf(
"[debug] id=%d tracker=%d options=\'%s\'\n", s->id, s->tracker, s->opt.c_str());
254 if(
context.lastError().length())
256 fprintf(stderr,
"tracker creation error: %s\n",
context.lastError().c_str());
276std::string
trim(
char* line,
int len)
280 for(s = 0; isspace(line[s]) && s < len; s++);
283 for(
int i = s; line[i] && i < len; i++)
285 if(line[i] ==
'#')
break;
286 if(!isspace(line[i])) e = i+1;
288 return std::string(line+s, e-s);
295 if(!str)
return false;
298 i = strtol(str, &endptr, 10);
299 if(*endptr || errno) {
300 fprintf(stderr,
"Error, expected an integer but got token: \"%s\"\n", str);
310 if(!str)
return false;
313 i = strtoul(str, &endptr, 10);
314 if(*endptr || errno) {
315 fprintf(stderr,
"Error, expected an unsigned integer but got token: \"%s\"\n", str);
325 if(!str)
return false;
330 }
else if(s ==
"false") {
336 b = i ? true :
false;
344 if(!str)
return false;
347 f = (float) strtod(str, &endptr);
348 if(*endptr || errno) {
349 fprintf(stderr,
"Error, expected a float but got token: \"%s\"\n", str);
367 std::map <std::string, std::string> keyvals;
368 std::stringstream _error;
377 bool contains(
const std::string &key)
379 return keyvals.find(key) != keyvals.end();
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;
391 bool parse(SensorInfo &si)
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();
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 }
409 Spec spec_rigid[] = {
410 {
"tracker",
"uint32_t", &si.tracker,
true },
411 {
"",
"", NULL,
false }
414 if(pop_int(
"sensor", si.sensor_id))
416 _error <<
"required key 'sensor' not found";
420 if(pop(
"type", spec_type))
422 _error <<
"required key 'type' not found";
427 if(spec_type ==
"rigid" || spec_type ==
"rigid_body")
429 si.type = OWL::Type::RIGID;
432 else if(spec_type ==
"point")
434 si.type = OWL::Type::MARKER;
438 _error <<
"unknown sensor type: " << spec_type;
442 for(
int i = 0; spec[i].dest; i++)
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));
453 _error <<
"unknown spec type: " << spec[i].type;
460 _error <<
"required key not found: " << spec[i].key;
466 _error <<
"error reading value for key \'" << spec[i].key <<
"'";
471 if(si.type == OWL::Type::RIGID)
475 if(!isnan(x) || !isnan(y) || !isnan(z))
477 if(isnan(x) || isnan(y) || isnan(z))
479 _error <<
"x,y,z keys must all be specified if any are specified.";
484 _error <<
"pos and x,y,z keys are mutually exclusive.";
487 std::stringstream pos; pos << x <<
',' << y <<
',' << z;
488 keyvals[
"pos"] = pos.str();
496 int pop(std::string key, std::string &val)
498 if(keyvals.find(key) == keyvals.end())
506 int pop_int(std::string key,
int &n)
509 if(pop(key, v))
return 1;
510 return read_int(v.c_str(), n) ? 0 : 2;
514 int pop_uint(std::string key, uint32_t &n)
517 if(pop(key, v))
return 1;
522 int pop_float(std::string key,
float &n)
525 if(pop(key, v))
return 1;
530 int pop_bool(std::string key,
bool &n)
533 if(pop(key, v))
return 1;
538 int parse_kv(std::string str)
543 std::vector<char> current_key;
544 std::vector<char> current_val;
551 while(isspace(str[i])) i++;
554 _error <<
"key names are not allowed to contain quotes or be contained in quotes.";
556 }
else if(isspace(str[i])) {
557 _error <<
"unexpected whitespace.";
559 }
else if(str[i] ==
'=') {
562 }
else current_key.push_back(str[i++]);
564 if(!current_key.size()) {
565 _error <<
"empty key name.";
580 _error <<
"misplaced quotes.";
583 }
else if(str[i] ==
'=') {
584 _error <<
"unexpected '=' char in value token.";
586 }
else if(!quoted && isspace(str[i]))
break;
587 else current_val.push_back(str[i++]);
590 _error <<
"unterminated quotes.";
593 if(str[i] && !isspace(str[i])) {
594 _error <<
"expected whitespace after value token.";
597 if(!current_val.size()) {
598 _error <<
"empty value string.";
601 std::string key = std::string(current_key.data(), current_key.size());
602 std::string val = std::string(current_val.data(), current_val.size());
604 if(keyvals.find(key) != keyvals.end())
606 _error <<
"duplicate key encountered: '" << key <<
"'";
620 const int BUFSIZE = 1024;
627 while(!(eof = (fgets(line, BUFSIZE, file) == NULL)))
630 ln =
trim(line, BUFSIZE);
633 if(!ln.length())
continue;
638 fprintf (stderr,
"Error, nested <owl> tag encountered. Aborting...\n");
644 }
else if (ln ==
"</owl>") {
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());
656 fprintf (stderr,
"Error, </owl> tag without <owl> tag. Aborting...\n");
663 int e = parser.parse_kv(ln);
665 fprintf(stderr,
"Error at character %d.\n", e);
670 if(parser.contains(
"sensor")) {
673 if(parser.parse(info))
676 if(
smgr->get_by_sensor(info.sensor_id))
678 fprintf(stderr,
"duplicate sensor defined: %d\n", info.sensor_id);
685 fprintf(stderr,
"%s\n", parser.error().c_str());
690 if(parser.pop(
"device",
device) == 2)
692 fprintf(stderr,
"error reading value for key 'device'\n");
695 if(parser.pop_bool(
"drop_frames",
drop_frames) == 2)
697 fprintf(stderr,
"error reading value for key 'drop_frames'\n");
700 if(parser.pop_bool(
"debug",
debug) == 2)
702 fprintf(stderr,
"error reading value for key 'debug'\n");
707 std::string new_options = parser.join();
708 if(new_options.length())
options += (
options.length()?
" ":
"") + parser.join();
712 if(eof) fprintf(stderr,
"Unexpected end of file.\n");
713 else fprintf(stderr,
"Unable to parse line: \"%s\"\n", ln.c_str());
721 if(!
context.isOpen())
return false;
723 context.streaming(enable ? 1 : 0);
724 printf(
"streaming: %d\n", enable);
726 if(
context.lastError().length())
728 fprintf(stderr,
"owl error: %s\n",
context.lastError().c_str());
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);
763 if(m.cond <= 0)
return;
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);
789 if(r.cond <= 0)
return;
806 fprintf(stderr,
"PhaseSpace: cannot write message: tossing\n");
817 fprintf(stderr,
"error: sensor %d exceeds max button count\n", sensor);
826 printf(
"[debug] button %d 0x%x\n",
d_sensor, value);
839 printf(
"[debug] analog button %d 0x%x\n",
d_sensor, value);
846const A*
find(
int id,
size_t& hint, std::vector<A> &data)
848 if(hint >= data.size() ||
id != data[hint].id)
850 for(
size_t i = 0; i < data.size(); i++)
852 const A &d = data[i];
867 if(!
context.isOpen())
return 0;
875 const OWL::Event *
event = NULL;
878 const OWL::Event *e = NULL;
882 if(e->type_id() == OWL::Type::FRAME)
event = e;
883 else if(e->type_id() == OWL::Type::ERROR)
887 fprintf(stderr,
"owl error event: %s\n", err.c_str());
888 if(e->name() ==
"fatal")
890 fprintf(stderr,
"stopping...\n");
896 else if(e->type_id() == OWL::Type::BYTE)
898 std::string s; e->get(s);
899 printf(
"%s: %s\n", e->name(), s.c_str());
901 if(strcmp(e->name(),
"done") == 0)
904 if(maxiter && ++count >= maxiter)
break;
915 lldiv_t divresult = lldiv(event->time(), 1000000);
925 OWL::Markers markers;
928 for(
const OWL::Event *e = event->begin(); e !=
event->end(); e++)
930 if(e->type_id() == OWL::Type::MARKER)
932 else if(e->type_id() == OWL::Type::RIGID)
936 int slave =
context.property<
int>(
"slave");
937 std::vector<const OWL::Marker*> reported_markers;
938 std::vector<const OWL::Rigid*> reported_rigids;
941 for(Sensors::iterator s =
smgr->begin(); s !=
smgr->end(); s++)
945 case OWL::Type::MARKER:
947 const OWL::Marker *m = find<OWL::Marker>(s->id, s->search_hint, markers);
950 if(slave) reported_markers.push_back(m);
954 case OWL::Type::RIGID:
956 const OWL::Rigid *r = find<OWL::Rigid>(s->id, s->search_hint, rigids);
959 if(slave) reported_rigids.push_back(r);
969 if(s->sensor_id > sensor) sensor = s->sensor_id;
976 if(sensor < 1000) sensor = 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())
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())
989 if(
context.lastError().length())
991 printf(
"owl error: %s\n",
context.lastError().c_str());
995 if(!
context.property<
int>(
"initialized"))
998 return (markers.size() || rigids.size()) ? 1 : 0;
1006 if(thistracker->
debug) {
1007 printf(
"[debug] vrpn_Tracker_PhaseSpace::handle_update_rate_request\n");
1009 vrpn_float64 update_rate = 0;
1011 thistracker->
context.frequency((
float) update_rate);
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...
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...
Analog server that can scale and clip its range to -1..1.
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 ...
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()
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)
virtual int get_report(void)
void report_marker(vrpn_int32 sensor, const OWL::Marker &m)
void report_rigid(vrpn_int32 sensor, const OWL::Rigid &r, bool is_stylus=false)
~vrpn_Tracker_PhaseSpace()
bool enableStreaming(bool enable)
virtual void send_report(void)
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_int32 update_rate_id
This structure is what is passed to a vrpn_Connection message callback.
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)
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)