vrpn_Tracker_GPS::vrpn_Tracker_GPS(const char *name,
vrpn_Connection *c,
const char *port,
long baud,
int utmFlag,
int testFileFlag,
const char* startSentence) :
vrpn_Tracker_Serial(name,c,port,baud)
{
sprintf(testfilename, "GPS-data.txt");
// This allow people to set an external flag whether they want to use real GPS or not
if (testFileFlag == 1) {
//MessageBox(NULL, "CFG file flag set to use sample GPS data","FYI",0);
testfile = fopen(testfilename,"r"); //comment this line out to get real data
} else {
//MessageBox(NULL, temp,"Live GPS data",0);
testfile = NULL;
}
useUTM = utmFlag;
if (strlen(startSentence) > 0) {
nmeaParser.setStartSentence((char*)startSentence);
}
// Set the hardware flow-control on the serial line in case
// the GPS unit requires it (some do).
if (serial_fd >= 0) {
vrpn_set_rts(serial_fd);
}
register_server_handlers(); //-eb
}
vrpn_Tracker_DeadReckoning_Rotation::vrpn_Tracker_DeadReckoning_Rotation(
std::string myName, vrpn_Connection *c, std::string origTrackerName,
vrpn_int32 numSensors, vrpn_float64 predictionTime, bool estimateVelocity)
: vrpn_Tracker_Server(myName.c_str(), c, numSensors)
, d_estimateVelocity(estimateVelocity)
{
// Do the things all tracker servers need to do.
num_sensors = numSensors;
register_server_handlers();
// Set values that don't need to be set in the list above, so that we
// don't worry about out-of-order warnings from the compiler in case we
// adjust the order in the header file in the future.
d_predictionTime = predictionTime;
d_numSensors = numSensors;
// If the name of the tracker we're using starts with a '*' character,
// we use our own connection to talk with it. Otherwise, we open a remote
// tracker with the specified name.
if (origTrackerName[0] == '*') {
d_origTracker = new vrpn_Tracker_Remote(&(origTrackerName.c_str()[1]), c);
}
else {
d_origTracker = new vrpn_Tracker_Remote(origTrackerName.c_str());
}
// Initialize the rotational state of each sensor. There has not bee
// an orientation report, so we set its time to 0.
// Set up callback handlers for the pose and pose velocity messages,
// from which we will extract the orientation and orientation velocity
// information.
for (vrpn_int32 i = 0; i < numSensors; i++) {
q_type no_rotation = { 0, 0, 0, 1 };
RotationState rs;
q_copy(rs.d_rotationAmount, no_rotation);
rs.d_rotationInterval = 1;
rs.d_receivedAngularVelocityReport = false;
rs.d_lastReportTime.tv_sec = 0;
rs.d_lastReportTime.tv_usec = 0;
d_rotationStates.push_back(rs);
}
// Register handler for all sensors, so we'll be told the ID
d_origTracker->register_change_handler(this, handle_tracker_report);
d_origTracker->register_change_handler(this, handle_tracker_velocity_report);
}
vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode(const char * name, vrpn_Connection * c,
unsigned num_sensor,
const char * address_in,
unsigned port_in)
: vrpn_Tracker(name, c), m_num_sensor(num_sensor), m_handle(NULL)
{
//printf("vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode()\n");
register_server_handlers();
{
std::string address("127.0.0.1");
std::size_t port = 32079;
if (NULL != address_in) {
address.assign(address);
}
if (port_in > 0) {
port = port_in;
}
sampler_type * sampler = new sampler_type(address, port);
try {
if (StaticManager().attach(*sampler)) {
// Attempt to read a single sample from
// the stream. Use a 1 second blocking call.
sampler_type::data_type data;
if (sampler->get_data_block(data, 1)) {
m_handle = sampler;
} else {
fprintf(stderr, "MotionNode driver failed to start sampling, device not currently reading\n");
}
}
} catch (std::exception & e) {
fprintf(stderr, "%\n", e.what());
}
}
if (NULL != m_handle) {
vrpn_Tracker::status = vrpn_TRACKER_SYNCING;
} else {
vrpn_Tracker::status = vrpn_TRACKER_FAIL;
}
}
vrpn_Tracker_ViewPoint::vrpn_Tracker_ViewPoint(const char* name, vrpn_Connection* c, bool smoothedData) :
vrpn_Tracker(name, c), vrpn_Analog(name, c), useSmoothedData(smoothedData)
{
// Check the DLL version
double version = VPX_GetDLLVersion();
if (VPX_VersionMismatch(VPX_SDK_VERSION)) {
fprintf(stderr, "vrpn_Tracker_ViewPoint::vrpn_Tracker_ViewPoint(): Warning, SDK version is %g, while DLL version is %g \n", version, VPX_SDK_VERSION);
}
else {
printf("vrpn_Tracker_ViewPoint::vrpn_Tracker_ViewPoint(): SDK version %g matches DLL version %g \n", version, VPX_SDK_VERSION);
}
// Two sensors, one for each eye
vrpn_Tracker::num_sensors = 2;
// Currently 3 analog channels per eye
const int channels_per_eye = 3;
// Total number of channels is two times the number of channels per eye
vrpn_Analog::num_channel = channels_per_eye * 2;
// VRPN stuff
register_server_handlers();
}
vrpn_Tracker_InterSense::vrpn_Tracker_InterSense(const char *name,
vrpn_Connection *c,
int commPort, const char *additional_reset_commands,
int is900_timestamps, int reset_at_start) :
vrpn_Tracker(name,c), do_is900_timestamps(is900_timestamps),
m_reset_at_start(reset_at_start)
{
#ifdef VRPN_INCLUDE_INTERSENSE
char errStr[1024];
int i;
register_server_handlers();
if (additional_reset_commands == NULL) {
sprintf(add_reset_cmd, "");
} else {
vrpn_strcpy(add_reset_cmd, additional_reset_commands);
}
// Initially, set to no buttons or analogs on the stations. The
// methods to add buttons and analogs must be called to add them.
for (i = 0; i < ISD_MAX_STATIONS; i++) {
is900_buttons[i] = NULL;
is900_analogs[i] = NULL;
}
m_CommPort = commPort;
m_Handle = ISD_OpenTracker(NULL, commPort, FALSE, FALSE);
if(m_Handle == -1)
{
sprintf(errStr,"Failed to open tracker '%s' on COM%d: ISLIB_OpenTracker returned -1",name,commPort);
status = vrpn_TRACKER_FAIL;
return;
}
// ISD_TRACKER_INFO_TYPE trackerInfo;
ISD_GetTrackerConfig(m_Handle,&m_TrackerInfo,FALSE);
for (i = 0; i < ISD_MAX_STATIONS; i++) {
if (set_sensor_output_format(i)) {
sprintf(errStr,"Failed to reset sensor %d on tracker '%s' on COM%d",i, name,commPort);
status = vrpn_TRACKER_FAIL;
return;
}
}
//what is the update rate of this tracker?
//we might want to set the update rate of the mainloop to based on this value.
//for now we just print it out.
getTrackerInfo(errStr);
vrpn_gettimeofday(×tamp, NULL);
VRPN_MSG_INFO(errStr);
fprintf(stderr,errStr);
status = vrpn_TRACKER_SYNCING;
#else
fprintf(stderr,"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
status = vrpn_TRACKER_FAIL;
#endif
}
vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode(const char *name,
vrpn_Connection *c,
unsigned num_sensor,
const char *address,
unsigned port)
: vrpn_Tracker(name, c), m_num_sensor(num_sensor), m_handle(NULL)
{
#if defined(TRACKER_MOTIONNODE_TEST)
printf("vrpn_Tracker_MotionNode {\n");
#endif // TRACKER_MOTIONNODE_TEST
register_server_handlers();
{
std::string remote_address = "127.0.0.1";
unsigned remote_port = 32079;
if (NULL != address) {
remote_address = address;
}
if (port > 0) {
remote_port = port;
}
sampler_type *sampler = NULL;
try { sampler = new sampler_type(address, port); }
catch (...) {
vrpn_Tracker::status = vrpn_TRACKER_FAIL;
return;
}
// Attempt to read a single sample from
// the stream.
for (int i=0; i<4; i++) {
sampler_type::data_type data;
if (sampler->get_data_block(data, m_num_sensor) && !data.empty()) {
m_handle = sampler;
sampler = NULL;
break;
}
}
if (NULL == m_handle) {
fprintf(stderr, "MotionNode driver failed to start sampling, device not currently reading\n");
}
#if defined(TRACKER_MOTIONNODE_TEST)
else {
printf(
"Connected to Motion data service at \"%s:%d\"\n",
remote_address.c_str(), remote_port);
}
#endif // TRACKER_MOTIONNODE_TEST
// Clean up resources now if the sampler was never
// copied into this object.
if ((NULL == m_handle) && (NULL != sampler)) {
try {
delete sampler;
} catch (...) {
fprintf(stderr, "vrpn_Tracker_MotionNode::vrpn_Tracker_MotionNode(): delete failed\n");
return;
}
}
}
if (NULL != m_handle) {
vrpn_Tracker::status = vrpn_TRACKER_SYNCING;
} else {
vrpn_Tracker::status = vrpn_TRACKER_FAIL;
}
#if defined(TRACKER_MOTIONNODE_TEST)
printf("} vrpn_Tracker_MotionNode, status=%d\n", vrpn_Tracker::status);
#endif // TRACKER_MOTIONNODE_TEST
}
