modularization/src/detection/detection_lidar_cnn_segmentation/main.cpp

#include <QCoreApplication>
#include <pcl/io/pcd_io.h>

#include "cnn_segmentation.h"
//#include "detect_lidar_type.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 <thread>

//#include "alog.h"

CNNSegmentation gcnn;

void * gpa;
void * gpdetect;
iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

std::thread * gpthread;

std::string gstrinput;
std::string gstroutput;

static iv::lidar::PointXYZ pcltoprotopoint(pcl::PointXYZ x)
{
    iv::lidar::PointXYZ lx;
    lx.set_x(x.x);
    lx.set_y(x.y);
    lx.set_z(x.z);
    return lx;
}

static void GetLidarObj(std::vector<Obstacle> objvec,iv::lidar::objectarray & lidarobjvec)
{
    int i;

    givlog->verbose("CNNOBJ","object size is %d",objvec.size());
    for(i=0;i<objvec.size();i++)
    {
        Obstacle x;
        iv::lidar::lidarobject lidarobj;
  //      iv::lidar::object xo;

        x = objvec.at(i);
        pcl::PointCloud<pcl::PointXYZI> in_cluster = *x.cloud_ptr;



        float min_x = std::numeric_limits<float>::max();
        float max_x = -std::numeric_limits<float>::max();
        float min_y = std::numeric_limits<float>::max();
        float max_y = -std::numeric_limits<float>::max();
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();
        float average_x = 0, average_y = 0, average_z = 0;
        float length, width, height;

        pcl::PointXYZ min_point;
        pcl::PointXYZ max_point;
  //      pcl::PointXYZ average_point;
        pcl::PointXYZ centroid;

        centroid.x = 0;
        centroid.y = 0;
        centroid.z = 0;

        for (auto pit = in_cluster.points.begin(); pit != in_cluster.points.end(); ++pit)
        {
            average_x += pit->x;
            average_y += pit->y;
            average_z += pit->z;
            centroid.x += pit->x;
            centroid.y += pit->y;
            centroid.z += pit->z;

            if (pit->x < min_x)
                min_x = pit->x;
            if (pit->y < min_y)
                min_y = pit->y;
            if (pit->z < min_z)
                min_z = pit->z;
            if (pit->x > max_x)
                max_x = pit->x;
            if (pit->y > max_y)
                max_y = pit->y;
            if (pit->z > max_z)
                max_z = pit->z;
        }

        // min, max points
        min_point.x = min_x;
        min_point.y = min_y;
        min_point.z = min_z;
        max_point.x = max_x;
        max_point.y = max_y;
        max_point.z = max_z;

        if (in_cluster.points.size() > 0)
        {
            centroid.x /= in_cluster.points.size();
            centroid.y /= in_cluster.points.size();
            centroid.z /= in_cluster.points.size();
        }
        length = max_point.x - min_point.x;
        width = max_point.y - min_point.y;
        height = max_point.z - min_point.z;

        iv::lidar::PointXYZ lx;
        iv::lidar::PointXYZ * plx;
        lx = pcltoprotopoint(centroid);
        plx = lidarobj.mutable_centroid();
        plx->CopyFrom(lx);
        lx = pcltoprotopoint(min_point);
        plx = lidarobj.mutable_min_point();
        plx->CopyFrom(lx);
        lx = pcltoprotopoint(max_point);
        plx = lidarobj.mutable_max_point();
        plx->CopyFrom(lx);




//        lidarobj.centroid = centroid;
//        lidarobj.min_point = min_point;
//        lidarobj.max_point = max_point;


        lx.set_x(min_point.x + length / 2);
        lx.set_y(min_point.y + width / 2);
        lx.set_z(min_point.z + height / 2);

        plx = lidarobj.mutable_position();
        plx->CopyFrom(lx);

//        lidarobj.position.x = min_point.x + length / 2;
//        lidarobj.position.y = min_point.y + width / 2;
//        lidarobj.position.z = min_point.z + height / 2;
        iv::lidar::Dimension ld;
        iv::lidar::Dimension * pld;
        ld.set_x((length < 0) ? -1 * length : length);
        ld.set_y((width < 0) ? -1 * width : width);
        ld.set_z((height < 0) ? -1 * height : height);
//        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
//        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
//        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);
        pld = lidarobj.mutable_dimensions();
        pld->CopyFrom(ld);

        lidarobj.set_mntype(x.meta_type);
        lidarobj.set_score(x.score);


        lidarobj.set_type_name(x.GetTypeString());

        int j;
        for(j=0;j<x.meta_type_probs.size();j++)
        {
            lidarobj.add_type_probs(x.meta_type_probs.at(j));
        }

        for(j=0;j<x.cloud_ptr->size();j++)
        {
            iv::lidar::PointXYZI * pcp = lidarobj.add_cloud();
            pcl::PointXYZI pp = x.cloud_ptr->at(j);
            pcp->set_x(pp.x);
            pcp->set_y(pp.y);
            pcp->set_z(pp.z);
            pcp->set_i(pp.intensity);
        }

        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
        po->CopyFrom(lidarobj);

        givlog->verbose("CNNOBJ","    object %d type:%s x:%6.3f y:%6.3f",i,
                        lidarobj.type_name().data(),lidarobj.position().x(),
                        lidarobj.position().y());


//        lidarobj.mnType = x.meta_type;

//        lidarobj.score = x.score;
//        lidarobj.type_probs = x.meta_type_probs;
//        lidarobj.cloud_ptr = x.cloud_ptr;

//        int ndatalen;
//        char * strx = lidarobj.serialize(ndatalen);

//        lidarobj.unserialize(strx,ndatalen);
//        delete strx;

//        lidarobjvec.push_back(lidarobj);

    }
}



//static char * lidarobjtostr( std::vector<iv::lidar::lidarobject> lidarobjvec,int & ntlen)
//{
//    int * nlens = new int[lidarobjvec.size()];
//    int i;
//    int ntotallen = 2*sizeof(int) + lidarobjvec.size() * sizeof(int);
//    for(i=0;i<lidarobjvec.size();i++)
//    {
//        *(nlens+i) = lidarobjvec.at(i).getserializelen();
//        ntotallen = ntotallen + *(nlens + i);
//    }
//    char * pRtn = new char[ntotallen];
//    int npos = 0;
//    int nobjsize = lidarobjvec.size();
//    memcpy(pRtn,&ntotallen,sizeof(int));npos = npos + sizeof(int);
//    memcpy(pRtn+npos,&nobjsize,sizeof(int));npos = npos + sizeof(int);
//    for(i=0;i<lidarobjvec.size();i++)
//    {
//        int nlenvalue = *(nlens + i);
//        memcpy(pRtn+npos,&nlenvalue,sizeof(int));
//        npos = npos + sizeof(int);
//    }
//    for(i=0;i<lidarobjvec.size();i++)
//    {
//        int ndatalen;
//        char * strser = lidarobjvec.at(i).serialize(ndatalen);
//        memcpy(pRtn+ npos,strser,*(nlens+i));
//        npos = npos + *(nlens+i);
//        delete strser;
//    }

//    if(ntotallen != npos)std::cout<<" total len is "<<ntotallen<<" pos is "<<npos<<std::endl;

//    delete nlens;
//    ntlen = ntotallen;
//    return pRtn;

//}

void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
{
    std::cout<<"pcd size is "<<pc_ptr->size()<<std::endl;
    pcl::PointIndices valid_idx;
    auto &indices = valid_idx.indices;
    indices.resize(pc_ptr->size());
    std::iota(indices.begin(), indices.end(), 0);

    std::vector<Obstacle> objvec;

    gcnn.segment(pc_ptr, valid_idx, objvec);
    givlog->verbose("obj size is %d", objvec.size());
    std::cout<<"obj size is "<<objvec.size()<<std::endl;

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

    double timex = pc_ptr->header.stamp;
    timex = timex/1000.0;
    lidarobjvec.set_timestamp(pc_ptr->header.stamp);

    int ntlen;
    std::string out = lidarobjvec.SerializeAsString();
 //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
    givlog->verbose("lenth is %d",out.length());
  //  delete strout;

//    int i;
//    for(i=0;i<objvec.size();i++)
//    {
//        Obstacle x;
//        x = objvec.at(i);

//        std::cout<<"obj "<<i<<" x is "<<x.GetTypeString()<<std::endl;

//        iv::lidar::lidarobject y;
//        y = lidarobjvec.at(i);
//        std::cout<<"     "<<" x is"<<y.position.x<<" y is "<<y.position.y<<" len x is "<<y.dimensions.x<<" len y is "<<y.dimensions.y<<std::endl;
//        std::cout<<"     "<<" type is "<<y.mnType<<std::endl;
//    }
}


int gnothavedatatime = 0;

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;
    }

    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;
    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;
//        if((p->x<-30)||(p->x>30)||(p->y>50)||(p->y<-50))
//        {

//        }
//        else
//        {
            memcpy(&xp,p,sizeof(pcl::PointXYZI));
            xp.z = xp.z;
            point_cloud->push_back(xp);

//        }
        p++;
//        xp.x = p->x;
//        xp.y = p->y;
//        xp.z = p->z;
//        xp.intensity = p->intensity;
//        point_cloud->push_back(xp);
//        p++;
    }

//    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;

    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_cnn_segmentation is ok");
                gfault->SetFaultState(0,0,"data is ok.");
                break;
            case 1:
                givlog->warn("more than 10 seconds not have lidar pointcloud.");
                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
                break;
            case 2:
                givlog->error("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;

}

//void testcall(const char * str)
//{
//    std::cout<<"call msg is "<<str<<std::endl;
//}

int main(int argc, char *argv[])
{
    showversion("detection_lidar_cnn_segmentation");
    QCoreApplication a(argc, argv);

    gfault = new iv::Ivfault("lidar_cnn_segmentation");
    givlog = new iv::Ivlog("lidar_cnn_segmentation");

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

//    ALOG("cnn.");
//    ALOG("VALUE = %d",1);

//    iv::Logger::Inst().SetCallback(testcall);

//    ALOG("cnn.");

//    iv::Logger::Inst().setlogfile("/home/nvidia/logger.txt");

//    ALOG("CNN");

    char * strhome = getenv("HOME");
    std::string protofile = strhome ;//
    protofile += "/models/lidar/model.prototxt";
    std::string weightfile = strhome;
    weightfile += "/models/lidar/model.caffemodel";

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/detection_lidar_cnn_segmentation.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
    protofile = xparam.GetParam("prototxt",protofile.data());
    weightfile = xparam.GetParam("caffemodel",weightfile.data());
    gstrinput = xparam.GetParam("input","lidar_pc");
    gstroutput = xparam.GetParam("output","lidar_cnn_dectect");

//    std::string protofile = "/home/yuchuli/qt/bq/models/lidar/model.prototxt";
//    std::string weightfile = "/home/yuchuli/qt/bq/models/lidar/model.caffemodel";
    gcnn.init(protofile,weightfile,60.0,0.6,512,512,false,true,0);



//    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
//    pcl::io::loadPCDFile ("/home/yuchuli/test_pcd.pcd", *cloud);

//    DectectOnePCD(cloud);

    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();
}