// If the time is before our current time (or there is no current
// time) then reset back to the beginning. Whether or not we did
// that, search forwards until we get to or past the time we are
// searching for.
// newtime is an elapsed time from the start of the file
int vrpn_File_Connection::jump_to_time(timeval newtime)
{
if( d_earliest_user_time_valid )
{ d_time = vrpn_TimevalSum( d_earliest_user_time, newtime ); }
else
{ d_time = vrpn_TimevalSum( d_start_time, newtime ); } // XXX get rid of this option - dtm
// If the time is earlier than where we are, or if we have
// run past the end (no current entry), jump back to
// the beginning of the file before searching.
if ( !d_currentLogEntry || vrpn_TimevalGreater(d_currentLogEntry->data.msg_time, d_time) ) {
reset();
}
// Search forwards, as needed. Do not play the messages as they are
// passed, just skip over them until we get to a message that has a
// time greater than or equal to the one we are looking for. That is,
// one whose time is not less than ours.
while (!vrpn_TimevalGreater(d_currentLogEntry->data.msg_time, d_time)) {
int ret = advance_currentLogEntry();
if ( ret != 0 ) {
return 0; // Didn't get where we were going!
}
}
return 1; // Got where we were going!
}
bool ArduinoComparer::computeLatency(
int arduinoChannel
, int deviceChannel
, double &outLatencySeconds
, bool arrivalTime ) const
{
// Bogus value in case we have to bail.
outLatencySeconds = -10e10;
if ( (m_deviceReports.size() == 0) || (m_arduinoReports.size() == 0) ) {
return false;
}
// Compute the start time, which is the lowest time value in either
// of the report lists.
struct timeval start;
if (arrivalTime) {
start = m_deviceReports[0].arrivalTime;
if (vrpn_TimevalGreater(start, m_arduinoReports[0].arrivalTime)) {
start = m_arduinoReports[0].arrivalTime;
}
} else {
start = m_deviceReports[0].sampleTime;
if (vrpn_TimevalGreater(start, m_arduinoReports[0].sampleTime)) {
start = m_arduinoReports[0].sampleTime;
}
}
// Construct trajectories for both the Arduino and the device.
Trajectory arduinoTrajectory(m_arduinoReports, start, arduinoChannel, arrivalTime);
Trajectory deviceTrajectory(m_deviceReports, start, deviceChannel, arrivalTime);
// Compute the sum of squared differences between the device values and
// the expected mapping for the nearest-time Arduino values for a temporal
// offset of 0.
double minOffset = 0;
double minError = computeError(arduinoTrajectory, deviceTrajectory,
minOffset);
// @todo Turn this from a brute-force search into an optimization routine.
for (double offset = -300e-3; offset <= 300e-3; offset += 1e-3) {
double err = computeError(arduinoTrajectory, deviceTrajectory, offset);
//std::cout << "XXX offset vs. error: " << offset << ", " << err << std::endl;
if (err < minError) {
minError = err;
minOffset = offset;
}
}
outLatencySeconds = minOffset;
return true;
}
// checks if there is at least one log entry that occurs
// between the current d_time and the given filetime
//
// [juliano 9/24/99] the above comment is almost right
// the upper bound of the interval is not open,
// but closed at time_we_want_to_play_to.
//
// this function checks if the next message to play back
// from the stream file has a timestamp LESSTHAN OR EQUAL TO
// the argument to this function (which is the time that we
// wish to play to). If it does, then a pos value is returned
//
// you can pause playback of a streamfile by ceasing to increment
// the value that is passed to this function. However, if the next
// message in the streamfile has the same timestamp as the previous
// one, it will get played anyway. Pause will not be achieved until
// all such messages have been played.
//
// Beware: make sure you put the correct timestamps on individual
// messages when recording them in chunks (batches)
// to a time to a streamfile.
//
int vrpn_File_Connection::need_to_play( timeval time_we_want_to_play_to )
{
// This read_entry() call may be useful to test the state, but
// it should be the case that d_currentLogEntry is non-NULL except
// when we are at the end. This is because read_entry() and the
// constructor now both read the next one in when they are finished.
if (!d_currentLogEntry) {
int retval = read_entry();
if (retval < 0) { return -1; } // error reading from file
if (retval > 0) { return 0; } // end of file; nothing to replay
d_currentLogEntry = d_logTail; // If read_entry() returns 0, this will be non-NULL
}
vrpn_HANDLERPARAM & header = d_currentLogEntry->data;
// [juliano 9/24/99] is this right?
// this is a ">" test, not a ">=" test
// consequently, this function keeps returning true until the
// next message is timestamped greater. So, if a group of
// messages share a timestamp, you cannot pause streamfile
// replay anywhere inside the group.
//
// this is true, but do you ever want to pause in the middle of
// such a group? This was a problem prior to fixing the
// timeval overflow bug, but now that it's fixed, (who cares?)
return vrpn_TimevalGreater( time_we_want_to_play_to, header.msg_time );
}
// virtual
void vrpn_RedundantTransmission::mainloop (void) {
queuedMessage * qm;
queuedMessage ** snitch;
timeval now;
if (!d_connection) {
return;
}
//fprintf(stderr, "mainloop: %d messages queued.\n", d_numMessagesQueued);
vrpn_gettimeofday(&now, NULL);
for (qm = d_messageList; qm; qm = qm->next) {
if ((qm->remainingTransmissions > 0) &&
vrpn_TimevalGreater(now, qm->nextValidTime)) {
d_connection->pack_message(qm->p.payload_len, qm->p.msg_time,
qm->p.type, qm->p.sender, qm->p.buffer,
vrpn_CONNECTION_LOW_LATENCY);
qm->nextValidTime = vrpn_TimevalSum(now, qm->transmissionInterval);
qm->remainingTransmissions--;
//fprintf(stderr, "Sending message; "
//"%d transmissions remaining at %d.%d int.\n",
//qm->remainingTransmissions, qm->transmissionInterval.tv_sec,
//qm->transmissionInterval.tv_usec);
}
}
snitch = &d_messageList;
qm = *snitch;
while (qm) {
if (!qm->remainingTransmissions) {
*snitch = qm->next;
delete [] (char *) qm->p.buffer;
delete qm;
qm = *snitch;
d_numMessagesQueued--;
} else {
snitch = &qm->next;
qm = *snitch;
}
}
if ((d_numMessagesQueued && !d_messageList) ||
(!d_numMessagesQueued && d_messageList)) {
fprintf(stderr, "vrpn_RedundantTransmission::mainloop(): "
"serious internal error.\n");
d_numMessagesQueued = 0;
d_messageList = NULL;
}
}
// plays all entries between d_time and end_filetime
// returns -1 on error, 0 on success
int vrpn_File_Connection::play_to_filetime(const timeval end_filetime)
{
vrpn_uint32 playback_this_iteration = 0;
if (vrpn_TimevalGreater(d_time, end_filetime)) {
// end_filetime is BEFORE d_time (our current time in the stream)
// so, we need to go backwards in the stream
// currently, this is implemented by
// * rewinding the stream to the beginning
// * playing log messages one at a time until we get to end_filetime
reset();
}
int ret;
while ((ret = playone_to_filetime(end_filetime)) == 0) {
// * you get here ONLY IF playone_to_filetime returned 0
// * that means that it played one entry
playback_this_iteration++;
if ((get_Jane_value() > 0) &&
(playback_this_iteration >= get_Jane_value())) {
// Early exit from the loop
// Don't reset d_time to end_filetime
return 0;
}
}
if (ret == 1) {
// playone_to_filetime finished or EOF no error for us
// Set log position to the exact requested ending time,
// don't leave it at the last log entry time
d_time = end_filetime;
ret = 0;
}
return ret;
}
void VRPNTrackerInstance::getAccReport(vrpn_TRACKERACCCB* cpy, timeval* ts, int in_sensor)
{
TrackerAccList::iterator it;
vrpn_TRACKERACCCB* last = NULL;
for ( it = acc.begin(); it != acc.end(); it++ )
{
vrpn_TRACKERACCCB* curr = *it;
if (curr->sensor == in_sensor)
{
if (ts == NULL)
{
*cpy = *curr;
return;
}
else if (vrpn_TimevalGreater(*ts,curr->msg_time))
{
if (last)
{
double val = vrpn_TimevalMsecs(vrpn_TimevalDiff(*ts, curr->msg_time))/vrpn_TimevalMsecs(vrpn_TimevalDiff(last->msg_time, curr->msg_time));
cpy->acc[0] = curr->acc[0] + val*(last->acc[0] - curr->acc[0]);
cpy->acc[1] = curr->acc[1] + val*(last->acc[1] - curr->acc[1]);
cpy->acc[2] = curr->acc[2] + val*(last->acc[2] - curr->acc[2]);
q_slerp(cpy->acc_quat, curr->acc_quat, last->acc_quat, val);
cpy->acc_quat_dt = curr->acc_quat_dt + val*(last->acc_quat_dt - curr->acc_quat_dt);
return;
}
else
{
*cpy = *curr;
return;
}
}
else
last = curr;
}
}
}
void vrpn_File_Connection::find_superlative_user_times( )
{
timeval high = {0, 0};
timeval low = {LONG_MAX, 999999L};
// Remember where we were when we asked this question
bool retval = store_stream_bookmark( );
if( retval == false )
{
#ifdef VERBOSE
printf( "vrpn_File_Connection::find_superlative_user_times: didn't successfully save bookmark.\n" );
#endif
return;
}
// Go to the beginning of the file and then run through all
// of the messages to find the one with the lowest/highest value
reset();
do {
if( d_currentLogEntry && (d_currentLogEntry->data.type >= 0) )
{
if( vrpn_TimevalGreater( d_currentLogEntry->data.msg_time, high ) )
{
high = d_currentLogEntry->data.msg_time;
}
if( vrpn_TimevalGreater(low, d_currentLogEntry->data.msg_time ) )
{
low = d_currentLogEntry->data.msg_time;
}
}
} while( d_currentLogEntry && ( advance_currentLogEntry( ) == 0 ) );
// We have our value. Set it and go back where
// we came from, but don't play the records along
// the way.
retval = return_to_bookmark( );
if( retval == false )
{
// oops. we've really screwed things up.
fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times messed up the location in the file stream.\n" );
reset( );
return;
}
if( high.tv_sec != LONG_MIN ) // we found something
{
d_highest_user_time = high;
d_highest_user_time_valid = true;
}
#ifdef VERBOSE
else
{
fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times: did not find a highest-time user message\n"
}
#endif
if( low.tv_sec != LONG_MAX ) // we found something
{
d_earliest_user_time = low;
d_earliest_user_time_valid = true;
}
#ifdef VERBOSE
else
{
fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times: did not find an earliest user message\n"
}
#endif
} // end find_superlative_user_times
// plays at most one entry which comes before end_filetime
// returns
// -1 on error (including EOF, call eof() to test)
// 0 for normal result (played one entry)
// 1 if we hit end_filetime
int vrpn_File_Connection::playone_to_filetime( timeval end_filetime )
{
vrpn_Endpoint * endpoint = d_endpoints[0];
timeval now;
int retval;
// If we don't have a currentLogEntry, then we've gone past the end of the
// file.
if (!d_currentLogEntry) {
return 1;
}
vrpn_HANDLERPARAM & header = d_currentLogEntry->data;
if (vrpn_TimevalGreater(header.msg_time, end_filetime)) {
// there are no entries to play after the current
// but before end_filetime
return 1;
}
// TCH July 2001
// XXX A big design decision: do we re-log messages exactly,
// or do we mark them with the time they were played back?
// Maybe this should be switchable, but the latter is what
// I need yesterday.
vrpn_gettimeofday(&now, NULL);
retval = endpoint->d_inLog->logIncomingMessage
(header.payload_len, now, header.type,
header.sender, header.buffer);
if (retval) {
fprintf(stderr, "Couldn't log \"incoming\" message during replay!\n");
return -1;
}
// advance current file position
d_time = header.msg_time;
// Handle this log entry
if (header.type >= 0) {
#ifdef VERBOSE
printf("vrpn_FC: Msg Sender (%s), Type (%s), at (%ld:%ld)\n",
endpoint->other_senders[header.sender].name,
endpoint->other_types[header.type].name,
header.msg_time.tv_sec, header.msg_time.tv_usec);
#endif
if (endpoint->local_type_id(header.type) >= 0) {
if (do_callbacks_for(endpoint->local_type_id(header.type),
endpoint->local_sender_id(header.sender),
header.msg_time, header.payload_len,
header.buffer)) {
return -1;
}
}
} else { // system handler
if (header.type != vrpn_CONNECTION_UDP_DESCRIPTION) {
if (doSystemCallbacksFor(header, endpoint)) {
fprintf(stderr, "vrpn_File_Connection::playone_to_filename: "
"Nonzero system return.\n");
return -1;
}
}
}
return advance_currentLogEntry();
}
//virtual
vrpn_bool vrpn_Shared_float64::shouldAcceptUpdate
(vrpn_float64 newValue, timeval when,
vrpn_bool isLocalSet) {
// Is this "new" change idempotent?
if ((d_mode & VRPN_SO_IGNORE_IDEMPOTENT) &&
(newValue == d_value)) {
return vrpn_FALSE;
}
// Is this "new" change older than the previous change?
if ((d_mode & VRPN_SO_IGNORE_OLD) &&
!vrpn_TimevalGreater(when, d_lastUpdate)) {
// If the timestamps of the new & previous changes are equal:
// - if we are the serializer, we can accept the local change
// - if we are not the serializer, local updates are to be rejected
if( vrpn_TimevalEqual( when, d_lastUpdate ) ) {
if( !d_isSerializer && isLocalSet ) {
return vrpn_FALSE;
}
}
else
return vrpn_FALSE;
}
// All other clauses of shouldAcceptUpdate depend on serialization
if (!(d_mode & VRPN_SO_DEFER_UPDATES)) {
return vrpn_TRUE;
}
// If we're not the serializer, don't accept local set() calls -
// forward those to the serializer. Non-local set() calls are
// messages from the serializer that we should accept.
if (!d_isSerializer) {
if (isLocalSet) {
yankDeferredUpdateCallbacks();
return vrpn_FALSE;
} else {
return vrpn_TRUE;
}
}
// We are the serializer.
if (isLocalSet) {
if (d_policy == vrpn_DENY_LOCAL) {
return vrpn_FALSE;
} else {
return vrpn_TRUE;
}
}
// Are we accepting all updates?
if (d_policy == vrpn_ACCEPT) {
return vrpn_TRUE;
}
// Does the user want to accept this one?
if ((d_policy == vrpn_CALLBACK) && d_policyCallback &&
(*d_policyCallback)(d_policyUserdata, newValue, when, this)) {
return vrpn_TRUE;
}
return vrpn_FALSE;
}
//virtual
vrpn_bool vrpn_Shared_int32::shouldAcceptUpdate
(vrpn_int32 newValue, timeval when,
vrpn_bool isLocalSet, vrpn_LamportTimestamp *) {
//fprintf(stderr, "In vrpn_Shared_int32::shouldAcceptUpdate(%s).\n", d_name);
/*
// this commented-out code uses Lamport logical clocks, and is
// disabled now b/c the implementation of Lamport clocks was
// never complete. We use standard timestamps instead.
vrpn_bool old, equal;
if (t) {
old = d_lastLamportUpdate && (*t < *d_lastLamportUpdate);
equal = d_lastLamportUpdate && (*t == *d_lastLamportUpdate);
} else {
old = !vrpn_TimevalGreater(when, d_lastUpdate);
equal = vrpn_TimevalEqual( when, d_lastUpdate );
}
*/
vrpn_bool old = !vrpn_TimevalGreater(when, d_lastUpdate);
vrpn_bool equal = vrpn_TimevalEqual( when, d_lastUpdate );
// Is this "new" change idempotent?
if ((d_mode & VRPN_SO_IGNORE_IDEMPOTENT) &&
(newValue == d_value)) {
//fprintf(stderr, "... was idempotent.\n");
return vrpn_FALSE;
}
// Is this "new" change older than the previous change?
if ( (d_mode & VRPN_SO_IGNORE_OLD) && old ) {
//fprintf(stderr, "... was outdated.\n");
return vrpn_FALSE;
}
// Is this "new" change older than the previous change?
if ((d_mode & VRPN_SO_IGNORE_OLD) && old) {
// If the timestamps of the new & previous changes are equal:
// - if we are the serializer, we can accept the local change
// - if we are not the serializer, local updates are to be rejected
if( equal ) {
if( !d_isSerializer && isLocalSet ) {
return vrpn_FALSE;
}
}
else
return vrpn_FALSE;
}
// All other clauses of shouldAcceptUpdate depend on serialization
if (!(d_mode & VRPN_SO_DEFER_UPDATES)) {
return vrpn_TRUE;
}
//fprintf(stderr, "... serializing: ");
// If we're not the serializer, don't accept local set() calls -
// forward those to the serializer. Non-local set() calls are
// messages from the serializer that we should accept.
if (!d_isSerializer) {
if (isLocalSet) {
//fprintf(stderr, "local update, not serializer, so reject.\n");
yankDeferredUpdateCallbacks();
return vrpn_FALSE;
} else {
//fprintf(stderr, "remote update, not serializer, so accept.\n");
return vrpn_TRUE;
}
}
// We are the serializer.
//fprintf(stderr, "serializer: ");
if (isLocalSet) {
//fprintf(stderr, "local update.\n");
if (d_policy == vrpn_DENY_LOCAL) {
return vrpn_FALSE;
} else {
return vrpn_TRUE;
}
}
// Are we accepting all updates?
if (d_policy == vrpn_ACCEPT) {
//fprintf(stderr, "policy is to accept.\n");
return vrpn_TRUE;
}
// Does the user want to accept this one?
if ((d_policy == vrpn_CALLBACK) && d_policyCallback &&
(*d_policyCallback)(d_policyUserdata, newValue, when, this)) {
//fprintf(stderr, "user callback accepts.\n");
return vrpn_TRUE;
}
//fprintf(stderr, "rejected.\n");
return vrpn_FALSE;
}
