src/detection/sensordata_save_orin/Readme.md

@@ -0,0 +1,14 @@
+## sensordata save on orin
+
+
+first,change the destination ip of the lidar32 to 192.168.1.103
+then,run surround-view 4 camera driver,./read4usb
+
+This project is to record and save lidar pointcloud to .pcd file, and the data will be named by time type.
+
+NEW: add frontcamera data
+
+You can edit * savePosition.xml * to change the directory of the pcd and png file
+
+NEW: add surround-view 4 camera
+

src/detection/sensordata_save_orin/imageBuffer.h

@@ -0,0 +1,83 @@
+#ifndef IMAGEBUFFER_H
+#define IMAGEBUFFER_H
+
+#include <opencv2/opencv.hpp>
+#include <mutex>
+#include <condition_variable>
+#include <queue>
+template<typename T>
+class ConsumerProducerQueue
+{
+
+public:
+    ConsumerProducerQueue(int mxsz,bool dropFrame) :
+            maxSize(mxsz),dropFrame(dropFrame)
+    { }
+
+    bool add(T request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if(dropFrame && isFull())
+        {
+            //lock.unlock();
+            //return false;
+            cpq.pop();
+            cpq.push(request);
+            cond.notify_all();
+            return true;
+
+        }
+        else {
+            cond.wait(lock, [this]() { return !isFull(); });
+            cpq.push(request);
+            //lock.unlock();
+            cond.notify_all();
+            return true;
+        }
+    }
+    void consume(T &request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        cond.wait(lock, [this]()
+        { return !isEmpty(); });
+        request = cpq.front();
+        cpq.pop();
+        //lock.unlock();
+        cond.notify_all();
+
+    }
+
+    bool isFull() const
+    {
+        return cpq.size() >= maxSize;
+    }
+
+    bool isEmpty() const
+    {
+        return cpq.size() == 0;
+    }
+
+    int length() const
+    {
+        return cpq.size();
+    }
+
+    void clear()
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        while (!isEmpty())
+        {
+            cpq.pop();
+        }
+        lock.unlock();
+        cond.notify_all();
+    }
+
+private:
+    std::condition_variable cond;  //条件变量允许通过通知进而实现线程同步
+    std::mutex mutex;     //提供了多种互斥操作，可以显式避免数据竞争
+    std::queue<T> cpq;    //容器适配器，它给予程序员队列的功能
+    int maxSize;
+    bool dropFrame;
+};
+#endif // IMAGEBUFFER_H

src/detection/sensordata_save_orin/main.cpp

@@ -0,0 +1,390 @@
+#include <QCoreApplication>
+#include <pcl/io/pcd_io.h>
+
+#include "object.pb.h"
+#include "objectarray.pb.h"
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+#include "ivfault.h"
+#include "ivlog.h"
+#include "ivexit.h"
+#include "ivversion.h"
+#include "rawpic.pb.cc"
+
+#include <string>
+#include <thread>
+#include <time.h>
+#include <opencv2/opencv.hpp>
+
+#include <sys/stat.h>
+#include <libgen.h>
+#include "imageBuffer.h"
+
+
+// save pointcloud data and front camera data
+void * gpa;
+void * gpcamera;
+void * gpcamera_front;
+void * gpcamera_back;
+void * gpcamera_left;
+void * gpcamera_right;
+
+std::string gstrinput = "lidar_pc";
+const std::string cameraname="image00";
+
+const std::string camerafront="picfront";
+const std::string camerarear="picrear";
+const std::string cameraleft="picleft";
+const std::string cameraright="picright";
+
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+std::string strpath = "./savePosition.xml";
+
+iv::xmlparam::Xmlparam xp(strpath);
+std::string strDir_pointcloud = xp.GetParam("pointcloud_save_position","/home/nvidia/saveData/ivd");
+std::string strDir_middle = xp.GetParam("middle_save_position","/home/nvidia/saveData/middle");
+
+std::string strDir_front = xp.GetParam("front_save_position","/home/nvidia/saveData/front");
+std::string strDir_back = xp.GetParam("back_save_position","/home/nvidia/saveData/back");
+std::string strDir_left = xp.GetParam("left_save_position","/home/nvidia/saveData/left");
+std::string strDir_right = xp.GetParam("right_save_position","/home/nvidia/saveData/right");
+
+
+ConsumerProducerQueue<pcl::PointCloud<pcl::PointXYZI>::Ptr> * pcdBuffer_middle =
+        new ConsumerProducerQueue<pcl::PointCloud<pcl::PointXYZI>::Ptr>(1,false);
+ConsumerProducerQueue<cv::Mat> * imageBuffer_middle =  new ConsumerProducerQueue<cv::Mat>(2,true);
+ConsumerProducerQueue<cv::Mat> * imageBuffer_front =  new ConsumerProducerQueue<cv::Mat>(2,true);
+ConsumerProducerQueue<cv::Mat> * imageBuffer_back =  new ConsumerProducerQueue<cv::Mat>(2,true);
+ConsumerProducerQueue<cv::Mat> * imageBuffer_left =  new ConsumerProducerQueue<cv::Mat>(2,true);
+ConsumerProducerQueue<cv::Mat> * imageBuffer_right =  new ConsumerProducerQueue<cv::Mat>(2,true);
+
+static bool check_exists(const std::string &file_path) {
+    return (access(file_path.c_str(), F_OK) != -1);
+  }
+
+  static std::string get_parent_path(const std::string &path) {
+    char buf[1024];
+    strcpy(buf, path.c_str());
+    char *result = dirname(buf);
+    return std::string(result);
+  }
+
+  //创建多级目录
+  static void create_directories(const std::string &path_dir) {
+    if (check_exists(path_dir)) {
+      return;
+    }
+    std::vector<std::string> path_list({path_dir});
+
+    std::string temp_dir_str = path_dir;
+    while (true) {
+      if ((temp_dir_str.find("/") != std::string::npos) &&
+          (temp_dir_str != "/")) {
+        temp_dir_str = get_parent_path(temp_dir_str);
+        if (!check_exists(temp_dir_str)) {
+          path_list.push_back(temp_dir_str);
+        } else {
+          break;
+        }
+      } else {
+        break;
+      }
+    }
+
+    for (auto iter = path_list.rbegin(); iter != path_list.rend(); ++iter) {
+      mkdir((*iter).c_str(), 0775);
+    }
+  }
+
+
+int pcdNum = 1;
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        givlog->verbose("ListenPointCloud data is small headsize = %d, data size is %d", *pHeadSize, nSize);
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    QTime xTime;
+    xTime.start();
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        memcpy(&xp,p,sizeof(pcl::PointXYZI));
+        xp.z = xp.z;
+        xp.intensity = xp.intensity;
+        if(xp.intensity>255)xp.intensity = 255;
+        if(xp.intensity<0)xp.intensity = 0;
+        point_cloud->push_back(xp);
+        p++;
+    }
+//    pcl::io::savePCDFileASCII(strDir+std::to_string(pcdNum)+".pcd", *point_cloud);
+//    time_t currentTime = std::time(NULL);
+//    char chCurrentTime[64];
+//    std::strftime(chCurrentTime, sizeof(chCurrentTime), "%Y%m%d%H%M%S", std::localtime(&currentTime));
+//    std::string stCurrentTime = chCurrentTime;
+
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string pcdname = std::to_string(millseconds);
+
+    pcdBuffer_middle->add(point_cloud);
+    //pcl::io::savePCDFileASCII(strDir_pointcloud+"/"+pcdname+".pcd", *point_cloud);
+    //std::cout<<"Saved "<<point_cloud->size()<<" data points to "<<strDir_pointcloud+"/"+pcdname+".pcd"<<std::endl;
+    pcdNum++;
+
+}
+
+void Listenpic(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
+    }
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string picname = std::to_string(millseconds);
+    if(!mat.empty())
+    {
+        imageBuffer_middle->add(mat);
+        //cv::imwrite(strDir_middle+"/"+picname+".jpg", mat);
+        //std::cout<<"Saved "<<" camera data to "<<strDir_middle+"/"+picname+".jpg"<<std::endl;
+    }
+    mat.release();
+}
+
+void Listenpic_front(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
+    }
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string picname = std::to_string(millseconds);
+
+    if(!mat.empty())
+    {
+        imageBuffer_front->add(mat);
+        //cv::imwrite(strDir_front+"/"+picname+".jpg", mat);
+        //std::cout<<"Saved "<<" camera data to "<<strDir_front+"/"+picname+".jpg"<<std::endl;
+    }
+    mat.release();
+}
+
+void Listenpic_back(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
+    }
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string picname = std::to_string(millseconds);
+
+    if(!mat.empty())
+    {
+        imageBuffer_back->add(mat);
+        //cv::imwrite(strDir_back+"/"+picname+".jpg", mat);
+        //std::cout<<"Saved "<<" camera data to "<<strDir_back+"/"+picname+".jpg"<<std::endl;
+    }
+    mat.release();
+}
+
+void Listenpic_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
+    }
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string picname = std::to_string(millseconds);
+
+    if(!mat.empty())
+    {
+        imageBuffer_left->add(mat);
+        //cv::imwrite(strDir_left+"/"+picname+".jpg", mat);
+        //std::cout<<"Saved "<<" camera data to "<<strDir_left+"/"+picname+".jpg"<<std::endl;
+    }
+    mat.release();
+}
+
+void Listenpic_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize<1000)return;
+    iv::vision::rawpic pic;
+    if(false == pic.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"picview Listenpic fail."<<std::endl;
+        return;
+    }
+    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
+    if(pic.type() == 1)
+        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
+    else
+    {
+        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
+        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
+    }
+    auto now = std::chrono::system_clock::now();
+    long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+    std::string picname = std::to_string(millseconds);
+
+    if(!mat.empty())
+    {
+        imageBuffer_right->add(mat);
+        //cv::imwrite(strDir_right+"/"+picname+".jpg", mat);
+        //std::cout<<"Saved "<<" camera data to "<<strDir_right+"/"+picname+".jpg"<<std::endl;
+    }
+    mat.release();
+}
+
+void exitfunc()
+{
+    std::cout<<"state thread closed."<<std::endl;
+    iv::modulecomm::Unregister(gpa);
+    iv::modulecomm::Unregister(gpcamera);
+    iv::modulecomm::Unregister(gpcamera_front);
+    iv::modulecomm::Unregister(gpcamera_back);
+    iv::modulecomm::Unregister(gpcamera_left);
+    iv::modulecomm::Unregister(gpcamera_right);
+    std::cout<<"exit func complete"<<std::endl;
+}
+
+int main(int argc, char *argv[])
+{
+    gfault = new iv::Ivfault("adciv_save");
+    givlog = new iv::Ivlog("adciv_save");
+    gfault->SetFaultState(0,0,"adciv_save initialize. ");
+    showversion("save_lidar_data");
+    QCoreApplication a(argc, argv);
+
+    create_directories(strDir_pointcloud);
+    create_directories(strDir_middle);
+    create_directories(strDir_front);
+    create_directories(strDir_back);
+    create_directories(strDir_left);
+    create_directories(strDir_right);
+
+    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
+    gpcamera= iv::modulecomm::RegisterRecv(&cameraname[0],Listenpic);
+    gpcamera_front= iv::modulecomm::RegisterRecv(&camerafront[0],Listenpic_front);
+    gpcamera_back= iv::modulecomm::RegisterRecv(&camerarear[0],Listenpic_back);
+    gpcamera_left= iv::modulecomm::RegisterRecv(&cameraleft[0],Listenpic_left);
+    gpcamera_right= iv::modulecomm::RegisterRecv(&cameraright[0],Listenpic_right);
+
+    cv::Mat picfront,picback,picleft,picright,picmiddle;
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    while(1)
+    {
+        auto now = std::chrono::system_clock::now();
+        long long millseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+        std::string filename = std::to_string(millseconds);
+
+        if (pcdBuffer_middle->isEmpty() || imageBuffer_front->isEmpty()|| imageBuffer_back->isEmpty() ||
+            imageBuffer_left->isEmpty()|| imageBuffer_right->isEmpty() || imageBuffer_middle->isEmpty())
+        {
+
+            std::cout<<"Wait data"<<std::endl;
+            if(imageBuffer_middle->isEmpty())
+                std::cout<<"middle senyun image is empty"<<std::endl;
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+        pcdBuffer_middle->consume(point_cloud);
+        imageBuffer_front->consume(picfront);
+        imageBuffer_back->consume(picback);
+        imageBuffer_left->consume(picleft);
+        imageBuffer_right->consume(picright);
+        imageBuffer_middle->consume(picmiddle);
+
+
+        if((!picfront.empty()) && (!picback.empty()) && (!picleft.empty()) && (!picright.empty())
+                && (!picmiddle.empty()))
+        {
+            pcl::io::savePCDFileASCII(strDir_pointcloud+"/"+filename+".pcd", *point_cloud);
+            cv::imwrite(strDir_front+"/"+filename+".jpg", picfront);
+            cv::imwrite(strDir_back+"/"+filename+".jpg", picback);
+            cv::imwrite(strDir_left+"/"+filename+".jpg", picleft);
+            cv::imwrite(strDir_right+"/"+filename+".jpg", picright);
+            cv::imwrite(strDir_middle+"/"+filename+".jpg", picmiddle);
+            std::cout<<"Saved "<<" camera data to "<<filename+".jpg"<<std::endl;
+        }
+
+    }
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+
+    return a.exec();
+}

src/detection/sensordata_save_orin/savePosition.xml

@@ -0,0 +1,10 @@
+<xml>	
+	<node name="bqev_save">
+		<param name="pointcloud_save_position" value="/home/nvidia/saveData/ivd" />
+		<param name="middle_save_position" value="/home/nvidia/saveData/middle" />
+		<param name="front_save_position" value="/home/nvidia/saveData/front" />
+		<param name="back_save_position" value="/home/nvidia/saveData/back" />
+		<param name="left_save_position" value="/home/nvidia/saveData/left" />
+		<param name="right_save_position" value="/home/nvidia/saveData/right" />
+	</node>
+</xml>

src/detection/sensordata_save_orin/sensordata_save_orin.pro

@@ -0,0 +1,91 @@
+QT -= gui
+
+QMAKE_LFLAGS += -no-pie
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = sensordata_save_orin
+TEMPLATE = app
+
+CONFIG += c++14 #console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+#DEFINES += USE_PLUS_MODULECOMM
+
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+
+SOURCES += main.cpp \
+    ../../include/msgtype/rawpic.pb.cc
+
+
+HEADERS += imageBuffer.h
+
+
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += /usr/include/opencv4
+
+INCLUDEPATH += /usr/include/pcl-1.10 \
+               ../../include/msgtype/rawpic.pb.h
+
+
+INCLUDEPATH += /usr/local/cuda-11.4/targets/aarch64-linux/include
+
+LIBS += -L/usr/local/cuda-11.4/targets/aarch64-linux/lib  # -lcublas
+
+
+LIBS += /home/nvidia/modularization/src/detection/build-adciv_save-unknown-Debug/libusb_cam_python.so
+
+INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
+
+LIBS += -lpcl_io -lpcl_common
+
+
+LIBS += -lboost_system  -lavutil  -lprotobuf -lcudnn
+
+
+LIBS += -lcudnn
+
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv_highgui.so \
+        /usr/lib/aarch64-linux-gnu/libopencv_core.so    \
+        /usr/lib/aarch64-linux-gnu/libopencv_imgproc.so \
+        /usr/lib/aarch64-linux-gnu/libopencv_imgcodecs.so \
+        /usr/lib/aarch64-linux-gnu/libopencv_videoio.so \
+        /usr/lib/aarch64-linux-gnu/libopencv_video.so \
+        /usr/lib/aarch64-linux-gnu/libopencv_calib3d.so \
+
+#unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe/arm64 -lcaffe -lcudnn
+
+HEADERS += \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+LIBS += -L/usr/lib/aarch64-linux-gnu/ -lglog
+
+
+
+
+
+

src/driver/read4usb/Readme.md

@@ -0,0 +1,12 @@
+driver_camera_ioctl用于usb摄像头驱动。主要代码来自ros版的usb_cam程序。使用的是ioctl。usb摄像头的framerate能到30。
+
+主要参数：
+msgname 共享内存名词，例如image00
+cameraname 设备名称，例如/dev/video0
+use_rawmjpeg 是否直接使用mjpeg数据，例如true
+image_width 宽度，比如1920
+image_height 高度，比如1080
+framerate 帧率，比如30
+
+
+

src/driver/read4usb/main.cpp

@@ -0,0 +1,370 @@
+#include <QCoreApplication>
+#include <thread>
+#include <QMutex>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core.hpp>
+
+#ifdef USE_OPENCV4
+//#include "opencv2/imgcodecs/legacy/constants_c.h"   //OpenCV4 use this line
+#include <opencv2/imgproc/types_c.h>   //OpenCV4 use this line
+#endif
+
+#include <opencv2/videoio.hpp>
+
+#include "usb_cam.h"
+
+#include <QTime>
+
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include <sys/types.h>                      // 下面四个头文件是linux系统编程特有的
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+
+#include <linux/videodev2.h>                // 操作摄像头设备
+
+#include "modulecomm.h"
+#include "rawpic.pb.h"
+#include "xmlparam.h"
+
+#include "ivversion.h"
+
+#include "ivexit.h"
+#include <signal.h>
+
+
+cv::Mat image1;
+
+QCoreApplication * gApp;
+
+
+void * gpa;
+
+int gindex = 0;
+
+
+int gcamindex = 1;
+
+std::string gstrcamera = "";
+
+std::string gmsgname = "usbpic";
+
+std::string gstrdevname = "/dev/video5";
+
+
+std::string gvideo_device_name_, gio_method_name_, gpixel_format_name_, gcamera_name_, gcamera_info_url_;
+//std::string start_service_name_, start_service_name_;
+bool gstreaming_status_;
+int gimage_width_, gimage_height_, gframerate_, gexposure_, gbrightness_, gcontrast_, gsaturation_, gsharpness_, gfocus_,
+    gwhite_balance_, ggain_;
+bool gautofocus_, gautoexposure_, gauto_white_balance_;
+
+bool gbuserawmjpeg = true;
+
+bool gbcompress = true;
+
+/**将uchar类型的数据转换为Mat类型*/
+int ucharToMat(uchar *p2,cv::Mat& src,int flag)
+{
+    int img_width = src.cols;
+    int img_height = src.rows;
+    //Mat img(Size(img_width, img_height), CV_8UC3);
+    int nsize = img_width * img_height * 3;
+    int xwid = img_width * 3;
+    for (int i = 0; i < nsize ; i++)
+    {
+        src.at<cv::Vec3b>(i / (xwid), (i % (xwid)) / 3)[i % 3] = p2[i];//BGR格式
+        //src.at<Vec3b>(i / (img_width * 3), (i % (img_width * 3)) / 3)[i % 3] = p2[i];//换为RGB使用
+    }
+    flag = 1;
+    return flag;
+}
+
+
+using namespace  usb_cam;
+void threadcapture()
+{
+
+    usb_cam::UsbCam camx;
+
+    // set the IO method
+    UsbCam::io_method io_method = UsbCam::io_method_from_string(gio_method_name_);
+    if(io_method == UsbCam::IO_METHOD_UNKNOWN)
+    {
+      qDebug("Unknown IO method '%s'", gio_method_name_.c_str());
+      return;
+    }
+
+    // set the pixel format
+    UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(gpixel_format_name_);
+    if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
+    {
+      qDebug("Unknown pixel format '%s'", gpixel_format_name_.c_str());
+      return;
+    }
+
+    // start the camera
+    camx.start(gvideo_device_name_.c_str(), io_method, pixel_format, gimage_width_,
+             gimage_height_, gframerate_);
+
+    // set camera parameters
+    if (gbrightness_ >= 0)
+    {
+      camx.set_v4l_parameter("brightness", gbrightness_);
+    }
+
+    if (gcontrast_ >= 0)
+    {
+      camx.set_v4l_parameter("contrast", gcontrast_);
+    }
+
+    if (gsaturation_ >= 0)
+    {
+      camx.set_v4l_parameter("saturation", gsaturation_);
+    }
+
+    if (gsharpness_ >= 0)
+    {
+      camx.set_v4l_parameter("sharpness", gsharpness_);
+    }
+
+    if (ggain_ >= 0)
+    {
+      camx.set_v4l_parameter("gain", ggain_);
+    }
+
+    // check auto white balance
+    if (gauto_white_balance_)
+    {
+      camx.set_v4l_parameter("white_balance_temperature_auto", 1);
+    }
+    else
+    {
+      camx.set_v4l_parameter("white_balance_temperature_auto", 0);
+      camx.set_v4l_parameter("white_balance_temperature", gwhite_balance_);
+    }
+
+    // check auto exposure
+    if (!gautoexposure_)
+    {
+      // turn down exposure control (from max of 3)
+      camx.set_v4l_parameter("exposure_auto", 1);
+      // change the exposure level
+      camx.set_v4l_parameter("exposure_absolute", gexposure_);
+    }
+
+    // check auto focus
+    if (gautofocus_)
+    {
+      camx.set_auto_focus(1);
+      camx.set_v4l_parameter("focus_auto", 1);
+    }
+    else
+    {
+      camx.set_v4l_parameter("focus_auto", 0);
+      if (gfocus_ >= 0)
+      {
+        camx.set_v4l_parameter("focus_absolute", gfocus_);
+      }
+    }
+
+    camx.set_useRawMJPEG(gbuserawmjpeg);
+
+    int nserbufsize = 20000000;
+
+    char * strser = new char[nserbufsize];
+
+    char * strbuf = new char[10000000];
+
+    IplImage *srcImg;
+
+
+    srcImg = cvCreateImage(cvSize(gimage_width_, gimage_height_), 8, 3);
+    cvZero(srcImg);
+    cv::Mat mat1(cvSize(gimage_width_, gimage_height_), CV_8UC3,srcImg->imageData) ;//(cvSize(WIDTH, HEIGHT), CV_8UC3);//, frame.imageData);;
+
+
+
+    while(1)
+    {
+        int nLen;
+        camx.grab_image(strbuf,&nLen,10000000);
+        qDebug("time %ld len: %ld",QDateTime::currentMSecsSinceEpoch(),nLen);
+
+
+//        iv::modulecomm::ModuleSendMsg(gpa,strbuf,nLen);
+//        continue;
+
+        iv::vision::rawpic pic;
+
+        qint64 time = QDateTime::currentMSecsSinceEpoch();
+        QTime xg;
+        xg.start();
+        pic.set_time(time);
+        pic.set_index(gindex);gindex++;
+
+
+        pic.set_elemsize(3);
+        pic.set_width(gimage_width_);
+        pic.set_height(gimage_height_);
+        pic.set_mattype(0);
+
+
+        if((gbuserawmjpeg) && (gpixel_format_name_ == "mjpeg"))
+        {
+            pic.set_picdata(strbuf,nLen);
+            pic.set_type(2);
+        }
+        else
+        {
+            if((gpixel_format_name_ != "mjpeg")&&(gbcompress))
+            {
+                std::vector<int> param = std::vector<int>(2);
+                param[0] = 1;//CV_IMWRITE_JPEG_QUALITY;
+                param[1] = 95; // default(95) 0-100
+                std::vector<unsigned char> buff;
+
+                cv::imencode(".jpg", mat1, buff, param);
+                pic.set_picdata(buff.data(),buff.size());
+                buff.clear();
+                pic.set_type(2);
+
+            }
+            else
+            {
+
+                cv::cvtColor(cv::Mat(gimage_height_, gimage_width_, CV_8UC3,strbuf), mat1,
+                             cv::COLOR_RGB2BGR);
+
+                //          ucharToMat((unsigned char *)strbuf, mat1, 0);
+
+                pic.set_elemsize(mat1.elemSize());
+                pic.set_width(mat1.cols);
+                pic.set_height(mat1.rows);
+                pic.set_mattype(mat1.type());
+                pic.set_picdata(mat1.data,mat1.rows*mat1.cols*mat1.elemSize());
+                //                pic.set_picdata(matcompress.data,matcompress.rows*matcompress.cols*matcompress.elemSize());
+                pic.set_type(1);
+            }
+        }
+
+        image1 = mat1;
+
+
+        int nSize = pic.ByteSize();
+
+        bool bser = pic.SerializeToArray(strser,nSize);
+        qDebug("pac time is %d size is %d",xg.elapsed(),nSize);
+
+        if(bser)iv::modulecomm::ModuleSendMsg(gpa,strser,nSize);
+        else
+        {
+            std::cout<<"usbcamera "<< "serialize error. "<<std::endl;
+        }
+    }
+
+}
+
+void ExitFunc()
+{
+    gApp->quit();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+}
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        ExitFunc();
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_camera_ioctl");
+    QCoreApplication a(argc, argv);
+    gApp = &a;
+
+    QString strpath = QCoreApplication::applicationDirPath();
+//    QString apppath = strpath;
+    if(argc < 2)
+        strpath = strpath + "/driver_camera_ioctl.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+
+    std::string xmlmsgname = xp.GetParam("msgname","image00");
+    gmsgname = xmlmsgname;
+
+    gstrcamera = xp.GetParam("cameraname","/dev/video0");
+
+     xp.GetParam("cameraname",gvideo_device_name_, std::string("/dev/video0"));
+ //   gvideo_device_name_ = xp.GetParam("video_device", std::string("/dev/video5"));
+    gbrightness_ =  xp.GetParam("brightness",  -1); //0-255, -1 "leave alone"
+    gcontrast_ = xp.GetParam("contrast",  -1); //0-255, -1 "leave alone"
+    gsaturation_ =  xp.GetParam("saturation",  -1); //0-255, -1 "leave alone"
+    gsharpness_ = xp.GetParam("sharpness",  -1); //0-255, -1 "leave alone"
+    // possible values: mmap, read, userptr
+    gio_method_name_ = xp.GetParam("io_method",  std::string("mmap"));
+    gimage_width_ = xp.GetParam("image_width", 1920);
+    gimage_height_ = xp.GetParam("image_height",  1080);
+    gframerate_ =  xp.GetParam("framerate",  30);
+    // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
+    gpixel_format_name_ = xp.GetParam("pixel_format",  std::string("mjpeg"));
+    // enable/disable autofocus
+    gautofocus_  = xp.GetParam("autofocus", false);
+
+
+    gfocus_ = xp.GetParam("focus", -1); //0-255, -1 "leave alone"
+    // enable/disable autoexposure
+    gautoexposure_ = xp.GetParam("autoexposure",  true);
+    gexposure_ = xp.GetParam("exposure",  100);
+    ggain_ = xp.GetParam("gain",  -1); //0-100?, -1 "leave alone"
+    // enable/disable auto white balance temperature
+    gauto_white_balance_ = xp.GetParam("auto_white_balance", true);
+    gwhite_balance_ = xp.GetParam("white_balance", 4000);
+
+    // load the camera info
+//     xp.GetParam("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
+    gcamera_name_ = xp.GetParam("camera_name",  std::string("head_camera"));
+    gcamera_info_url_ = xp.GetParam("camera_info_url",  std::string(""));
+
+    gbuserawmjpeg = xp.GetParam("use_rawmjpeg",true);
+
+    gbcompress = xp.GetParam("bcompress",true);
+
+
+
+    gpa = iv::modulecomm::RegisterSend(gmsgname.data(),20000000,1);
+    std::thread * mthread =  new  std::thread(threadcapture) ;//new std::thread(VideoThread,0);
+
+
+    while(1)
+    {
+        if (image1.cols>0){
+            cv::imshow("result",image1);
+        }
+        cv::waitKey(1);
+    }
+
+    (void)mthread;
+//    (void)conthread;
+//    (void)compressthread;
+
+    iv::ivexit::RegIVExitCall(ExitFunc);
+    signal(SIGINT,signal_handler);
+
+    int nrc = a.exec();
+
+    iv::modulecomm::Unregister(gpa);
+
+    std::cout<<"driver_camera_ioctl quit."<<nrc<<std::endl;
+
+    return nrc;
+}

src/driver/read4usb/main_new.cpp

@@ -0,0 +1,312 @@
+#include <QCoreApplication>
+#include <thread>
+#include <QMutex>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core.hpp>
+
+#ifdef USE_OPENCV4
+//#include "opencv2/imgcodecs/legacy/constants_c.h"   //OpenCV4 use this line
+#include <opencv2/imgproc/types_c.h>   //OpenCV4 use this line
+#endif
+
+#include <opencv2/videoio.hpp>
+#include "usb_cam.h"
+#include <QTime>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <sys/types.h>                      // 下面四个头文件是linux系统编程特有的
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+
+#include <linux/videodev2.h>                // 操作摄像头设备
+
+#include "modulecomm.h"
+#include "rawpic.pb.h"
+#include "xmlparam.h"
+
+#include "ivversion.h"
+
+#include "ivexit.h"
+#include <signal.h>
+
+
+cv::Mat matimage;
+
+QCoreApplication * gApp;
+
+
+void * gpa_front;
+void * gpa_back;
+void * gpa_left;
+void * gpa_right;
+
+int gindex_front = 0;
+int gindex_back = 0;
+int gindex_left = 0;
+int gindex_right = 0;
+
+std::string gio_method_name_, gpixel_format_name_, gcamera_name_, gcamera_info_url_;
+std::string device_front,device_back,device_left,device_right;
+std::string gmsgname_front, gmsgname_back, gmsgname_left, gmsgname_right;
+//std::string start_service_name_, start_service_name_;
+bool gstreaming_status_;
+int gimage_width_, gimage_height_, gframerate_, gexposure_, gbrightness_, gcontrast_, gsaturation_, gsharpness_, gfocus_,
+    gwhite_balance_, ggain_;
+bool gautofocus_, gautoexposure_, gauto_white_balance_;
+
+bool gbuserawmjpeg = true;
+
+bool gbcompress = false;
+
+/**将uchar类型的数据转换为Mat类型*/
+int ucharToMat(uchar *p2,cv::Mat& src,int flag)
+{
+    int img_width = src.cols;
+    int img_height = src.rows;
+    //Mat img(Size(img_width, img_height), CV_8UC3);
+    int nsize = img_width * img_height * 3;
+    int xwid = img_width * 3;
+    for (int i = 0; i < nsize ; i++)
+    {
+        src.at<cv::Vec3b>(i / (xwid), (i % (xwid)) / 3)[i % 3] = p2[i];//BGR格式
+        //src.at<Vec3b>(i / (img_width * 3), (i % (img_width * 3)) / 3)[i % 3] = p2[i];//换为RGB使用
+    }
+    flag = 1;
+    return flag;
+}
+
+
+using namespace  usb_cam;
+void threadcapture(std::string devicename,void * pHandle)
+{
+
+    int gindex = 0;
+    usb_cam::UsbCam camx;
+    // set the IO method
+    UsbCam::io_method io_method = UsbCam::io_method_from_string(gio_method_name_);
+    if(io_method == UsbCam::IO_METHOD_UNKNOWN)
+    {
+      qDebug("Unknown IO method '%s'", gio_method_name_.c_str());
+      return;
+    }
+    // set the pixel format
+    UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(gpixel_format_name_);
+    if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
+    {
+      qDebug("Unknown pixel format '%s'", gpixel_format_name_.c_str());
+      return;
+    }
+
+    // start the camera
+    camx.start(devicename.c_str(), io_method, pixel_format, gimage_width_,
+             gimage_height_, gframerate_);
+
+    // set camera parameters
+    if (gbrightness_ >= 0)
+    {
+      camx.set_v4l_parameter("brightness", gbrightness_);
+    }
+
+    if (gcontrast_ >= 0)
+    {
+      camx.set_v4l_parameter("contrast", gcontrast_);
+    }
+
+    if (gsaturation_ >= 0)
+    {
+      camx.set_v4l_parameter("saturation", gsaturation_);
+    }
+
+    if (gsharpness_ >= 0)
+    {
+      camx.set_v4l_parameter("sharpness", gsharpness_);
+    }
+
+    if (ggain_ >= 0)
+    {
+      camx.set_v4l_parameter("gain", ggain_);
+    }
+
+    // check auto white balance
+    if (gauto_white_balance_)
+    {
+      camx.set_v4l_parameter("white_balance_temperature_auto", 1);
+    }
+    else
+    {
+      camx.set_v4l_parameter("white_balance_temperature_auto", 0);
+      camx.set_v4l_parameter("white_balance_temperature", gwhite_balance_);
+    }
+
+    // check auto exposure
+    if (!gautoexposure_)
+    {
+      // turn down exposure control (from max of 3)
+      camx.set_v4l_parameter("exposure_auto", 1);
+      // change the exposure level
+      camx.set_v4l_parameter("exposure_absolute", gexposure_);
+    }
+
+    // check auto focus
+    if (gautofocus_)
+    {
+      camx.set_auto_focus(1);
+      camx.set_v4l_parameter("focus_auto", 1);
+    }
+    else
+    {
+      camx.set_v4l_parameter("focus_auto", 0);
+      if (gfocus_ >= 0)
+      {
+        camx.set_v4l_parameter("focus_absolute", gfocus_);
+      }
+    }
+
+    camx.set_useRawMJPEG(gbuserawmjpeg);
+
+    int nserbufsize = 20000000;
+
+    char * strser = new char[nserbufsize];
+
+    char * strbuf = new char[10000000];
+
+    IplImage *srcImg;
+    srcImg = cvCreateImage(cvSize(gimage_width_, gimage_height_), 8, 3);
+    cvZero(srcImg);
+    cv::Mat mat1(cvSize(gimage_width_, gimage_height_), CV_8UC3,srcImg->imageData) ;//(cvSize(WIDTH, HEIGHT), CV_8UC3);//, frame.imageData);;
+    while(1)
+    {
+        int nLen;
+        camx.grab_image(strbuf,&nLen,10000000);
+        qDebug("time %ld len: %ld",QDateTime::currentMSecsSinceEpoch(),nLen);
+        iv::vision::rawpic pic;
+
+        qint64 time = QDateTime::currentMSecsSinceEpoch();
+        QTime xg;
+        xg.start();
+        pic.set_time(time);
+        pic.set_index(gindex);gindex++;
+
+        pic.set_elemsize(3);
+        pic.set_width(gimage_width_);
+        pic.set_height(gimage_height_);
+        pic.set_mattype(0);
+        if (nLen > 0)
+        {
+            if((gbuserawmjpeg) && (gpixel_format_name_ == "mjpeg"))
+            {
+                pic.set_picdata(strbuf,nLen);
+                pic.set_type(2);
+            }
+            int nSize = pic.ByteSize();
+            bool bser = pic.SerializeToArray(strser,nSize);
+            qDebug("pac time is %d size is %d",xg.elapsed(),nSize);
+
+            if(bser)iv::modulecomm::ModuleSendMsg(pHandle,strser,nSize);
+            else
+            {
+                std::cout<<"usbcamera "<< "serialize error. "<<std::endl;
+            }
+        }
+        else
+            std::cout<<"pic lenght is 0"<<std::endl;
+    }
+
+}
+
+void ExitFunc()
+{
+    gApp->quit();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+}
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        ExitFunc();
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_camera_ioctl");
+    QCoreApplication a(argc, argv);
+    gApp = &a;
+
+    QString strpath = QCoreApplication::applicationDirPath();
+//    QString apppath = strpath;
+    if(argc < 2)
+        strpath = strpath + "./read4usb.xml";
+    else
+        strpath = argv[1];
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+
+    gmsgname_front = xp.GetParam("msgfront","picfront");
+    gmsgname_back = xp.GetParam("msgback","picrear");
+    gmsgname_left = xp.GetParam("msgleft","picleft");
+    gmsgname_right = xp.GetParam("msgright","picright");
+
+    device_front = xp.GetParam("front", std::string("/dev/video0"));
+    device_back = xp.GetParam("back", std::string("/dev/video2"));
+    device_left = xp.GetParam("left", std::string("/dev/video4"));
+    device_right = xp.GetParam("right", std::string("/dev/video6"));
+
+
+    gbrightness_ =  xp.GetParam("brightness",  -1); //0-255, -1 "leave alone"
+    gcontrast_ = xp.GetParam("contrast",  -1); //0-255, -1 "leave alone"
+    gsaturation_ =  xp.GetParam("saturation",  -1); //0-255, -1 "leave alone"
+    gsharpness_ = xp.GetParam("sharpness",  -1); //0-255, -1 "leave alone"
+    // possible values: mmap, read, userptr
+    gio_method_name_ = xp.GetParam("io_method",  std::string("mmap"));
+    gimage_width_ = xp.GetParam("image_width", 640);
+    gimage_height_ = xp.GetParam("image_height",  480);
+    gframerate_ =  xp.GetParam("framerate",  30);
+    // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
+    gpixel_format_name_ = xp.GetParam("pixel_format",  std::string("mjpeg"));
+    // enable/disable autofocus
+    gautofocus_  = xp.GetParam("autofocus", false);
+    gfocus_ = xp.GetParam("focus", -1); //0-255, -1 "leave alone"
+    // enable/disable autoexposure
+    gautoexposure_ = xp.GetParam("autoexposure",  true);
+    gexposure_ = xp.GetParam("exposure",  100);
+    ggain_ = xp.GetParam("gain",  -1); //0-100?, -1 "leave alone"
+    // enable/disable auto white balance temperature
+    gauto_white_balance_ = xp.GetParam("auto_white_balance", true);
+    gwhite_balance_ = xp.GetParam("white_balance", 4000);
+
+    // load the camera info
+    gbuserawmjpeg = xp.GetParam("use_rawmjpeg",true);
+    gbcompress = xp.GetParam("bcompress",true);
+
+    gpa_front = iv::modulecomm::RegisterSend(gmsgname_front.data(),1000000,1);
+    std::thread * mthread1 =  new  std::thread(threadcapture,device_front,gpa_front);//new std::thread(VideoThread,0);
+
+    gpa_back = iv::modulecomm::RegisterSend(gmsgname_back.data(),1000000,1);
+    std::thread * mthread2 =  new  std::thread(threadcapture,device_back,gpa_back);//
+
+    gpa_left = iv::modulecomm::RegisterSend(gmsgname_left.data(),1000000,1);
+    std::thread * mthread3 =  new  std::thread(threadcapture,device_left,gpa_left);//
+
+    gpa_right = iv::modulecomm::RegisterSend(gmsgname_right.data(),1000000,1);
+    std::thread * mthread4 =  new  std::thread(threadcapture,device_right,gpa_right);//
+
+    (void)mthread1;
+    (void)mthread2;
+    (void)mthread3;
+    (void)mthread4;
+    iv::ivexit::RegIVExitCall(ExitFunc);
+    signal(SIGINT,signal_handler);
+    int nrc = a.exec();
+    iv::modulecomm::Unregister(gpa_front);
+    iv::modulecomm::Unregister(gpa_back);
+    iv::modulecomm::Unregister(gpa_left);
+    iv::modulecomm::Unregister(gpa_right);
+    std::cout<<"driver_camera_ioctl quit."<<nrc<<std::endl;
+
+    return nrc;
+}

src/driver/read4usb/read4usb.pro

@@ -0,0 +1,54 @@
+
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main_new.cpp \
+           usb_cam.cpp
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+
+}
+
+
+INCLUDEPATH += /home/linaro/opencv3
+
+
+#contains(QMAKE_HOST.arch, aarch64){
+#    DEFINES += USE_OPENCV4
+#}
+
+INCLUDEPATH += /usr/include/x86_64-linux-gnu
+
+
+DEFINES += USE_OPENCV4
+
+HEADERS += \
+    usb_cam.h
+
+
+LIBS +=-lpostproc -lswresample -lswscale -lavfilter -lavdevice -lavformat -lavcodec -lavutil -lm -ldl
+
+LIBS += -livprotoif

src/driver/read4usb/read4usb.xml

@@ -0,0 +1,16 @@
+<xml>	
+	<node name="driver_camera_ioctl">
+		<param name="msgfront" value="picfront" />
+		<param name="msgback" value="picrear" />
+		<param name="msgleft" value="picleft" />
+		<param name="msgright" value="picright" />
+		<param name="image_width" value = "640" />
+		<param name="image_height" value="480" />
+		<param name="use_rawmjpeg" value = "true" />
+		<param name="framerate" value="30" />
+		<param name="front" value="/dev/video0" />
+		<param name="back" value="/dev/video2" />
+		<param name="left" value="/dev/video4" />
+		<param name="right" value="/dev/video6" />
+	</node>
+</xml>

src/driver/read4usb/usb_cam.cpp

@@ -0,0 +1,1310 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Robert Bosch LLC.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Robert Bosch nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#define __STDC_CONSTANT_MACROS
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <fcntl.h>              /* low-level i/o */
+#include <unistd.h>
+#include <errno.h>
+#include <malloc.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+//#include <ros/ros.h>
+#include <boost/lexical_cast.hpp>
+//#include <sensor_msgs/fill_image.h>
+
+#include <usb_cam.h>
+
+//#include <QDateTime>
+#include <iostream>
+
+#define CLEAR(x) memset (&(x), 0, sizeof (x))
+
+namespace usb_cam {
+
+static void errno_exit(const char * s)
+{
+    std::cout<<s<<" error "<<errno<<" "<<strerror(errno)<<std::endl;
+//  qDebug("%s error %d, %s", s, errno, strerror(errno));
+  exit(EXIT_FAILURE);
+}
+
+static int xioctl(int fd, int request, void * arg)
+{
+  int r;
+
+  do
+    r = ioctl(fd, request, arg);
+  while (-1 == r && EINTR == errno);
+
+  return r;
+}
+
+const unsigned char uchar_clipping_table[] = {
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, // -128 - -121
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, // -120 - -113
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, // -112 - -105
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, // -104 -  -97
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -96 -  -89
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -88 -  -81
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -80 -  -73
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -72 -  -65
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -64 -  -57
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -56 -  -49
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -48 -  -41
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -40 -  -33
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -32 -  -25
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -24 -  -17
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //  -16 -   -9
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0, //   -8 -   -1
+    0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30,
+    31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59,
+    60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88,
+    89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113,
+    114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136,
+    137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
+    160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182,
+    183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205,
+    206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228,
+    229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251,
+    252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263
+    255, 255, 255, 255, 255, 255, 255, 255, // 264-271
+    255, 255, 255, 255, 255, 255, 255, 255, // 272-279
+    255, 255, 255, 255, 255, 255, 255, 255, // 280-287
+    255, 255, 255, 255, 255, 255, 255, 255, // 288-295
+    255, 255, 255, 255, 255, 255, 255, 255, // 296-303
+    255, 255, 255, 255, 255, 255, 255, 255, // 304-311
+    255, 255, 255, 255, 255, 255, 255, 255, // 312-319
+    255, 255, 255, 255, 255, 255, 255, 255, // 320-327
+    255, 255, 255, 255, 255, 255, 255, 255, // 328-335
+    255, 255, 255, 255, 255, 255, 255, 255, // 336-343
+    255, 255, 255, 255, 255, 255, 255, 255, // 344-351
+    255, 255, 255, 255, 255, 255, 255, 255, // 352-359
+    255, 255, 255, 255, 255, 255, 255, 255, // 360-367
+    255, 255, 255, 255, 255, 255, 255, 255, // 368-375
+    255, 255, 255, 255, 255, 255, 255, 255, // 376-383
+    };
+const int clipping_table_offset = 128;
+
+/** Clip a value to the range 0<val<255. For speed this is done using an
+ * array, so can only cope with numbers in the range -128<val<383.
+ */
+static unsigned char CLIPVALUE(int val)
+{
+  // Old method (if)
+  /*   val = val < 0 ? 0 : val; */
+  /*   return val > 255 ? 255 : val; */
+
+  // New method (array)
+  return uchar_clipping_table[val + clipping_table_offset];
+}
+
+/**
+ * Conversion from YUV to RGB.
+ * The normal conversion matrix is due to Julien (surname unknown):
+ *
+ * [ R ]   [  1.0   0.0     1.403 ] [ Y ]
+ * [ G ] = [  1.0  -0.344  -0.714 ] [ U ]
+ * [ B ]   [  1.0   1.770   0.0   ] [ V ]
+ *
+ * and the firewire one is similar:
+ *
+ * [ R ]   [  1.0   0.0     0.700 ] [ Y ]
+ * [ G ] = [  1.0  -0.198  -0.291 ] [ U ]
+ * [ B ]   [  1.0   1.015   0.0   ] [ V ]
+ *
+ * Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB
+ *                   do not get you back to the same RGB!)
+ * [ R ]   [  1.0   0.0     1.136 ] [ Y ]
+ * [ G ] = [  1.0  -0.396  -0.578 ] [ U ]
+ * [ B ]   [  1.0   2.041   0.002 ] [ V ]
+ *
+ */
+static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r,
+                    unsigned char* g, unsigned char* b)
+{
+  const int y2 = (int)y;
+  const int u2 = (int)u - 128;
+  const int v2 = (int)v - 128;
+  //std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
+
+  // This is the normal YUV conversion, but
+  // appears to be incorrect for the firewire cameras
+  //   int r2 = y2 + ( (v2*91947) >> 16);
+  //   int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
+  //   int b2 = y2 + ( (u2*115999) >> 16);
+  // This is an adjusted version (UV spread out a bit)
+  int r2 = y2 + ((v2 * 37221) >> 15);
+  int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15);
+  int b2 = y2 + ((u2 * 66883) >> 15);
+  //std::cerr << "   RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
+
+  // Cap the values.
+  *r = CLIPVALUE(r2);
+  *g = CLIPVALUE(g2);
+  *b = CLIPVALUE(b2);
+}
+
+void uyvy2rgb(char *YUV, char *RGB, int NumPixels)
+{
+  int i, j;
+  unsigned char y0, y1, u, v;
+  unsigned char r, g, b;
+  for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
+  {
+    u = (unsigned char)YUV[i + 0];
+    y0 = (unsigned char)YUV[i + 1];
+    v = (unsigned char)YUV[i + 2];
+    y1 = (unsigned char)YUV[i + 3];
+    YUV2RGB(y0, u, v, &r, &g, &b);
+    RGB[j + 0] = r;
+    RGB[j + 1] = g;
+    RGB[j + 2] = b;
+    YUV2RGB(y1, u, v, &r, &g, &b);
+    RGB[j + 3] = r;
+    RGB[j + 4] = g;
+    RGB[j + 5] = b;
+  }
+}
+
+static void mono102mono8(char *RAW, char *MONO, int NumPixels)
+{
+  int i, j;
+  for (i = 0, j = 0; i < (NumPixels << 1); i += 2, j += 1)
+  {
+    //first byte is low byte, second byte is high byte; smash together and convert to 8-bit
+    MONO[j] = (unsigned char)(((RAW[i + 0] >> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0));
+  }
+}
+
+static void yuyv2rgb(char *YUV, char *RGB, int NumPixels)
+{
+  int i, j;
+  unsigned char y0, y1, u, v;
+  unsigned char r, g, b;
+
+  for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
+  {
+    y0 = (unsigned char)YUV[i + 0];
+    u = (unsigned char)YUV[i + 1];
+    y1 = (unsigned char)YUV[i + 2];
+    v = (unsigned char)YUV[i + 3];
+    YUV2RGB(y0, u, v, &r, &g, &b);
+    RGB[j + 0] = r;
+    RGB[j + 1] = g;
+    RGB[j + 2] = b;
+    YUV2RGB(y1, u, v, &r, &g, &b);
+    RGB[j + 3] = r;
+    RGB[j + 4] = g;
+    RGB[j + 5] = b;
+  }
+}
+
+void rgb242rgb(char *YUV, char *RGB, int NumPixels)
+{
+  memcpy(RGB, YUV, NumPixels * 3);
+}
+
+
+UsbCam::UsbCam()
+  : io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL),
+    avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL),
+    avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL), is_capturing_(false) {
+}
+UsbCam::~UsbCam()
+{
+  shutdown();
+}
+
+int UsbCam::init_mjpeg_decoder(int image_width, int image_height)
+{
+  avcodec_register_all();
+
+  avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
+  if (!avcodec_)
+  {
+      std::cout<<" Could not find MJPEG decoder "<<std::endl;
+ //   qDebug("Could not find MJPEG decoder");
+    return 0;
+  }
+
+  avcodec_context_ = avcodec_alloc_context3(avcodec_);
+#if LIBAVCODEC_VERSION_MAJOR < 55
+  avframe_camera_ = avcodec_alloc_frame();
+  avframe_rgb_ = avcodec_alloc_frame();
+#else
+  avframe_camera_ = av_frame_alloc();
+  avframe_rgb_ = av_frame_alloc();
+#endif
+
+  avpicture_alloc((AVPicture *)avframe_rgb_, AV_PIX_FMT_RGB24, image_width, image_height);
+
+  avcodec_context_->codec_id = AV_CODEC_ID_MJPEG;
+  avcodec_context_->width = image_width;
+  avcodec_context_->height = image_height;
+
+#if LIBAVCODEC_VERSION_MAJOR > 52
+  avcodec_context_->pix_fmt = AV_PIX_FMT_YUV422P;
+  avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
+#endif
+
+  avframe_camera_size_ = avpicture_get_size(AV_PIX_FMT_YUV422P, image_width, image_height);
+  avframe_rgb_size_ = avpicture_get_size(AV_PIX_FMT_RGB24, image_width, image_height);
+
+  /* open it */
+  if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0)
+  {
+      std::cout<<" Could not find MJPEG decoder "<<std::endl;
+//    qDebug("Could not open MJPEG Decoder");
+    return 0;
+  }
+  return 1;
+}
+
+void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
+{
+  int got_picture;
+
+  memset(RGB, 0, avframe_rgb_size_);
+
+#if LIBAVCODEC_VERSION_MAJOR > 52
+  int decoded_len;
+  AVPacket avpkt;
+  av_init_packet(&avpkt);
+
+  avpkt.size = len;
+  avpkt.data = (unsigned char*)MJPEG;
+  decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_, &got_picture, &avpkt);
+
+  if (decoded_len < 0)
+  {
+      std::cout<<" Error while decoding frame."<<std::endl;
+//    qDebug("Error while decoding frame.");
+    return;
+  }
+#else
+  avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture, (uint8_t *) MJPEG, len);
+#endif
+
+  if (!got_picture)
+  {
+      std::cout<<" Webcam: expected picture but didn't get it... "<<std::endl;
+//    qDebug("Webcam: expected picture but didn't get it...");
+    return;
+  }
+
+  int xsize = avcodec_context_->width;
+  int ysize = avcodec_context_->height;
+  int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
+  if (pic_size != avframe_camera_size_)
+  {
+      std::cout<<"outbuf size mismatch.  pic_size:"<<pic_size<<" bufsize: "<<avframe_camera_size_<<std::endl;
+//    qDebug("outbuf size mismatch.  pic_size: %d bufsize: %d", pic_size, avframe_camera_size_);
+    return;
+  }
+
+  video_sws_ = sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL,
+			      NULL,  NULL);
+  sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0, ysize, avframe_rgb_->data,
+            avframe_rgb_->linesize);
+  sws_freeContext(video_sws_);
+
+  int size = avpicture_layout((AVPicture *)avframe_rgb_, AV_PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size_);
+  if (size != avframe_rgb_size_)
+  {
+      std::cout<<" webcam: avpicture_layout error:  "<<size<<std::endl;
+//    qDebug("webcam: avpicture_layout error: %d", size);
+    return;
+  }
+}
+
+void UsbCam::process_image(const void * src, int len, camera_image_t *dest)
+{
+  if (pixelformat_ == V4L2_PIX_FMT_YUYV)
+  {
+    if (monochrome_)
+    { //actually format V4L2_PIX_FMT_Y16, but xioctl gets unhappy if you don't use the advertised type (yuyv)
+      mono102mono8((char*)src, dest->image, dest->width * dest->height);
+    }
+    else
+    {
+      yuyv2rgb((char*)src, dest->image, dest->width * dest->height);
+    }
+  }
+  else if (pixelformat_ == V4L2_PIX_FMT_UYVY)
+    uyvy2rgb((char*)src, dest->image, dest->width * dest->height);
+  else if (pixelformat_ == V4L2_PIX_FMT_MJPEG)
+  {
+      if(mbNotDecodeJpeg)
+      {
+          dest->image_size = len;
+          memcpy(dest->image,(char *)src,dest->image_size);
+      }
+      else
+      {
+        mjpeg2rgb((char*)src, len, dest->image, dest->width * dest->height);
+      }
+  }
+  else if (pixelformat_ == V4L2_PIX_FMT_RGB24)
+    rgb242rgb((char*)src, dest->image, dest->width * dest->height);
+  else if (pixelformat_ == V4L2_PIX_FMT_GREY)
+    memcpy(dest->image, (char*)src, dest->width * dest->height);
+}
+
+int UsbCam::read_frame()
+{
+  struct v4l2_buffer buf;
+  unsigned int i;
+  int len;
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      len = read(fd_, buffers_[0].start, buffers_[0].length);
+      if (len == -1)
+      {
+        switch (errno)
+        {
+          case EAGAIN:
+            return 0;
+
+          case EIO:
+            /* Could ignore EIO, see spec. */
+
+            /* fall through */
+
+          default:
+            errno_exit("read");
+        }
+      }
+
+      process_image(buffers_[0].start, len, image_);
+
+      break;
+
+    case IO_METHOD_MMAP:
+      CLEAR(buf);
+
+      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+      buf.memory = V4L2_MEMORY_MMAP;
+
+      if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
+      {
+        switch (errno)
+        {
+          case EAGAIN:
+            return 0;
+
+          case EIO:
+            /* Could ignore EIO, see spec. */
+
+            /* fall through */
+
+          default:
+            errno_exit("VIDIOC_DQBUF");
+        }
+      }
+
+      assert(buf.index < n_buffers_);
+      len = buf.bytesused;
+      process_image(buffers_[buf.index].start, len, image_);
+
+      if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
+        errno_exit("VIDIOC_QBUF");
+
+      break;
+
+    case IO_METHOD_USERPTR:
+      CLEAR(buf);
+
+      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+      buf.memory = V4L2_MEMORY_USERPTR;
+
+      if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
+      {
+        switch (errno)
+        {
+          case EAGAIN:
+            return 0;
+
+          case EIO:
+            /* Could ignore EIO, see spec. */
+
+            /* fall through */
+
+          default:
+            errno_exit("VIDIOC_DQBUF");
+        }
+      }
+
+      for (i = 0; i < n_buffers_; ++i)
+        if (buf.m.userptr == (unsigned long)buffers_[i].start && buf.length == buffers_[i].length)
+          break;
+
+      assert(i < n_buffers_);
+      len = buf.bytesused;
+      process_image((void *)buf.m.userptr, len, image_);
+
+      if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
+        errno_exit("VIDIOC_QBUF");
+
+      break;
+  }
+
+  return 1;
+}
+
+bool UsbCam::is_capturing() {
+  return is_capturing_;
+}
+
+void UsbCam::stop_capturing(void)
+{
+  if(!is_capturing_) return;
+
+  is_capturing_ = false;
+  enum v4l2_buf_type type;
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      /* Nothing to do. */
+      break;
+
+    case IO_METHOD_MMAP:
+    case IO_METHOD_USERPTR:
+      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+      if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type))
+        errno_exit("VIDIOC_STREAMOFF");
+
+      break;
+  }
+}
+
+void UsbCam::start_capturing(void)
+{
+
+  if(is_capturing_) return;
+
+  unsigned int i;
+  enum v4l2_buf_type type;
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      /* Nothing to do. */
+      break;
+
+    case IO_METHOD_MMAP:
+      for (i = 0; i < n_buffers_; ++i)
+      {
+        struct v4l2_buffer buf;
+
+        CLEAR(buf);
+
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = i;
+
+        if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
+          errno_exit("VIDIOC_QBUF");
+      }
+
+      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+      if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
+        errno_exit("VIDIOC_STREAMON");
+
+      break;
+
+    case IO_METHOD_USERPTR:
+      for (i = 0; i < n_buffers_; ++i)
+      {
+        struct v4l2_buffer buf;
+
+        CLEAR(buf);
+
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_USERPTR;
+        buf.index = i;
+        buf.m.userptr = (unsigned long)buffers_[i].start;
+        buf.length = buffers_[i].length;
+
+        if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
+          errno_exit("VIDIOC_QBUF");
+      }
+
+      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+      if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
+        errno_exit("VIDIOC_STREAMON");
+
+      break;
+  }
+  is_capturing_ = true;
+}
+
+void UsbCam::uninit_device(void)
+{
+  unsigned int i;
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      free(buffers_[0].start);
+      break;
+
+    case IO_METHOD_MMAP:
+      for (i = 0; i < n_buffers_; ++i)
+        if (-1 == munmap(buffers_[i].start, buffers_[i].length))
+          errno_exit("munmap");
+      break;
+
+    case IO_METHOD_USERPTR:
+      for (i = 0; i < n_buffers_; ++i)
+        free(buffers_[i].start);
+      break;
+  }
+
+  free(buffers_);
+}
+
+void UsbCam::init_read(unsigned int buffer_size)
+{
+  buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
+
+  if (!buffers_)
+  {
+      std::cout<<"Out of memory"<<std::endl;
+//    qDebug("Out of memory");
+    exit(EXIT_FAILURE);
+  }
+
+  buffers_[0].length = buffer_size;
+  buffers_[0].start = malloc(buffer_size);
+
+  if (!buffers_[0].start)
+  {
+      std::cout<<"Out of memory"<<std::endl;
+//    qDebug("Out of memory");
+    exit(EXIT_FAILURE);
+  }
+}
+
+void UsbCam::init_mmap(void)
+{
+  struct v4l2_requestbuffers req;
+
+  CLEAR(req);
+
+  req.count = 4;
+  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  req.memory = V4L2_MEMORY_MMAP;
+
+  if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
+  {
+    if (EINVAL == errno)
+    {
+      std::cout<<camera_dev_ << " does not support memory mapping"<<std::endl;
+      exit(EXIT_FAILURE);
+    }
+    else
+    {
+      errno_exit("VIDIOC_REQBUFS");
+    }
+  }
+
+  if (req.count < 2)
+  {
+    std::cout<<"Insufficient buffer memory on " << camera_dev_<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_));
+
+  if (!buffers_)
+  {
+    std::cout<<"Out of memory"<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_)
+  {
+    struct v4l2_buffer buf;
+
+    CLEAR(buf);
+
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory = V4L2_MEMORY_MMAP;
+    buf.index = n_buffers_;
+
+    if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf))
+      errno_exit("VIDIOC_QUERYBUF");
+
+    buffers_[n_buffers_].length = buf.length;
+    buffers_[n_buffers_].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
+				      MAP_SHARED /* recommended */,
+				      fd_, buf.m.offset);
+
+    if (MAP_FAILED == buffers_[n_buffers_].start)
+      errno_exit("mmap");
+  }
+}
+
+void UsbCam::init_userp(unsigned int buffer_size)
+{
+  struct v4l2_requestbuffers req;
+  unsigned int page_size;
+
+  page_size = getpagesize();
+  buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
+
+  CLEAR(req);
+
+  req.count = 4;
+  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  req.memory = V4L2_MEMORY_USERPTR;
+
+  if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
+  {
+    if (EINVAL == errno)
+    {
+      std::cout<<camera_dev_ << " does not support "
+                "user pointer i/o"<<std::endl;
+      exit(EXIT_FAILURE);
+    }
+    else
+    {
+      errno_exit("VIDIOC_REQBUFS");
+    }
+  }
+
+  buffers_ = (buffer*)calloc(4, sizeof(*buffers_));
+
+  if (!buffers_)
+  {
+      std::cout<<"Out of memory"<<std::endl;
+//    qDebug("Out of memory");
+    exit(EXIT_FAILURE);
+  }
+
+  for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_)
+  {
+    buffers_[n_buffers_].length = buffer_size;
+    buffers_[n_buffers_].start = memalign(/* boundary */page_size, buffer_size);
+
+    if (!buffers_[n_buffers_].start)
+    {
+        std::cout<<"Out of memory"<<std::endl;
+//      qDebug("Out of memory");
+      exit(EXIT_FAILURE);
+    }
+  }
+}
+
+void UsbCam::init_device(int image_width, int image_height, int framerate)
+{
+  struct v4l2_capability cap;
+  struct v4l2_cropcap cropcap;
+  struct v4l2_crop crop;
+  struct v4l2_format fmt;
+  unsigned int min;
+
+  if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap))
+  {
+    if (EINVAL == errno)
+    {
+      std::cout<<camera_dev_ << " is no V4L2 device"<<std::endl;
+      exit(EXIT_FAILURE);
+    }
+    else
+    {
+      errno_exit("VIDIOC_QUERYCAP");
+    }
+  }
+
+  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
+  {
+    std::cout<<camera_dev_ << " is no video capture device"<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      if (!(cap.capabilities & V4L2_CAP_READWRITE))
+      {
+        std::cout<<camera_dev_ << " does not support read i/o"<<std::endl;
+        exit(EXIT_FAILURE);
+      }
+
+      break;
+
+    case IO_METHOD_MMAP:
+    case IO_METHOD_USERPTR:
+      if (!(cap.capabilities & V4L2_CAP_STREAMING))
+      {
+        std::cout<<camera_dev_ << " does not support streaming i/o"<<std::endl;
+        exit(EXIT_FAILURE);
+      }
+
+      break;
+  }
+
+  /* Select video input, video standard and tune here. */
+
+  CLEAR(cropcap);
+
+  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+  if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap))
+  {
+    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    crop.c = cropcap.defrect; /* reset to default */
+
+    if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop))
+    {
+      switch (errno)
+      {
+        case EINVAL:
+          /* Cropping not supported. */
+          break;
+        default:
+          /* Errors ignored. */
+          break;
+      }
+    }
+  }
+  else
+  {
+    /* Errors ignored. */
+  }
+
+  CLEAR(fmt);
+
+//  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+//  fmt.fmt.pix.width = 640;
+//  fmt.fmt.pix.height = 480;
+//  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+//  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+
+  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  fmt.fmt.pix.width = image_width;
+  fmt.fmt.pix.height = image_height;
+  fmt.fmt.pix.pixelformat = pixelformat_;
+  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+
+  if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt))
+    errno_exit("VIDIOC_S_FMT");
+
+  /* Note VIDIOC_S_FMT may change width and height. */
+
+  /* Buggy driver paranoia. */
+  min = fmt.fmt.pix.width * 2;
+  if (fmt.fmt.pix.bytesperline < min)
+    fmt.fmt.pix.bytesperline = min;
+  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+  if (fmt.fmt.pix.sizeimage < min)
+    fmt.fmt.pix.sizeimage = min;
+
+  image_width = fmt.fmt.pix.width;
+  image_height = fmt.fmt.pix.height;
+
+  struct v4l2_streamparm stream_params;
+  memset(&stream_params, 0, sizeof(stream_params));
+  stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0)
+    errno_exit("Couldn't query v4l fps!");
+
+  std::cout<<"Capability flag: "<<stream_params.parm.capture.capability<<std::endl;
+//  qDebug("Capability flag: 0x%x", stream_params.parm.capture.capability);
+
+  stream_params.parm.capture.timeperframe.numerator = 1;
+  stream_params.parm.capture.timeperframe.denominator = framerate;
+  if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0)
+  {
+      std::cout<<"Couldn't set camera framerate"<<std::endl;
+  }
+ //   qDebug("Couldn't set camera framerate");
+  else
+  {
+      std::cout<<"Set framerate to be "<<framerate<<std::endl;
+//      qDebug("Set framerate to be %i", framerate);
+  }
+
+
+  switch (io_)
+  {
+    case IO_METHOD_READ:
+      init_read(fmt.fmt.pix.sizeimage);
+      break;
+
+    case IO_METHOD_MMAP:
+      init_mmap();
+      break;
+
+    case IO_METHOD_USERPTR:
+      init_userp(fmt.fmt.pix.sizeimage);
+      break;
+  }
+}
+
+void UsbCam::close_device(void)
+{
+  if (-1 == close(fd_))
+    errno_exit("close");
+
+  fd_ = -1;
+}
+
+void UsbCam::open_device(void)
+{
+  struct stat st;
+
+  if (-1 == stat(camera_dev_.c_str(), &st))
+  {
+    std::cout<<"Cannot identify '" << camera_dev_ << "': " << errno << ", " << strerror(errno)<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  if (!S_ISCHR(st.st_mode))
+  {
+    std::cout<<camera_dev_ << " is no device"<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+
+  fd_ = open(camera_dev_.c_str(), O_RDWR /* required */| O_NONBLOCK, 0);
+
+  if (-1 == fd_)
+  {
+    std::cout<<"Cannot open '" << camera_dev_ << "': " << errno << ", " << strerror(errno)<<std::endl;
+    exit(EXIT_FAILURE);
+  }
+}
+
+void UsbCam::start(const std::string& dev, io_method io_method,
+		   pixel_format pixel_format, int image_width, int image_height,
+		   int framerate)
+{
+  camera_dev_ = dev;
+
+  io_ = io_method;
+  monochrome_ = false;
+  if (pixel_format == PIXEL_FORMAT_YUYV)
+    pixelformat_ = V4L2_PIX_FMT_YUYV;
+  else if (pixel_format == PIXEL_FORMAT_UYVY)
+    pixelformat_ = V4L2_PIX_FMT_UYVY;
+  else if (pixel_format == PIXEL_FORMAT_MJPEG)
+  {
+    pixelformat_ = V4L2_PIX_FMT_MJPEG;
+    init_mjpeg_decoder(image_width, image_height);
+  }
+  else if (pixel_format == PIXEL_FORMAT_YUVMONO10)
+  {
+    //actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels), but we need to use the advertised type (yuyv)
+    pixelformat_ = V4L2_PIX_FMT_YUYV;
+    monochrome_ = true;
+  }
+  else if (pixel_format == PIXEL_FORMAT_RGB24)
+  {
+    pixelformat_ = V4L2_PIX_FMT_RGB24;
+  }
+  else if (pixel_format == PIXEL_FORMAT_GREY)
+  {
+    pixelformat_ = V4L2_PIX_FMT_GREY;
+    monochrome_ = true;
+  }
+  else
+  {
+    std::cout<<"Unknown pixel format."<<std::endl;
+//    qDebug("Unknown pixel format.");
+    exit(EXIT_FAILURE);
+  }
+
+
+  open_device();
+  init_device(image_width, image_height, framerate);
+  start_capturing();
+
+  image_ = (camera_image_t *)calloc(1, sizeof(camera_image_t));
+
+  image_->width = image_width;
+  image_->height = image_height;
+  image_->bytes_per_pixel = 3;      //corrected 11/10/15 (BYTES not BITS per pixel)
+
+  image_->image_size = image_->width * image_->height * image_->bytes_per_pixel;
+  image_->is_new = 0;
+  image_->image = (char *)calloc(image_->image_size, sizeof(char));
+  memset(image_->image, 0, image_->image_size * sizeof(char));
+}
+
+void UsbCam::shutdown(void)
+{
+  stop_capturing();
+  uninit_device();
+  close_device();
+
+  if (avcodec_context_)
+  {
+    avcodec_close(avcodec_context_);
+    av_free(avcodec_context_);
+    avcodec_context_ = NULL;
+  }
+  if (avframe_camera_)
+    av_free(avframe_camera_);
+  avframe_camera_ = NULL;
+  if (avframe_rgb_)
+    av_free(avframe_rgb_);
+  avframe_rgb_ = NULL;
+  if(image_)
+    free(image_);
+  image_ = NULL;
+}
+
+//void UsbCam::grab_image(sensor_msgs::Image* msg)
+//{
+//  // grab the image
+//  grab_image();
+//  // stamp the image
+//  msg->header.stamp = ros::Time::now();
+//  // fill the info
+//  if (monochrome_)
+//  {
+//    fillImage(*msg, "mono8", image_->height, image_->width, image_->width,
+//        image_->image);
+//  }
+//  else
+//  {
+//    fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width,
+//        image_->image);
+//  }
+//}
+
+void UsbCam::grab_image(char *strdata, int *pnlen,const int nSize)
+{
+    grab_image();
+    if(image_->image_size > nSize)
+    {
+        std::cout<<"image size is "<<image_->image_size<<" . data buffer is "<<nSize<<" small."<<std::endl;
+ //       qDebug("image size is %d . data buffer is %d small.",image_->image_size,nSize);
+        *pnlen = 0;
+        return;
+    }
+    memcpy(strdata,image_->image,image_->image_size);
+    *pnlen = image_->image_size;
+}
+
+void UsbCam::grab_image()
+{
+  fd_set fds;
+  struct timeval tv;
+  int r;
+
+  FD_ZERO(&fds);
+  FD_SET(fd_, &fds);
+
+  /* Timeout. */
+  tv.tv_sec = 5;
+  tv.tv_usec = 0;
+
+  r = select(fd_ + 1, &fds, NULL, NULL, &tv);
+
+  if (-1 == r)
+  {
+    if (EINTR == errno)
+      return;
+
+    errno_exit("select");
+  }
+
+  if (0 == r)
+  {
+      std::cout<<"select timeout."<<std::endl;
+ //   qDebug("select timeout");
+    exit(EXIT_FAILURE);
+  }
+
+  read_frame();
+  image_->is_new = 1;
+}
+
+// enables/disables auto focus
+void UsbCam::set_auto_focus(int value)
+{
+  struct v4l2_queryctrl queryctrl;
+  struct v4l2_ext_control control;
+
+  memset(&queryctrl, 0, sizeof(queryctrl));
+  queryctrl.id = V4L2_CID_FOCUS_AUTO;
+
+  if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl))
+  {
+    if (errno != EINVAL)
+    {
+      perror("VIDIOC_QUERYCTRL");
+      return;
+    }
+    else
+    {
+        std::cout<<" V4L2_CID_FOCUS_AUTO is not supported"<<std::endl;
+  //    qDebug("V4L2_CID_FOCUS_AUTO is not supported");
+      return;
+    }
+  }
+  else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
+  {
+      std::cout<<" V4L2_CID_FOCUS_AUTO is not supported"<<std::endl;
+//    qDebug("V4L2_CID_FOCUS_AUTO is not supported");
+    return;
+  }
+  else
+  {
+    memset(&control, 0, sizeof(control));
+    control.id = V4L2_CID_FOCUS_AUTO;
+    control.value = value;
+
+    if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control))
+    {
+      perror("VIDIOC_S_CTRL");
+      return;
+    }
+  }
+}
+
+/**
+* Set video device parameter via call to v4l-utils.
+*
+* @param param The name of the parameter to set
+* @param param The value to assign
+*/
+void UsbCam::set_v4l_parameter(const std::string& param, int value)
+{
+  set_v4l_parameter(param, boost::lexical_cast<std::string>(value));
+}
+/**
+* Set video device parameter via call to v4l-utils.
+*
+* @param param The name of the parameter to set
+* @param param The value to assign
+*/
+void UsbCam::set_v4l_parameter(const std::string& param, const std::string& value)
+{
+  // build the command
+  std::stringstream ss;
+  ss << "v4l2-ctl --device=" << camera_dev_ << " -c " << param << "=" << value << " 2>&1";
+  std::string cmd = ss.str();
+
+  // capture the output
+  std::string output;
+  int buffer_size = 256;
+  char buffer[buffer_size];
+  FILE *stream = popen(cmd.c_str(), "r");
+  if (stream)
+  {
+    while (!feof(stream))
+      if (fgets(buffer, buffer_size, stream) != NULL)
+        output.append(buffer);
+    pclose(stream);
+    // any output should be an error
+    if (output.length() > 0)
+    {
+        std::cout<<output<<std::endl;
+//      qDebug("%s", output.c_str());
+    }
+  }
+  else
+  {
+      std::cout<<"usb_cam_node could not run '"<<cmd<<"'"<<std::endl;
+ //     qDebug("usb_cam_node could not run '%s'", cmd.c_str());
+  }
+
+}
+
+UsbCam::io_method UsbCam::io_method_from_string(const std::string& str)
+{
+  if (str == "mmap")
+    return IO_METHOD_MMAP;
+  else if (str == "read")
+    return IO_METHOD_READ;
+  else if (str == "userptr")
+    return IO_METHOD_USERPTR;
+  else
+    return IO_METHOD_UNKNOWN;
+}
+
+UsbCam::pixel_format UsbCam::pixel_format_from_string(const std::string& str)
+{
+    if (str == "yuyv")
+      return PIXEL_FORMAT_YUYV;
+    else if (str == "uyvy")
+      return PIXEL_FORMAT_UYVY;
+    else if (str == "mjpeg")
+      return PIXEL_FORMAT_MJPEG;
+    else if (str == "yuvmono10")
+      return PIXEL_FORMAT_YUVMONO10;
+    else if (str == "rgb24")
+      return PIXEL_FORMAT_RGB24;
+    else if (str == "grey")
+      return PIXEL_FORMAT_GREY;
+    else
+      return PIXEL_FORMAT_UNKNOWN;
+}
+
+void UsbCam::set_useRawMJPEG(bool bUse)
+{
+    mbNotDecodeJpeg = bUse;
+}
+
+}

src/driver/read4usb/usb_cam.h

@@ -0,0 +1,161 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Robert Bosch LLC.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Robert Bosch nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#ifndef USB_CAM_USB_CAM_H
+#define USB_CAM_USB_CAM_H
+
+#include <asm/types.h>          /* for videodev2.h */
+
+extern "C"
+{
+#include <linux/videodev2.h>
+#include <libavcodec/avcodec.h>
+#include <libswscale/swscale.h>
+#include <libavutil/mem.h>
+}
+
+// legacy reasons
+#include <libavcodec/version.h>
+#if LIBAVCODEC_VERSION_MAJOR < 55
+#define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG
+#endif
+
+#include <string>
+#include <sstream>
+
+//#include <sensor_msgs/Image.h>
+
+namespace usb_cam {
+
+class UsbCam {
+ public:
+  typedef enum
+  {
+    IO_METHOD_READ, IO_METHOD_MMAP, IO_METHOD_USERPTR, IO_METHOD_UNKNOWN,
+  } io_method;
+
+  typedef enum
+  {
+    PIXEL_FORMAT_YUYV, PIXEL_FORMAT_UYVY, PIXEL_FORMAT_MJPEG, PIXEL_FORMAT_YUVMONO10, PIXEL_FORMAT_RGB24, PIXEL_FORMAT_GREY, PIXEL_FORMAT_UNKNOWN
+  } pixel_format;
+
+  UsbCam();
+  ~UsbCam();
+
+  // start camera
+  void start(const std::string& dev, io_method io, pixel_format pf,
+		    int image_width, int image_height, int framerate);
+  // shutdown camera
+  void shutdown(void);
+
+  // grabs a new image from the camera
+//  void grab_image(sensor_msgs::Image* image);
+
+  void grab_image(char * strdata,int * pnlen,const int nsize);
+
+  // enables/disable auto focus
+  void set_auto_focus(int value);
+
+  // Set video device parameters
+  void set_v4l_parameter(const std::string& param, int value);
+  void set_v4l_parameter(const std::string& param, const std::string& value);
+
+  static io_method io_method_from_string(const std::string& str);
+  static pixel_format pixel_format_from_string(const std::string& str);
+
+  void stop_capturing(void);
+  void start_capturing(void);
+  bool is_capturing();
+
+  void set_useRawMJPEG(bool bUse);
+
+ private:
+  typedef struct
+  {
+    int width;
+    int height;
+    int bytes_per_pixel;
+    int image_size;
+    char *image;
+    int is_new;
+  } camera_image_t;
+
+  struct buffer
+  {
+    void * start;
+    size_t length;
+  };
+
+
+  int init_mjpeg_decoder(int image_width, int image_height);
+  void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels);
+  void process_image(const void * src, int len, camera_image_t *dest);
+  int read_frame();
+  void uninit_device(void);
+  void init_read(unsigned int buffer_size);
+  void init_mmap(void);
+  void init_userp(unsigned int buffer_size);
+  void init_device(int image_width, int image_height, int framerate);
+  void close_device(void);
+  void open_device(void);
+  void grab_image();
+  bool is_capturing_;
+
+
+  std::string camera_dev_;
+  unsigned int pixelformat_;
+  bool monochrome_;
+  io_method io_;
+  int fd_;
+  buffer * buffers_;
+  unsigned int n_buffers_;
+  AVFrame *avframe_camera_;
+  AVFrame *avframe_rgb_;
+  AVCodec *avcodec_;
+  AVDictionary *avoptions_;
+  AVCodecContext *avcodec_context_;
+  int avframe_camera_size_;
+  int avframe_rgb_size_;
+  struct SwsContext *video_sws_;
+  camera_image_t *image_;
+
+  bool mbNotDecodeJpeg = true;
+
+};
+
+}
+
+#endif
+