modularization/src/fusion/lidar_radar_fusion_cnn/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 "gpsimu.pb.h"
#include "lidarbuffer.h"
#include "xmlparam.h"
#include <QThread>
#include <QString>
#include <QMutex>
#include "eigen3/Eigen/Dense"
//#include "data_manager.h"
#include "fusion.hpp"
#include "Tracking.hpp"
#include "transformation.h"
#include "mobileye.pb.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;

iv::radar::radarobjectarray radarobjvec;
iv::lidar::objectarray lidar_obj;
iv::mobileye::mobileye mobileye_info;
iv::mobileye::obs obs_info;
iv::mobileye::lane lane_info;
iv::mobileye::tsr tsr_info;


QTime gTime;
using namespace std;
int gntemp = 0;
iv::fusion::fusionobjectarray li_ra_fusion;
void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion);
TrackerSettings settings;
CTracker tracker(settings);
bool m_isTrackerInitialized = false;

static bool gbUseLastLidar = false;
static lidarbuffer * gplidarbuffer;


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

    iv::radar::radarobjectarray radarobj;
    if(nSize<1)return;
    if(false == radarobj.ParseFromArray(strdata,nSize))
    {
        std::cout<<" Listenesrfront fail."<<std::endl;
        return;
    }
    else{
        //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
    }
    //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
    gMutex.lock();
    //    for(int i=0;i<radarobj.obj_size();i++)
    //    {
    //        if(abs(radarobj.obj(i).x())<2 && abs(radarobj.obj(i).vy()) >10)
    //        {
    //            std::cout<<"x    y      vx    vy  "<<radarobj.obj(i).x()<<"          "
    //                    <<radarobj.obj(i).y()<<"         "
    //                    <<radarobj.obj(i).vx()<<"     "
    //                    <<radarobj.obj(i).vy()<<"    "<<std::endl;

    //        }

    //    }
//std::cout<<"radar size:"<<nSize<<std::endl;
    radarobjvec.CopyFrom(radarobj);
    gMutex.unlock();
}

void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::lidar::objectarray lidarobj;

    if(nSize<1)return;
    if(false == lidarobj.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
        return;
    }

//    static int a = 0;
//    if(a != 0)
//    {
//        lidarobj.clear_obj();
//    }
//    a++;
//    if(a == 2)a = 0;


    std::cout<<" before obj size: "<<lidarobj.obj_size()<<std::endl;
    if(gbUseLastLidar)
    {
        iv::lidar::objectarray lidarobjraw;
        lidarobjraw.CopyFrom(lidarobj);
        gplidarbuffer->FilterLidarObj(lidarobj);
        gplidarbuffer->AddLidarObj(lidarobjraw);

    }


    std::cout<<" after obj size: "<<lidarobj.obj_size()<<std::endl;


    //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
    gMutex.lock();
//        std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
    for(int i = 0; i < lidarobj.obj_size();i++) {
        if(abs(lidarobj.obj(i).centroid().x())<4){
            std::cout<<lidarobj.obj(i).velocity_linear_x()<<std::endl;
        }
    }
    datafusion(lidarobj,radarobjvec,li_ra_fusion);
    gMutex.unlock();
}

void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::mobileye::mobileye mobileye;

    if(nSize<1)return;
    if(false == mobileye.ParseFromArray(strdata,nSize))
    {
        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
        return;
    }
    //    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
    //    for (int m_index = 0; m_index< mobileye.xobj_size(); m_index++)
    //    {
    //        std::cout<<" p_x  p_y  v_x  v_y   "<<mobileye.xobj(m_index).pos_y()<< "  "<<mobileye.xobj(m_index).pos_x()
    //                <<"    "<<mobileye.xobj(m_index).obs_rel_vel_x() * tan(mobileye.xobj(m_index).obsang())
    //               <<"   "<<mobileye.xobj(m_index).obs_rel_vel_x()<<"                                   \n"
    //               << mobileye.xobj(m_index).obsang()<<std::endl;
    //    }

    gMutex.lock();
    mobileye_info.CopyFrom(mobileye);
    gMutex.unlock();
}

int ccccc =0;
void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
{
    //    gMutex.lock();
    //    Transformation::RadarPross(radarobjvec);

    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);


    //    li_ra_fusion.Clear();
    //      std::cout<<" RLFusion     begin    "<<std::endl;
//    AddMobileye(li_ra_fusion,mobileye_info);
    //    std::cout<<" RLFusion     end      "<<std::endl;
//    mobileye_info.clear_xobj();
    //        std::cout<< "     fusion size  "<<li_ra_fusion.obj_size()<<std::endl;
    //    for(int i=0;i<li_ra_fusion.obj_size();i++)
    //    {
    //        if(abs(li_ra_fusion.obj(i).vel_relative().y())>10 )
    //        {
    //            std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.-obj(i).centroid().x()<<"          "
    //                    <<li_ra_fusion.obj(i).centroid().y()<<"         "
    //                    <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
    //                    <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
    //                    <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;

    //        }

    //    }


    //    int nsize =0;
    //    for(int nradar = 0; nradar<radarobjvec.obj_size(); nradar++)
    //    {
    //        if(radarobjvec.obj(nradar).bvalid() == false) continue;
    //        if(sqrt(abs(radarobjvec.obj(nradar).x()))< 4 && radarobjvec.obj(nradar).y()< 20 ) nsize = nsize +1;
    //    }

    //        std::cout<<"   fusion_obj_size     radar_obj_size   :   "<<li_ra_fusion.obj_size()<<"     "<<nsize<<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;
//        }
//    }
//    iv::fusion::fusionobjectarray trackedobjvec = Tracking(li_ra_fusion, tracker);
    //    std::<<"track    end"<<std::endl;

    //--------------------------------------------  end tracking  --------------------------------------------------
    //    gMutex.unlock();
    iv::fusion::fusionobjectarray out_fusion = li_ra_fusion;
    ObsToNormal(out_fusion);
    string out;

    if(out_fusion.obj_size() == 0)
    {
        //        std::cout<<"   fake   obj"<<std::endl;

        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_ = out_fusion.add_obj();
        fake_obj_->CopyFrom(fake_obj);
        out = out_fusion.SerializeAsString();
    }else
    {
        out = out_fusion.SerializeAsString();
        //        for (int k = 0; k<trackedobjvec.obj_size();k++){
        //            std::cout<<" size   x    y    vy :   "<<trackedobjvec.obj_size()<<"   "
        //                    <<trackedobjvec.obj(k).centroid().x()<< "   "<<trackedobjvec.obj(k).centroid().y()<<"    "
        //                   <<trackedobjvec.obj(k).vel_relative().y()<<"   "<<trackedobjvec.obj(k).nomal_centroid_size() <<std::endl;
        //        }
    }
    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
    //    gMutex.unlock();
}

void ListenGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenGPS Parse error."<<std::endl;
        return;
    }

    if(gbUseLastLidar)
    {
        gplidarbuffer->AddGPS(xgpsimu);
    }
}

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

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


    tracker.setSettings(settings);

    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
    std::string strgpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
    std::string struselast = xparam.GetParam("uselast","false");
    std::string strlooktime = xparam.GetParam("LookTime","3000"); //3000ms
    std::string strlookprob = xparam.GetParam("LookProb","0.3");
    std::string strlidaroffx = xparam.GetParam("LidarOffX","0.0");
    std::string strlidaroffy = xparam.GetParam("LidarOffY","0.0");
    std::string strdisthresh = xparam.GetParam("disthresh","1.0");

    if(struselast == "true")
    {
        gbUseLastLidar = true;
        gplidarbuffer = new lidarbuffer(atoi(strlooktime.data()),atof(strlookprob.data()),
                                        atof(strlidaroffx.data()),atof(strlidaroffy.data()),atof(strdisthresh.data()));
    }
    else
    {
        gbUseLastLidar = false;
    }

    void * pa = iv::modulecomm::RegisterRecv(strgpsmsgname.data(),ListenGPS);

    void *gpa;
    gpa = iv::modulecomm::RegisterRecv("radar_front",Listenesrfront);
    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
    return a.exec();
}