dump_send.cpp
Go to the documentation of this file.
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 #include <ros/ros.h>
00009 #include "sensor_msgs/Image.h"
00010 #include "sensor_msgs/image_encodings.h"
00011 extern "C"
00012 {
00013 #include "avilib.h"
00014 }
00015 
00016 const unsigned WIDTH = 160, HEIGHT = 120, FPS = 10;
00017 static bool done = false;
00018 
00019 void sigint_handler(int sig)
00020 {
00021   done = true;
00022 }
00023 
00024 int main(int argc, char **argv)
00025 {
00026   if (argc < 2)
00027   {
00028     fprintf(stderr, "usage: view DEVICE\n");
00029     return 1;
00030   }
00031   ros::init(argc, argv, "uvc_cam");
00032   ros::NodeHandle n;
00033   ros::Publisher pub = n.advertise<sensor_msgs::Image>("image", 1);
00034 
00035   avi_t avi;
00036   ros::Time t_prev(ros::Time::now());
00037   char fname[200], log_fname[200];
00038   sprintf(fname, "%.3f.avi", t_prev.toSec());
00039   sprintf(log_fname, "%.3f.txt", t_prev.toSec());
00040   FILE *time_log = fopen(log_fname, "w");
00041   if (!time_log)
00042     throw std::runtime_error("couldn't open frame time log");
00043   if (AVI_open_output_file(&avi, fname) < 0)
00044     throw std::runtime_error("couldn't open AVI file");
00045   AVI_set_video(&avi, WIDTH, HEIGHT, FPS, "RGB");
00046 
00047   uvc_cam::Cam cam(argv[1], uvc_cam::Cam::MODE_RGB);
00048   int count = 0, keyframe = 1;
00049   signal(SIGINT, sigint_handler);
00050   while (n.ok() && !done)
00051   {
00052     unsigned char *frame = NULL;
00053     uint32_t bytes_used;
00054     int buf_idx = cam.grab(&frame, bytes_used);
00055     //printf("      %d byte image\n", bytes_used);
00056     if (frame)
00057     {
00058       sensor_msgs::Image image; 
00059       
00060       image.header.stamp = ros::Time::now();
00061 //      image.label = "UVC Camera Image";
00062       image.encoding = sensor_msgs::image_encodings::RGB8;
00063 //      image.depth = "uint8";
00064 
00065       image.height = HEIGHT;
00066       image.width = WIDTH;
00067 
00068 //      image.is_bigendian = ;
00069       image.step = 3 * WIDTH;
00070             
00071 //      std_msgs::UInt8MultiArray & multi_arr = image.uint8_data;
00072 //      multi_arr.layout.dim.resize(3);
00073 //      multi_arr.layout.dim[0].label  = "height";
00074 //      multi_arr.layout.dim[0].size   = HEIGHT;
00075 //      multi_arr.layout.dim[0].stride = 3 * HEIGHT * WIDTH;
00076 //      multi_arr.layout.dim[1].label  = "width";
00077 //      multi_arr.layout.dim[1].size   = WIDTH;
00078 //      multi_arr.layout.dim[1].stride = 3 * WIDTH;
00079 //      multi_arr.layout.dim[2].label  = "channel";
00080 //      multi_arr.layout.dim[2].size   = 3;
00081 //      multi_arr.layout.dim[2].stride = 3;
00082       
00083 //      multi_arr.data.resize(3 * HEIGHT * WIDTH);
00084 //      memcpy(&multi_arr.data[0], frame, WIDTH*HEIGHT*3);
00085       
00086 //      uint8_t* bgr = &multi_arr.data[0];
00087 
00088       image.data.resize( image.step * image.height );
00089       uint8_t* bgr = &(image.data[0]);
00090       
00091       for (uint32_t y = 0; y < HEIGHT; y++)
00092         for (uint32_t x = 0; x < WIDTH; x++)
00093           {
00094             uint8_t *p = frame + y * WIDTH * 3 + x * 3;
00095             uint8_t *q = bgr   + y * WIDTH * 3 + x * 3;
00096             q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
00097           }
00098       pub.publish(image);
00099      
00100       fprintf(time_log, "%.6f\n", ros::Time::now().toSec());
00101       //AVI_write_frame(&avi, frame, bytes_used, keyframe);
00102       AVI_write_frame(&avi, frame, 3*HEIGHT*WIDTH, keyframe);
00103 
00104       //fwrite(frame, bytes_used, 1, f);
00105 
00106       cam.release(buf_idx);
00107       if (keyframe) keyframe = 0;
00108       /*
00109       memcpy(surf->pixels, frame, WIDTH*HEIGHT*3);
00110       cam.release(buf_idx);
00111       */
00112     }
00113     if (count++ % FPS == 0)
00114     {
00115       ros::Time t(ros::Time::now());
00116       ros::Duration d(t - t_prev);
00117       printf("%.1f fps\n", (double)FPS / d.toSec());
00118       t_prev = t;
00119     }
00120   }
00121   fclose(time_log);
00122   AVI_close(&avi);
00123   return 0;
00124 }


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