vrpn_tracker.cpp

#include <iostream>
#include <algorithm>
#include <functional>

#include "vrpn_Tracker.h"
#include "quat.h"

#include "vrpn_tracker.hpp"

namespace microcart
{
	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;
};

void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
		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;
						(*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);
			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;
		}
	}
}