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