00001 #include <unistd.h>
00002 #include <cstdio>
00003 #include <cassert>
00004 #include <ros/time.h>
00005 #include "uvc_cam/uvc_cam.h"
00006 #include <signal.h>
00007 #include <stdexcept>
00008
00009 const unsigned WIDTH = 640, HEIGHT = 480, FPS = 30;
00010 static bool done = false;
00011
00012 void sigint_handler(int sig)
00013 {
00014 done = true;
00015 }
00016
00017 int main(int argc, char **argv)
00018 {
00019 if (argc < 2)
00020 {
00021 fprintf(stderr, "usage: view DEVICE\n");
00022 return 1;
00023 }
00024 ros::Time t_prev(ros::Time::now());
00025 char fname[200], log_fname[200];
00026 sprintf(fname, "%.3f.raw", t_prev.toSec());
00027 FILE *f = fopen(fname, "w");
00028 uvc_cam::Cam cam(argv[1], uvc_cam::Cam::MODE_YUYV, WIDTH, HEIGHT, FPS);
00029 int count = 0, keyframe = 1;
00030 signal(SIGINT, sigint_handler);
00031 while (!done)
00032 {
00033 unsigned char *frame = NULL;
00034 uint32_t bytes_used;
00035 int buf_idx = cam.grab(&frame, bytes_used);
00036 if (frame)
00037 {
00038 double d = ros::Time::now().toSec();
00039 fwrite(&d, sizeof(double), 1, f);
00040 fwrite(frame, WIDTH*HEIGHT*2, 1, f);
00041 cam.release(buf_idx);
00042 }
00043 if (count++ % FPS == 0)
00044 {
00045 ros::Time t(ros::Time::now());
00046 ros::Duration d(t - t_prev);
00047 printf("%10d %.1f fps\n", count, (double)FPS / d.toSec());
00048 t_prev = t;
00049 }
00050 }
00051 fclose(f);
00052 printf("goodbye\n");
00053 return 0;
00054 }
00055
