Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef USB_CAM_USB_CAM_H
00037 #define USB_CAM_USB_CAM_H
00038
00039 #include <asm/types.h>
00040
00041 extern "C"
00042 {
00043 #include <linux/videodev2.h>
00044 #include <libavcodec/avcodec.h>
00045 #include <libswscale/swscale.h>
00046 #include <libavutil/mem.h>
00047 }
00048
00049
00050 #include <libavcodec/version.h>
00051 #if LIBAVCODEC_VERSION_MAJOR < 55
00052 #define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG
00053 #endif
00054
00055 #include <string>
00056 #include <sstream>
00057
00058 #include <sensor_msgs/Image.h>
00059
00060 namespace usb_cam {
00061
00062 class UsbCam {
00063 public:
00064 typedef enum
00065 {
00066 IO_METHOD_READ, IO_METHOD_MMAP, IO_METHOD_USERPTR, IO_METHOD_UNKNOWN,
00067 } io_method;
00068
00069 typedef enum
00070 {
00071 PIXEL_FORMAT_YUYV, PIXEL_FORMAT_UYVY, PIXEL_FORMAT_MJPEG, PIXEL_FORMAT_YUVMONO10, PIXEL_FORMAT_RGB24, PIXEL_FORMAT_UNKNOWN
00072 } pixel_format;
00073
00074 UsbCam();
00075 ~UsbCam();
00076
00077
00078 void start(const std::string& dev, io_method io, pixel_format pf,
00079 int image_width, int image_height, int framerate);
00080
00081 void shutdown(void);
00082
00083
00084 void grab_image(sensor_msgs::Image* image);
00085
00086
00087 void set_auto_focus(int value);
00088
00089
00090 void set_v4l_parameter(const std::string& param, int value);
00091 void set_v4l_parameter(const std::string& param, const std::string& value);
00092
00093 static io_method io_method_from_string(const std::string& str);
00094 static pixel_format pixel_format_from_string(const std::string& str);
00095
00096 private:
00097 typedef struct
00098 {
00099 int width;
00100 int height;
00101 int bytes_per_pixel;
00102 int image_size;
00103 char *image;
00104 int is_new;
00105 } camera_image_t;
00106
00107 struct buffer
00108 {
00109 void * start;
00110 size_t length;
00111 };
00112
00113
00114 int init_mjpeg_decoder(int image_width, int image_height);
00115 void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels);
00116 void process_image(const void * src, int len, camera_image_t *dest);
00117 int read_frame();
00118 void stop_capturing(void);
00119 void start_capturing(void);
00120 void uninit_device(void);
00121 void init_read(unsigned int buffer_size);
00122 void init_mmap(void);
00123 void init_userp(unsigned int buffer_size);
00124 void init_device(int image_width, int image_height, int framerate);
00125 void close_device(void);
00126 void open_device(void);
00127 void grab_image();
00128
00129
00130 std::string camera_dev_;
00131 unsigned int pixelformat_;
00132 bool monochrome_;
00133 io_method io_;
00134 int fd_;
00135 buffer * buffers_;
00136 unsigned int n_buffers_;
00137 AVFrame *avframe_camera_;
00138 AVFrame *avframe_rgb_;
00139 AVCodec *avcodec_;
00140 AVDictionary *avoptions_;
00141 AVCodecContext *avcodec_context_;
00142 int avframe_camera_size_;
00143 int avframe_rgb_size_;
00144 struct SwsContext *video_sws_;
00145 camera_image_t *image_;
00146
00147 };
00148
00149 }
00150
00151 #endif
00152