Commit b174ab89 authored by mkelly2's avatar mkelly2

Reverting vrpn changes

parent 21c3977a
......@@ -138,6 +138,11 @@ static void cb(struct ucart_vrpn_TrackerData * td) {
int main(int argc, char **argv)
{
if (NUM_TRACKABLES != 1) {
printf("Error: Only 1 trackable is supported\n");
exit(-1);
}
int activity;
int i,j;
......@@ -279,13 +284,13 @@ int main(int argc, char **argv)
}
//TODO - remove tracker instance
//tracker = trackables[0].tracker;
printf("Attempting to Start VRPN thread... ");
if (ucart_vrpn_start()) {
warnx("Failed\n");
}
else {
printf("Success\n");
}
//printf("Attempting to Start VRPN thread... ");
//if (ucart_vrpn_start()) {
// warnx("Failed\n");
//}
//else {
// printf("Success\n");
//}
} else {
printf("Ignoring VRPN information...\n");
}
......@@ -344,11 +349,11 @@ int main(int argc, char **argv)
// Free VRPN trackers and stop VRPN Tracker Thread
if(!getenv(NOVRPN_ENV)){
if (ucart_vrpn_stop()) {
warnx("VRPN thread stop failed...\n");
} else {
printf("VRPN thread stopped\n");
}
//if (ucart_vrpn_stop()) {
// warnx("VRPN thread stop failed...\n");
//} else {
// printf("VRPN thread stopped\n");
//}
for (i = 0; i < NUM_TRACKABLES; i++) {
ucart_vrpn_tracker_freeInstance(trackables[i].tracker);
}
......@@ -407,10 +412,10 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
}
for (int i = 0; i < NUM_TRACKABLES; i++) {
if (strcmp(trackables[i].server_name, info->server_name) == 0) {
//if (strcmp(trackables[i].server_name, info->server_name) == 0) {
writeQuadIndex(packet, psize, i);
break;
}
// break;
//}
}
}
......
#include <iostream>
#include <algorithm>
#include <functional>
#include <cstring>
#include "vrpn_Tracker.h"
#include "quat.h"
#include "vrpn_tracker.hpp"
static microcart::Tracker_handler handler;
#include "vrpn_tracker.hpp"
namespace microcart
{
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
Tracker_handler::Tracker_handler() :
stop_flag(0),
num_trackers(0)
{
stop_flag = 0;
num_trackers = 0;
}
Tracker_handler::~Tracker_handler() {}
void Tracker_handler::vrpn_start() {
stop_flag = 0;
vrpn_thread = std::thread(&Tracker_handler::vrpn_loop, this);
}
void Tracker_handler::vrpn_stop() {
stop_flag = 1;
vrpn_thread.join();
}
TrackerData::TrackerData(std::string server) :
x(0.0), y(0.0), z(0.0),
pitch(0.0), roll(0.0), yaw(0.0),
fps(0.0), timestamp()
{
strncpy(server_name, server.c_str(), 256);
}
TrackerData::TrackerData(const char * server) :
x(0.0), y(0.0), z(0.0),
pitch(0.0), roll(0.0), yaw(0.0),
fps(0.0), timestamp()
{
strncpy(server_name, server, 256);
}
Tracker::Tracker(std::string server) :
remote(server.c_str()),
trackerData(server)
{
}
Tracker::Tracker(const char * server) :
remote(server),
trackerData(server)
{
remote.register_change_handler(this, vrpn_cb);
}
Tracker::~Tracker()
{
//TODO - may not be needed
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
}
}
const struct TrackerData Tracker::getData(void)
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
return trackerData;
}
void Tracker_handler::vrpn_loop(void)
{
while (1) {
for (int i = 0; i < NUM_TRACKABLES; i++) {
trackers[i]->remote.mainloop();
{
std::lock_guard<std::mutex> guard(trackers[i]->vrpn_mutex);
if (stop_flag) {
return;
}
}
}
}
}
void Tracker::callback(const vrpn_TRACKERCB t)
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
q_vec_type euler;
q_to_euler(euler, t.quat);
trackerData.x = (double) t.pos[0];
trackerData.y = (double) t.pos[1];
trackerData.z = (double) t.pos[2];
trackerData.roll = (double) euler[2];
trackerData.pitch = (double) euler[1];
trackerData.yaw = (double) euler[0];
timeval elapsed_time;
timersub(&t.msg_time, &trackerData.timestamp, &elapsed_time);
trackerData.timestamp = t.msg_time;
double elapsed_time_usec = (double) elapsed_time.tv_usec +
((1000000.0) * (double) elapsed_time.tv_sec);
trackerData.fps = 1.0 / elapsed_time_usec;
auto td = trackerData;
for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
(*i)(td);
}
}
void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
{
cb_vector.push_back(cb);
}
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t)
{
Tracker * inst = (Tracker *)(param);
inst->callback(t);
}
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
TrackerData::TrackerData() :
x(0.0), y(0.0), z(0.0),
pitch(0.0), roll(0.0), yaw(0.0),
fps(0.0), timestamp()
{
}
Tracker::Tracker(std::string server) :
remote(server.c_str()),
stop_flag(0),
trackerData()
{
}
Tracker::Tracker(const char * server) :
remote(server),
stop_flag(0),
trackerData()
{
remote.register_change_handler(this, vrpn_cb);
stop_flag = 0;
vrpn_thread = std::thread(&Tracker::vrpn_loop, this);
}
Tracker::~Tracker()
{
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
stop_flag = 1;
}
vrpn_thread.join();
}
const struct TrackerData Tracker::getData(void)
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
return trackerData;
}
void Tracker::vrpn_loop(void)
{
while (1) {
remote.mainloop();
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
if (stop_flag) {
return;
}
}
}
}
void Tracker::callback(const vrpn_TRACKERCB t)
{
std::lock_guard<std::mutex> guard(vrpn_mutex);
q_vec_type euler;
q_to_euler(euler, t.quat);
trackerData.x = (double) t.pos[0];
trackerData.y = (double) t.pos[1];
trackerData.z = (double) t.pos[2];
trackerData.roll = (double) euler[2];
trackerData.pitch = (double) euler[1];
trackerData.yaw = (double) euler[0];
timeval elapsed_time;
timersub(&t.msg_time, &trackerData.timestamp, &elapsed_time);
trackerData.timestamp = t.msg_time;
double elapsed_time_usec = (double) elapsed_time.tv_usec +
((1000000.0) * (double) elapsed_time.tv_sec);
trackerData.fps = 1.0 / elapsed_time_usec;
auto td = trackerData;
for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
(*i)(td);
}
}
void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
{
cb_vector.push_back(cb);
}
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t)
{
Tracker * inst = (Tracker *)(param);
inst->callback(t);
}
}
/* C Interface stuff */
struct ucart_vrpn_tracker {
microcart::Tracker * t;
microcart::Tracker * t;
};
void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
const microcart::TrackerData &td)
const microcart::TrackerData &td)
{
struct ucart_vrpn_TrackerData data;
data.x = td.x;
data.y = td.y;
data.z = td.z;
data.pitch = td.pitch;
data.roll = td.roll;
data.yaw = td.yaw;
data.fps = td.fps;
strncpy(data.server_name, td.server_name, 256);
(*cb)(&data);
struct ucart_vrpn_TrackerData data;
data.x = td.x;
data.y = td.y;
data.z = td.z;
data.pitch = td.pitch;
data.roll = td.roll;
data.yaw = td.yaw;
data.fps = td.fps;
(*cb)(&data);
}
extern "C"
{
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
const char * server)
{
try {
auto inst = new struct ucart_vrpn_tracker;
inst->t = new microcart::Tracker(server);
handler.trackers[handler.num_trackers] = inst->t;
handler.num_trackers++;
return inst;
} catch(...) {
return NULL;
}
}
void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst)
{
delete inst->t;
delete inst;
}
int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
void (*cb)(struct ucart_vrpn_TrackerData *))
{
try {
auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
inst->t->addCallback(new_cb);
} catch(...) {
return -1;
}
return 0;
}
int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker *inst,
struct ucart_vrpn_TrackerData *td)
{
try {
auto data = inst->t->getData();
td->x = data.x;
td->y = data.y;
td->z = data.z;
td->pitch = data.pitch;
td->roll = data.roll;
td->yaw = data.yaw;
strncpy(td->server_name, data.server_name, 256);
return 0;
} catch(...) {
return -1;
}
}
int ucart_vrpn_start() {
if (handler.num_trackers > 0) {
handler.vrpn_start();
return 0;
}
return 1;
}
int ucart_vrpn_stop() {
if (handler.stop_flag != 1) {
handler.vrpn_stop();
return 0;
}
return 1;
}
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
const char * server)
{
try {
auto inst = new struct ucart_vrpn_tracker;
inst->t = new microcart::Tracker(server);
return inst;
} catch(...) {
return NULL;
}
}
void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst)
{
delete inst->t;
delete inst;
}
int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
void (*cb)(struct ucart_vrpn_TrackerData *))
{
try {
auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
inst->t->addCallback(new_cb);
} catch(...) {
return -1;
}
return 0;
}
int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker *inst,
struct ucart_vrpn_TrackerData *td)
{
try {
auto data = inst->t->getData();
td->x = data.x;
td->y = data.y;
td->z = data.z;
td->pitch = data.pitch;
td->roll = data.roll;
td->yaw = data.yaw;
return 0;
} catch(...) {
return -1;
}
}
}
......@@ -13,8 +13,6 @@
#include <vector>
#include "vrpn_Tracker.h"
#include "config.h"
#endif
#include <sys/time.h>
......@@ -23,34 +21,31 @@
extern "C"
{
#endif
struct ucart_vrpn_tracker;
struct ucart_vrpn_TrackerData {
float x;
float y;
float z;
struct ucart_vrpn_tracker;
struct ucart_vrpn_TrackerData {
float x;
float y;
float z;
float pitch;
float roll;
float yaw;
float pitch;
float roll;
float yaw;
double fps;
struct timeval timestamp;
char server_name[256];
};
double fps;
struct timeval timestamp;
};
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
const char * server);
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
const char * server);
void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);
void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);
int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
void (*cb)(struct ucart_vrpn_TrackerData *));
int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
void (*cb)(struct ucart_vrpn_TrackerData *));
int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
struct ucart_vrpn_TrackerData * td);
int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
struct ucart_vrpn_TrackerData * td);
int ucart_vrpn_start();
int ucart_vrpn_stop();
#ifdef __cplusplus
}
#endif
......@@ -58,57 +53,46 @@ extern "C"
#ifdef __cplusplus
namespace microcart
{
struct TrackerData {
public:
float x;
float y;
float z;
float pitch;
float roll;
float yaw;
double fps;
timeval timestamp;
TrackerData();
};
class Tracker {
public:
const struct TrackerData getData(void);
void callback(const vrpn_TRACKERCB t);
Tracker(std::string server);
Tracker(const char * server);
~Tracker();
void addCallback(std::function<void(const TrackerData &)> cb);
private:
int stop_flag;
TrackerData trackerData;
std::thread vrpn_thread;
std::mutex vrpn_mutex;
vrpn_Tracker_Remote remote;
void vrpn_loop(void);
std::vector<std::function<void(const TrackerData &)>> cb_vector;
};
struct TrackerData {
public:
float x;
float y;
float z;
float pitch;
float roll;
float yaw;
double fps;
timeval timestamp;
char server_name[256];
TrackerData(const char * server);
TrackerData(std::string server);
};
class Tracker {
public:
const struct TrackerData getData(void);
void callback(const vrpn_TRACKERCB t);
Tracker(std::string server);
Tracker(const char * server);
~Tracker();
vrpn_Tracker_Remote remote;
std::mutex vrpn_mutex;
void addCallback(std::function<void(const TrackerData &)> cb);
private:
TrackerData trackerData;
std::vector<std::function<void(const TrackerData &)>> cb_vector;
};
class Tracker_handler {
public:
Tracker_handler();
~Tracker_handler();
void vrpn_start();
void vrpn_stop();
int num_trackers;
int stop_flag;
Tracker * trackers[NUM_TRACKABLES];
private:
std::thread vrpn_thread;
void vrpn_loop(void);
};
}
/* __cplusplus */
#endif
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment