modify fusion and aovid obstacle

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -25,7 +25,7 @@ using namespace std;
 extern iv::Ivlog * givlog;
 
 #define PI (3.1415926535897932384626433832795)
-
+#define AVOID_NEW 1
 iv::decition::DecideGps00::DecideGps00() {
 
 }
@@ -104,7 +104,8 @@ int obsLostTimeAvoid = 0;
 
 // double avoidTime = 0;
 
-
+int avoidXNewPre=0;
+int avoidXNew=0;
 double avoidX =0;
 int    turnLampFlag=0;  //  if <0:left , if >0:right
 float roadWidth = 3.5;
@@ -192,6 +193,8 @@ std::vector<iv::Point2D> traceOriLeft,traceOriRight;
 bool qiedianCount = false;
 
 static int obstacle_avoid_flag=0;
+
+
 //日常展示
 
 iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
@@ -923,6 +926,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     obsDistanceAvoid = -1;
     esrIndexAvoid = -1;
     roadPre = -1;
+    avoidXNewPre=0;
     //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;
 
     gpsTraceOri.clear();
@@ -1052,6 +1056,20 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }else{
          gpsMapLine[PathPoint]->runMode=0;
     }
+
+#ifdef AVOID_NEW
+    if(obstacle_avoid_flag==1){
+
+    }else{
+        avoidXNew=0;
+        roadNow = roadOri;
+        if(vehState==backOri){
+            vehState=normalRun;
+        }
+    }
+    givlog->debug("decition_brain","avoidXNew: %d",
+                    avoidXNew);
+#else
     if(obstacle_avoid_flag==1){
         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
     }else{
@@ -1061,6 +1079,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             vehState=normalRun;
         }
     }
+#endif
+
     if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
                                                 ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
     {
@@ -1131,6 +1151,33 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+#ifdef AVOID_NEW
+    if(gpsTraceNow.size()==0){
+        if (avoidXNew==0)
+        {
+            if(vehState == backOri)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
+            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+            gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+            gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+#else
     if(gpsTraceNow.size()==0){
         if (roadNow==roadOri)
         {
@@ -1161,6 +1208,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+#endif
+
+
+
     if(vehState==normalRun)
     {
         if(gpsTraceNow.size()>200){
@@ -1397,11 +1448,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         dSpeed = min(25.0,dSpeed);
 
     }
-
+#ifdef AVOID_NEW
+    if((vehState==avoiding)||(vehState==backOri)||(avoidXNew!=0)||(vehState==preAvoid))
+    {
+        dSpeed = min(8.0,dSpeed);
+    }
+#else
     if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
     {
         dSpeed = min(8.0,dSpeed);
     }
+#endif
 
     if((gpsMapLine[PathPoint]->speed)>0.001)
     {
@@ -1560,7 +1617,45 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
     //   计算回归原始路线
+#ifdef AVOID_NEW
+    int avoidXPre=20;
+    if (avoidXNew!=0)
+    {
+        if(useOldAvoid){
+            int avoidLeft_value=0;
+            int avoidRight_value=0;
+            int* avoidLeft_p=&avoidLeft_value;
+            int* avoidRight_p=&avoidRight_value;
+            computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+            avoidXPre=computeBackDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNew);
+            givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
+                            vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXPre);
+        }
+    }
+    if ((avoidXNew!=0 && avoidXPre!=20))
+    {
+
+        if((avoidXPre-avoidXNew)<0)
+            turnLampFlag<0;
+        else if((avoidXPre-avoidXNew)>0)
+            turnLampFlag>0;
+        else
+            turnLampFlag=0;
+
+
+        //double back_offset=avoidXPre-avoidXNew;
+        if(useOldAvoid){
+            gpsTraceAvoidXY.clear();
+            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidXPre,now_gps_ins,gpsTraceNow);
+            startBackGpsIns = now_gps_ins;
+        }
+
+        vehState = backOri;
+        avoidXNew=0;
+        hasCheckedBackLidar = false;
 
+    }
+#else
     if ((roadNow!=roadOri))
     {
         if(useFrenet){
@@ -1608,6 +1703,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
 
     }
+#endif
+
 
     static bool avoid_speed_flag=false;
     /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
@@ -1712,7 +1809,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         cout<<"\n====================preAvoid==================\n"<<endl;
         /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
-        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
+        if (obsDistance>ServiceCarStatus.aocfDis)
         {
             // vehState = avoidObs; 0929
             vehState=normalRun;
@@ -1721,9 +1818,26 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
             if(useOldAvoid){
+#ifdef AVOID_NEW
+                int avoidLeft_value=0;
+                int avoidRight_value=0;
+                int* avoidLeft_p=&avoidLeft_value;
+                int* avoidRight_p=&avoidRight_value;
+                computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+                //computeAvoidBoundary(0,2,4.5,2.4,avoidLeft_p,avoidRight_p);
+                //avoidLeft_value=-5;
+                //avoidRight_value=0;
+                avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);
+                givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
+                                vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXNewPre);
+                givlog->debug("decition_brain","avoidXNewPre1: %d",avoidXNewPre);
+
+#else
                 roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
               //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
                  avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+
+#endif
             }
             else if(useFrenet){
                 double offsetL = -(roadSum - 1)*roadWidth;
@@ -1731,6 +1845,34 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
             }
 
+
+
+#ifdef AVOID_NEW
+            if (avoidXNewPre ==0 && obsDistance<1.5 && obsDistance>0)
+            {
+                vehState = avoidObs;
+            }
+            else if (avoidXNewPre != 0)
+            {
+                if(useOldAvoid){
+                    avoidXNew=avoidXNewPre;
+                     if(avoidXNew<0)
+                         turnLampFlag<0;
+                     else if(avoidXNew>0)
+                         turnLampFlag>0;
+                     else
+                         turnLampFlag=0;
+
+                    gpsTraceNow.clear();
+                    gpsTraceAvoidXY.clear();
+                    gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                    startAvoidGpsIns = now_gps_ins;
+                }
+                vehState = avoiding;
+                obstacle_avoid_flag=1;
+                hasCheckedAvoidLidar = false;
+            }
+#else
             if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
             {
                 //  vehState = waitAvoid; 0929
@@ -1776,6 +1918,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
 
             }
+#endif
         }
     }
 
@@ -3949,3 +4092,146 @@ double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gp
     }
     return dist_to_end;
 }
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight ){
+
+    double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);
+    *avoidXRight=((int)RightBoundary);
+    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    *avoidXLeft=(-(int)LeftBoundary);
+}
+
+int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight) {
+
+    int signed_record_avoidX=0,record_avoidX=20;
+    double obs_cur_distance=500,record_obstacle_distance;
+
+    obs_property.clear();
+    for (int i =  avoidLeft; i <= avoidRight; i++)
+    {
+        obsvalue x_value;
+        obsvalue *x=&x_value;
+        x_value.avoid_distance=i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, i);
+        computeObsOnRoadNew(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per,x);
+        obs_property.push_back(x_value);
+        if(i==0)
+            obs_cur_distance=x_value.obs_distance;
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    for(int j=0;j<obs_property.size();j++){
+        if(obs_property[j].obs_distance>100){
+                if(abs(obs_property[j].avoid_distance)<record_avoidX){
+                        record_avoidX=abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX=obs_property[j].avoid_distance;
+                }
+        }
+    }
+
+   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+   {
+          int obs_test=abs(signed_record_avoidX)-1;
+
+          if(obs_property[obs_test].obs_distance>100){
+                    signed_record_avoidX-=1;
+                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+          }
+   }
+    if((signed_record_avoidX!=0)&&(obs_cur_distance<50))
+    {
+
+                        avoidMinDistance= obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+                        startAvoidGps=now_gps_ins;
+                        return signed_record_avoidX;
+    }
+    return 0;
+}
+
+void iv::decition::DecideGps00::computeObsOnRoadNew(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva) {
+
+    double obs=-1,obsSd=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0){
+        obsva->obs_distance=500;
+    }else{
+        obsva->obs_distance=obs;
+    }
+}
+
+int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow) {
+
+    obs_property.clear();
+    double record_avoidX=20,signed_record_avoidX=0,obs_cur_distance=500;
+    for (int i =  avoidLeft; i <= avoidRight; i++)
+    {
+        obsvalue x_value;
+        obsvalue *x=&x_value;
+        x_value.avoid_distance=i;
+        gpsTraceBack.clear();
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, i);
+        computeObsOnRoadNew(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per,x);
+        obs_property.push_back(x_value);
+        if(i==0)
+            obs_cur_distance=x_value.obs_distance;
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+                        return 0;
+    return 20;
+}
+
+
+
+

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -19,8 +19,7 @@
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
-
-#include "common/tracepointstation.h"
+#include <common/tracepointstation.h>
 
 namespace iv {
 namespace decition {
@@ -79,6 +78,9 @@ public:
     std::vector<int> v2xTrafficVector;
     iv::GPS_INS startAvoidGpsIns,startBackGpsIns;
 
+    double avoidMinDistance;
+    iv::GPS_INS startAvoidGps;
+
     ///kkcai, 2020-01-03
     //bool isFirstRun = true;
     static bool isFirstRun;
@@ -122,6 +124,14 @@ public:
 
     bool changingDangWei=false;
 
+    struct obsvalue{
+        int avoid_distance;
+        double obs_distance;
+    };
+    std::vector<obsvalue> obs_property;
+
+
+
     BaseController *pid_Controller;
     BaseAdapter *ge3_Adapter;
     BaseAdapter *qingyuan_Adapter;
@@ -217,6 +227,12 @@ public:
     void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
     float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
     double computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint);
+    void   computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft,int* avoidXRight );
+    int computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight);
+    void computeObsOnRoadNew(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva);
+    int computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow);
+
 
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;
@@ -254,6 +270,7 @@ extern bool rapidStop;
 extern int gpsMissCount;
 extern bool changeRoad;
 extern double avoidX;
+extern int avoidXNew;
 extern bool parkBesideRoad;
 extern double steerSpeed;
 extern bool transferPieriod;

src/fusion/fusion_pointcloud_bus/fusion_pointcloud_bus.yaml

@@ -1,14 +1,13 @@
 lidar:
   - left
   - right
-  - rear
-
+  - ignore
 left:
   memname: lidarpc_left
   offset:
-    x: -1.2
-    y: 0.0
-    z: 1.0
+    x: -1
+    y: 0
+    z: -1.0
   maximum:
     x: 100
     y: 100.0
@@ -20,23 +19,9 @@ left:
 right:
   memname: lidarpc_right
   offset:
-    x: 1.2
-    y: 0.0
-    z: 1.0
-  maximum:
-    x: 100
-    y: 100.0
-    z: 100
-  minimum:
-    x: -100
-    y: -100.0
-    z: -100
-rear:
-  memname: lidarpc_rear
-  offset:
-    x: 0.0
-    y: -6.3
-    z: 2.8
+    x: 1
+    y: 0
+    z: -1.0
   maximum:
     x: 100
     y: 100.0
@@ -46,8 +31,10 @@ rear:
     y: -100.0
     z: -100
 ignore:
-  xmin: -1.2
-  ymin: -10
-  xmax: 1.2
-  ymax: 0
+  xmin: -1.3
+  ymin: -5.0
+  xmax: 1.3
+  ymax: 0.5
+  zmin: -1.8
+  zmax: 3.0;
 output: lidar_pc

src/fusion/lidar_radar_fusion/fusion.hpp

@@ -6,6 +6,7 @@
 #include "opencv2/opencv.hpp"
 #include "perceptionoutput.h"
 #include "Eigen/Eigen"
+#include "mobileye.pb.h"
 
 namespace iv {
 namespace fusion {
@@ -16,20 +17,21 @@ static float time_step = 0.3;
 static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
 
 typedef  std::vector<iv::Perception::PerceptionOutput> LidarObject;
+typedef  std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> LidarObjectPtr;
 float ComputeDis(cv::Point2f po1, cv::Point2f po2)
 {
     return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
 }
 
-void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array,
+void Get_AssociationMat(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarray& radar_object_array,
                         std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
 {
-    std::cout<<"       begin     "<<std::endl;
-    int nlidar = lidar_object_vector.size();
+//    std::cout<<" is      ok     "<<std::endl;
+    int nlidar = lidar_object_vector_ptr->size();
     int nradar = radar_object_array.obj_size();
     match_idx.clear();
     radar_idx.clear();
-    for(int i = 0; i<lidar_object_vector.size(); i++)
+    for(int i = 0; i<lidar_object_vector_ptr->size(); i++)
     {
         match_index match;
         match.nlidar = i;
@@ -39,7 +41,7 @@ void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjecta
         {
 //            std::cout<<"  is     ok   "<<std::endl;
             if(radar_object_array.obj(j).bvalid() == false) continue;
-            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector.at(i))))
+            if(!(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_vector_ptr->at(i))))
             {
                 fuindex.push_back(j);
             }
@@ -48,8 +50,8 @@ void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjecta
         {
             std::vector<float> dis;
             cv::Point2f po1;
-            po1.x = lidar_object_vector.at(i).location.x;
-            po1.y = lidar_object_vector.at(i).location.y;
+            po1.x = lidar_object_vector_ptr->at(i).location.x;
+            po1.y = lidar_object_vector_ptr->at(i).location.y;
             for(std::vector<int>::iterator d = fuindex.begin(); d !=fuindex.end(); d++)
             {
                 cv::Point2f po2;
@@ -81,46 +83,50 @@ void Get_AssociationMat(LidarObject& lidar_object_vector,iv::radar::radarobjecta
             radar_idx.push_back(k);
         }
     }
-    std::cout<<"      end     "<<std::endl;
 }
 
-void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+void RLfusion(LidarObjectPtr& lidar_object_vector_ptr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
+//    std::cout<<"     RLfusion         begin  "<<std::endl;
     lidar_radar_fusion_object_array.Clear();
     std::vector<match_index> match_idx;
     std::vector<int> radar_idx;
-    Get_AssociationMat(lidar_object_vector,radar_object_array,match_idx,radar_idx);
+//    std::cout<<"     association       begin           "<<std::endl;
+    Get_AssociationMat(lidar_object_vector_ptr,radar_object_array,match_idx,radar_idx);
+//    std::cout<<"     association       end  "<<std::endl;
     for(int i =0; i< match_idx.size(); i++)
     {
+//        std::cout<<"  lidar    oook  "<<std::endl;
         iv::fusion::fusionobject fusion_object;
-        fusion_object.set_id(lidar_object_vector.at(match_idx.at(i).nlidar).tracker_id);
-        fusion_object.set_type_name(labels[lidar_object_vector.at(match_idx.at(i).nlidar).label]);
-        fusion_object.set_prob(lidar_object_vector.at(match_idx.at(i).nlidar).robustness);
+        fusion_object.set_id(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).tracker_id);
+        fusion_object.set_type_name(labels[lidar_object_vector_ptr->at(match_idx.at(i).nlidar).label]);
+        fusion_object.set_prob(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).robustness);
         if(match_idx.at(i).nradar == -1000)
         {
+
 //            std::cout<<"  lidar    ok  "<<std::endl;
-            fusion_object.set_yaw(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
-            fusion_object.set_lifetime(lidar_object_vector.at(match_idx.at(i).nlidar).visible_life);
+            fusion_object.set_yaw(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw);
+            fusion_object.set_lifetime(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).visible_life);
 
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
-            vel_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x);
-            vel_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y);
+            vel_relative.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).velocity.x);
+            vel_relative.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).velocity.y);
             vel_relative_ = fusion_object.mutable_vel_relative();
             vel_relative_->CopyFrom(vel_relative);
 
             iv::fusion::VelXY vel_abs;
             iv::fusion::VelXY *vel_abs_;
-            vel_abs.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.x);
-            vel_abs.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).velocity_abs.y);
+            vel_abs.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).velocity_abs.x);
+            vel_abs.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).velocity_abs.y);
             vel_abs_ = fusion_object.mutable_vel_abs();
             vel_abs_->CopyFrom(vel_abs);
 
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x);
-            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y);
-            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
+            centroid.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.x);
+            centroid.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.y);
+            centroid.set_z(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.z);
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
         }else {
@@ -144,39 +150,39 @@ void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& rada
 
             iv::fusion::PointXYZ centroid;
             iv::fusion::PointXYZ *centroid_;
-            centroid.set_x(lidar_object_vector.at(match_idx.at(i).nradar).location.x);
-            centroid.set_y(lidar_object_vector.at(match_idx.at(i).nradar).location.y);
-            centroid.set_z(lidar_object_vector.at(match_idx.at(i).nradar).location.z);
+            centroid.set_x(radar_object_array.obj(match_idx.at(i).nradar).x());
+            centroid.set_y(radar_object_array.obj(match_idx.at(i).nradar).y());
+            centroid.set_z(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.z);
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
         }
         iv::fusion::AccXY acc_relative;
         iv::fusion::AccXY *acc_relative_;
-        acc_relative.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.x);
-        acc_relative.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).acceleration_abs.y);
+        acc_relative.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).acceleration_abs.x);
+        acc_relative.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).acceleration_abs.y);
         acc_relative_ = fusion_object.mutable_acc_relative();
         acc_relative_->CopyFrom(acc_relative);
 
         iv::fusion::PointXYZ min_point;
         iv::fusion::PointXYZ *min_point_;
-        min_point.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.x);
-        min_point.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).nearest_point.y);
+        min_point.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).nearest_point.x);
+        min_point.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).nearest_point.y);
         min_point_ = fusion_object.mutable_min_point();
         min_point_->CopyFrom(min_point);
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
-        dimension.set_x(lidar_object_vector.at(match_idx.at(i).nlidar).size.x);
-        dimension.set_y(lidar_object_vector.at(match_idx.at(i).nlidar).size.y);
-        dimension.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).size.z);
+        dimension.set_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.x);
+        dimension.set_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.y);
+        dimension.set_z(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.z);
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
-        if((lidar_object_vector.at(match_idx.at(i).nlidar).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
+        if((lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.x>0)&&(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.y>0))
         {
-            int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
+            int xp = (int)((lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
             if(xp == 0)xp=1;
-            int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.y/0.2)/2.0);
+            int yp = (int)((lidar_object_vector_ptr->at(match_idx.at(i).nlidar).size.y/0.2)/2.0);
             if(yp == 0)yp=1;
             int ix,iy;
             for(ix = 0; ix<(xp*2); ix++)
@@ -187,65 +193,20 @@ void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& rada
                     iv::fusion::NomalXYZ *nomal_centroid_;
                     float nomal_x = ix*0.2 - xp*0.2;
                     float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = lidar_object_vector.at(match_idx.at(i).nlidar).location.z;
-                    float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
-                    float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(i).yaw);
-                    nomal_centroid.set_nomal_x(lidar_object_vector.at(match_idx.at(i).nlidar).location.x + s);
-                    nomal_centroid.set_nomal_y(lidar_object_vector.at(match_idx.at(i).nlidar).location.y+ t);
+                    float nomal_z = lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.z;
+                    float s = nomal_x*cos(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw);
+                    float t = nomal_x*sin(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector_ptr->at(i).yaw);
+                    nomal_centroid.set_nomal_x(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.x + s);
+                    nomal_centroid.set_nomal_y(lidar_object_vector_ptr->at(match_idx.at(i).nlidar).location.y+ t);
                     nomal_centroid_ = fusion_object.add_nomal_centroid();
                     nomal_centroid_->CopyFrom(nomal_centroid);
                 }
             }
         }
-        for(int k = 0; k<10; k++)
-        {
-//            std::cout<<"fusion    begin"<<std::endl;
-            iv::fusion::PointXYZ point_forcaste;
-            iv::fusion::PointXYZ *point_forcaste_;
-            float forcast_x = lidar_object_vector.at(match_idx.at(i).nlidar).location.x + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.x*time_step*(k+1);
-            float forcast_y = lidar_object_vector.at(match_idx.at(i).nlidar).location.y + lidar_object_vector.at(match_idx.at(i).nlidar).velocity.y*time_step*(k+1);
-            point_forcaste.set_x(forcast_x);
-            point_forcaste.set_y(forcast_y);
-            point_forcaste.set_z(lidar_object_vector.at(match_idx.at(i).nlidar).location.z);
-            point_forcaste_ = fusion_object.add_point_forecast();
-            point_forcaste_->CopyFrom(point_forcaste);
-
-            iv::fusion::NomalForecast forcast_normal;
-            iv::fusion::NomalForecast *forcast_normal_;
-            forcast_normal.set_forecast_index(i);
-//            std::cout<<"fusion     end"<<std::endl;
-            if((lidar_object_vector.at(i).size.x>0)&&(lidar_object_vector.at(match_idx.at(i).nlidar).size.y>0))
-            {
-                int xp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.x/0.2)/2.0);
-                if(xp == 0)xp=1;
-                int yp = (int)((lidar_object_vector.at(match_idx.at(i).nlidar).size.y/0.2)/2.0);
-                if(yp == 0)yp=1;
-                int ix,iy;
-                for(ix = 0; ix<(xp*2); ix++)
-                {
-                    for(iy = 0; iy<(yp*2); iy++)
-                    {
-                        iv::fusion::NomalXYZ nomal_forcast;
-                        iv::fusion::NomalXYZ *nomal_forcast_;
-                        float nomal_x = ix*0.2 - xp*0.2;
-                        float nomal_y = iy*0.2 - yp*0.2;
-                        float nomal_z = lidar_object_vector.at(match_idx.at(i).nlidar).location.z;
-                        float s = nomal_x*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) - nomal_y*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
-                        float t = nomal_x*sin(lidar_object_vector.at(match_idx.at(i).nlidar).yaw) + nomal_y*cos(lidar_object_vector.at(match_idx.at(i).nlidar).yaw);
-                        nomal_forcast.set_nomal_x(forcast_x + s);
-                        nomal_forcast.set_nomal_y(forcast_y + t);
-                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                        nomal_forcast_->CopyFrom(nomal_forcast);
-                    }
-                }
-            }
-            forcast_normal_=fusion_object.add_forecast_nomals();
-            forcast_normal_->CopyFrom(forcast_normal);
-        }
-
         iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
         pobj->CopyFrom(fusion_object);
     }
+//     std::cout<<"     RLfusion       center  "<<std::endl;
         for(int j = 0; j< radar_idx.size() ; j++){
             if(radar_object_array.obj(j).bvalid() == false) continue;
             if(abs(radar_object_array.obj(j).x())<4 && radar_object_array.obj(j).y()< 100) continue;
@@ -297,54 +258,73 @@ void RLfusion(LidarObject& lidar_object_vector,iv::radar::radarobjectarray& rada
                     nomal_centroid_->CopyFrom(nomal_centroid);
                 }
             }
+            iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+            obj->CopyFrom(fusion_obj);
+        }  
+//        std::cout<<"     RLfusion       end  "<<std::endl;
+}
 
-            for(int k = 0; k<10; k++)
-            {
-//                std::cout<<"fusion    begin"<<std::endl;
-                iv::fusion::PointXYZ point_forcaste;
-                iv::fusion::PointXYZ *point_forcaste_;
-                float forcast_x = radar_object_array.obj(j).x() + radar_object_array.obj(j).vx()*time_step*(k+1);
-                float forcast_y = radar_object_array.obj(j).y() + radar_object_array.obj(j).vy()*time_step*(k+1);
-                point_forcaste.set_x(forcast_x);
-                point_forcaste.set_y(forcast_y);
-                point_forcaste.set_z(1.0);
-                point_forcaste_ = fusion_obj.add_point_forecast();
-                point_forcaste_->CopyFrom(point_forcaste);
+void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
+{
+    int time_step = 0.3;
+//    lidar_radar_fusion_object_array.Clear();
+    for(int j = 0; j< xobs_info.xobj_size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
+        vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
 
-                iv::fusion::NomalForecast forcast_normal;
-                iv::fusion::NomalForecast *forcast_normal_;
-                forcast_normal.set_forecast_index(j);
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(-(xobs_info.xobj(j).pos_y()));
+        centroid.set_y(xobs_info.xobj(j).pos_x());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
 
-                int xp = (int)((0.5/0.2)/2.0);
-                if(xp == 0)xp=1;
-                int yp = (int)((0.5/0.2)/2.0);
-                if(yp == 0)yp=1;
-                int ix,iy;
-                for(ix = 0; ix<(xp*2); ix++)
-                {
-                    for(iy = 0; iy<(yp*2); iy++)
-                    {
-                        iv::fusion::NomalXYZ nomal_forcast;
-                        iv::fusion::NomalXYZ *nomal_forcast_;
-                        float nomal_x = ix*0.2 - xp*0.2;
-                        float nomal_y = iy*0.2 - yp*0.2;
-                        float nomal_z = 1.0;
-                        float s = nomal_x*cos(radar_object_array.obj(j).angle()) - nomal_y*sin(radar_object_array.obj(j).angle());
-                        float t = nomal_x*sin(radar_object_array.obj(j).angle()) + nomal_y*cos(radar_object_array.obj(j).angle());
-                        nomal_forcast.set_nomal_x(forcast_x + s);
-                        nomal_forcast.set_nomal_y(forcast_y + t);
-                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                        nomal_forcast_->CopyFrom(nomal_forcast);
-                    }
-                }
-                forcast_normal_=fusion_obj.add_forecast_nomals();
-                forcast_normal_->CopyFrom(forcast_normal);
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(xobs_info.xobj(j).obswidth());
+        dimension.set_y(2.0);
+        dimension.set_z(1.0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
+        if(xp == 0)xp=1;
+        int yp = (int)((2.0/0.2)/2.0);
+        if(yp == 0)yp=1;
+        int ix,iy;
+        for(ix = 0; ix<(xp*2); ix++)
+        {
+            for(iy = 0; iy<(yp*2); iy++)
+            {
+                iv::fusion::NomalXYZ nomal_centroid;
+                iv::fusion::NomalXYZ *nomal_centroid_;
+                float nomal_x = ix*0.2 - xp*0.2;
+                float nomal_y = iy*0.2 - yp*0.2;
+                float nomal_z = 1.0;
+                float s = nomal_x*cos(0)
+                        - nomal_y*sin(0);
+                float t = nomal_x*sin(0)
+                        + nomal_y*cos(0);
+                nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
+                nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
+                nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                nomal_centroid_->CopyFrom(nomal_centroid);
             }
+        }
+
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
 
-            iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
-            obj->CopyFrom(fusion_obj);
-        }  
 }
+
 }
 }
 

src/fusion/lidar_radar_fusion/fusion_probabilities.cpp

@@ -20,12 +20,12 @@ int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::rad
 {
 //    std::cout<<"  lidar   x   y   "<<lidarobject.nearest_point.x<<"           "<<lidarobject.nearest_point.y<<std::endl;
 //    std::cout<<"  radar   x   y   "<<radarPoint.x()<<"     "<<radarPoint.y()<<std::endl;
-    if(!((radarPoint.x()  >=(lidarobject.nearest_point.x))&&( radarPoint.y() >= (lidarobject.nearest_point.y))&&
-         (radarPoint.x()  <=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radarPoint.y()<=(lidarobject.location.y + lidarobject.size.y * 0.5 + 1))))
+    if((radarPoint.x()  >=(lidarobject.nearest_point.x))&&( radarPoint.y() >= (lidarobject.nearest_point.y))&&
+         (radarPoint.x()  <=(lidarobject.location.x + lidarobject.size.x*0.5))&&(radarPoint.y()<=(lidarobject.location.y + lidarobject.size.y * 0.5 + 1)))
     {
-        return 0;
-    } else {
         return 1;
+    } else {
+        return 0;
     }
 
 

src/fusion/lidar_radar_fusion/lidar_radar_fusion.pro

@@ -29,6 +29,11 @@ HEADERS += \
     Tracker/track.h \
     Tracker/VOTTracker.hpp \
     Tracker/Tracking.h \
+    ../../include/msgtype/vbox.pb.h \
+    ../../include/msgtype/mobileye_tsr.pb.h \
+    ../../include/msgtype/mobileye_obs.pb.h \
+    ../../include/msgtype/mobileye_lane.pb.h \
+    ../../include/msgtype/mobileye.pb.h
 
 
 SOURCES += \
@@ -45,6 +50,10 @@ SOURCES += \
     Tracker/HungarianAlg.cpp \
     Tracker/Kalman.cpp \
     Tracker/track.cpp \
+    ../../include/msgtype/mobileye_tsr.pb.cc \
+    ../../include/msgtype/mobileye_obs.pb.cc \
+    ../../include/msgtype/mobileye_lane.pb.cc \
+    ../../include/msgtype/mobileye.pb.cc
 
 
 INCLUDEPATH += /usr/include/eigen3

src/fusion/lidar_radar_fusion/main.cpp

@@ -15,6 +15,8 @@
 #include "Tracking.h"
 #include "perceptionoutput.h"
 #include "transformation.h"
+#include "mobileye.pb.h"
+
 using namespace iv;
 using namespace fusion;
 
@@ -25,8 +27,10 @@ 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;
+typedef  std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> LidarObjectPtr;
 
 iv::radar::radarobjectarray radarobjvec;
+iv::mobileye::mobileye mobileye_info;
 LidarObject lidar_obj;
 
 
@@ -34,7 +38,7 @@ QTime gTime;
 using namespace std;
 int gntemp = 0;
 iv::fusion::fusionobjectarray li_ra_fusion;
-void datafusion();
+void datafusion(iv::radar::radarobjectarray& radar_obj,LidarObjectPtr lidar_object_vector_ptr,iv::mobileye::mobileye& mobileye);
 TrackerSettings settings;
 CTracker tracker(settings);
 bool m_isTrackerInitialized = false;
@@ -77,18 +81,45 @@ void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int
         pdata++;
     }
     gMutex.lock();
-    lidar_obj.swap(*lidar_per);
+//    lidar_obj.swap(*lidar_per);
 //    std::cout<<"lidar   objs      :"<<lidar_obj.size()<<std::endl;
-    datafusion();
+    datafusion(radarobjvec,lidar_per,mobileye_info);
     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()
+void datafusion(iv::radar::radarobjectarray& radar_obj,LidarObjectPtr lidar_object_vector_ptr,iv::mobileye::mobileye& mobileye)
 {
 //    gMutex.lock();
 //    Transformation::RadarPross(radarobjvec);
-    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    RLfusion(lidar_object_vector_ptr,radarobjvec,li_ra_fusion);
+    AddMobileye(li_ra_fusion,mobileye);
 //    std::cout<<li_ra_fusion.obj_size()<<std::endl;
 
 //    gMutex.lock();
@@ -142,5 +173,7 @@ int main(int argc, char *argv[])
     gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
 //    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
+
     return a.exec();
 }