int CMOOSSerialPort::ReadNWithTimeOut2(char *pData, int nLen, double dfTimeOut, double * pTime )
{
int nSpace = nLen; //space left in buffer
int nRead = 0; //total number of chars read
bool bQuit = false; //exit flag on complete message
double dfStopTime=MOOSLocalTime()+dfTimeOut;
while (MOOSLocalTime()<dfStopTime && !bQuit)
{
//try the read
int nGrabbed = GrabN(pData+nRead,nSpace);
if (nGrabbed == 0)
{
// wait a while...maybe it is on its way!
MOOSPause(10);
}
else if(nGrabbed<0)
{
return -1; // Signal error
}
else
{
// Ensures that we only grab the time for the first byte.
if(nRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
//great, so increment out buffer pointer
//and check to see if this is a complete
// nomad reply
nSpace-=nGrabbed;
nRead+=nGrabbed;
if(nSpace<=0)
bQuit = true;
}
}
return nRead;
}
int CMOOSSerialPort::ReadNWithTimeOut(char *pData, int nLen, double dfTimeOut,double * pTime )
{
int nSpace = nLen; //space left in buffer
int nRead = 0; //total number of chars read
bool bQuit = false; // exit flag on complete message
double dfStopTime=MOOSLocalTime()+dfTimeOut;
while (MOOSLocalTime()<dfStopTime && !bQuit)
{
//try the read
int nGrabbed = GrabN(pData+nRead,nSpace);
if (nGrabbed == 0)
{
// wait a while...maybe it is on its way!
MOOSPause(10);
}
else if(nGrabbed<0)
{
MOOSTrace("Grab FAILED %s\n",MOOSHERE);
MOOSPause(10);
}
else
{
if(nRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
nSpace-=nGrabbed;
nRead+=nGrabbed;
if(nSpace<=0)
bQuit = true;
}
}
return nRead;
}
bool OnNewMail(MOOSMSG_LIST & Mail){
//process it
MOOSMSG_LIST::iterator q;
for(q=Mail.begin();q!=Mail.end();q++)
{
//are we a ponger
if(q->GetKey()=="ex1010-ping")
Notify("ex1010-pong",q->GetDouble());
if(q->GetKey()=="ex1010-pong"){
double latency =(MOOSLocalTime()-q->GetDouble())/2;
mean_latency_+= latency;
if(count_>2)//this simply removes case of stale data in DB
max_latency_ = std::max(latency,max_latency_);
if(count_<burstsize_-1)
Notify("ex1010-ping",MOOSLocalTime());
}
count_++;
}
return true;
}
bool Iterate()
{
if(pinger_)
{
std::cout<<"ping-ponged "<<count_<<" messages ("<<2*count_<<") messages exchanged\n";
std::cout<<" mean latency of "<<mean_latency_*1000000.0/count_<<" us\n";
std::cout<<" max latency of "<<max_latency_*1000000.0<<" us\n";
//excite the system once in while...
Notify("ex1010-ping",MOOSLocalTime());
count_ = 0;mean_latency_ = 0.0;
}
return true;
}
bool Camera::Iterate()
{
AppCastingMOOSApp::Iterate();
Mat m_capture_frame(LARGEUR_IMAGE_CAMERA, HAUTEUR_IMAGE_CAMERA, CV_8UC(3));
if(m_vc_v4l2.read(m_capture_frame))
{
if(m_inverser_image)
flip(m_capture_frame, m_capture_frame, -1);
Notify((char*)(m_image_name).c_str(), (void*)m_capture_frame.data, 3 * LARGEUR_IMAGE_CAMERA * HAUTEUR_IMAGE_CAMERA, MOOSLocalTime());
//imwrite("test.jpeg", m_capture_frame);
if(m_affichage_image)
{
imshow(m_display_name, m_capture_frame);
waitKey(1);
}
retractRunWarning("No frame grabbed.");
}
else
reportRunWarning("No frame grabbed.");
// MOOSDebugWrite("No frame grabbed.");
AppCastingMOOSApp::PostReport();
return(true);
}
