modularization/src/detection/detection_lidar_PointPillars_MultiHead_1025/main.cpp

#include <QCoreApplication>
#include <QDateTime>
#include <iostream>
#include "pointpillars.h"
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include "xmlparam.h"
#include "modulecomm.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivexit.h"
#include "ivversion.h"
#include <thread>
#include "objectarray.pb.h"
//#include "ivbacktrace.h"
#include "Tracking.hpp"
iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

std::thread * gpthread;
PointPillars * pPillars = nullptr ;
void * gpa;
void * gpdetect;
int gnothavedatatime = 0;
const int kNumPointFeature = 5;
const int kOutputNumBoxFeature = 7;
std::string gstrinput;
std::string gstroutput;

TrackerSettings settings;
CTracker tracker(settings);
bool m_isTrackerInitialized = false;

void PclToArray(
        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
        float* out_points_array, const float normalizing_factor) {
    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
        pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
        out_points_array[i * 4 + 0] = point.x;
        out_points_array[i * 4 + 1] = point.y;
        out_points_array[i * 4 + 2] = point.z;
        out_points_array[i * 4 + 3] =
                static_cast<float>(point.intensity / normalizing_factor);
    }
}

void PclXYZITToArray(
        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
        float* out_points_array, const float normalizing_factor) {
    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
        pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
        out_points_array[i * 5 + 0] = point.x;
        out_points_array[i * 5 + 1] = point.y;
        out_points_array[i * 5 + 2] = point.z;
        out_points_array[i * 5 + 3] =
                static_cast<float>(point.intensity / normalizing_factor);
        out_points_array[i * 5 + 4] = 0;
    }
}

void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
                 std::vector<float> out_scores,iv::lidar::objectarray & lidarobjvec)
{
    int i;
    int obj_size = out_detections.size()/kOutputNumBoxFeature;
    //    givlog->verbose("OBJ","object size is %d",obj_size);
    for(i=0;i<obj_size;i++)
    {
        iv::lidar::lidarobject lidarobj;
        if (out_scores.at(i) < 0.10) continue;

        lidarobj.set_tyaw(out_detections.at(i*7+6));
        iv::lidar::PointXYZ centroid;
        iv::lidar::PointXYZ * _centroid;
        centroid.set_x(out_detections.at(i*7));
        centroid.set_y(out_detections.at(i*7+1));
        centroid.set_z(out_detections.at(i*7+2));
        _centroid = lidarobj.mutable_centroid();
        _centroid->CopyFrom(centroid);

        iv::lidar::PointXYZ min_point;
        iv::lidar::PointXYZ * _min_point;
        min_point.set_x(0);
        min_point.set_y(0);
        min_point.set_z(0);
        _min_point = lidarobj.mutable_min_point();
        _min_point->CopyFrom(min_point);

        iv::lidar::PointXYZ max_point;
        iv::lidar::PointXYZ * _max_point;
        max_point.set_x(0);
        max_point.set_y(0);
        max_point.set_z(0);
        _max_point = lidarobj.mutable_max_point();
        _max_point->CopyFrom(max_point);

        iv::lidar::PointXYZ position;
        iv::lidar::PointXYZ * _position;
        position.set_x(out_detections.at(i*7));
        position.set_y(out_detections.at(i*7+1));
        position.set_z(out_detections.at(i*7+2));
        _position = lidarobj.mutable_position();
        _position->CopyFrom(position);
        lidarobj.set_mntype(out_labels.at(i));
        // label 2  8
        if(out_labels.at(i)==2){
            lidarobj.set_mntype(8);
        }else if(out_labels.at(i)==8){
            lidarobj.set_mntype(2);
        }
        lidarobj.set_score(out_scores.at(i));
        lidarobj.add_type_probs(out_scores.at(i));

        iv::lidar::PointXYZI point_cloud;
        iv::lidar::PointXYZI * _point_cloud;
        point_cloud.set_x(out_detections.at(i*7));
        point_cloud.set_y(out_detections.at(i*7+1));
        point_cloud.set_z(out_detections.at(i*7+2));
        point_cloud.set_i(0);

        _point_cloud = lidarobj.add_cloud();
        _point_cloud->CopyFrom(point_cloud);

        iv::lidar::Dimension ld;
        iv::lidar::Dimension * pld;
        ld.set_x(out_detections.at(i*7+3));// w
        ld.set_y(out_detections.at(i*7+4));// l
        ld.set_z(out_detections.at(i*7+5));// h
        pld = lidarobj.mutable_dimensions();
        pld->CopyFrom(ld);

        //        std::cout<<"x y z   :  "<<out_detections.at(i*7+3)<<"    "<< out_detections.at(i*7+4)<<"    "<<out_detections.at(i*7+5)<<std::endl;
        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
        po->CopyFrom(lidarobj);
    }

}

void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
{
    std::shared_ptr<float> points_array_ptr = std::shared_ptr<float>(new float[pc_ptr->size() * kNumPointFeature]);
    //   float* points_array = new float[pc_ptr->size() * kNumPointFeature];
    PclXYZITToArray(pc_ptr, points_array_ptr.get(), 1.0);

    int    in_num_points = pc_ptr->width;

    std::vector<float> out_detections;
    std::vector<int> out_labels;
    std::vector<float> out_scores;

    QTime xTime;

    xTime.start();

    cudaDeviceSynchronize();
    pPillars->DoInference(points_array_ptr.get(), in_num_points, &out_detections, &out_labels , &out_scores);
    cudaDeviceSynchronize();

    //    givlog->verbose("obj size is %d", num_objects);
    //    std::cout<<"obj size is "<<num_objects<<std::endl;

    //    std::vector<iv::lidar::lidarobject> lidarobjvec;
    iv::lidar::objectarray lidarobjvec;
    GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);

    double timex = pc_ptr->header.stamp;
    timex = timex/1000.0;
    lidarobjvec.set_timestamp(pc_ptr->header.stamp);
    //---------------------------------------------  init tracker  -------------------------------------------------
    if (!m_isTrackerInitialized)
    {
        m_isTrackerInitialized = InitTracker(tracker);
        if (!m_isTrackerInitialized)
        {
            std::cerr << "Tracker initialize error!!!" << std::endl;
        }
    }
    iv::lidar::objectarray trackedobjvec = Tracking(lidarobjvec, tracker);
    //    std::<<"track    end"<<std::endl;

    //    --------------------------------------------  end tracking  --------------------------------------------------
    int ntlen;
    std::string out = trackedobjvec.SerializeAsString();
    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());

    //    givlog->verbose("lenth is %d",out.length());
}

void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    //    std::cout<<" is  ok  ------------  "<<std::endl;
    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;
    }

    gnothavedatatime = 0;
    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;
    std::shared_ptr<char> str_ptr;
    str_ptr.reset(strName);
    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;
        point_cloud->push_back(xp);
        p++;
    }

    DectectOnePCD(point_cloud);
    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
    gfault->SetFaultState(0, 0, "ok");

}

bool gbstate = true;
void statethread()
{
    int nstate = 0;
    int nlaststate = 0;
    while (gbstate)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        if(gnothavedatatime < 100000) gnothavedatatime++;

        if (gnothavedatatime  < 100){
            nstate = 0;
        }
        if (gnothavedatatime > 1000)
        {
            nstate = 1;
        }
        if (gnothavedatatime > 6000)
        {
            nstate = 2;
        }
        if (nstate != nlaststate) {
            switch (nstate) {
            case 0:
                givlog->info("detection_lidar_pointpillar is ok");
                gfault->SetFaultState(0,0,"data is ok.");
                break;
            case 1:
                givlog->info(" more than 10 seconds not have lidar pointcloud.");
                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
                break;
            case 2:
                givlog->info(" more than 60 seconds not have lidar pointcloud.");
                gfault->SetFaultState(2,2, "more than 60 seconds not have lidar pointcloud.");
                break;
            default:
                break;
            }
        }
    }
}

void exitfunc()
{
    gbstate = false;
    gpthread->join();
    std::cout<<" state thread closed."<<std::endl;
    iv::modulecomm::Unregister(gpa);
    iv::modulecomm::Unregister(gpdetect);
    std::cout<<"exit func complete"<<std::endl;
}
#include <QFile>
bool trtisexist(std::string strpfe,std::string strbackbone)
{
    QFile xFile;
    xFile.setFileName(strpfe.data());
    if(xFile.exists() == false)
    {
        return false;
    }
    xFile.setFileName(strbackbone.data());
    if(xFile.exists() == false)
    {
        return false;
    }
    return true;
}
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    //    RegisterIVBackTrace();
    tracker.setSettings(settings);
    gfault = new iv::Ivfault("lidar_pointpillar");
    givlog = new iv::Ivlog("lidar_pointpillar");

    gfault->SetFaultState(0,0,"pointpillar initialize. ");

    char * strhome = getenv("HOME");
    std::string pfe_file = strhome;
    pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
    std::string pfe_trt_file = pfe_file.substr(0, pfe_file.find(".")) + ".trt";

    std::string backbone_file = strhome;
    backbone_file += "/models/lidar/cbgs_pp_multihead_backbone.onnx";
    std::string backbone_trt_file = backbone_file.substr(0, backbone_file.find(".")) + ".trt";

    bool btrtexist = trtisexist(pfe_trt_file,backbone_trt_file);


    QString strpath = QCoreApplication::applicationDirPath();
    std::string pp_config = strpath.toStdString() ;
    pp_config += "/cfgs/cbgs_pp_multihead.yaml";
    if (argc < 2)
        strpath = strpath + "/detection_lidar_pointpillar.xml";
    else
        strpath = argv[1];

    std::cout<<pp_config<<std::endl;

    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
    pfe_file = xparam.GetParam("pfe_file",pfe_file.data());
    backbone_file = xparam.GetParam("backbone_file",backbone_file.data());
    gstrinput = xparam.GetParam("input","lidar_pc");
    gstroutput = xparam.GetParam("output","lidar_pointpillar");

    if(btrtexist == false)
    {

        std::cout<<"use onnx model."<<std::endl;
        pPillars = new PointPillars(
                    0.4,
                    0.2,
                    true,
                    pfe_file,
                    backbone_file,
                    pp_config
                    );
    }
    else
    {
        std::cout<<"use trt model."<<std::endl;
        pPillars = new PointPillars(
                    0.1,
                    0.2,
                    false,
                    pfe_trt_file,
                    backbone_trt_file,
                    pp_config
                    );
    }
    std::cout<<"PointPillars Init OK."<<std::endl;

    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
    gpthread = new std::thread(statethread);

    iv::ivexit::RegIVExitCall(exitfunc);
    return a.exec();
}