int main(int argc, char **argv)
{
UdpConnectReceive(UDP_HOST, UDP_PORT);
std::list<UdpPacket> udp_packets;
uint8_t *image = NULL;
int width, height, channels;
struct timespec ts1, ts2;
uint32_t count = 0;
double dt_acc = 0;
printf("connected!\n");
while(1)
{
UdpReceiveGetPackets(udp_packets);
if (0)//udp_packets.begin()==udp_packets.end())
printf("no packets received\n");
for(std::list<UdpPacket>::iterator it = udp_packets.begin(); it != udp_packets.end(); it++)
{
count++;
clock_gettime(CLOCK_MONOTONIC, &ts2);
double dt = (ts2.tv_sec - ts1.tv_sec)*1000 + (ts2.tv_nsec - ts1.tv_nsec)/1e6;
dt_acc += dt;
printf("dt: %f ms\n", dt);
clock_gettime(CLOCK_MONOTONIC, &ts1);
//parse imu data
//decompress image data
jpeg_decompress(&(it->data[12*4]), it->data.size(), &image, &width, &height, &channels);
//printf("width: %d, height: %d\n", width, height);
#if 1
if(channels == 1)
imgproc(image, width, height);
else
printf("Expecting monochrome image, got image with channels = %d\n", channels);
#endif
int N = 10;
if(count % N == 0)
{
//printf("dt: %f ms\n", dt_acc/N);
count = 0;
dt_acc = 0;
}
printf("received packet\n");
}
usleep(1000);
}
return 0;
}
void xf_Bitmap_Decompress(rdpContext* context, rdpBitmap* bitmap,
BYTE* data, int width, int height, int bpp, int length,
BOOL compressed, int codec_id)
{
UINT16 size;
BYTE* src;
BYTE* dst;
int yindex;
int xindex;
BOOL status;
RFX_MESSAGE* msg;
xfContext* xfc = (xfContext*) context;
size = width * height * ((bpp + 7) / 8);
if (!bitmap->data)
bitmap->data = (BYTE*) _aligned_malloc(size, 16);
else
bitmap->data = (BYTE*) _aligned_realloc(bitmap->data, size, 16);
switch (codec_id)
{
case RDP_CODEC_ID_NSCODEC:
DEBUG_WARN( "xf_Bitmap_Decompress: nsc not done\n");
break;
case RDP_CODEC_ID_REMOTEFX:
rfx_context_set_pixel_format(xfc->rfx, RDP_PIXEL_FORMAT_B8G8R8A8);
msg = rfx_process_message(xfc->rfx, data, length);
if (!msg)
{
DEBUG_WARN( "xf_Bitmap_Decompress: rfx Decompression Failed\n");
}
else
{
for (yindex = 0; yindex < height; yindex++)
{
src = msg->tiles[0]->data + yindex * 64 * 4;
dst = bitmap->data + yindex * width * 3;
for (xindex = 0; xindex < width; xindex++)
{
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
src++;
}
}
rfx_message_free(xfc->rfx, msg);
}
break;
case RDP_CODEC_ID_JPEG:
if (!jpeg_decompress(data, bitmap->data, width, height, length, bpp))
{
DEBUG_WARN( "xf_Bitmap_Decompress: jpeg Decompression Failed\n");
}
break;
default:
if (compressed)
{
status = bitmap_decompress(data, bitmap->data, width, height, length, bpp, bpp);
if (!status)
{
DEBUG_WARN( "xf_Bitmap_Decompress: Bitmap Decompression Failed\n");
}
}
else
{
freerdp_image_flip(data, bitmap->data, width, height, bpp);
}
break;
}
bitmap->compressed = FALSE;
bitmap->length = size;
bitmap->bpp = bpp;
}
void gdi_Bitmap_Decompress(rdpContext* context, rdpBitmap* bitmap,
BYTE* data, int width, int height, int bpp, int length,
BOOL compressed, int codecId)
{
BOOL status;
UINT16 size;
BYTE* src;
BYTE* dst;
int yindex;
int xindex;
rdpGdi* gdi;
RFX_MESSAGE* msg;
size = width * height * ((bpp + 7) / 8);
if (!bitmap->data)
bitmap->data = (BYTE*) malloc(size);
else
bitmap->data = (BYTE*) realloc(bitmap->data, size);
switch (codecId)
{
case RDP_CODEC_ID_NSCODEC:
gdi = context->gdi;
nsc_process_message(gdi->nsc_context, bpp, width, height, data, length);
freerdp_image_flip(((NSC_CONTEXT*) gdi->nsc_context)->BitmapData, bitmap->data, width, height, bpp);
break;
case RDP_CODEC_ID_REMOTEFX:
gdi = context->gdi;
rfx_context_set_pixel_format(gdi->rfx_context, RDP_PIXEL_FORMAT_B8G8R8A8);
msg = rfx_process_message(gdi->rfx_context, data, length);
if (!msg)
{
fprintf(stderr, "gdi_Bitmap_Decompress: rfx Decompression Failed\n");
}
else
{
for (yindex = 0; yindex < height; yindex++)
{
src = msg->tiles[0]->data + yindex * 64 * 4;
dst = bitmap->data + yindex * width * 3;
for (xindex = 0; xindex < width; xindex++)
{
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
src++;
}
}
rfx_message_free(gdi->rfx_context, msg);
}
break;
case RDP_CODEC_ID_JPEG:
#ifdef WITH_JPEG
if (!jpeg_decompress(data, bitmap->data, width, height, length, bpp))
{
fprintf(stderr, "gdi_Bitmap_Decompress: jpeg Decompression Failed\n");
}
#endif
break;
default:
if (compressed)
{
status = bitmap_decompress(data, bitmap->data, width, height, length, bpp, bpp);
if (!status)
{
fprintf(stderr, "gdi_Bitmap_Decompress: Bitmap Decompression Failed\n");
}
}
else
{
freerdp_image_flip(data, bitmap->data, width, height, bpp);
}
break;
}
bitmap->width = width;
bitmap->height = height;
bitmap->compressed = FALSE;
bitmap->length = size;
bitmap->bpp = bpp;
}
PyObject *RovioCam::updateMMap(int decompress) {
char *header, *type, *creator, *fmt, *image_buffer;
long format, compression, newWidth, newHeight, timeStamp, frameNum, unknown1;
long chanWidth, chanHeight, layer, chanID, unknown2, size;
//printf("In updateMMap 1\n");
lock.ReadLock();
//printf("In updateMMap 2\n");
// get an image from socket
// Got type=TekkotsuImage
// Got format=0
// Got compression=1
// Got newWidth=104
// Got newHeight=80
// Got timest=121465
// Got frameNum=3185
//printf("receiving...\n");
if (tcp) {
header = sock->read(4); // \r\0\0\0
//printf("In updateMMap 3: '%s'\n", header);
type = sock->readUntil((char)0); // "TekkotsuImage"
//printf("type: '%s'\n", type);
format = convert(sock->read(4));
//printf("format: %ld\n", format);
compression = convert(sock->read(4));
//printf("compression: %ld\n", compression);
newWidth = convert(sock->read(4));
//printf("newWidth: %ld\n", newWidth);
newHeight = convert(sock->read(4));
//printf("newHeight: %ld\n", newHeight);
timeStamp = convert(sock->read(4));
//printf("timeStamp: %ld\n", timeStamp);
frameNum = convert(sock->read(4));
//printf("frameNum: %ld\n", frameNum);
unknown1 = convert(sock->read(4));
//printf("unknown1: %ld\n", unknown1);
//// Got creator=FbkImage
//// Got chanwidth=104
//// Got chanheight=80
//// Got layer=3
//// Got chan_id=0
//// Got fmt=JPEGColor
//// read JPEG: len=2547
creator = sock->readUntil((char)0); // creator
//printf("creator: %s\n", creator);
chanWidth = convert(sock->read(4));
//printf("chanWidth: %ld\n", chanWidth);
chanHeight = convert(sock->read(4));
//printf("chanHeight: %ld\n", chanHeight);
layer = convert(sock->read(4));
//printf("layer: %ld\n", layer);
chanID = convert(sock->read(4));
//printf("chanID: %ld\n", chanID);
unknown2 = convert(sock->read(4));
//printf("unknown2: %ld\n", unknown2);
fmt = sock->readUntil((char)0); // fmt
//printf("fmt: %s\n", fmt);
size = convert(sock->read(4));
image_buffer = sock->read(size);
} else { // UDP
printf("reading...\n");
// wait until answer comes back, for up to 1 second
char *st = sock->read(MAXBUFSIZE);
printf("done!\n");
printf("making a fake socket...\n");
FakeSocket fakesock = FakeSocket(st);
header = fakesock.read(4); // \r\0\0\0
printf("In updateMMap 3: '%s'\n", header);
type = fakesock.readUntil((char)0); // "TekkotsuImage"
printf("type: '%s'\n", type);
format = convert(fakesock.read(4));
printf("format: %ld\n", format);
compression = convert(fakesock.read(4));
printf("compression: %ld\n", compression);
newWidth = convert(fakesock.read(4));
printf("newWidth: %ld\n", newWidth);
newHeight = convert(fakesock.read(4));
printf("newHeight: %ld\n", newHeight);
timeStamp = convert(fakesock.read(4));
printf("timeStamp: %ld\n", timeStamp);
frameNum = convert(fakesock.read(4));
printf("frameNum: %ld\n", frameNum);
unknown1 = convert(fakesock.read(4));
printf("unknown1: %ld\n", unknown1);
//// Got creator=FbkImage
//// Got chanwidth=104
//// Got chanheight=80
//// Got layer=3
//// Got chan_id=0
//// Got fmt=JPEGColor
//// read JPEG: len=2547
creator = fakesock.readUntil((char)0); // creator
printf("creator: %s\n", creator);
chanWidth = convert(fakesock.read(4));
printf("chanWidth: %ld\n", chanWidth);
chanHeight = convert(fakesock.read(4));
printf("chanHeight: %ld\n", chanHeight);
layer = convert(fakesock.read(4));
printf("layer: %ld\n", layer);
chanID = convert(fakesock.read(4));
printf("chanID: %ld\n", chanID);
unknown2 = convert(fakesock.read(4));
printf("unknown2: %ld\n", unknown2);
fmt = fakesock.readUntil((char)0); // fmt
printf("fmt: %s\n", fmt);
size = convert(fakesock.read(4));
printf("size: %ld\n", size);
}
// convert image from JPEG to RGB in mmap
if (width == 0 && height == 0) {
width = newWidth;
height = newHeight;
printf("New Rovio image size: %d x %d; %ld\n", width, height, size);
lock.ReadUnlock();
return PyInt_FromLong(0);
}
if (decompress) {
if (size > 0 && size < 10000) {
//printf("decompressing...\n");
jpeg_decompress((unsigned char *)image, (width * height * depth),
(unsigned char *)image_buffer, (int) size);
//printf("done!\n");
lock.ReadUnlock();
return PyInt_FromLong(size);
} else {
printf("Rovio camera bad JPEG size: %ld\n", size);
lock.ReadUnlock();
return PyInt_FromLong(0);
}
}
lock.ReadUnlock();
return PyInt_FromLong(0);
}
GLOBAL int
read_JPEG_file (char * filename)
{
/* These three structs contain JPEG parameters and working data.
* They must survive for the duration of parameter setup and one
* call to jpeg_decompress; typically, making them local data in the
* calling routine is the best strategy.
*/
struct Decompress_info_struct cinfo;
struct Decompress_methods_struct dc_methods;
struct External_methods_struct e_methods;
/* Select the input and output files.
* In this example we want to open the input file before doing anything else,
* so that the setjmp() error recovery below can assume the file is open.
* Note that cinfo.output_file is only used if your output handling routines
* use it; otherwise, you can just make it NULL.
* VERY IMPORTANT: use "b" option to fopen() if you are on a machine that
* requires it in order to read binary files.
*/
if ((cinfo.input_file = fopen(filename, "rb")) == NULL) {
fprintf(stderr, "can't open %s\n", filename);
return 0;
}
cinfo.output_file = NULL; /* if no actual output file involved */
/* Initialize the system-dependent method pointers. */
cinfo.methods = &dc_methods; /* links to method structs */
cinfo.emethods = &e_methods;
/* Here we supply our own error handler; compare to use of standard error
* handler in the previous write_JPEG_file example.
*/
emethods = &e_methods; /* save struct addr for possible access */
e_methods.error_exit = error_exit; /* supply error-exit routine */
e_methods.trace_message = trace_message; /* supply trace-message routine */
e_methods.trace_level = 0; /* default = no tracing */
e_methods.num_warnings = 0; /* no warnings emitted yet */
e_methods.first_warning_level = 0; /* display first corrupt-data warning */
e_methods.more_warning_level = 3; /* but suppress additional ones */
/* prepare setjmp context for possible exit from error_exit */
if (setjmp(setjmp_buffer)) {
/* If we get here, the JPEG code has signaled an error.
* Memory allocation has already been cleaned up (see free_all call in
* error_exit), but we need to close the input file before returning.
* You might also need to close an output file, etc.
*/
fclose(cinfo.input_file);
return 0;
}
/* Here we use the standard memory manager provided with the JPEG code.
* In some cases you might want to replace the memory manager, or at
* least the system-dependent part of it, with your own code.
*/
jselmemmgr(&e_methods); /* select std memory allocation routines */
/* If the decompressor requires full-image buffers (for two-pass color
* quantization or a noninterleaved JPEG file), it will create temporary
* files for anything that doesn't fit within the maximum-memory setting.
* You can change the default maximum-memory setting by changing
* e_methods.max_memory_to_use after jselmemmgr returns.
* On some systems you may also need to set up a signal handler to
* ensure that temporary files are deleted if the program is interrupted.
* (This is most important if you are on MS-DOS and use the jmemdos.c
* memory manager back end; it will try to grab extended memory for
* temp files, and that space will NOT be freed automatically.)
* See jcmain.c or jdmain.c for an example signal handler.
*/
/* Here, set up the pointer to your own routine for post-header-reading
* parameter selection. You could also initialize the pointers to the
* output data handling routines here, if they are not dependent on the
* image type.
*/
dc_methods.d_ui_method_selection = d_ui_method_selection;
/* Set up default decompression parameters. */
j_d_defaults(&cinfo, TRUE);
/* TRUE indicates that an input buffer should be allocated.
* In unusual cases you may want to allocate the input buffer yourself;
* see jddeflts.c for commentary.
*/
/* At this point you can modify the default parameters set by j_d_defaults
* as needed; for example, you can request color quantization or force
* grayscale output. See jdmain.c for examples of what you might change.
*/
/* Set up to read a JFIF or baseline-JPEG file. */
/* This is the only JPEG file format currently supported. */
jselrjfif(&cinfo);
/* Here we go! */
jpeg_decompress(&cinfo);
/* That's it, son. Nothin' else to do, except close files. */
/* Here we assume only the input file need be closed. */
fclose(cinfo.input_file);
/* You might want to test e_methods.num_warnings to see if bad data was
* detected. In this example, we just blindly forge ahead.
*/
return 1; /* indicate success */
/* Note: if you want to decompress more than one image, we recommend you
* repeat this whole routine. You MUST repeat the j_d_defaults()/alter
* parameters/jpeg_decompress() sequence, as some data structures allocated
* in j_d_defaults are freed upon exit from jpeg_decompress.
*/
}
LRESULT CALLBACK cap_stream_cb(HWND hwnd, VIDEOHDR *pframe)
{
cap_cx *cx;
void *temp = NULL;
int iskeyframe;
size_t datalen;
void *pdata;
size_t outdatalen;
void *poutdata;
int width;
int height;
size_t rgblen;
void *prgb;
int quein;
char message[256];
DWORD decodetime;
if(!(cx = cap_assoc_get(hwnd)))
return TRUE;
if(cx->started)
{
/*
* Get captured frame
*/
pdata = pframe->lpData;
datalen = (size_t)(pframe->dwBytesUsed);
iskeyframe = pframe->dwFlags & VHDR_KEYFRAME;
width = cx->pfmt->bmiHeader.biWidth;
height = cx->pfmt->bmiHeader.biHeight;
//start decoding frame time
decodetime = GetTickCount();
/*
* Use VCM decompressor
*/
if(cx->mode == CAP_USE_IC)
{
// alloc buffer for decompressed frame
outdatalen = cx->poutfmt->bmiHeader.biSizeImage;
if(!(poutdata = malloc(outdatalen)))
{
return TRUE;
}
// decompress frame
if(ICDecompress(
cx->ic,
iskeyframe ? 0 : ICDECOMPRESS_NOTKEYFRAME,
&(cx->pfmt->bmiHeader),
pdata,
&(cx->poutfmt->bmiHeader),
poutdata) != ICERR_OK)
{
free(poutdata);
return TRUE;
}
// to dealloc
temp = poutdata;
// get decompressed frame
pdata = poutdata;
datalen = outdatalen;
width = cx->poutfmt->bmiHeader.biWidth;
height = cx->poutfmt->bmiHeader.biHeight;
}
/*
* Use pixel decoder
*/
if( (cx->mode == CAP_USE_PIXDEC) ||
(cx->mode == CAP_USE_IC) )
{
// alloc buffer
rgblen = width*height*3;
if(!(prgb = malloc(rgblen)))
{
free(temp);
return TRUE;
}
// decode frame
if(!cx->pix_func(prgb, pdata, width, height))
{
free(prgb);
free(temp);
return TRUE;
}
// dealloc encoded frame buffer
free(temp);
}
/*
* Use JPEG decompressor
*/
if(cx->mode == CAP_USE_JPEG)
{
if(!(prgb = jpeg_decompress(pdata, datalen, &width, &height)))
{
return TRUE;
}
}
//output statistics
decodetime = GetTickCount() - decodetime;
sprintf(message, "Frame decode time = %d ms, input length = %d, output length = %d\n",
decodetime, pframe->dwBytesUsed, rgblen);
OutputDebugString(message);
/*
* Put frame in buffer
*/
// check buffer have free slot
if((quein = (cx->que_in + 1) % CAP_QUE) == cx->que_out)
{
OutputDebugString("Capture buffer overflow\n");
free(prgb);
return TRUE;
}
// put frame to free slot
cx->que[cx->que_in].rgb = prgb;
cx->que[cx->que_in].width = width;
cx->que[cx->que_in].height = height;
cx->que[cx->que_in].length = rgblen;
cx->que_in = quein;
}
return TRUE;
}
