save sensor data(lidar,senyun-camera,4 camera)on orin.

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
+
+
+
+
+
+