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:
950 if(slave) reported_markers.push_back(m);
954 case OWL::Type::RIGID:
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...
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)
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
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
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)