123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220 |
- #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 "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{
-
- }
-
- gMutex.lock();
-
-
-
-
-
-
-
-
-
-
- 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;
- }
-
- gMutex.lock();
-
- 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;
- }
-
-
-
-
-
-
-
-
- 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)
- {
-
-
- RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-
-
- AddMobileye(li_ra_fusion,mobileye_info);
-
- mobileye_info.clear_xobj();
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- 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);
-
-
-
- iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
- ObsToNormal(out_fusion);
- string out;
- if(out_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_ = out_fusion.add_obj();
- fake_obj_->CopyFrom(fake_obj);
- out = out_fusion.SerializeAsString();
- }else
- {
- out = out_fusion.SerializeAsString();
-
-
-
-
-
- }
- iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
-
- }
- 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();
- }
|