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


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;

    //        }

    //    }

    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;
    }
    //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
    gMutex.lock();
    //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<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 = trackedobjvec;
    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();
}


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_pointpillar",Listenlidarcnndetect);
    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
    return a.exec();
}