Trajectory::Trajectory(
const std::vector<DeviceThreadReport> &reports //< Holds the values to fill in
, struct timeval start //< Defines 0 seconds
, int index //< Which value to use from the reports
, bool arrivalTime //< Use arrival time rather than reported time
)
{
if (index < 0) { return; }
// Insert all reports that have a value with the specified index.
// Their time is with respect to the base time.
for (size_t i = 0; i < reports.size(); i++) {
if (reports[i].values.size() > index) {
Entry e;
e.m_value = reports[i].values[index];
if (arrivalTime) {
e.m_time = vrpn_TimevalDurationSeconds(reports[i].arrivalTime, start);
} else {
e.m_time = vrpn_TimevalDurationSeconds(reports[i].sampleTime, start);
}
m_entries.push_back(e);
}
}
// Sort the resulting vector of values.
std::sort(m_entries.begin(), m_entries.end());
}
void vrpn_IMU_SimpleCombiner::mainloop()
{
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the analogs that are defined and the button
// so that we will get all of the values fresh.
if (d_acceleration.ana != NULL) { d_acceleration.ana->mainloop(); }
if (d_rotational_vel.ana != NULL) { d_rotational_vel.ana->mainloop(); }
if (d_magnetometer.ana != NULL) { d_magnetometer.ana->mainloop(); }
// Update the matrix based on the change in time since the last
// update and the current values.
struct timeval now;
vrpn_gettimeofday(&now, NULL);
double delta_t = vrpn_TimevalDurationSeconds(now, d_prev_update_time);
update_matrix_based_on_values(delta_t);
d_prev_update_time = now;
// See if it has been long enough since our last report.
// If so, generate a new one.
double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (interval >= d_update_interval) {
// pack and deliver tracker report with info from the current matrix;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"cannot write pose message: tossing\n");
}
len = encode_vel_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
velocity_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"cannot write velocity message: tossing\n");
}
} else {
fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
"No valid connection\n");
}
// We just sent a report, so reset the time
d_prevtime = now;
}
}
void VRPN_CALLBACK vrpn_Tracker_FilterOneEuro::handle_tracker_update(void *userdata, const vrpn_TRACKERCB info)
{
// Get pointer to the object we're dealing with.
vrpn_Tracker_FilterOneEuro *me = static_cast<vrpn_Tracker_FilterOneEuro *>(userdata);
// See if this sensor is within our range. If not, we ignore it.
if (info.sensor >= me->d_channels) {
return;
}
// Filter the position and orientation and then report the filtered value
// for this channel. Keep track of the delta-time, and update our current
// time so we get the right one next time.
double dt = vrpn_TimevalDurationSeconds(info.msg_time, me->d_last_report_times[info.sensor]);
if (dt <= 0) { dt = 1; } // Avoid divide-by-zero in case of fluke.
vrpn_float64 pos[3];
vrpn_float64 quat[4];
memcpy(pos, info.pos, sizeof(pos));
memcpy(quat, info.quat, sizeof(quat));
const vrpn_float64 *filtered = me->d_filters[info.sensor].filter(dt, pos);
q_vec_copy(me->pos, filtered);
const double *q_filtered = me->d_qfilters[info.sensor].filter(dt, quat);
q_normalize(me->d_quat, q_filtered);
me->timestamp = info.msg_time;
me->d_sensor = info.sensor;
me->d_last_report_times[info.sensor] = info.msg_time;
// Send the filtered report.
char msgbuf[512];
int len = me->encode_to(msgbuf);
if (me->d_connection->pack_message(len, me->timestamp, me->position_m_id,
me->d_sender_id, msgbuf,vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
}
}
void vrpn_Tracker_ThalmicLabsMyo::_report_lock() {
vrpn_gettimeofday(&_timestamp, NULL);
double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
vrpn_Button::timestamp = _timestamp;
buttons[0] = _locked;
vrpn_Button::report_changes();
}
void vrpn_Tracker_ThalmicLabsMyo::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& rotation)
{
if (myo != _myo)
return;
if (!d_connection) {
return;
}
vrpn_gettimeofday(&_timestamp, NULL);
double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
vrpn_Tracker::timestamp = _timestamp;
vrpn_Analog::timestamp = _timestamp;
d_quat[0] = rotation.x();
d_quat[1] = rotation.y();
d_quat[2] = rotation.z();
d_quat[3] = rotation.w();
// do the same as analog, with euler angles (maybe offset from when OnArmSync?)
q_vec_type euler;
q_to_euler(euler, d_quat);
channel[ANALOG_ROTATION_X] = euler[Q_ROLL];
channel[ANALOG_ROTATION_Y] = euler[Q_PITCH];
channel[ANALOG_ROTATION_Z] = euler[Q_YAW];
char msgbuf[1000];
int len = vrpn_Tracker::encode_to(msgbuf);
if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
}
_analogChanged = true;
}
int main(void)
{
struct timeval last_time, this_time;
double skip;
vrpn_gettimeofday(&last_time, NULL);
printf("Current time in seconds:microseconds = %lu:%lu\n",
last_time.tv_sec, last_time.tv_usec);
printf("Checking for monotonicity and step size\n");
printf(" (should be no further output if things are working perfectly)\n");
fflush(stdout);
vrpn_gettimeofday(&last_time, NULL);
while (true) {
vrpn_gettimeofday(&this_time, NULL);
skip = vrpn_TimevalDurationSeconds(this_time, last_time);
if (skip > 200e-6) {
printf("Skipped forward %lg microseconds\n", skip*1e6);
}
if (skip < 0) {
printf("** Backwards %lg microseconds\n", skip*1e6);
}
last_time = this_time;
}
return 0;
}
void vrpn_Tracker_ThalmicLabsMyo::onAccelerometerData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& accel)
{
if (myo != _myo)
return;
if (!d_connection) {
return;
}
vrpn_gettimeofday(&_timestamp, NULL);
double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
vrpn_Tracker::timestamp = _timestamp;
vrpn_Analog::timestamp = _timestamp;
acc[0] = accel[0];
acc[1] = accel[1];
acc[2] = accel[2];
// same for analog
channel[ANALOG_ACCEL_X] = accel[0];
channel[ANALOG_ACCEL_Y] = accel[1];
channel[ANALOG_ACCEL_Z] = accel[2];
char msgbuf[1000];
int len = encode_acc_to(msgbuf);
if (d_connection->pack_message(len, _timestamp, accel_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "Thalmic Lab's myo tracker: can't write message: tossing\n");
}
_analogChanged = true;
}
void vrpn_Tracker_ButtonFly::mainloop()
{
int i;
struct timeval now;
double interval; // How long since the last report, in secs
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the buttons that are defined and the analog
// scale values if they are defined so that we will get all of
// the values fresh.
for (i = 0; i < d_num_axes; i++) {
d_axes[i].btn->mainloop();
}
if (d_vel_scale != NULL) { d_vel_scale->mainloop(); };
if (d_rot_scale != NULL) { d_rot_scale->mainloop(); };
// See if it has been long enough since our last report.
// If so, generate a new one.
vrpn_gettimeofday(&now, NULL);
interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (shouldReport(interval)) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
d_prevtime = now;
// Set the time on the report to now.
vrpn_Tracker::timestamp = now;
// pack and deliver tracker report;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker ButtonFly: cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker ButtonFly: No valid connection\n");
}
// We're not always sending reports, but we still want to
// update the matrix so that we don't integrate over
// too long a timespan when we do finally report a change.
} else if (interval >= d_update_interval) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
d_prevtime = now;
}
}
void vrpn_Oculus::mainloop()
{
vrpn_gettimeofday(&d_timestamp, NULL);
// See if it has been long enough to send another keep-alive
if (vrpn_TimevalDurationSeconds(d_timestamp, d_lastKeepAlive) >=
d_keepAliveSeconds) {
writeKeepAlive();
d_lastKeepAlive = d_timestamp;
}
update();
server_mainloop();
}
void vrpn_Tracker_DeadReckoning_Rotation::handle_tracker_report(void *userdata,
const vrpn_TRACKERCB info)
{
// Find the pointer to the class that registered the callback and get a
// reference to the RotationState we're supposed to be using.
vrpn_Tracker_DeadReckoning_Rotation *me =
static_cast<vrpn_Tracker_DeadReckoning_Rotation *>(userdata);
if (info.sensor >= me->d_numSensors) {
me->send_text_message(vrpn_TEXT_WARNING)
<< "Received tracker message from sensor " << info.sensor
<< " but I only have " << me->d_numSensors
<< "sensors. Discarding.";
return;
}
vrpn_Tracker_DeadReckoning_Rotation::RotationState &state =
me->d_rotationStates[info.sensor];
if (!state.d_receivedAngularVelocityReport && me->d_estimateVelocity) {
// If we have not received any velocity reports, then we estimate
// the angular velocity using the last report (if any). The new
// combined rotation T3 = T2 * T1, where T2 is the difference in
// rotation between the last time (T1) and now (T3). We want to
// solve for T2 (so we can keep applying it going forward). We
// find it by right-multiuplying both sides of the equation by
// T1i (inverse of T1): T3 * T1i = T2.
if (state.d_lastReportTime.tv_sec != 0) {
q_type inverted;
q_invert(inverted, state.d_lastOrientation);
q_mult(state.d_rotationAmount, info.quat, inverted);
state.d_rotationInterval = vrpn_TimevalDurationSeconds(
info.msg_time, state.d_lastReportTime);
// If we get a zero or negative rotation interval, we're
// not going to be able to handle it, so we set things back
// to no rotation over a unit-time interval.
if (state.d_rotationInterval < 0) {
state.d_rotationInterval = 1;
q_make(state.d_rotationAmount, 0, 0, 0, 1);
}
}
}
// Keep track of the position, orientation and time for the next report
q_vec_copy(state.d_lastPosition, info.pos);
q_copy(state.d_lastOrientation, info.quat);
state.d_lastReportTime = info.msg_time;
// We have new data, so we send a new prediction.
me->sendNewPrediction(info.sensor);
}
void vrpn_Tracker_ThalmicLabsMyo::onGyroscopeData(myo::Myo* myo, uint64_t timestamp, const myo::Vector3<float>& gyro)
{
if (myo != _myo)
return;
vrpn_gettimeofday(&_timestamp, NULL);
double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
vrpn_Analog::timestamp = _timestamp;
channel[ANALOG_GYRO_X] = gyro[0];
channel[ANALOG_GYRO_Y] = gyro[1];
channel[ANALOG_GYRO_Z] = gyro[2];
_analogChanged = true;
}
void vrpn_Tracker_ThalmicLabsMyo::onPose(myo::Myo* myo, uint64_t timestamp, myo::Pose pose)
{
if (myo != _myo)
return;
vrpn_gettimeofday(&_timestamp, NULL);
double dt = vrpn_TimevalDurationSeconds(_timestamp, vrpn_Button::timestamp);
vrpn_Button::timestamp = _timestamp;
// std::cout << "Myo switched to pose " << pose.toString() << "." << std::endl;
buttons[0] = _locked;
// reset all buttons to 0. Maybe we should only do this if rest is on ?
for (int i = 1; i < libmyo_num_poses + 1; ++i)
buttons[i] = 0;
buttons[pose.type() + 1] = 1;
vrpn_Button::report_changes();
}
// Request some log files and wait up to a second for the report of these
// files. Return true if we got a report and the names match.
bool test_logfile_names(const char *local_in, const char *local_out,
const char *remote_in, const char *remote_out)
{
struct timeval start;
struct timeval now;
// Mark no report and the request logging with the specified
// parameters.
g_got_report = false;
if (!g_logger->send_logging_request(local_in, local_out, remote_in, remote_out)) {
fprintf(stderr, "test_logfile_names: Logging request send failed\n");
return false;
}
// Mainloop the logger for up to five seconds waiting for a response.
// If we don't get a response, this is a failure.
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
vrpn_SleepMsecs(1);
} while ( !g_got_report && (vrpn_TimevalDurationSeconds(now, start) < 5.0));
if (!g_got_report) {
fprintf(stderr, "test_logfile_names: Timeout waiting for report of logging from server\n");
return false;
}
// Check to see if the names are the same. Return true if they all are.
if ( (strcmp(g_local_in, local_in) == 0) &&
(strcmp(g_local_out, local_out) == 0) &&
(strcmp(g_remote_in, remote_in) == 0) &&
(strcmp(g_remote_out, remote_out) == 0) ) {
return true;
} else {
return false;
}
}
void vrpn_Tracker_WiimoteHead::mainloop() {
struct timeval now;
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() the wiimote to get fresh values
if (d_ana != NULL) {
d_ana->mainloop();
}
// See if we have new data, or if it has been too long since our last
// report. Send a new report in either case.
vrpn_gettimeofday(&now, NULL);
double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (_should_report(interval)) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_pose();
report();
}
}
void vrpn_IMU_Magnetometer::mainloop()
{
struct timeval now;
double interval; // How long since the last report, in secs
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the analogs that are defined
// so that we will get all of the values fresh.
if (d_vector.ana != NULL) { d_vector.ana->mainloop(); };
// Keep track of the minimum and maximum values of
// our reports.
for (size_t i = 0; i < 3; i++) {
if (d_vector.values[i] < d_mins[i]) {
d_mins[i] = d_vector.values[i];
}
if (d_vector.values[i] > d_maxes[i]) {
d_maxes[i] = d_vector.values[i];
}
}
// See if it has been long enough since our last report.
// If so, generate a new one.
vrpn_gettimeofday(&now, NULL);
interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (interval >= d_update_interval) {
// Set the time on the report
timestamp = d_vector.time;
// Compute the unit vector by finding the normalized
// distance between the min and max for each axis and
// converting it to the range (-1,1) and then normalizing
// the resulting vector. Watch out for min and max
// being the same.
for (size_t i = 0; i < 3; i++) {
double diff = d_vector.values[i] - d_mins[i];
if (diff == 0) {
channel[i] = 0;
}
else {
channel[i] = -1 + 2 * diff / (d_maxes[i] - d_mins[i]);
}
}
double len = sqrt( channel[0] * channel[0] +
channel[1] * channel[1] + channel[2] * channel[2]);
if (len == 0) {
channel[0] = 1;
channel[1] = 0;
channel[2] = 0;
} else {
channel[0] /= len;
channel[1] /= len;
channel[2] /= len;
}
// pack and deliver analog unit vector report;
if (d_connection) {
if (d_report_changes) {
vrpn_Analog_Server::report_changes();
} else {
vrpn_Analog_Server::report();
}
} else {
fprintf(stderr,"vrpn_IMU_Magnetometer: "
"No valid connection\n");
}
// We just sent a report, so reset the time
d_prevtime = now;
}
// We're not always sending reports, but we still want to
// update the interval clock so that we don't integrate over
// too long a timespan when we do finally report a change.
if (interval >= d_update_interval) {
d_prevtime = now;
}
}
void vrpn_Tracker_AnalogFly::mainloop()
{
struct timeval now;
double interval; // How long since the last report, in secs
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() all of the analogs that are defined and the button
// so that we will get all of the values fresh.
if (d_x.ana != NULL) { d_x.ana->mainloop(); };
if (d_y.ana != NULL) { d_y.ana->mainloop(); };
if (d_z.ana != NULL) { d_z.ana->mainloop(); };
if (d_sx.ana != NULL) { d_sx.ana->mainloop(); };
if (d_sy.ana != NULL) { d_sy.ana->mainloop(); };
if (d_sz.ana != NULL) { d_sz.ana->mainloop(); };
if (d_reset_button != NULL) { d_reset_button->mainloop(); };
if (d_clutch_button != NULL) { d_clutch_button->mainloop(); };
// See if it has been long enough since our last report.
// If so, generate a new one.
vrpn_gettimeofday(&now, NULL);
interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (shouldReport(interval)) {
// Set the time on the report to now, if not an absolute
// tracker. Absolute trackers have their time values set
// to match the time at which their analog devices gave the
// last report.
if (!d_absolute) {
vrpn_Tracker::timestamp = now;
}
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_matrix_based_on_values(interval);
// pack and deliver tracker report;
if (d_connection) {
char msgbuf[1000];
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, vrpn_Tracker::timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker AnalogFly: "
"cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker AnalogFly: "
"No valid connection\n");
}
// We just sent a report, so reset the time
d_prevtime = now;
}
// We're not always sending reports, but we still want to
// update the interval clock so that we don't integrate over
// too long a timespan when we do finally report a change.
if (interval >= d_update_interval) {
d_prevtime = now;
}
}
int main(int argc, char *argv[]) {
/* XXX Checking how well the two clocks track each other
unsigned long lsec, lusec;
unsigned long fsec, fusec;
long dsec, dusec;
int i;
for (i = 0; i < 10; i++) {
get_time_using_GetLocalTime(lsec, lusec);
get_time_using_ftime(fsec, fusec);
dsec = lsec - fsec;
dusec = lusec - fusec;
printf("L: %u:%u, F: %u:%u, Difference: %u:%ld\n",
lsec, lusec, fsec, fusec, dsec, dusec);
Sleep(1000);
}
*/
/* Check the behavior of the clock */
SYSTEMTIME sstart, stime; // System time in funky structure
FILETIME ftime; // Time in 100-nsec intervals since Jan 1 1601
LARGE_INTEGER tics; // ftime stored into a 64-bit quantity
GetLocalTime(&sstart);
stime = sstart;
while (stime.wSecond - sstart.wSecond < 2) {
GetLocalTime(&stime);
printf("Seconds %2d, Milliseconds %4d\n", static_cast<int>(stime.wSecond),
static_cast<int>(stime.wMilliseconds));
SystemTimeToFileTime(&stime, &ftime);
// Copy the data into a structure that can be treated as a 64-bit
// integer
tics.HighPart = ftime.dwHighDateTime;
tics.LowPart = ftime.dwLowDateTime;
printf(" Converted to 64-bit: %llu\n", tics.QuadPart);
// Convert the 64-bit time into seconds and microseconds since July 1
// 1601
unsigned long sec, usec;
sec = (long)(tics.QuadPart / 10000000L);
usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
printf(" Converted to secs and usecs: %6lu:%6lu\n", sec, usec);
}
/* Checking the vrpn_gettimeofday() function for monotonicity and step size */
struct timeval last_time, this_time;
double skip;
vrpn_gettimeofday(&last_time, NULL);
printf("Checking for monotonicity and step size\n");
printf(" (should be no further output if things are working perfectly)\n");
while (true) {
vrpn_gettimeofday(&this_time, NULL);
skip = vrpn_TimevalDurationSeconds(this_time, last_time);
if (skip > 200e-6) {
printf("Skipped forward %lg microseconds\n", skip*1e6);
}
if (skip < 0) {
printf("** Backwards %lg microseconds\n", skip*1e6);
}
last_time = this_time;
}
return 0;
}
int main (int argc, char * argv [])
{
const char * devicename = "Logger0@localhost";
struct timeval start;
struct timeval now;
int ret = 0;
// parse args
if (argc == 1) {
// Fine, use defaults.
} else if (argc > 2) {
fprintf(stderr, "Usage: %s Device_name\n"
" Device_name: VRPN name of data source to contact\n"
" example: Logger0@localhost\n",
argv[0]);
exit(0);
} else {
devicename = argv[1];
}
// Open the logger and set up its callback handler.
fprintf(stderr, "Logger's name is %s.\n", devicename);
g_logger = new vrpn_Auxiliary_Logger_Remote (devicename);
g_logger->register_report_handler(NULL, handle_log_report);
// Main loop for half a second to get things started on the
// connection.
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 0.5);
// Try to create four named log files. Wait for a second after
// creation to give it something to log.
if (!test_logfile_names("temp1_local_in", "temp1_local_out",
"temp1_remote_in", "temp1_remote_out") ) {
fprintf(stderr,"Error creating first set of logs\n");
ret = -1;
}
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 5.0);
// send a log-file-name request
g_logger->send_logging_status_request( );
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 5.0);
// Try to create blank log files (no log should be made). Wait for a second after
// creation to give it something to log.
if (!test_logfile_names("", "", "", "") ) {
fprintf(stderr,"Error turning off logs\n");
ret = -1;
}
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 5.0);
// Try to create just one named log file. Wait for a second after
// creation to give it something to log.
if (!test_logfile_names("temp2_local_in", "", "", "") ) {
fprintf(stderr,"Error creating second set of logs\n");
ret = -1;
}
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 5.0);
// Try to create blank log files (no log should be made). Wait for a second after
// creation to give it something to log.
if (!test_logfile_names("", "", "", "") ) {
fprintf(stderr,"Error turning off logs\n");
ret = -1;
}
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDurationSeconds(now, start) < 5.0);
// Done.
if (ret == 0) {
printf("Success!\n");
} else {
printf("Make sure that files with the requested names don't already exist.\n");
}
delete g_logger;
return ret;
} /* main */
int main(int argc, const char *argv[])
{
// Parse the command line.
size_t realParams = 0;
int count = 10;
bool arrivalTime = false;
for (size_t i = 1; i < argc; i++) {
if (argv[i] == std::string("-count")) {
if (++i > argc) {
std::cerr << "Error: -count parameter requires value" << std::endl;
Usage(argv[0]);
}
count = atoi(argv[i]);
if (count < 1) {
std::cerr << "Error: -count parameter must be >= 1, found "
<< argv[i] << std::endl;
Usage(argv[0]);
}
} else if (argv[i] == std::string("-arrivalTime")) {
arrivalTime = true;
} else if (argv[i][0] == '-') {
Usage(argv[0]);
} else switch (++realParams) {
case 1:
g_arduinoPortName = argv[i];
break;
case 2:
g_arduinoChannel = atoi(argv[i]);
break;
default:
Usage(argv[0]);
}
}
if (realParams != 2) {
Usage(argv[0]);
}
// Construct the thread to handle the photosensor
// reading from the Ardiuno.
DeviceThreadVRPNAnalog arduino(CreateStreamingServer);
//-----------------------------------------------------------------
// Wait until we get at least one report from the device
// or timeout. Make sure the report sizes are large enough
// to support the channels we're reading.
struct timeval start, now;
vrpn_gettimeofday(&start, NULL);
size_t arduinoCount = 0;
std::vector<DeviceThreadReport> r;
if (g_verbosity > 0) {
std::cout << "Waiting for reports from Arduino:" << std::endl;
}
double lastArduinoValue;
do {
r = arduino.GetReports();
if (r.size() > 0) {
if (r[0].values.size() <= g_arduinoChannel) {
std::cerr << "Report size from Arduino: " << r[0].values.size()
<< " is too small for requested channel: " << g_arduinoChannel << std::endl;
return -3;
}
lastArduinoValue = r.back().values[g_arduinoChannel];
}
arduinoCount += r.size();
vrpn_gettimeofday(&now, NULL);
} while ( (arduinoCount == 0)
&& (vrpn_TimevalDurationSeconds(now, start) < 20) );
if (arduinoCount == 0) {
std::cerr << "No reports from Arduino" << std::endl;
return -5;
}
//-----------------------------------------------------------------
// Construct a RenderManager to use to send images to the screen,
// and register the display update we'll use to make
// the images (black or white) to test the rendering latency.
if (g_verbosity > 0) {
std::cout << "Run an OSVR server for us to connect to and place"
<< " the photosensor in front of the screen at the location"
<< " whose latency you want to render." << std::endl;
}
osvr::clientkit::ClientContext context(
"com.reliasolve.RenderManagerLatencyTest");
osvr::renderkit::RenderManager *render =
osvr::renderkit::createRenderManager(context.get(),"OpenGL");
if ((render == nullptr) ||
(!render->doingOkay())) {
std::cerr << "Could not create RenderManager" << std::endl;
return 1;
}
// Set callback to handle setting up rendering in a display
render->SetDisplayCallback(SetupDisplay);
// Open the display and make sure this worked.
osvr::renderkit::RenderManager::OpenResults ret = render->OpenDisplay();
if (ret.status == osvr::renderkit::RenderManager::OpenStatus::FAILURE) {
std::cerr << "Could not open display" << std::endl;
delete render;
return 2;
}
//-----------------------------------------------------------------
// Wait a bit for any DirectMode switching/power on to happen.
vrpn_SleepMsecs(1000);
//-----------------------------------------------------------------
// Render a dark scene for half a second and then read the Arduino value
// to get a baseline for dark. Half a second is an arbitrary number that
// should be larger than the latency present in the system.
// Then render another dark scene right at the end so our vertical
// retrace timing should be correct for the bright scene.
g_red = g_green = g_blue = 0;
render->Render();
vrpn_SleepMsecs(500);
r = arduino.GetReports();
if (r.size() == 0) {
std::cerr << "Could not read Arduino value after dark rendering" << std::endl;
delete render;
return 3;
}
double dark = r.back().values[g_arduinoChannel];
if (g_verbosity > 1) {
std::cout << "Dark-screen photosensor value: " << dark << std::endl;
}
render->Render();
//-----------------------------------------------------------------
// Render a bright scene for half a second and then read the Arduino value
// to get a baseline for bright. Half a second is an arbitrary number that
// should be larger than the latency present in the system.
// Then render another bright scene right at the end so our vertical
// retrace timing should be correct for the bright scene.
g_red = g_green = g_blue = 1;
render->Render();
vrpn_SleepMsecs(500);
r = arduino.GetReports();
if (r.size() == 0) {
std::cerr << "Could not read Arduino value after bright rendering" << std::endl;
delete render;
return 4;
}
double bright = r.back().values[g_arduinoChannel];
if (g_verbosity > 1) {
std::cout << "Bright-screen photosensor value: " << bright << std::endl;
}
double threshold = (dark + bright) / 2;
if (threshold - dark < 10) {
std::cerr << "Bright/dark difference insufficient: " << threshold - dark
<< std::endl;
return 5;
}
if (g_verbosity > 1) {
std::cout << "Threshold photosensor value: " << threshold << std::endl;
}
render->Render();
//-----------------------------------------------------------------
// Do as many iterations as we're asked for, reporting the latency
// between when we asked for rendering and when we saw the screen
// brightness go from below halfway between dark and bright to
// above halfway between.
std::vector<double> pre_delays_ms, post_delays_ms;
for (size_t i = 0; i < count; i++) {
// Render dark and wait long enough for it to settle.
// As above, do another render after the sleep so we're running
// the sytem as if it were rendering every frame.
g_red = g_green = g_blue = 0;
render->Render();
vrpn_SleepMsecs(500);
r = arduino.GetReports();
render->Render();
// Store the time, render bright, store the after-render time,
// and then wait for the bright to have finished.
// As above, do another render after the sleep so we're running
// the sytem as if it were rendering every frame.
g_red = g_green = g_blue = 1;
struct timeval pre_render;
vrpn_gettimeofday(&pre_render, NULL);
render->Render();
struct timeval post_render;
vrpn_gettimeofday(&post_render, NULL);
vrpn_SleepMsecs(500);
r = arduino.GetReports();
render->Render();
// Find where we cross the threshold from dark to bright and
// report latency to pre-render and post-render times.
for (size_t t = 1; t < r.size(); t++) {
if ((r[t - 1].values[g_arduinoChannel] < threshold) &&
(r[t].values[g_arduinoChannel] >= threshold)) {
if (g_verbosity > 1) {
if (arrivalTime) {
pre_delays_ms.push_back(vrpn_TimevalDurationSeconds(r[t].arrivalTime, pre_render) * 1e3);
post_delays_ms.push_back(vrpn_TimevalDurationSeconds(r[t].arrivalTime, post_render) * 1e3);
} else {
pre_delays_ms.push_back(vrpn_TimevalDurationSeconds(r[t].sampleTime, pre_render) * 1e3);
post_delays_ms.push_back(vrpn_TimevalDurationSeconds(r[t].sampleTime, post_render) * 1e3);
}
std::cout << "Latency from pre-render: "
<< pre_delays_ms[i]
<< "ms, from post-render: "
<< post_delays_ms[i]
<< std::endl;
}
break;
}
}
}
//-----------------------------------------------------------------
// Compute and report statistics on the measurements.
print_stats("Pre-delay (ms)", pre_delays_ms);
print_stats("Post-delay (ms)", post_delays_ms);
//-----------------------------------------------------------------
// We're done. Shut down the threads and exit.
return 0;
}
