fix(fusion,driver,detection,controller):fix many error

deploy-agx.sh

@@ -2,7 +2,7 @@
 
 Qtgccdir='/usr/lib/aarch64-linux-gnu/qt5'
 QtPlatformdir=$Qtgccdir/plugins/platforms
-QtLibDir=$Qtgccdir/lib
+QtLibDir=$Qtgccdir/../
 
 if [ "$#" -lt 1 ]; then
 	echo "没有输入"

src/common/modulecomm/modulecomm.pro

@@ -16,7 +16,7 @@ unix:system("./../../../include/linuxsystemtest.sh ")
 unix:include(./../../../include/systemdef.pri)
 win32: DEFINES += SYSTEM_WIN
 
-DEFINES += USE_GROUPUDP
+# DEFINES += USE_GROUPUDP
 
 if(contains(DEFINES,USE_GROUPUDP)){
 QT += network

src/controller/controller_hapo/main.cpp

@@ -212,17 +212,18 @@ void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsign
     }
     else
     {
+        std::cout<<"  shift "<<xrc.shift()<<" acc "<<xrc.acc()<<" brake: "<<xrc.brake()<<" wheel: "<<xrc.wheel()<<std::endl;
         gbAutoDriving = false;
         gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_REMOTE);
-        xdecition.set_torque(xrc.acc());
+        xdecition.set_torque(xrc.acc()*0.08);
         xdecition.set_brake(xrc.brake());
         xdecition.set_wheelangle(xrc.wheel()*5.5f);
         if(xrc.shift() > 0) //D shift
-            xdecition.set_gear(1); //0P 1D 2R 3N
+            xdecition.set_gear(4); //0P 1D 2R 3N
         else if(xrc.shift() < 0)
             xdecition.set_gear(2);
-        else
-            xdecition.set_gear(3);
+//        else
+//            xdecition.set_gear(3);
         xdecition.set_handbrake(0);
         xdecition.set_mode(1);
         gMutex.lock();

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -1911,7 +1911,7 @@ static int file_num;
     }
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.1)//&&(realspeed>1)//&& (avoid_speed_flag==true)        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
 //        ObsTimeStart=GetTickCount();
@@ -2021,7 +2021,7 @@ static int file_num;
                 //computeAvoidBoundary(0,2,4.5,2.4,avoidLeft_p,avoidRight_p);
                 //avoidLeft_value=-5;
                 //avoidRight_value=0;
-                computeObsBackward(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);//2020418
+               // computeObsBackward(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);//2020418
                 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);
@@ -2349,12 +2349,11 @@ static int file_num;
 
         if(ServiceCarStatus.msysparam.mvehtype=="haomo")
         {
-//            if(obsDistance>ServiceCarStatus.socfDis && obsDistance<15)
-//            {
-//                dSpeed = min(3.0,dSpeed);
-//            }
-//            else
-            if((obsDistance>ServiceCarStatus.socfDis-4) &&obsDistance<ServiceCarStatus.socfDis) //
+            if(obsDistance>ServiceCarStatus.socfDis && obsDistance<15)
+            {
+                dSpeed = min(5.0,dSpeed);
+            }
+          else if((obsDistance>ServiceCarStatus.socfDis-4) &&obsDistance<ServiceCarStatus.socfDis) //
             {
 //               dSpeed=0;
                dSpeed = min(3.0,dSpeed);
@@ -4631,73 +4630,6 @@ void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSu
 
 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;
-//    }
-//    int record_vector=0;
-//    for(int j=0;j<obs_property.size();j++){
-//        if(obs_property[j].obs_distance>100){
-//            if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
-//                break;
-//            }
-//                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;
-//                        record_vector=j;
-//                }
-//        }
-//    }
-
-//   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
-//   {
-//          int obs_test=record_vector-1;
-
-//          if(obs_property[obs_test].obs_distance>100){
-//                    signed_record_avoidX-=1;
-//                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
-//          }
-//   }
-//   if(front_car_id>0){
-//           if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
-//                    signed_record_avoidX=front_car.avoidX;
-//                    return signed_record_avoidX;
-//           }
-
-//           if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
-//                    signed_record_avoidX=front_car.avoidX;
-//                    return 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;
-
-    //20200418，add,begin
     int signed_record_avoidX=0,record_avoidX=20;
     double obs_cur_distance=500,record_obstacle_distance;
 
@@ -4720,57 +4652,29 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
     {
         hasCheckedAvoidLidar = true;
     }
-    int record_j=0;
+    int record_vector=0;
     for(int j=0;j<obs_property.size();j++){
         if(obs_property[j].obs_distance>100){
+            if((signed_record_avoidX<0)&&(obs_property[j].avoid_distance>0)){
+                break;
+            }
                 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;
-                        record_j=j;
+                        record_vector=j;
                 }
         }
     }
 
-//   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
-//   {
-//          int obs_test=record_j-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){
-           if(signed_record_avoidX-1>=avoidLeft){
-               int obs_test=record_j-1;
-               int signed_record_avoidX_minusone=signed_record_avoidX-1;
-               if(obs_property[obs_test].obs_distance>100){
-                   for(int k=signed_record_avoidX_minusone;k<0;k++){
-                       if(obs_back_property[k].avoid_ok==false){
-                                signed_record_avoidX=0;
-                                break;
-                       }
-                       if(k==-1){
-                                signed_record_avoidX-=1;
-                       }
-                   }
-               }
-           }else{
-               for(int k=signed_record_avoidX;k<0;k++){
-                   if(obs_back_property[k].avoid_ok==false){
-                            signed_record_avoidX=0;
-                            break;
-                   }
-               }
-           }
-   }else if(signed_record_avoidX>0){
-       for(int k=1;k<signed_record_avoidX;k++){
-           if(obs_back_property[k].avoid_ok==false){
-                    signed_record_avoidX=0;
-                    break;
-           }
-       }
-   }
+   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+   {
+          int obs_test=record_vector-1;
 
+          if(obs_property[obs_test].obs_distance>100){
+                    signed_record_avoidX-=1;
+                    givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+          }
+   }
    if(front_car_id>0){
            if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
                     signed_record_avoidX=front_car.avoidX;
@@ -4791,6 +4695,101 @@ int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridP
                         return signed_record_avoidX;
     }
     return 0;
+
+    //20200418，add,begin
+//    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;
+//    }
+//    int record_j=0;
+//    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;
+//                        record_j=j;
+//                }
+//        }
+//    }
+
+////   if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+////   {
+////          int obs_test=record_j-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){
+//           if(signed_record_avoidX-1>=avoidLeft){
+//               int obs_test=record_j-1;
+//               int signed_record_avoidX_minusone=signed_record_avoidX-1;
+//               if(obs_property[obs_test].obs_distance>100){
+//                   for(int k=signed_record_avoidX_minusone;k<0;k++){
+//                       if(obs_back_property[k].avoid_ok==false){
+//                                signed_record_avoidX=0;
+//                                break;
+//                       }
+//                       if(k==-1){
+//                                signed_record_avoidX-=1;
+//                       }
+//                   }
+//               }
+//           }else{
+//               for(int k=signed_record_avoidX;k<0;k++){
+//                   if(obs_back_property[k].avoid_ok==false){
+//                            signed_record_avoidX=0;
+//                            break;
+//                   }
+//               }
+//           }
+//   }else if(signed_record_avoidX>0){
+//       for(int k=1;k<signed_record_avoidX;k++){
+//           if(obs_back_property[k].avoid_ok==false){
+//                    signed_record_avoidX=0;
+//                    break;
+//           }
+//       }
+//   }
+
+//   if(front_car_id>0){
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return signed_record_avoidX;
+//           }
+
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return 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,

src/detection/detection_lidar_ukf_pda/imm_ukf_pda.cpp

@@ -543,8 +543,8 @@ void ImmUkfPda::staticClassification()
   for (size_t i = 0; i < targets_.size(); i++)
   {
     // targets_[i].x_merge_(2) is referred for estimated velocity
-//    double current_velocity = std::abs(targets_[i].x_merge_(2));
-    double current_velocity = targets_[i].x_merge_(2);
+    double current_velocity = std::abs(targets_[i].x_merge_(2));
+//    double current_velocity = targets_[i].x_merge_(2);
     targets_[i].vel_history_.push_back(current_velocity);
     if (targets_[i].tracking_num_ == TrackingState::Stable && targets_[i].lifetime_ > life_time_thres_)
     {
@@ -712,8 +712,6 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
     dd.set_id(targets_[i].ukf_id_);
 //    dd.id = targets_[i].ukf_id_;
     dd.set_velocity_linear_x(tv);
-//    std::cout<<" -------------------------------------------------------"<<std::endl;
-//    std::cout<<"   vel    linear    "<<tv<<std::endl;
    // dd.velocity_linear_x = tv;
     dd.set_acceleration_linear_y(tyaw_rate);
    // dd.acceleration_linear_y = tyaw_rate;
@@ -737,7 +735,11 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
 
       iv::lidar::PointXYZ * pp = dd.mutable_position();
       pp->set_x(tx);
-      pp->set_y((ty));
+      pp->set_y(ty);
+//      if (abs(tx)<1){
+//      std::cout<<" -------------------------------------------------------"<<std::endl;
+//      std::cout<<"   x     y       v    "<<tx<<"    "<<ty<<"         "<<tv<<std::endl;
+//      }
 //      dd.position.x = tx;
 //      dd.position().y() = ty;
 

src/detection/detection_ultrasonic/main.cpp

@@ -61,7 +61,7 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         p->CopyFrom(area);
     }else{
         int tmp[2];
-        tmp[0] = xmsg.sigobjdist_flmiddle();
+        tmp[0] = 50000;//xmsg.sigobjdist_flmiddle();
         tmp[1] = xmsg.sigobjdist_frmiddle();
         int min_dist = min(tmp,2);
         iv::ultrasonic::Area area;
@@ -118,7 +118,7 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         tmp[0] = xmsg.sigobjdist_rlcorner();
         tmp[1] = xmsg.sigobjdist_rlside();
         tmp[2] = xmsg.sigobjdist_flside();
-        tmp[3] = xmsg.sigobjdist_flcorner();
+        tmp[3] = 50000;//xmsg.sigobjdist_flcorner();
         int min_dist = min(tmp,4);
         iv::ultrasonic::Area area;
         area.set_id(4);

src/driver/driver_camera_ioctl/main.cpp

@@ -304,8 +304,8 @@ int main(int argc, char *argv[])
     gsharpness_ = xp.GetParam("sharpness",  -1); //0-255, -1 "leave alone"
     // possible values: mmap, read, userptr
     gio_method_name_ = xp.GetParam("io_method",  std::string("mmap"));
-    gimage_width_ = xp.GetParam("image_width", 1920);
-    gimage_height_ = xp.GetParam("image_height",  1080);
+    gimage_width_ = xp.GetParam("image_width", 1280);
+    gimage_height_ = xp.GetParam("image_height",  720);
     gframerate_ =  xp.GetParam("framerate",  30);
     // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
     gpixel_format_name_ = xp.GetParam("pixel_format",  std::string("mjpeg"));

src/driver/driver_camera_ioctl/usb_cam.cpp

@@ -951,6 +951,7 @@ void UsbCam::init_device(int image_width, int image_height, int framerate)
   image_width = fmt.fmt.pix.width;
   image_height = fmt.fmt.pix.height;
 
+
   struct v4l2_streamparm stream_params;
   memset(&stream_params, 0, sizeof(stream_params));
   stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

src/driver/driver_radar_continental_ARS408_SRR308/main.cpp

@@ -166,26 +166,31 @@ int main(int argc, char *argv[])
     shmCANSend.mnBufferSize = 100000;
     shmCANSend.mnBufferCount = 3;
     shmCANSend.mpa = iv::modulecomm::RegisterSend(shmCANSend.mstrmsgname,shmCANSend.mnBufferSize,shmCANSend.mnBufferCount);
+    std::cout<<"shm CANSend name:"<<shmCANSend.mstrmsgname<<std::endl;
 
     strncpy(shmRadar.mstrmsgname,setupConfig.strMemRadar.data(),255);
     shmRadar.mnBufferSize = 100000;
     shmRadar.mnBufferCount = 1;
     shmRadar.mpa = iv::modulecomm::RegisterSend(shmRadar.mstrmsgname,shmRadar.mnBufferSize,shmRadar.mnBufferCount);
+    std::cout<<"shm Radar name:"<<shmRadar.mstrmsgname<<std::endl;
 
     strncpy(shmCANRecv.mstrmsgname,setupConfig.strMemCANRecv.data(),255);
     shmCANRecv.mnBufferSize = 100000;
     shmCANRecv.mnBufferCount = 3;
     shmCANRecv.mpa = iv::modulecomm::RegisterRecv(shmCANRecv.mstrmsgname,ListenCANMsg);
+    std::cout<<"shm CANRecv name:"<<shmCANRecv.mstrmsgname<<std::endl;
 
     strncpy(shmGPSIMU.mstrmsgname,setupConfig.strMemGPSIMU.data(),255);
     shmGPSIMU.mnBufferSize = 100000;
     shmGPSIMU.mnBufferCount = 3;
     shmGPSIMU.mpa = iv::modulecomm::RegisterRecv(shmGPSIMU.mstrmsgname,ListenGPSIMUMsg);
+    std::cout<<"shm GPSIMU name:"<<shmGPSIMU.mstrmsgname<<std::endl;
 
     strncpy(shmChassis.mstrmsgname,setupConfig.strMemChassis.data(),255);
     shmChassis.mnBufferSize = 100000;
     shmChassis.mnBufferCount = 3;
     shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisMsg);
+    std::cout<<"shm Chassis name:"<<shmChassis.mstrmsgname<<std::endl;
 
     can_send_producer = new CANSend_Producer(&CANSend);
     can_send_producer->start();

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.cpp

@@ -113,7 +113,7 @@ void CTracker::UpdateTrackingState(
             if (assignment[i] != -1)
             {
 #ifdef DEBUG_SHOW
-                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
+//                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
 #endif
                 if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
                 {
@@ -124,14 +124,14 @@ void CTracker::UpdateTrackingState(
             else
             {
 #ifdef DEBUG_SHOW
-                std::cout<<-1<<", ";
+//                std::cout<<-1<<", ";
 #endif
                 // If track have no assigned detect, then increment skipped frames counter.
                 m_tracks[i]->SkippedFrames()++;
             }
         }
 #ifdef DEBUG_SHOW
-                std::cout<<std::endl;
+//                std::cout<<std::endl;
 #endif
         // If track didn't get detects long time, remove it.
         for (size_t i = 0; i < m_tracks.size();)

src/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.hpp

@@ -22,7 +22,7 @@ bool InitTracker(CTracker& tracker)
     settings.m_accelNoiseMag = 0.5f; // 卡尔曼的噪声放大器
     settings.m_distThres = 30.f; // 匹配算法中的距离阈值
     settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f; // 目标的最小面积半径（像素）
-    settings.m_maximumAllowedSkippedFrames = 3; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
+    settings.m_maximumAllowedSkippedFrames = 2; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
     settings.m_maxTraceLength = 5; // 最大跟踪长度，即历史轨迹保留的最大长度
 
     tracker.setSettings(settings);
@@ -37,7 +37,7 @@ bool InitTracker(CTracker& tracker)
 iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
 {
 #ifdef DEBUG_SHOW
-    std::cout<<"-------------------------------------------------"<<std::endl;
+//    std::cout<<"-------------------------------------------------"<<std::endl;
 #endif
     iv::fusion::fusionobjectarray trackedobjvec;
     trackedobjvec.clear_obj();
@@ -66,7 +66,7 @@ iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjv
     tracker.Update(regions, cv::UMat(), 30);
     auto tracks = tracker.GetTracks();
 #ifdef DEBUG_SHOW
-    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
+//    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
 #endif
     for (size_t i = 0; i < tracks.size(); i++)
     {
@@ -165,7 +165,7 @@ iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjv
 #endif
     }
 #ifdef DEBUG_SHOW
-    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
+//    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
 #endif
 //    for (size_t i = 0; i < trackedobjvec.obj_size(); i++)
 //    {

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -129,9 +129,8 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
-#ifdef DEBUG_SHOW
-            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
-#endif
+//            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
             fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
@@ -213,7 +212,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 //    std::cout<<"   fusion   end    "<<std::endl;
 }
 
-
 void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
 {
     int time_step = 0.3;
@@ -243,7 +241,7 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         centroid_ = fusion_obj.mutable_centroid();
         centroid_->CopyFrom(centroid);
 #ifdef DEBUG_SHOW
-        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+//        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
 #endif
 
         iv::fusion::Dimension dimension;
@@ -259,13 +257,51 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 
 }
 
+void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::radar::radarobjectarray& radar_info)
+{
+    for(int j = 0; j< radar_info.obj_size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_sensor_type(1);
+        fusion_obj.set_yaw(0);
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(radar_info.obj(j).vx());
+        vel_relative.set_y(radar_info.obj(j).vy());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(radar_info.obj(j).x());
+        centroid.set_y(radar_info.obj(j).y());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+//        std::cout<<"radar_back: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(0.5);
+        dimension.set_y(0.5);
+        dimension.set_z(0.5);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+}
+}
+
 void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
+//    int cont = 0;
     for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
     {
 //        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
 //        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
-        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)
+                || lidar_radar_fusion_object_array.obj(i).centroid().y() <-1.0) continue;
 
         if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
                 (lidar_radar_fusion_object_array.obj(i).dimensions().y()>0))
@@ -291,8 +327,10 @@ void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
                     nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                     nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                     if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
-                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <1.0 &&
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) >-1.0) continue;
                     else{
+
                         nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                         nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                         iv::fusion::fusionobject &fusion_obj = (iv::fusion::fusionobject &)lidar_radar_fusion_object_array.obj(i);
@@ -303,8 +341,9 @@ void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
             }
 
     }
-}}
-
+}
+//    std::cout<<"   count    "<<cont<<std::endl;
+}
 
 }
 }

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -25,7 +25,8 @@ typedef  iv::radar::radarobjectarray RadarDataType;
 typedef  iv::lidar::objectarray LidarDataType;
 typedef  std::chrono::system_clock::time_point TimeType;
 
-iv::radar::radarobjectarray radarobjvec;
+iv::radar::radarobjectarray radar_front;
+iv::radar::radarobjectarray radar_back;
 iv::lidar::objectarray lidar_obj;
 iv::mobileye::mobileye mobileye_info;
 iv::mobileye::obs obs_info;
@@ -37,7 +38,7 @@ 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);
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radar_front, iv::fusion::fusionobjectarray& li_ra_fusion);
 TrackerSettings settings;
 CTracker tracker(settings);
 bool m_isTrackerInitialized = false;
@@ -56,7 +57,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
     else{
         //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
     }
-    //        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
+//            std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
     gMutex.lock();
     //    for(int i=0;i<radarobj.obj_size();i++)
     //    {
@@ -71,7 +72,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
 
     //    }
 
-    radarobjvec.CopyFrom(radarobj);
+    radar_front.CopyFrom(radarobj);
     gMutex.unlock();
 }
 
@@ -87,8 +88,32 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
     }
     //    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);
+    //    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
+    datafusion(lidarobj,radar_front,li_ra_fusion);
+    gMutex.unlock();
+}
+
+void Listenradarback(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();
+//    std::cout<<"     radar  obj "<<radarobj.obj_size()<<std::endl;
+        for (int m_index = 0; m_index< radarobj.obj_size(); m_index++)
+        {
+            if(abs(radarobj.obj(m_index).x()) < 1) {
+
+            std::cout<<" p_x  p_y  v_x  v_y   "<<radarobj.obj(m_index).x()<<"    "<<radarobj.obj(m_index).y()<<std::endl;}
+        }
+
+    radar_back.CopyFrom(radarobj);
     gMutex.unlock();
 }
 
@@ -123,6 +148,12 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     //    Transformation::RadarPross(radarobjvec);
 
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+//    std::cout<<"   obj      size   "<<li_ra_fusion.obj_size()<<std::endl;
+//    for (int k = 0; k<li_ra_fusion.obj_size();k++){
+//        if(abs(li_ra_fusion.obj(k).centroid().x())<1){
+//        std::cout<<" size   x    y    vx   vy :   "<<li_ra_fusion.obj(k).centroid().x()<< "   "<<li_ra_fusion.obj(k).centroid().y()<<"    "
+//               <<li_ra_fusion.obj(k).vel_relative().x()<<"   "<<li_ra_fusion.obj(k).vel_relative().y() <<std::endl;
+//    }
 
 
     //    li_ra_fusion.Clear();
@@ -130,34 +161,8 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 //    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;
+      AddRadarBack(li_ra_fusion,radar_back);
+      radar_back.clear_obj();
     //---------------------------------------------  init tracker  -------------------------------------------------
     if (!m_isTrackerInitialized)
     {
@@ -172,7 +177,7 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     //--------------------------------------------  end tracking  --------------------------------------------------
     //    gMutex.unlock();
-    iv::fusion::fusionobjectarray out_fusion = li_ra_fusion;
+    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
     ObsToNormal(out_fusion);
     string out;
 
@@ -196,14 +201,19 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
     }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;
-        //        }
+
     }
+
+        for (int k = 0; k<out_fusion.obj_size();k++){
+            if(abs(out_fusion.obj(k).centroid().x())<1 && out_fusion.obj(k).centroid().y()>5 ){
+            std::cout<<" size   x    y    vx   vy :   "<<out_fusion.obj(k).centroid().x()<< "   "<<out_fusion.obj(k).centroid().y()<<"    "
+                   <<out_fusion.obj(k).velocity_linear_x()<<std::endl;
+        }
+        }
+
     iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
     //    gMutex.unlock();
+
 }
 
 
@@ -213,7 +223,8 @@ int main(int argc, char *argv[])
     tracker.setSettings(settings);
 
     void *gpa;
-    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("radar_front",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("radar_back",Listenradarback);
     gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
     return a.exec();