modularization/src/fusion/lidar_radar_fusion/main.cpp

#include <QCoreApplication>
#include <QDateTime>
#include <iostream>
#include "modulecomm.h"
#include "radarobjectarray.pb.h"
#include "objectarray.pb.h"
#include "fusionobjectarray.pb.h"
#include "fusionobject.pb.h"
#include <QThread>
#include <QString>
#include <QMutex>
#include "eigen3/Eigen/Dense"
//#include "data_manager.h"
#include "fusion.hpp"
#include "Tracking.h"
#include "perceptionoutput.h"
#include "transformation.h"
using namespace iv;
using namespace fusion;

void *gfu = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
static QMutex gMutex;

typedef  iv::radar::radarobjectarray RadarDataType;
typedef  iv::lidar::objectarray LidarDataType;
typedef  std::chrono::system_clock::time_point TimeType;
typedef  std::vector<iv::Perception::PerceptionOutput> LidarObject;

iv::radar::radarobjectarray radarobjvec;
LidarObject lidar_obj;


QTime gTime;
using namespace std;
int gntemp = 0;
iv::fusion::fusionobjectarray li_ra_fusion;
void datafusion();
TrackerSettings settings;
CTracker tracker(settings);
bool m_isTrackerInitialized = false;
//iv::DataManager data_manager;

void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    std::cout<<"radar is ok"<<std::endl;
    iv::radar::radarobjectarray radarobj;
    if(nSize<1)return;
    if(false == radarobj.ParseFromArray(strdata,nSize))
    {
        std::cout<<" Listenesrfront fail."<<std::endl;
        return;
    }
    else{
        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
    }
    gMutex.lock();
    radarobjvec.CopyFrom(radarobj);
    gMutex.unlock();
}

void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    qDebug("size is %d",nSize);

    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);


    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
    int i;
    for(i=0;i<nCount;i++)
    {
        iv::Perception::PerceptionOutput temp;
        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
        lidar_per->push_back(temp);
        pdata++;
    }
    gMutex.lock();
    lidar_obj.swap(*lidar_per);
//    std::cout<<"lidar   objs      :"<<lidar_obj.size()<<std::endl;
    datafusion();
    gMutex.unlock();
}

int ccccc =0;
void datafusion()
{
//    gMutex.lock();
//    Transformation::RadarPross(radarobjvec);
    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
//    std::cout<<li_ra_fusion.obj_size()<<std::endl;

//    gMutex.lock();
//    std::cout<<" track   begin"<<std::endl;
    //---------------------------------------------  init tracker  -------------------------------------------------
    if (!m_isTrackerInitialized)
    {
        m_isTrackerInitialized = InitTracker(tracker);
        if (!m_isTrackerInitialized)
        {
            std::cerr << "Tracker initialize error!!!" << std::endl;
        }
    }
    Tracking(li_ra_fusion, tracker);
//    std::cout<<"track    end"<<std::endl;

    //--------------------------------------------  end tracking  ---------------------------------------------------
//    gMutex.unlock();
    string out;

    if(li_ra_fusion.obj_size() == 0)
    {
        iv::fusion::fusionobject fake_obj;
        iv::fusion::fusionobject *fake_obj_;
        iv::fusion::PointXYZ fake_cen;
        iv::fusion::PointXYZ *fake_cen_;
        fake_cen.set_x(10000);
        fake_cen.set_y(10000);
        fake_cen.set_z(10000);
        fake_cen_ = fake_obj.mutable_centroid();
        fake_cen_ ->CopyFrom(fake_cen);

        fake_obj_ = li_ra_fusion.add_obj();
        fake_obj_->CopyFrom(fake_obj);
        out = li_ra_fusion.SerializeAsString();
    }else
    {
        out = li_ra_fusion.SerializeAsString();
    }
    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
//    gMutex.unlock();
}


int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    tracker.setSettings(settings);

    void *gpa;
    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
//    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
    gpa = iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
    return a.exec();
}