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;
}
/**
* Reads image data from an Aravis ArvBuffer and saves a png file to filename
* TODO: Add error checking and all that stuff (this code is demonstrative)
*/
void arv_save_png(ArvBuffer * buffer, const char * filename)
{
// TODO: This only works on image buffers
assert(arv_buffer_get_payload_type(buffer) == ARV_BUFFER_PAYLOAD_TYPE_IMAGE);
size_t buffer_size;
char * buffer_data = (char*)arv_buffer_get_data(buffer, &buffer_size); // raw data
int width; int height;
arv_buffer_get_image_region(buffer, NULL, NULL, &width, &height); // get width/height
int bit_depth = ARV_PIXEL_FORMAT_BIT_PER_PIXEL(arv_buffer_get_image_pixel_format(buffer)); // bit(s) per pixel
//TODO: Deal with non-png compliant pixel formats?
// EG: ARV_PIXEL_FORMAT_MONO_14 is 14 bits per pixel, so conversion to PNG loses data
int arv_row_stride = width * bit_depth/8; // bytes per row, for constructing row pointers
int color_type = PNG_COLOR_TYPE_GRAY; //TODO: Check for other types?
// boilerplate libpng stuff without error checking (setjmp? Seriously? How many kittens have to die?)
png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
png_infop info_ptr = png_create_info_struct(png_ptr);
FILE * f = fopen(filename, "wb");
png_init_io(png_ptr, f);
png_set_IHDR(png_ptr, info_ptr, width, height, bit_depth, color_type,
PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
png_write_info(png_ptr, info_ptr);
// Need to create pointers to each row of pixels for libpng
png_bytepp rows = (png_bytepp)(png_malloc(png_ptr, height*sizeof(png_bytep)));
int i =0;
for (i = 0; i < height; ++i)
rows[i] = (png_bytep)(buffer_data + (height - i)*arv_row_stride);
// Actually write image
png_write_image(png_ptr, rows);
png_write_end(png_ptr, NULL); // cleanup
fclose(f);
}
bool arv_buffer_save_raw(ArvBuffer * buffer, const char * filename)
{
if (get_free_space() < 50000000) return false;
size_t buffer_size;
char * buffer_data = (char*)arv_buffer_get_data(buffer, &buffer_size); // raw data
int width; int height;
arv_buffer_get_image_region(buffer, NULL, NULL, &width, &height); // get width/height
int bit_depth = ARV_PIXEL_FORMAT_BIT_PER_PIXEL(arv_buffer_get_image_pixel_format(buffer)); // bit(s) per pixel
FILE * f = fopen(filename, "wb");
assert(f != NULL);
fwrite(buffer_data, 1, buffer_size, f);
fclose(f);
return true;
}
bool CvCaptureCAM_Aravis::grabFrame()
{
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 (tries == max_tries)
return false;
size_t buffer_size;
framebuffer = (void*)arv_buffer_get_data (arv_buffer, &buffer_size);
arv_buffer_get_image_region (arv_buffer, NULL, NULL, &width, &height);
arv_stream_push_buffer(stream, arv_buffer);
return true;
}
static GstFlowReturn
gst_aravis_create (GstPushSrc * push_src, GstBuffer ** buffer)
{
GstAravis *gst_aravis;
ArvBuffer *arv_buffer;
int arv_row_stride;
int width, height;
char *buffer_data;
size_t buffer_size;
guint64 timestamp_ns;
gst_aravis = GST_ARAVIS (push_src);
do {
arv_buffer = arv_stream_timeout_pop_buffer (gst_aravis->stream, gst_aravis->buffer_timeout_us);
if (arv_buffer != NULL && arv_buffer_get_status (arv_buffer) != ARV_BUFFER_STATUS_SUCCESS)
arv_stream_push_buffer (gst_aravis->stream, arv_buffer);
} while (arv_buffer != NULL && arv_buffer_get_status (arv_buffer) != ARV_BUFFER_STATUS_SUCCESS);
if (arv_buffer == NULL)
return GST_FLOW_ERROR;
*buffer = gst_buffer_new ();
buffer_data = (char *) arv_buffer_get_data (arv_buffer, &buffer_size);
arv_buffer_get_image_region (arv_buffer, NULL, NULL, &width, &height);
arv_row_stride = width * ARV_PIXEL_FORMAT_BIT_PER_PIXEL (arv_buffer_get_image_pixel_format (arv_buffer)) / 8;
timestamp_ns = arv_buffer_get_timestamp (arv_buffer);
/* Gstreamer requires row stride to be a multiple of 4 */
if ((arv_row_stride & 0x3) != 0) {
int gst_row_stride;
size_t size;
void *data;
int i;
gst_row_stride = (arv_row_stride & ~(0x3)) + 4;
size = height * gst_row_stride;
data = g_malloc (size);
for (i = 0; i < height; i++)
memcpy (((char *) data) + i * gst_row_stride, buffer_data + i * arv_row_stride, arv_row_stride);
GST_BUFFER_DATA (buffer) = data;
GST_BUFFER_MALLOCDATA (buffer) = data;
GST_BUFFER_SIZE (buffer) = size;
} else {
GST_BUFFER_DATA (*buffer) = buffer_data;
GST_BUFFER_MALLOCDATA (*buffer) = NULL;
GST_BUFFER_SIZE (*buffer) = buffer_size;
}
if (!gst_base_src_get_do_timestamp(GST_BASE_SRC(push_src))) {
if (gst_aravis->timestamp_offset == 0) {
gst_aravis->timestamp_offset = timestamp_ns;
gst_aravis->last_timestamp = timestamp_ns;
}
GST_BUFFER_TIMESTAMP (*buffer) = timestamp_ns - gst_aravis->timestamp_offset;
GST_BUFFER_DURATION (*buffer) = timestamp_ns - gst_aravis->last_timestamp;
gst_aravis->last_timestamp = timestamp_ns;
}
arv_stream_push_buffer (gst_aravis->stream, arv_buffer);
gst_buffer_set_caps (*buffer, gst_aravis->fixed_caps);
return GST_FLOW_OK;
}
bool CameraGigeAravis::grabImage(Frame &newFrame){
ArvBuffer *arv_buffer;
//exp = arv_camera_get_exposure_time(camera);
arv_buffer = arv_stream_timeout_pop_buffer(stream,2000000); //us
char *buffer_data;
size_t buffer_size;
if(arv_buffer == NULL){
throw runtime_error("arv_buffer is NULL");
return false;
}else{
try{
if(arv_buffer_get_status(arv_buffer) == ARV_BUFFER_STATUS_SUCCESS){
//BOOST_LOG_SEV(logger, normal) << "Success to grab a frame.";
buffer_data = (char *) arv_buffer_get_data (arv_buffer, &buffer_size);
//Timestamping.
//string acquisitionDate = TimeDate::localDateTime(microsec_clock::universal_time(),"%Y:%m:%d:%H:%M:%S");
//BOOST_LOG_SEV(logger, normal) << "Date : " << acquisitionDate;
boost::posix_time::ptime time = boost::posix_time::microsec_clock::universal_time();
string acquisitionDate = to_iso_extended_string(time);
//BOOST_LOG_SEV(logger, normal) << "Date : " << acqDateInMicrosec;
Mat image;
CamPixFmt imgDepth = MONO8;
int saturateVal = 0;
if(pixFormat == ARV_PIXEL_FORMAT_MONO_8){
//BOOST_LOG_SEV(logger, normal) << "Creating Mat 8 bits ...";
image = Mat(mHeight, mWidth, CV_8UC1, buffer_data);
imgDepth = MONO8;
saturateVal = 255;
}else if(pixFormat == ARV_PIXEL_FORMAT_MONO_12){
//BOOST_LOG_SEV(logger, normal) << "Creating Mat 16 bits ...";
image = Mat(mHeight, mWidth, CV_16UC1, buffer_data);
imgDepth = MONO12;
saturateVal = 4095;
//double t3 = (double)getTickCount();
if(shiftBitsImage){
//BOOST_LOG_SEV(logger, normal) << "Shifting bits ...";
unsigned short * p;
for(int i = 0; i < image.rows; i++){
p = image.ptr<unsigned short>(i);
for(int j = 0; j < image.cols; j++)
p[j] = p[j] >> 4;
}
//BOOST_LOG_SEV(logger, normal) << "Bits shifted.";
}
//t3 = (((double)getTickCount() - t3)/getTickFrequency())*1000;
//cout << "> Time shift : " << t3 << endl;
}
//BOOST_LOG_SEV(logger, normal) << "Creating frame object ...";
newFrame = Frame(image, gain, exp, acquisitionDate);
//BOOST_LOG_SEV(logger, normal) << "Setting date of frame ...";
//newFrame.setAcqDateMicro(acqDateInMicrosec);
//BOOST_LOG_SEV(logger, normal) << "Setting fps of frame ...";
newFrame.mFps = fps;
newFrame.mFormat = imgDepth;
//BOOST_LOG_SEV(logger, normal) << "Setting saturated value of frame ...";
newFrame.mSaturatedValue = saturateVal;
newFrame.mFrameNumber = frameCounter;
frameCounter++;
//BOOST_LOG_SEV(logger, normal) << "Re-pushing arv buffer in stream ...";
arv_stream_push_buffer(stream, arv_buffer);
return true;
}else{
switch(arv_buffer_get_status(arv_buffer)){
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()
