bool CvCaptureCAM_Aravis::grabFrame()
{
// remove content of previous frame
framebuffer = NULL;
if(stream) {
ArvBuffer *arv_buffer = NULL;
int max_tries = 10;
int tries = 0;
for(; tries < max_tries; tries ++) {
arv_buffer = arv_stream_timeout_pop_buffer (stream, 200000);
if (arv_buffer != NULL && arv_buffer_get_status (arv_buffer) != ARV_BUFFER_STATUS_SUCCESS) {
arv_stream_push_buffer (stream, arv_buffer);
} else break;
}
if(arv_buffer != NULL && tries < max_tries) {
size_t buffer_size;
framebuffer = (void*)arv_buffer_get_data (arv_buffer, &buffer_size);
// retieve image size properites
arv_buffer_get_image_region (arv_buffer, &xoffset, &yoffset, &width, &height);
// retieve image ID set by camera
frameID = arv_buffer_get_frame_id(arv_buffer);
arv_stream_push_buffer(stream, arv_buffer);
return true;
}
}
return false;
}
static void NewBuffer_callback (ArvStream *pStream, ApplicationData *pApplicationdata)
{
static uint64_t cm = 0L; // Camera time prev
uint64_t cn = 0L; // Camera time now
#ifdef TUNING
static uint64_t rm = 0L; // ROS time prev
#endif
uint64_t rn = 0L; // ROS time now
static uint64_t tm = 0L; // Calculated image time prev
uint64_t tn = 0L; // Calculated image time now
static int64_t em = 0L; // Error prev.
int64_t en = 0L; // Error now between calculated image time and ROS time.
int64_t de = 0L; // derivative.
int64_t ie = 0L; // integral.
int64_t u = 0L; // Output of controller.
int64_t kp1 = 0L; // Fractional gains in integer form.
int64_t kp2 = 1024L;
int64_t kd1 = 0L;
int64_t kd2 = 1024L;
int64_t ki1 = -1L; // A gentle pull toward zero.
int64_t ki2 = 1024L;
static uint32_t iFrame = 0; // Frame counter.
ArvBuffer *pBuffer;
#ifdef TUNING
std_msgs::Int64 msgInt64;
int kp = 0;
int kd = 0;
int ki = 0;
if (global.phNode->hasParam(ros::this_node::getName()+"/kp"))
{
global.phNode->getParam(ros::this_node::getName()+"/kp", kp);
kp1 = kp;
}
if (global.phNode->hasParam(ros::this_node::getName()+"/kd"))
{
global.phNode->getParam(ros::this_node::getName()+"/kd", kd);
kd1 = kd;
}
if (global.phNode->hasParam(ros::this_node::getName()+"/ki"))
{
global.phNode->getParam(ros::this_node::getName()+"/ki", ki);
ki1 = ki;
}
#endif
pBuffer = arv_stream_try_pop_buffer (pStream);
if (pBuffer != NULL)
{
if (arv_buffer_get_status(pBuffer) == ARV_BUFFER_STATUS_SUCCESS)
{
sensor_msgs::Image msg;
pApplicationdata->nBuffers++;
size_t currSize = arv_buffer_get_image_width(pBuffer) * arv_buffer_get_image_height(pBuffer) * global.nBytesPixel;
std::vector<uint8_t> this_data(currSize);
memcpy(&this_data[0], arv_buffer_get_data(pBuffer, &currSize), currSize);
// Camera/ROS Timestamp coordination.
cn = (uint64_t)arv_buffer_get_timestamp(pBuffer); // Camera now
rn = ros::Time::now().toNSec(); // ROS now
if (iFrame < 10)
{
cm = cn;
tm = rn;
}
// Control the error between the computed image timestamp and the ROS timestamp.
en = (int64_t)tm + (int64_t)cn - (int64_t)cm - (int64_t)rn; // i.e. tn-rn, but calced from prior values.
de = en-em;
ie += en;
u = kp1*(en/kp2) + ki1*(ie/ki2) + kd1*(de/kd2); // kp<0, ki<0, kd>0
// Compute the new timestamp.
tn = (uint64_t)((int64_t)tm + (int64_t)cn-(int64_t)cm + u);
#ifdef TUNING
ROS_WARN("en=%16ld, ie=%16ld, de=%16ld, u=%16ld + %16ld + %16ld = %16ld", en, ie, de, kp1*(en/kp2), ki1*(ie/ki2), kd1*(de/kd2), u);
ROS_WARN("cn=%16lu, rn=%16lu, cn-cm=%8ld, rn-rm=%8ld, tn-tm=%8ld, tn-rn=%ld", cn, rn, cn-cm, rn-rm, (int64_t)tn-(int64_t)tm, tn-rn);
msgInt64.data = tn-rn; //cn-cm+tn-tm; //
global.ppubInt64->publish(msgInt64);
rm = rn;
#endif
// Save prior values.
cm = cn;
tm = tn;
em = en;
// Construct the image message.
msg.header.stamp.fromNSec(tn);
msg.header.seq = arv_buffer_get_frame_id(pBuffer);
msg.header.frame_id = global.config.frame_id;
msg.width = global.widthRoi;
msg.height = global.heightRoi;
msg.encoding = global.pszPixelformat;
msg.step = msg.width * global.nBytesPixel;
msg.data = this_data;
// get current CameraInfo data
global.camerainfo = global.pCameraInfoManager->getCameraInfo();
global.camerainfo.header.stamp = msg.header.stamp;
global.camerainfo.header.seq = msg.header.seq;
global.camerainfo.header.frame_id = msg.header.frame_id;
global.camerainfo.width = global.widthRoi;
global.camerainfo.height = global.heightRoi;
global.publisher.publish(msg, global.camerainfo);
}
else
ROS_WARN ("Frame error: %s", szBufferStatusFromInt[arv_buffer_get_status(pBuffer)]);
arv_stream_push_buffer (pStream, pBuffer);
iFrame++;
}
} // NewBuffer_callback()
