bool process_frame()
{
static ros::Time lastTime;
Output_GetFrameNumber OutputFrameNum = MyClient.GetFrameNumber();
if ((OutputFrameNum.Result != Result::Success) and (OutputFrameNum.Result != Result::NoFrame))
{
ROS_ERROR("GetFrameNumber() returned %d", int(OutputFrameNum.Result));
return false;
}
if (frame_datum == 0 or (fabs(latest_time_bias) > time_bias_reset)) // this is the first frame we've gotten
{
frame_datum = OutputFrameNum.FrameNumber;
time_datum = now_time;
ROS_INFO("Time bias reset (bias was %f): marking VICON frame number %d as datum time %d.%d", latest_time_bias, frame_datum, time_datum.sec,
time_datum.nsec);
time_bias_reset_count++;
}
//frameCount++;
// ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
int frameDiff = 0;
if (lastFrameNumber != 0)
{
frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
frameCount += frameDiff;
if ((frameDiff) > 1)
{
droppedFrameCount += frameDiff - 1;
double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
ROS_DEBUG_STREAM(
(frameDiff - 1) << " more (total " << droppedFrameCount << "/" << frameCount << ", "
<< droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
}
}
lastFrameNumber = OutputFrameNum.FrameNumber;
if (frameDiff == 0)
{
return false;
}
else
{
freq_status.tick();
//double latencyInMs = MyClient.GetLatencyTotal().Total * 1000;
ros::Time frame_time = time_datum + ros::Duration((lastFrameNumber - frame_datum) / vicon_capture_rate);
latest_time_bias = (frame_time - now_time).toSec();
process_subjects(frame_time);
process_markers(frame_time, lastFrameNumber);
lastTime = now_time;
return true;
}
}
bool process_frame()
{
static ros::Time lastTime;
Output_GetFrameNumber OutputFrameNum = msvcbridge::GetFrameNumber();
//frameCount++;
//ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
int frameDiff = 0;
if (lastFrameNumber != 0)
{
frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
frameCount += frameDiff;
if ((frameDiff) > 1)
{
droppedFrameCount += frameDiff;
double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
ROS_DEBUG_STREAM(frameDiff << " more (total " << droppedFrameCount << "/" << frameCount << ", "
<< droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
}
}
lastFrameNumber = OutputFrameNum.FrameNumber;
if (frameDiff == 0)
{
return false;
}
else
{
freq_status_.tick();
ros::Duration vicon_latency(msvcbridge::GetLatencyTotal().Total);
process_subjects(now_time - vicon_latency);
process_markers(now_time - vicon_latency, lastFrameNumber);
lastTime = now_time;
return true;
}
}