camera.cpp

用于机器人自动低分辨路的地图测绘程序。用于机器人控制测绘。分为远端控制端和本地控制端。控制电机为标准舵机。

/*
    Robot Interface
    (C) 2006 Jason Hunt
    [e-mail]
    
    File: camera.cpp
*/


#include <windows.h>
#include <stdio.h>

#include "camera.h"
#include "utility.h"
#include "types.h"
#include "compress.h"
#include "display.h"
#include "vfwmod.h"
#include "network.h"


camera_class camera;

/* Camera constuctor */
camera_class::camera_class(void)
{
    InitializeCriticalSection(&camera_thread_lock);
    InitializeCriticalSection(&compressed_buffer_lock);

    // Create event for image request
	request_image_sem = CreateEvent(NULL, 0, 0, NULL);
	image_ready = CreateEvent(NULL, 0, 0, NULL);


    capture_win = NULL;                // Handle to the capture window
    camera_dc= NULL;                          // Camera device context

    compressed_buffer = NULL;         // Buffer for compressed data
    transcode_buffer = NULL;          // Buffer for transcoding
    camera_buffer = NULL;       // Buffer for camera data

    x_size = 0;         // Size of camera image
    y_size = 0;

    thread_running = false;         // True if thread running


    camera_number = 0;                  // Camera index
 
    max_compressed = 0;                 // Max compressed size

    frames = 0;         // Numbers of frames so far in this second
    update_rate = 0;    // Frames per second

    compressed_image_size = 0;
}
/* End of camera constuctor */ 



/* Start camera system */
void camera_class::start( HWND hwnd )
{
    char buff[400];
  
    capture_win = capCreateCaptureWindow ("Preview", 0, 0, 0, 320, 260, HWND_DESKTOP, 0); 

//     capture_win = capCreateCaptureWindow ("Preview", WS_VISIBLE, 0, 0, 320, 260,
//              HWND_DESKTOP                            , 0); 
                                       
    if (!capture_win)
    {
        add_line("Unable to create capture window");
        return;
    }

    // Setup Frame callback 
    // capSetCallbackOnFrame(capture_win, frame_callback);
               
    // Used for displaying a list of devices 
    char device_name[160];
    char device_version[160];

    if (capGetDriverDescription (camera_number, device_name, sizeof (device_name), 
                                 device_version, sizeof (device_version)))
    { 
        sprintf(buff, "Camera: %s", device_name);
        add_line(buff);                      
    }

    capDriverConnect(capture_win, 0);       // connect to driver    

//    capPreviewRate(capture_win, 20);         // Max preview rate
    capPreview(capture_win, FALSE);          // Show preview     

    setup_camera(hwnd, false);   // setup buffers
    
    // Set thread as running
	thread_running = true;
		
    // Create camera thread
	thread_handle  = CreateThread(NULL, 0, (unsigned long (__stdcall *)(void *))this->run_thread, (void *)this, 0, &thread_id);      

    // Request first image
    SetEvent(request_image_sem);  
}
/* End of Start camera */


/* Stop camera system */
void camera_class::stop( void )
{
    // Stop Serial Thread
    EnterCriticalSection(&camera_thread_lock);
    thread_running = false;
    LeaveCriticalSection(&camera_thread_lock);

    // Wait for therad to exit
    WaitForSingleObject(thread_handle,INFINITE);

    CloseHandle(request_image_sem);
 
    if (camera_buffer)
        free(camera_buffer);      

    if (compressed_buffer)
       free(compressed_buffer);  
 
    if (!capture_win) return;
 
      SendMessage(capture_win, WM_CAP_DRIVER_DISCONNECT, 0, 0);   
}
/* End of stop_camera */



/* Called once a second for calulate frame rate */
void camera_class::update( void )
{
    update_rate = frames;       // Set update rate
    frames = 0;                 // Reset frame counter
}
/* End of update */





void camera_class::request_image( void )
{    
    SetEvent(request_image_sem); 
    frames++;    
}

/* Send image to client */
void camera_class::send_image( void )
{
    // Compress the current frame 
    // Offset of 9 makes room for the header
    compress_frame((byte *)camera_buffer, (byte *)(compressed_buffer+9),
                   (byte *)transcode_buffer, max_compressed,
                   &compressed_image_size, x_size, y_size);        
                               
    // Construct packet header
    compressed_buffer[0] = 0x60; // Server to client command for image block
    
    compressed_buffer[1] = x_size >> 8;       // x size high byte
    compressed_buffer[2] = x_size & 0xff;     // x size low byte

    compressed_buffer[3] = y_size >> 8;       // y size high byte
    compressed_buffer[4] = y_size & 0xff;     // y size low byte
            
    compressed_buffer[5] = (compressed_image_size >> 24) & 0xff; // image data 4th byte (high)
    compressed_buffer[6] = (compressed_image_size >> 16) & 0xff; // image data 3rd byte 
    compressed_buffer[7] = (compressed_image_size >> 8) & 0xff;  // image data 2nd byte 
    compressed_buffer[8] = (compressed_image_size) & 0xff;       // image data 1st byte (low)
            
    network_send(compressed_buffer, compressed_image_size + 9);
}

/* End of send image */



/* Function to start the member thread */
int camera_class::run_thread(void * p_this)
{
    return ((camera_class*)(p_this))->camera_thread(0);    
}
/* End of run_thread */


/* Capture frame and wait for client request */
DWORD WINAPI camera_class::camera_thread( LPVOID lpParam ) 
{
    static unsigned int count = 0;

    bool done = false;
	while(!done)
	{
        if (capture_win && camera_buffer)
        {
            // Wait for 1/10 of a second for a request, otherwise discard frame 
           if (WaitForSingleObject(request_image_sem, 100) == WAIT_OBJECT_0)   // See if the event was singaled
            {               
                //EnterCriticalSection(&compressed_buffer_lock);
                capGrabFrame(capture_win);  // Capture frame and wait for callback
                capEditCopy(capture_win);
                OpenClipboard(NULL);
                HBITMAP tmp =(HBITMAP) GetClipboardData(CF_BITMAP);
                CloseClipboard();

                // Transfor into image array
                GetDIBits(camera_dc, tmp, 0, y_size, camera_buffer, &camera_buffer_info, DIB_RGB_COLORS);
                DeleteObject(tmp);
                send_image();           // Send the image to the client
            }                
        }

        EnterCriticalSection(&camera_thread_lock);
        if (!thread_running) done = true;
        LeaveCriticalSection(&camera_thread_lock);
    }

    return(0);
}
/* End of camera thread */


/* Setup buffer for camera image */
void camera_class::setup_buffer( void )
{    
    if (camera_buffer != NULL) free(camera_buffer);    

    camera_buffer = (color_type *) malloc(x_size * y_size * sizeof(color_type));
        
    if (camera_buffer == NULL)
    {            
        add_line("Unable to create camera buffer");
        return;
    }        

    if (compressed_buffer != NULL) free(compressed_buffer);    

    max_compressed = x_size * y_size * sizeof(color_type) * 2;

    compressed_buffer = (byte *) malloc(max_compressed);
        
    if (compressed_buffer == NULL)
    {            
        add_line("Unable to create compressed buffer");
        return;
    }        


    if (transcode_buffer != NULL) free(transcode_buffer);    

    transcode_buffer = (byte *) malloc(x_size * y_size * sizeof(color_type));
        
    if (transcode_buffer == NULL)
    {            
        add_line("Unable to create transcode buffer");
        return;
    } 

    // Set the new Bitmap information
    camera_buffer_info.bmiHeader.biSize = sizeof(camera_buffer_info.bmiHeader);
    camera_buffer_info.bmiHeader.biWidth = x_size;
    camera_buffer_info.bmiHeader.biHeight = y_size; 
    camera_buffer_info.bmiHeader.biPlanes = 1;
    camera_buffer_info.bmiHeader.biBitCount = 32;
    camera_buffer_info.bmiHeader.biCompression = BI_RGB;
    camera_buffer_info.bmiHeader.biSizeImage = x_size * y_size  * 4;
    camera_buffer_info.bmiHeader.biClrUsed = 0;
    camera_buffer_info.bmiHeader.biClrImportant = 0;
}
/* End of camera_setup_buffer */


/* Setup the camera buffers and parameters */
void camera_class::setup_camera( HWND hwnd, bool ask_info)
{        
    // Get main DC
    HDC hdc = GetDC(hwnd);

  	// Create Memory device contextand bitmap
    camera_dc  = CreateCompatibleDC(hdc);	

    ReleaseDC(hwnd, hdc);       // Main DC is no longer needed
    
    
    // Get video format information
    if (ask_info)
    {
        capDlgVideoSource(capture_win);
        capDlgVideoFormat(capture_win);         
    }

    // Get video format information
    capGetVideoFormat(capture_win, &camera_buffer_info, sizeof(camera_buffer_info));
   
    if ( camera_buffer_info.bmiHeader.biWidth > 0 &&
         camera_buffer_info.bmiHeader.biHeight > 0)  
    {
        char buff[400];
        sprintf(buff, "Size (%dx%d)", camera_buffer_info.bmiHeader.biWidth,
                                      camera_buffer_info.bmiHeader.biHeight);
                                      add_line(buff);
    }
    else
    {
        add_line("Camera not detected");

    }

    x_size = camera_buffer_info.bmiHeader.biWidth;
    y_size = camera_buffer_info.bmiHeader.biHeight;   

    setup_buffer();
}
/* End of setup camera */