view.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <unistd.h>
00003 #include <cstdio>
00004 #include <cassert>
00005 #include <ros/time.h>
00006 #include "uvc_cam/uvc_cam.h"
00007 #include "SDL/SDL.h"
00008 
00009 unsigned WIDTH = 640, HEIGHT = 480, FPS = 30;
00010 
00011 void save_photo(uint8_t *frame)
00012 {
00013   static int image_num = 0;
00014   char fnbuf[500];
00015   snprintf(fnbuf, sizeof(fnbuf), "image%06d.ppm", image_num++);
00016   printf("saving %s\n", fnbuf);
00017   FILE *f = fopen(fnbuf, "wb");
00018   if (!f)
00019   {
00020     printf("couldn't open %s\n", fnbuf);
00021     return;
00022   }
00023   fprintf(f, "P6\n%d %d\n255\n", WIDTH, HEIGHT);
00024   fwrite(frame, 1, WIDTH * HEIGHT * 3, f);
00025   fclose(f);
00026 }
00027 
00028 int main(int argc, char **argv)
00029 {
00030   if (argc != 2 && argc != 4 && argc != 5)
00031   {
00032     fprintf(stderr, "usage: view DEVICE WIDTH HEIGHT\n");
00033     return 1;
00034   }
00035 
00036   if(argc == 4)
00037   {
00038     WIDTH = atoi(argv[2]);
00039     HEIGHT= atoi(argv[3]);
00040   }
00041   if(argc == 5)
00042   {
00043     WIDTH = atoi(argv[2]);
00044     HEIGHT= atoi(argv[3]);
00045     FPS = atoi(argv[4]);
00046   }
00047 
00048   ros::Time::init();
00049   ROS_INFO("opening camera device...");
00050   uvc_cam::Cam cam(argv[1], uvc_cam::Cam::MODE_RGB, WIDTH, HEIGHT, FPS);
00051   ROS_INFO("\n\n\nsuccess!\n\n\n");
00052   if (SDL_Init(SDL_INIT_VIDEO) < 0)
00053   {
00054     fprintf(stderr, "sdl init error: %s\n", SDL_GetError());
00055     return 1;
00056   }
00057   atexit(SDL_Quit);
00058   SDL_Surface *surf = SDL_SetVideoMode(WIDTH, HEIGHT, 24, SDL_HWSURFACE);
00059   assert(surf);
00060   SDL_WM_SetCaption("hello world", "hello world");
00061   ros::Time t_prev(ros::Time::now());
00062   int count = 0;
00063   uint8_t *bgr_frame = new uint8_t[WIDTH*HEIGHT*3];
00064   for (bool done = false; !done;)
00065   {
00066     unsigned char *frame = NULL;
00067     uint32_t bytes_used;
00068     int buf_idx = cam.grab(&frame, bytes_used);
00069     if (count++ % 30 == 0)
00070     {
00071       ros::Time t(ros::Time::now());
00072       ros::Duration d(t - t_prev);
00073       printf("%.1f fps\n", 30.0 / d.toSec());
00074       t_prev = t;
00075     }
00076     if (frame)
00077     {
00078       for (uint32_t y = 0; y < HEIGHT; y++)
00079         for (uint32_t x = 0; x < WIDTH; x++)
00080         {
00081           uint8_t *p = frame     + y * WIDTH * 3 + x * 3;
00082           uint8_t *q = bgr_frame + y * WIDTH * 3 + x * 3;
00083           q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
00084         }
00085       memcpy(surf->pixels, bgr_frame, WIDTH*HEIGHT*3);
00086       cam.release(buf_idx);
00087     }
00088     //usleep(1000);
00089     SDL_UpdateRect(surf, 0, 0, WIDTH, HEIGHT);
00090     SDL_Event event;
00091     while (SDL_PollEvent(&event))
00092     {
00093       switch (event.type)
00094       {
00095         case SDL_KEYDOWN:
00096           switch(event.key.keysym.sym)
00097           {
00098             case SDLK_ESCAPE: done = true; break;
00099             case SDLK_SPACE:  save_photo(frame); break;
00100             default: break;
00101           }
00102           break;
00103         case SDL_QUIT:
00104           done = true;
00105           break;
00106       }
00107     }
00108   }
00109   delete[] bgr_frame;
00110   return 0;
00111 }
00112 


corobot_camera
Author(s):
autogenerated on Wed Aug 26 2015 11:09:42