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()