static void image_move(dword key)
{
// cancel z mode
in_move_z_mode = false;
if ((key & config.imgkey[14] && !(key & config.imgkey[15]))
|| (key & config.imgkey2[14] && !(key & config.imgkey2[15]))
) {
image_left();
}
if ((key & config.imgkey[15] && !(key & config.imgkey[14])) ||
(key & config.imgkey2[15] && !(key & config.imgkey2[14]))
) {
image_right();
}
if ((key & config.imgkey[12] && !(key & config.imgkey[13])) ||
(key & config.imgkey2[12] && !(key & config.imgkey2[13]))
) {
image_up();
}
if ((key & config.imgkey[13] && !(key & config.imgkey[12])) ||
(key & config.imgkey2[13] && !(key & config.imgkey2[12]))) {
image_down();
}
thumb = (config.thumb == conf_thumb_scroll);
img_needrp = thumb | img_needrp;
}
// draw_bitmap () - Draw a bitmap
void
CImageButtonWithStyle::draw_bitmap (HDC hDC, const CRect& Rect, DWORD style)
{
HBITMAP hBitmap = GetBitmap();
if (hBitmap == NULL)
return;
// determine size of bitmap image
BITMAPINFO bmi;
memset(&bmi, 0, sizeof (BITMAPINFO));
bmi.bmiHeader.biSize = sizeof (BITMAPINFOHEADER);
GetDIBits(hDC, hBitmap, 0, 0, NULL, &bmi, DIB_RGB_COLORS);
// determine position of top-left corner of bitmap (positioned according to style)
int x = image_left(bmi.bmiHeader.biWidth, Rect, style);
int y = image_top(bmi.bmiHeader.biHeight, Rect, style);
// Draw the bitmap
DrawState(hDC, NULL, NULL, (LPARAM) hBitmap, 0, x, y, bmi.bmiHeader.biWidth, bmi.bmiHeader.biHeight,
(style & WS_DISABLED) != 0 ? (DST_BITMAP | DSS_DISABLED) : (DST_BITMAP | DSS_NORMAL));
}
// draw_icon () - Draw an icon
void
CImageButtonWithStyle::draw_icon (HDC hDC, const CRect& Rect, DWORD style)
{
HICON hIcon = GetIcon();
if (hIcon == NULL)
return;
// determine size of icon image
ICONINFO ii;
GetIconInfo(hIcon, &ii);
BITMAPINFO bmi;
memset(&bmi, 0, sizeof (BITMAPINFO));
bmi.bmiHeader.biSize = sizeof (BITMAPINFOHEADER);
int cx = 0;
int cy = 0;
if (ii.hbmColor != NULL)
{
// icon has separate image and mask bitmaps - use size directly
GetDIBits(hDC, ii.hbmColor, 0, 0, NULL, &bmi, DIB_RGB_COLORS);
cx = bmi.bmiHeader.biWidth;
cy = bmi.bmiHeader.biHeight;
}
else
{
// icon has singel mask bitmap which is twice as high as icon
GetDIBits(hDC, ii.hbmMask, 0, 0, NULL, &bmi, DIB_RGB_COLORS);
cx = bmi.bmiHeader.biWidth;
cy = bmi.bmiHeader.biHeight / 2;
}
// determine position of top-left corner of icon
int x = image_left(cx, Rect, style);
int y = image_top(cy, Rect, style);
// Draw the icon
DrawState(hDC, NULL, NULL, (LPARAM) hIcon, 0, x, y, cx, cy,
(style & WS_DISABLED) != 0 ? (DST_ICON | DSS_DISABLED) : (DST_ICON | DSS_NORMAL));
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
ros::Time prev_time = ros::Time::now();
ros::Duration force_wait_duration(1.0f / float(process_fps));
while (ok) {
unsigned char *frame_left = NULL, *frame_right = NULL;
uint32_t bytes_used_left, bytes_used_right;
ros::Time capture_time = ros::Time::now();
int left_idx = cam_left->grab(&frame_left, bytes_used_left);
int right_idx = cam_right->grab(&frame_right, bytes_used_right);
/* Read in every frame the camera generates, but only send each
* (skip_frames + 1)th frame.
* Alternatively, define this behavior with the process_fps parameter.
* process_fps is used when the camera will not respond to the FPS
* setting and it's actual FPS is unknown.
* These reduces effective frame rate, processing time and network usage
* while keeping the images synchronized within the true framerate.
*/
if ((skip_frames == 0 || frames_to_skip == 0) &&
((ros::Time::now() - prev_time) >= force_wait_duration)) {
if (frame_left && frame_right) {
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = 3 * width;
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = 3 * width;
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
if (rotate_left)
rotate(&image_left->data[0], frame_left, width * height);
else if (vertical)
rotateVertical(&image_left->data[0], frame_left, width, height);
else
memcpy(&image_left->data[0], frame_left, width * height * 3);
if (rotate_right)
rotate(&image_right->data[0], frame_right, width * height);
else if (vertical)
rotateVertical(&image_right->data[0], frame_right, width, height);
else
memcpy(&image_right->data[0], frame_right, width * height * 3);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
prev_time = capture_time;
}
frames_to_skip = skip_frames;
} else if (frames_to_skip > 0) {
frames_to_skip--;
}
if (frame_left)
cam_left->release(left_idx);
if (frame_right)
cam_right->release(right_idx);
}
}
DEFINE_LOADER_PLUGIN_LOAD(p, st, vw
#if !defined(IDENTIFY_BEFORE_LOAD)
__attribute__((unused))
#endif
, c
#if !defined(IDENTIFY_BEFORE_LOAD)
__attribute__((unused))
#endif
, priv
#if !defined(IDENTIFY_BEFORE_LOAD)
__attribute__((unused))
#endif
)
{
jas_image_t *ji;
jas_stream_t *js;
unsigned char *d;
char *buf = NULL;
int k, cmp[3];
unsigned int i, j;
int tlx, tly;
int vs, hs;
//debug_message("JasPer: load() called\n");
#ifdef IDENTIFY_BEFORE_LOAD
{
LoaderStatus status;
if ((status = identify(p, st, vw, c, priv)) != LOAD_OK)
return status;
stream_rewind(st);
}
#endif
/* Read whole stream into buffer... */
{
char *tmp;
int size = 0, len;
int bufsize = 65536;
for (;;) {
if ((tmp = realloc(buf, bufsize)) == NULL) {
free(buf);
return LOAD_ERROR;
}
buf = tmp;
len = stream_read(st, (unsigned char *)(buf + size), bufsize - size);
size += len;
if (len < bufsize - size)
break;
bufsize += 65536;
}
if ((js = jas_stream_memopen(buf, size)) == NULL) {
free(buf);
return LOAD_ERROR;
}
}
/* loading... */
if ((ji = jas_image_decode(js, -1, 0)) == NULL) {
err_message_fnc("jas_image_decode() failed.\n");
goto error_clear;
}
/* colorspace conversion */
{
jas_cmprof_t *jc;
jas_image_t *new_ji;
if ((jc = jas_cmprof_createfromclrspc(JAS_CLRSPC_SRGB)) == NULL)
goto error_destroy_free;
if ((new_ji = jas_image_chclrspc(ji, jc, JAS_CMXFORM_INTENT_PER)) == NULL)
goto error_destroy_free;
jas_image_destroy(ji);
ji = new_ji;
}
jas_stream_close(js);
free(buf);
debug_message("JasPer: jas_image_decode() OK: (%ld,%ld)\n", jas_image_cmptwidth(ji, 0), jas_image_cmptheight(ji, 0));
/* convert to enfle format */
p->bits_per_pixel = 24;
p->type = _RGB24;
p->depth = 24;
cmp[0] = jas_image_getcmptbytype(ji, JAS_IMAGE_CT_COLOR(JAS_CLRSPC_CHANIND_RGB_R));
cmp[1] = jas_image_getcmptbytype(ji, JAS_IMAGE_CT_COLOR(JAS_CLRSPC_CHANIND_RGB_G));
cmp[2] = jas_image_getcmptbytype(ji, JAS_IMAGE_CT_COLOR(JAS_CLRSPC_CHANIND_RGB_B));
/* dimension */
image_width(p) = jas_image_cmptwidth(ji, cmp[0]);
image_height(p) = jas_image_cmptheight(ji, cmp[0]);
image_left(p) = 0;
image_top(p) = 0;
image_bpl(p) = image_width(p) * 3;
tlx = jas_image_cmpttlx(ji, cmp[0]);
tly = jas_image_cmpttly(ji, cmp[0]);
vs = jas_image_cmptvstep(ji, cmp[0]);
hs = jas_image_cmpthstep(ji, cmp[0]);
debug_message("JasPer: tlx %d tly %d vs %d hs %d ncomponents %d\n", tlx, tly, vs, hs, jas_image_numcmpts(ji));
/* memory allocation */
if ((d = memory_alloc(image_image(p), image_bpl(p) * image_height(p))) == NULL) {
err_message("No enough memory (%d bytes)\n", image_bpl(p) * image_height(p));
goto error_destroy_free;
}
for (i = 0; i < image_height(p); i++) {
for (j = 0; j < image_width(p); j++) {
for (k = 0; k < 3; k++)
*d++ = jas_image_readcmptsample(ji, cmp[k], j, i);
}
}
jas_image_destroy(ji);
return LOAD_OK;
error_destroy_free:
jas_image_destroy(ji);
error_clear:
return LOAD_ERROR;
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
FlyCapture2::Error error;
while (ok) {
// Retrieve an image
error = cam_left.RetrieveBuffer( &rawImage_l );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
error = cam_right.RetrieveBuffer( &rawImage_r );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
ros::Time capture_time = ros::Time::now();
// Convert the raw image
error = rawImage_l.Convert( FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage_l );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
error = rawImage_r.Convert( FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage_r );
if (error != FlyCapture2::PGRERROR_OK) {
// PrintError( error );
continue;
}
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = convertedImage_l.GetRows();
image_left->width = convertedImage_l.GetCols();
image_left->step = convertedImage_l.GetStride();
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_left->header.frame_id = frame;
int data_size = convertedImage_l.GetDataSize();
image_left->data.resize(data_size);
memcpy(&image_left->data[0], convertedImage_l.GetData(), data_size);
image_right->height = convertedImage_r.GetRows();
image_right->width = convertedImage_r.GetCols();
image_right->step = convertedImage_r.GetStride();
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_right->header.frame_id = frame;
data_size = convertedImage_r.GetDataSize();
image_right->data.resize(data_size);
memcpy(&image_right->data[0], convertedImage_r.GetData(), data_size);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
}
}
/* converts giflib format image into enfle format image */
static int
gif_convert(Image *p, GIF_info *g_info, GIF_image *image)
{
GIF_ct *ct;
int i, if_animated;
//int transparent_disposal;
#if 0
if (image->next != NULL) {
if ((p->next = image_create()) == NULL) {
image_destroy(p);
return 0;
}
if (!gif_convert(p->next, g_info, image->next)) {
image_destroy(p);
return 0;
}
} else
p->next = NULL;
#endif
//swidth = g_info->sd->width;
//sheight = g_info->sd->height;
image_left(p) = image->id->left;
image_top(p) = image->id->top;
image_width(p) = image->id->width;
image_height(p) = image->id->height;
#if 0
if (image_width(p) > swidth || image_height(p) > sheight) {
show_message("screen (%dx%d) but image (%dx%d)\n",
swidth, sheight, p->width, p->height);
swidth = image_width(p);
sheight = image_height(p);
}
#endif
p->ncolors = image->id->lct_follows ? 1 << image->id->depth : 1 << g_info->sd->depth;
p->type = _INDEX;
//p->delay = image->gc->delay ? image->gc->delay : 1;
if_animated = g_info->npics > 1 ? 1 : 0;
debug_message("GIF: %d pics animation %d\n", g_info->npics, if_animated);
#if 0
if (image->gc->transparent) {
p->transparent_disposal = if_animated ? _TRANSPARENT : transparent_disposal;
p->transparent.index = image->gc->transparent_index;
} else
p->transparent_disposal = _DONOTHING;
p->image_disposal = image->gc->disposal;
p->background.index = g_info->sd->back;
#endif
if (image->id->lct_follows)
ct = image->id->lct;
else if (g_info->sd->gct_follows)
ct = g_info->sd->gct;
else {
fprintf(stderr, "Null color table..\n");
ct = NULL;
}
for (i = 0; i < (int)p->ncolors; i++) {
p->colormap[i][0] = ct->cell[i]->value[0];
p->colormap[i][1] = ct->cell[i]->value[1];
p->colormap[i][2] = ct->cell[i]->value[2];
}
image_bpl(p) = image_width(p);
if (!image_image(p))
image_image(p) = memory_create();
if (memory_alloc(image_image(p), image_bpl(p) * image_height(p)) == NULL)
return 0;
memcpy(memory_ptr(image_image(p)), image->data, image_bpl(p) * image_height(p));
return 1;
}
/**
* Independent thread that grabs images and publishes them.
*/
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
ROS_INFO("feedImages");
cv::Mat left, right;
// ok is a global that goes false in destructor
while (ok) {
//unsigned char *frame_left = NULL, *frame_right = NULL;
//uint32_t bytes_used_left, bytes_used_right;
ros::Time capture_time = ros::Time::now();
//int left_idx = cam_left->grab(&frame_left, bytes_used_left);
//int right_idx = cam_right->grab(&frame_right, bytes_used_right);
left = cam_left->getImage();
right = cam_left->getImage();
// Read in every frame the camera generates, but only send each
// (skip_frames + 1)th frame. This reduces effective frame
// rate, processing time and network usage while keeping the
// images synchronized within the true framerate.
//
if (skip_frames == 0 || frames_to_skip == 0) {
//if (frame_left && frame_right) {
if(left.data && right.data){
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = 3 * width;
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = 3 * width;
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
/*
if (rotate_left)
rotate(&image_left->data[0], frame_left, width * height);
else
memcpy(&image_left->data[0], frame_left, width * height * 3);
if (rotate_right)
rotate(&image_right->data[0], frame_right, width * height);
else
memcpy(&image_right->data[0], frame_right, width * height * 3);
*/
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
}
frames_to_skip = skip_frames;
} else {
frames_to_skip--;
}
//if (frame_left) cam_left->release(left_idx);
//if (frame_right) cam_right->release(right_idx);
}
}
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
dynamic_reconfigure::Server<see3cam::cameraParamsConfig> server;
dynamic_reconfigure::Server<see3cam::cameraParamsConfig>::CallbackType f;
f = boost::bind(&see3cam::StereoCamera::callback, this, _1, _2);
server.setCallback(f);
while (ok) {
unsigned char *frame_left = NULL, *frame_right = NULL;
uint32_t bytes_used_left, bytes_used_right;
int channels = 2;
if (encoding == "rgb8") {
channels = 3;
}
if (encoding == "mono8") {
channels = 1;
}
int left_idx = cam_left->grab(&frame_left, bytes_used_left);
int right_idx = cam_right->grab(&frame_right, bytes_used_right);
ros::Time capture_time = ros::Time::now();
if (frame_left && frame_right) {
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = channels * width;
image_left->encoding = encoding;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = channels * width;
image_right->encoding = encoding;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
if (rotate_left)
switch (channels) {
case 1:
RotateMono(&image_left->data[0], frame_left, width * height);
break;
case 3:
RotateRgb(&image_left->data[0], frame_left, width * height);
break;
default:
RotateYuv(&image_left->data[0], frame_left, width * height);
}
else
memcpy(&image_left->data[0], frame_left, image_left->step * height);
if (rotate_right)
switch (channels) {
case 1:
RotateMono(&image_right->data[0], frame_right, width * height);
break;
case 3:
RotateRgb(&image_right->data[0], frame_right, width * height);
break;
default:
RotateYuv(&image_right->data[0], frame_right, width * height);
}
else
memcpy(&image_right->data[0], frame_right, image_right->step * height);
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
} else {
// Grab failure - need to restart camera driver.
if (cam_left)
delete cam_left;
if (cam_right)
delete cam_right;
sleep (1);
cam_left = new uvc_cam::Cam(left_device.c_str(), mode, width, height, fps);
cam_right = new uvc_cam::Cam(right_device.c_str(), mode, width, height, fps);
sleep (1);
// Force dynamic reconfigure to update
std::string cmd = "rosrun dynamic_reconfigure dynparam set " + ros::this_node::getName() + " exposureauto 1";
printf((cmd + "\n").c_str());
system(cmd.c_str());
}
if (frame_left)
cam_left->release(left_idx);
if (frame_right)
cam_right->release(right_idx);
}
}
