junsheng xianchang xiugai tijiao 20220627

src/decition/common/common/car_status.h

@@ -166,6 +166,7 @@ namespace iv {
         double mfEPSOff = 0.0;
         float socfDis=8;   //停障触发距离
         float aocfDis=35;   //避障触发距离//20
+        //float aocfDis=20;
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90
@@ -257,6 +258,8 @@ namespace iv {
        bool mbFullFillTask = 2;  //到达终点1  未达终点0
        bool mbArriveInitPos=0;//到达初始位置1,未到达是0
        bool mbArriveServicePos=0;//到达维护位置1,未到达是0
+
+       QTime mUltrasonicUpdateTimer;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -1884,6 +1884,7 @@ void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndata
         }
     }
 
+    ServiceCarStatus.mUltrasonicUpdateTimer.restart();
 
 }
 void iv::decition::BrainDecition::UpdateFSMSkipCmd(const char *pdata, const int ndatasize)

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -53,6 +53,7 @@ int iv::decition::DecideGps00::gpsMapParkIndex = 0;
 double iv::decition::DecideGps00::lastDistance = MaxValue;
 double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
 double iv::decition::DecideGps00::obsDistance = -1;
+int iv::decition::DecideGps00::obsType = 1;
 double iv::decition::DecideGps00::obsSpeed=0;
 double iv::decition::DecideGps00::obsDistanceAvoid = -1;
 double iv::decition::DecideGps00::lastDistanceAvoid = -1;
@@ -1746,11 +1747,16 @@ static int file_num;
             //            useOldAvoid = true;
         }
        // else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
-         else if (useOldAvoid && avoidDistance>20) {
+         else if (useOldAvoid && avoidDistance>20 && obsType == 1) {
         // vehState = avoidObs; 0929
             vehState = normalRun;
             turnLampFlag=0;
         }
+        else if (useOldAvoid && avoidDistance>15 && obsType == 2) {
+       // vehState = avoidObs; 0929
+           vehState = normalRun;
+           turnLampFlag=0;
+       }
         else if (turnLampFlag>0)
         {
             gps_decition->leftlamp = false;
@@ -1781,7 +1787,12 @@ static int file_num;
             //            useOldAvoid = true;
         }
        // else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
-        else if (useOldAvoid && backDistance>20) {//abs(gpsTraceNow[60].x)<0.05
+        else if (useOldAvoid && backDistance>20 && obsType == 1) {//abs(gpsTraceNow[60].x)<0.05
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+            obstacle_avoid_flag=0;
+        }
+        else if (useOldAvoid && backDistance>15 && obsType == 2) {//abs(gpsTraceNow[60].x)<0.05
             // vehState = avoidObs; 0929
             vehState = normalRun;
             obstacle_avoid_flag=0;
@@ -1903,8 +1914,9 @@ static int file_num;
     if(PathPoint+400<gpsMapLine.size()){
         int road_permit_sum=0;
         for(int k=PathPoint;k<PathPoint+400;k++){
-                if((gpsMapLine[k]->mbnoavoid==false)&&(gpsMapLine[k]->roadSum>1))
-                                 road_permit_sum++;
+                //if((gpsMapLine[k]->mbnoavoid==false)&&(gpsMapLine[k]->roadSum>1))
+            if(gpsMapLine[k]->mbnoavoid==false)
+                road_permit_sum++;
         }
         if(road_permit_sum>=400)
             road_permit_avoid=true;
@@ -2389,13 +2401,19 @@ static int file_num;
     }
 
     //超声波逻辑,begin
+    if(ServiceCarStatus.mUltrasonicUpdateTimer.elapsed() > 10000)
+    {
+        ServiceCarStatus.mbUltraDis[0]=ServiceCarStatus.mbUltraDis[1]=ServiceCarStatus.mbUltraDis[2]=ServiceCarStatus.mbUltraDis[3]=200;
+        ServiceCarStatus.mUltrasonicUpdateTimer.restart();
+    }
+
    if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
    {
       dSpeed=min(dSpeed,3.0);
    }
    else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
    {
-       minDecelerate=min(-1.0f,minDecelerate);
+       minDecelerate=min(-1.1f,minDecelerate);
    }
 
 /*   if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
@@ -2409,6 +2427,7 @@ static int file_num;
    {
       // minDecelerate=min(-0.5f,minDecelerate);
        dSpeed=min(dSpeed,5.0);
+       std::cout<<"   ====================================12================================"<<std::endl;
    }
 
    if(ServiceCarStatus.daocheMode)
@@ -2787,12 +2806,12 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     traceOriLeft.clear();
     traceOriRight.clear();
 
-    if (gpsMapLine.size() > 200 && PathPoint >= 0) {
+    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
         int aimIndex;
         if(circleMode){
-            aimIndex=PathPoint+200;
+            aimIndex=PathPoint+800;
         }else{
-            aimIndex=min((PathPoint+200),(int)gpsMapLine.size());
+            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
         }
 
         for (int i = PathPoint; i < aimIndex; i++)
@@ -3735,6 +3754,7 @@ void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr
 
     //   qDebug("time 3 is %d ",xTime.elapsed());
 
+    obsType = obsPoint.obs_type;
 }
 
 

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.h

@@ -64,6 +64,7 @@ public:
     static double lastVt;
     static double lastU;
     static double obsDistance;
+    static int obsType;
     static double obsSpeed;
     static double lidarDistanceAvoid;
     static double obsDistanceAvoid;

src/detection/detection_ground_filter/ground_plane_fitting_segmenter.hpp

@@ -27,8 +27,8 @@ bool sortByAxisZAsc(PointType p1, PointType p2) {
 
 struct SegmenterParams {
     // Ground Plane Fitting
-    int gpf_sensor_model = 64;
-    double gpf_sensor_height = 1.73;
+    int gpf_sensor_model = 32;
+    double gpf_sensor_height = 1.60;
     // fitting multiple planes
     int gpf_num_segment;
     // number of iterations

src/detection/detection_ground_filter/main.cpp

@@ -2,7 +2,7 @@
 #include "modulecomm.h"
 #include "base_segmenter.hpp"
 #include "ground_plane_fitting_segmenter.hpp"
-void * gpoint_out= iv::modulecomm::RegisterSend("point_no_ground",20000000,1);
+void * gpoint_out= iv::modulecomm::RegisterSend("lidar_pc",20000000,1);
 std::unique_ptr<lidar::segmenter::BaseSegmenter> ground_remover_;
 std::unique_ptr<lidar::segmenter::BaseSegmenter> segmenter_;
 
@@ -48,8 +48,8 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
     for(i=0;i<nPCount;i++)
     {
         pcl::PointXYZI xp;
-        xp.x = p->y;
-        xp.y = p->x;
+        xp.x = p->x;
+        xp.y = p->y;
         xp.z = p->z;
         xp.intensity = p->intensity;
         point_cloud->push_back(xp);
@@ -67,23 +67,13 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 }
 
 
-
-
-
-
-
-
-
-
-
-
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
     void * gpoint;
     lidar::segmenter::SegmenterParams params;
     // Ground Plane Fitting
-    params.gpf_sensor_model = 64;
+    params.gpf_sensor_model = 32;
     params.gpf_sensor_height = 0.90;
     // fitting multiple planes
     params.gpf_num_segment = 1;
@@ -91,7 +81,7 @@ int main(int argc, char *argv[])
     params.gpf_num_iter = 10;
     // number of points used to estimate the lowest point representative(LPR)
     // double of senser model???
-    params.gpf_num_lpr =128 ;
+    params.gpf_num_lpr =32 ;
     params.gpf_th_lprs = 0.08;
     // threshold for points to be considered initial seeds
     params.gpf_th_seeds = 0.1;
@@ -100,7 +90,7 @@ int main(int argc, char *argv[])
     params.gpf_th_gnds = 0.3;
     ground_remover_ = std::unique_ptr<lidar::segmenter::BaseSegmenter>(
         new lidar::segmenter::GroundPlaneFittingSegmenter(params));
-    gpoint = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+    gpoint = iv::modulecomm::RegisterRecv("lidar_pc_in",ListenPointCloud);
 
     return a.exec();
 }

src/detection/detection_lidar_PointPillars_MultiHead/detection_lidar_PointPillars_MultiHead.pro

@@ -161,24 +161,26 @@ unix:INCLUDEPATH += /usr/include/pcl-1.8
 
 LIBS += -lboost_system
 
-unix:LIBS +=  -lpcl_common\
-        -lpcl_features\
-        -lpcl_filters\
-        -lpcl_io\
-        -lpcl_io_ply\
-        -lpcl_kdtree\
-        -lpcl_keypoints\
-        -lpcl_octree\
-        -lpcl_outofcore\
-        -lpcl_people\
-        -lpcl_recognition\
-        -lpcl_registration\
-        -lpcl_sample_consensus\
-        -lpcl_search\
-        -lpcl_segmentation\
-        -lpcl_surface\
-        -lpcl_tracking\
-        -lpcl_visualization
+#unix:LIBS +=  -lpcl_common\
+#        -lpcl_features\
+#        -lpcl_filters\
+#        -lpcl_io\
+#        -lpcl_io_ply\
+#        -lpcl_kdtree\
+#        -lpcl_keypoints\
+#        -lpcl_octree\
+#        -lpcl_outofcore\
+#        -lpcl_people\
+#        -lpcl_recognition\
+#        -lpcl_registration\
+#        -lpcl_sample_consensus\
+#        -lpcl_search\
+#        -lpcl_segmentation\
+#        -lpcl_surface\
+#        -lpcl_tracking\
+#        -lpcl_visualization\
+unix:LIBS += /usr/lib/aarch64-linux-gnu/libpcl*.so
+
 
 INCLUDEPATH += /usr/include/opencv4/
 LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -33,8 +33,6 @@ const int kNumPointFeature = 5;
 const int kOutputNumBoxFeature = 7;
 std::string gstrinput;
 std::string gstroutput;
-Eigen::Matrix3d rotation_matrix;
-Eigen::Vector3d trans_matrix;
 TrackerSettings settings;
 CTracker tracker(settings);
 bool m_isTrackerInitialized = false;
@@ -306,40 +304,17 @@ void PclToArray(
 }
 
 void PclXYZITToArray(
-        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
-        float* out_points_array, const float normalizing_factor) {
-
-    /////shuffle the index array/////
-    bool shuffle = true;
-    int point_num = in_pcl_pc_ptr->size();
-    std::vector<int>indices(point_num);
-    std::iota(indices.begin(),indices.end(),0);
-    if(shuffle)
-    {
-//        unsigned seed = 0;
-//        std::shuffle(indices.begin(),indices.end(),std::default_random_engine(seed));
-
-        std::random_device rd;
-        std::mt19937 g(rd());
-        std::shuffle(indices.begin(),indices.end(),g);
-
-    }
-    /////shuffle the index array/////
-
-    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
-        //pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
-        int indice = indices[i];
-        pcl::PointXYZI point = in_pcl_pc_ptr->at(indice);
-        Eigen::Vector3d new_point, old_point;
-        old_point<<point.x, point.y, point.z;
-        new_point = rotation_matrix * (old_point) + trans_matrix;
-        out_points_array[i * 5 + 0] = new_point[0];
-        out_points_array[i * 5 + 1] = new_point[1];
-        out_points_array[i * 5 + 2] = new_point[2];
-        out_points_array[i * 5 + 3] =
-                static_cast<float>(point.intensity / normalizing_factor);
-        out_points_array[i * 5 + 4] = 0;
-    }
+    const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
+    float* out_points_array, const float normalizing_factor) {
+  for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
+    pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
+    out_points_array[i * 5 + 0] = point.x;
+    out_points_array[i * 5 + 1] = point.y;
+    out_points_array[i * 5 + 2] = point.z;
+    out_points_array[i * 5 + 3] =
+        static_cast<float>(point.intensity / normalizing_factor);
+    out_points_array[i * 5 + 4] = 0;
+  }
 }
 
 void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
@@ -720,7 +695,7 @@ int main(int argc, char *argv[])
     iv::xmlparam::Xmlparam xparam(strpath.toStdString());
     pfe_file = xparam.GetParam("pfe_file",pfe_file.data());
     backbone_file = xparam.GetParam("backbone_file",backbone_file.data());
-    gstrinput = xparam.GetParam("input","lidar_pc");
+    gstrinput = xparam.GetParam("input","lidar_pc_in");
     gstroutput = xparam.GetParam("output","lidar_pointpillar");
 
     if(btrtexist == false)
@@ -749,13 +724,6 @@ int main(int argc, char *argv[])
                     ));
     }
     std::cout<<"PointPillars Init OK."<<std::endl;
-    Eigen::AngleAxisd r_z ( 0.0356, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
-    Eigen::AngleAxisd r_y ( -0.0149, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
-    Eigen::AngleAxisd r_x ( -0.0168, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
-    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
-    // 四元数-->>旋转矩阵
-    rotation_matrix = q_zyx.toRotationMatrix();
-    trans_matrix << 0, 1.13, 0.28;
     gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
     gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
     gpthread = new std::thread(statethread);

src/detection/detection_lidar_cnn_segmentation/detection_lidar_cnn_segmentation.pro

@@ -36,7 +36,8 @@ SOURCES += main.cpp \
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += /usr/include/opencv4
+INCLUDEPATH += /usr/include/opencv4/
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 
 #INCLUDEPATH += /opt/ros/kinetic/include/opencv-3.3.1-dev
 
@@ -48,10 +49,7 @@ LIBS += -lpcl_io -lpcl_common
 
 LIBS += -lboost_system  -lavutil  -lprotobuf -lcudnn
 
-
-LIBS += -lcudnn
-
-unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe/arm64 -lcaffe -lcudnn
+unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe/arm64 -lcaffe
 
 HEADERS += \
     cluster2d.h \

src/detection/detection_lidar_grid/detection_lidar_grid.pro

@@ -58,11 +58,15 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         perception_lidar_vv7.cpp \
-    perceptionoutput.cpp
+    perceptionoutput.cpp \
+    ../../include/msgtype/fusionobject.pb.cc \
+    ../../include/msgtype/fusionobjectarray.pb.cc
 
 HEADERS += \
         perception_lidar_vv7.h \
-    perceptionoutput.h
+    perceptionoutput.h \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h
 
 unix {
     target.path = /usr/lib

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -9,7 +9,7 @@
 #include <iostream>
 #include <QDateTime>
 #include <QMutex>
-
+#include "Eigen/Dense"
 #include <pcl/conversions.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -19,6 +19,7 @@
 #include "ivbacktrace.h"
 
 #include "perceptionoutput.h"
+#include "fusionobjectarray.pb.h"
 
 #include <iostream>
 #include <fstream>
@@ -48,6 +49,8 @@ static char gstr_skipymin[256];
 static char gstr_skipymax[256];
 
 
+
+
 static int g_robosys = 10;
 
 //#define OUT_GROUND
@@ -118,15 +121,14 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
 
 
 
-static float SENSOR_HEIGHT = 2.1;
-static float VEHICLE_HEIGHT = 2.6;
+static float SENSOR_HEIGHT = 1.60;
+static float VEHICLE_HEIGHT = 1.80;
 //static float local_max_slope_ = 10;
 //static float general_max_slope_ = 3;
 //static float concentric_divider_distance_ = 1.5;
 //static float min_height_threshold_ = 0.05;
 //static float reclass_distance_threshold_ = 10.0;
 
-
 static float GRID_SIZE = 2.0;
 static float SMALLGRID_SIZE = 0.2;
 static float Height_Thesh_Ratio = 0.1;
@@ -137,12 +139,12 @@ static float VEC_MIN = -100.0;
 static float HOR_MIN = -100.0;
 static int g_seq = 0;
 
-static float skip_xmin = -0.9;
-static float skip_ymin = -2.3;
-static float skip_xmax = 0.9;
-static float skip_ymax = 2.3;
+static float skip_xmin = -0.5;
+static float skip_ymin = -1.13;
+static float skip_xmax = 0.5;
+static float skip_ymax = 0.3;
 
-static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs)
+static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::shared_ptr<iv::fusion::fusionobjectarray> lidar_obs)
 {
 
 //    std::cout<<"enter gird obs"<<std::endl;
@@ -359,8 +361,44 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
 
     for(i=0;i<NYC;i++)
     {
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_id(i);
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(0);
+        centroid.set_y(0);
+        centroid.set_z(0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(0);
+        dimension.set_y(0);
+        dimension.set_z(0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(0);
+        vel_relative.set_y(0);
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        fusion_obj.set_velocity_linear_x(-3);
+
+//        std::cout<<":     "<<NYC<<std::endl;
+
         for(j=0;j<NXC;j++)
         {
+            iv::fusion::NomalXYZ nomal_centroid;
+            iv::fusion::NomalXYZ *nomal_centroid_;
+            float low;
+            float high;
+            float nomal_z;
+            float nomal_x;
+            float nomal_y;
             iv::LidarGrid * p = pobsgrid + i*NXC+j;
             if(p->ob>=3)
             {
@@ -370,24 +408,26 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
                 }
                 else
                 {
-//                    temp.nomal_x = (j-125)*0.2+0.1;
-//                    temp.nomal_y = (i-250)*0.2+0.1;
-
-
-                    iv::ObstacleBasic temp;
-                    temp.low = p->low;
-                    temp.high = p->high;
-                    temp.nomal_z = p->high-p->low;
-                    temp.nomal_x = j*SMALLGRID_SIZE + 0.1+HOR_MIN;
-                    temp.nomal_y = i*SMALLGRID_SIZE +0.1 +VEC_MIN;
-                    lidar_obs->push_back(temp);
+                    low = p->low;
+                    high = p->high;
+                    nomal_z = p->high-p->low;
+                    nomal_x = j*SMALLGRID_SIZE + 0.1+HOR_MIN;
+                    nomal_y = i*SMALLGRID_SIZE +0.1 +VEC_MIN;
                 }
             }
+            if (abs(nomal_x)<0.4 && nomal_y<1.0 && nomal_y>-0.3) continue;
+            nomal_centroid.set_nomal_x(nomal_x);
+            nomal_centroid.set_nomal_y(nomal_y);
+            nomal_centroid.set_nomal_z(nomal_z);
+            nomal_centroid_ = fusion_obj.add_nomal_centroid();
+            nomal_centroid_->CopyFrom(nomal_centroid);
         }
+        iv::fusion::fusionobject *obj_ = lidar_obs->add_obj();
+        obj_->CopyFrom(fusion_obj);
+
     }
 
     delete pobsgrid;
-
     delete floor_find;
     delete floor_h_raw;
     delete floor_h;
@@ -439,8 +479,9 @@ static void ListenPointCloud(const char * strdata,const unsigned int nSize,const
         p++;
     }
 
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+//    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
 
+    std::shared_ptr<iv::fusion::fusionobjectarray> lidar_obs(new iv::fusion::fusionobjectarray);
     GridGetObs(point_cloud,lidar_obs);
 //    perception_lidar_vv7_ProcObs(point_cloud,lidar_obs);
 
@@ -448,17 +489,24 @@ static void ListenPointCloud(const char * strdata,const unsigned int nSize,const
 
     if(g_robosys == 0)
     {
-        if(lidar_obs->size() == 0)
+        if(lidar_obs->obj_size() == 0)
         {
-            iv::ObstacleBasic temp;
-            temp.low = 0;
-            temp.high = 3.0;
-            temp.nomal_z = 3.0;
-            temp.nomal_x = 1000;
-            temp.nomal_y = 1000;
-            lidar_obs->push_back(temp);
+            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_ = lidar_obs->add_obj();
+            fake_obj_->CopyFrom(fake_obj);
         }
-        if(lidar_obs->size() >0)iv::modulecomm::ModuleSendMsg(glidar_obs,(char *)(lidar_obs->data()),lidar_obs->size()*sizeof(iv::ObstacleBasic));
+
+        std::string out = lidar_obs->SerializeAsString();
+        if(lidar_obs->obj_size() >0)iv::modulecomm::ModuleSendMsg(glidar_obs,out.data(),out.length());
     }
     else
     {
@@ -645,6 +693,7 @@ int main(int argc, char *argv[])
     gfault = new iv::Ivfault("lidar_grid");
     givlog = new iv::Ivlog("lidar_grid");
 
+
     snprintf(gstr_yaml,255,"");
 
     int nRtn = GetOptLong(argc,argv);
@@ -653,14 +702,14 @@ int main(int argc, char *argv[])
         return 0;
     }
 
-    snprintf(gstr_sensorheight,255,"2.0");
-    snprintf(gstr_vehicleheight,255,"2.3");
+    snprintf(gstr_sensorheight,255,"1.60");
+    snprintf(gstr_vehicleheight,255,"1.80");
     snprintf(gstr_inputmemname,255,"lidar_pc");
-    snprintf(gstr_outputmemname,255,"lidar_obs");
-    snprintf(gstr_skipxmin,255,"-0.9");
-    snprintf(gstr_skipxmax,255,"0.9");
-    snprintf(gstr_skipymin,255,"-2.3");
-    snprintf(gstr_skipymax,255,"2.3");
+    snprintf(gstr_outputmemname,255,"grid_fusion");
+    snprintf(gstr_skipxmin,255,"-0.5");
+    snprintf(gstr_skipxmax,255,"0.5");
+    snprintf(gstr_skipymin,255,"-1.13");
+    snprintf(gstr_skipymax,255,"0.3");
 
 
     givlog->verbose("yaml is %s ",gstr_yaml);

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] = 50000;//xmsg.sigobjdist_flmiddle();
+        tmp[0] = 5000;//xmsg.sigobjdist_flmiddle();
         tmp[1] = xmsg.sigobjdist_frmiddle();
         int min_dist = min(tmp,2);
         iv::ultrasonic::Area area;
@@ -78,9 +78,9 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         p->CopyFrom(area);
     }else{
         int tmp[4];
-        tmp[0] = xmsg.sigobjdist_frcorner();
+        tmp[0] = 5000;//xmsg.sigobjdist_frcorner();
         tmp[1] = xmsg.sigobjdist_frside();
-        tmp[2] = xmsg.sigobjdist_rrside();
+        tmp[2] = 5000;//xmsg.sigobjdist_rrside();
         tmp[3] = xmsg.sigobjdist_rrcorner();
         int min_dist = min(tmp,4);
         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] = 50000;//xmsg.sigobjdist_flcorner();
+        tmp[3] = 5000;//xmsg.sigobjdist_flcorner();
         int min_dist = min(tmp,4);
         iv::ultrasonic::Area area;
         area.set_id(4);

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -189,7 +189,7 @@ void FSMUnit::ListenBrainStateMsg(const char * strdata,const unsigned int nSize,
         return;
     }
 
-    FSMState = xdata.mbfsm_state(); //0:待命停车 1:任务中 2:人工接管 3:停工停车 4:返程 5:返库
+    FSMState = xdata.mbfsm_state(); ///< 0:待命停车 1:任务中 2:人工接管 3:停工停车 4:返程 5:返库
 //    std::cout<<"fsm status: "<<(unsigned int)FSMState<<std::endl;
 
     emit refreshFSMStatus();
@@ -198,9 +198,15 @@ void FSMUnit::ListenBrainStateMsg(const char * strdata,const unsigned int nSize,
 void FSMUnit::navagationSwitchChanged_Slot(bool status)
 {
     if(status == true && FSMState == 0)
+    {
         missionCMD = iv::brain::MissionCMD::MISSION_START;
+        std::cout<<"mission start"<<std::endl;
+    }
     else if(status == false && FSMState == 1)
+    {
         missionCMD = iv::brain::MissionCMD::MISSION_CANCEL;
+        std::cout<<"mission canceled"<<std::endl;
+    }
     else
         missionCMD = iv::brain::MissionCMD::MISSION_RESERVED;
 }
@@ -242,7 +248,10 @@ void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
 void FSMUnit::setMissionFinished_Slot(void)
 {
     if(FSMState == 1)
-            missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
+    {
+        missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
+        std::cout<<"mission finished"<<std::endl;
+    }
     currentDestination.latitude = 0.0;
     currentDestination.longitude = 0.0;
 }
@@ -289,7 +298,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             waitingStationArrived = false;
         }
-        emit useStatusChanged(true);
+        emit useStatusChanged(0);
         emit setAllowPlan(true);
         break;
     case 1: ///< 1任务中
@@ -301,13 +310,15 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(1);
         emit setAllowPlan(false);
         break;
     case 2: ///< 2人工接管
         if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER)
-        {void sendPathPlanRequest(void);
+        {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(2);
         emit setAllowPlan(false);
         break;
     case 3: ///< 3停工停车
@@ -319,7 +330,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             maintainStationArrived = false;
         }
-        emit useStatusChanged(false);
+        emit useStatusChanged(3);
         emit setAllowPlan(false);
         break;
     case 4: ///< 4返程
@@ -327,6 +338,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(4);
         emit setAllowPlan(false);
         break;
     case 5: ///< 5返库
@@ -338,6 +350,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(5);
         emit setAllowPlan(false);
         break;
     default:
@@ -362,6 +375,7 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
 
         if(distance > 5.0)
         {
+            std::cout<<"destination miss distance in FSM mode 4:"<<distance<<std::endl;
             emit sendPathPlanRequest(waitingStation.latitude,waitingStation.longitude);
         }
         GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);

src/driver/driver_cloud_grpc_client_BS/fsm_unit.h

@@ -100,7 +100,7 @@ private:
 
 signals:
     void refreshFSMStatus(void);
-    void useStatusChanged(bool status);
+    void useStatusChanged(unsigned int status);
     void setAllowPlan(bool isAllow);
     void sendPathPlanRequest(double latitude,double longitude);
 

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -211,7 +211,7 @@ int main(int argc, char *argv[])
     QObject::connect(vehicleuploaddata,&DataExchangeClient::setFSMDestination,fsmunit,&FSMUnit::setFSMDestination_Slot);
     QObject::connect(vehicleuploaddata,&DataExchangeClient::setMissionFinished,fsmunit,&FSMUnit::setMissionFinished_Slot);
     QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::remoteCtrlModeChanged,fsmunit,&FSMUnit::remoteCtrlModeChanged_Slot);
-    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::navagationSwitchChanged,fsmunit,&FSMUnit::navagationSwitchChanged_Slot);
+    QObject::connect(vehicleuploadmap,&VehicleUploadMapClient::navagationSwitchChanged,fsmunit,&FSMUnit::navagationSwitchChanged_Slot);
     QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::useStatusCMDChanged,fsmunit,&FSMUnit::useStatusCMDChanged_Slot);
 
     return a.exec();

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -274,9 +274,12 @@ std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
         {
             modeChangeCMD = reply.modecmd();
             useStatusCMD = reply.usestatuscmd();//使用状态修改命令
+//            std::cout<<"use status cmd is "<<useStatusCMD<<std::endl;
+
             deactivatePosition = reply.deactivateposition(); //停用站点 等号重载了copyFrom
             speedCMD = reply.speedcmd(); //平台设定的期望运行速度
             navagationSwitch = reply.navagationswitch(); //开始-停止导航
+//            std::cout<<"navagationswitch cmd is "<<navagationSwitch<<std::endl;
         }
         return "changeCtrlMode RPC successed";
     } else {
@@ -299,10 +302,11 @@ void VehicleChangeCtrlModeClient::updateCtrolMode(void)
     else
         emit remoteCtrlModeChanged(true);
 
-    if(navagationSwitch == org::jeecg::defsControl::grpc::NavSwitch::NAV_START)
-        emit navagationSwitchChanged(true);
-    else
-        emit navagationSwitchChanged(false);
+//    if(navagationSwitch == org::jeecg::defsControl::grpc::NavSwitch::NAV_START)
+//        emit navagationSwitchChanged(true);
+//    else
+//        emit navagationSwitchChanged(false);
+///< postcar have not nav switch
 
     if(useStatusCMD == org::jeecg::defsControl::grpc::UseStatus::ENABLING)
         emit useStatusCMDChanged(true);
@@ -429,9 +433,12 @@ void VehicleUploadMapClient::run()
             {
                 std::string reply = uploadMap();
 //                std::cout<< reply <<std::endl;
-                if(isNeedMap == true && allowPlan == true)
+                if(isNeedMap == true)
                 {
-                    updateMapPOIData();
+                    if(allowPlan == true)
+                        updateMapPOIData();
+                    else
+                        isNeedMap = false;
                 }
             }
             lastTime = xTime.elapsed();
@@ -448,6 +455,7 @@ void VehicleUploadMapClient::uploadPath_Finished_Slot(std::string pathID)
     std::cout<<"Path ID:"<<pathID<<" upload finished"<<std::endl;
     if(isNeedMap == true)
     {
+        emit navagationSwitchChanged(true);
         isNeedMap = false;
     }
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_control.h

@@ -104,7 +104,7 @@ private:
 signals:
     void ctrlMode_Changed(org::jeecg::defsControl::grpc::CtrlMode ctrlMode);
     void remoteCtrlModeChanged(bool status);
-    void navagationSwitchChanged(bool status);
+//    void navagationSwitchChanged(bool status);
     void useStatusCMDChanged(bool status);
 };
 
@@ -133,6 +133,7 @@ private:
 
 signals:
     void patrolPOI_Recieved(std::string pathID);
+    void navagationSwitchChanged(bool status);
 
 public slots:
     void uploadPath_Finished_Slot(std::string pathID);

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -751,7 +751,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     }
 
     remainPathLength -= (speed * timeInterval_ms/1000.0);
-    if(remainPathLength > 0.0)std::cout<<"remainLength: "<<remainPathLength<<std::endl;
+//    if(remainPathLength > 0.0)std::cout<<"remainLength: "<<remainPathLength<<std::endl;
     if(remainPathLength <= 0.0)
         remainPathLength = 0.0;
 
@@ -766,9 +766,9 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
-        if(distance < 20.0)
-            std::cout<<"distance: "<<distance<<std::endl;
-        if(distance < 1.5)
+//        if(distance < 20.0)
+//            std::cout<<"distance: "<<distance<<std::endl;
+        if(distance < 2.5)
         {
             isArrived = 2;
             remainPathLength = 0.0;
@@ -832,7 +832,7 @@ void DataExchangeClient::run()
             realLastTime = xTime.elapsed();
             updateData(realInterval);
             std::string reply = uploadVehicleInfo();
-//            std::cout<< reply <<std::endl;
+            std::cout<< reply <<std::endl;
 //            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
             lastTime = xTime.elapsed();
         }
@@ -904,7 +904,31 @@ void DataExchangeClient::destination_Recieved_Slot(void)
     if(fabs(destinationPosition.latitude()) > 0.000001 \
             && fabs(destinationPosition.longitude()) > 0.000001)
     {
-
+        double localPositionX = 0.0;
+        double localPositionNextX = 0.0;
+        double localPositionY = 0.0;
+        double localPositionNextY = 0.0;
+        GaussProjCal(destinationPosition.longitude(),destinationPosition.latitude(),&localPositionX,&localPositionY);
+        GaussProjCal(destinationSaved.longitude(),destinationSaved.latitude(),&localPositionNextX,&localPositionNextY);
+        double deltaX = localPositionNextX - localPositionX;
+        double deltaY = localPositionNextY - localPositionY;
+        double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
+        if(distance > 5.0 && FSMStatus == 1)
+        {
+            std::cout<<std::setprecision(12)<<"destination change into"<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
+//            isArrived = 0;
+//            remainPathLength = 0.0;
+//            destinationSaved.Clear();
+
+//            xodrobj xodrDest;
+//            xodrDest.flon = destinationPosition.longitude();
+//            xodrDest.flat = destinationPosition.latitude();
+//            xodrDest.lane = 1;
+
+//            iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
+//            if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
+//            std::cout<<"patrol path re-calculating"<<std::endl;
+        }
     }
 }
 
@@ -932,10 +956,12 @@ void DataExchangeClient::destination_Refreshed_Timeout(void)
     std::cout<<"path plan timeout."<<std::endl;
 }
 
-void DataExchangeClient::useStatusChanged_Slot(bool status)
+void DataExchangeClient::useStatusChanged_Slot(unsigned int status)
 {
-    if(status)
-        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
-    else
+    if(status == 3)
         useStatus = org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING;
+    else
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+
+    FSMStatus = status; ///< 0:待命停车 1:任务中 2:人工接管 3:停工停车 4:返程 5:返库
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -165,6 +165,7 @@ private:
     org::jeecg::defsDetails::grpc::CtrlMode modeFeedback = org::jeecg::defsDetails::grpc::CtrlMode::CMD_AUTO; //mode Feedback
 
     org::jeecg::defsDetails::grpc::UseStatus useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+    unsigned int FSMStatus = 0;
     double remainPathLength = 0.0;
     org::jeecg::defsDetails::grpc::VehicleClass vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
 
@@ -187,7 +188,7 @@ signals:
 
 public slots:
     void patrolPOI_Recieved_Slot(std::string pathID);
-    void useStatusChanged_Slot(bool status);
+    void useStatusChanged_Slot(unsigned int status);
 
 private slots:
     void destination_Recieved_Slot(void);

src/driver/driver_lidar_rs32/lidar_driver_rs32.cpp

@@ -11,6 +11,7 @@
 //#include <pcl/conversions.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <Eigen/Dense>
 
 #include "modulecomm.h"
 
@@ -23,13 +24,15 @@
 int vv7;
 #endif
 
+Eigen::Matrix3d rotation_matrix;
+Eigen::Vector3d trans_matrix;
+
 
 
 #define Lidar_Pi 3.1415926535897932384626433832795
 #define Lidar32 (unsigned long)3405883584//192.168.1.203
 #define Lidar_roll_ang (90)*Lidar_Pi/180.0
 
-
 std::thread * g_prs32Thread;
 std::thread * g_prs32ProcThread;
 
@@ -97,8 +100,6 @@ public:
     }
 };
 
-
-
 rs32_Buf  * g_rs32_Buf;
 char * g_RawData_Buf;
 int gnRawPos = 0;
@@ -237,8 +238,13 @@ void process_rs32obs(char * strdata,int nLen)
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());
 
-
-
+    Eigen::AngleAxisd r_z ( 0.0356, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+    Eigen::AngleAxisd r_y ( -0.0149, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+    Eigen::AngleAxisd r_x ( -0.0168, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
+    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
+    // 四元数-->>旋转矩阵
+    rotation_matrix = q_zyx.toRotationMatrix();
+    trans_matrix << 0, 0, 0;
 
 //    point_cloud->header.stamp =
 //            pcl_conversions::toPCL(sweep_data->header).stamp;
@@ -330,11 +336,13 @@ void process_rs32obs(char * strdata,int nLen)
                             point.x  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*cos(Ang + frollang);
                             point.y  = Range*cos(V_theta[pointi] / 180 * Lidar_Pi)*sin(Ang + frollang);
                             point.z  = Range*sin(V_theta[pointi] / 180 * Lidar_Pi);
+                            if(point.z>1.5) continue;
                             if(binclix)
                             {
                                 double y,z;
                                 y = point.y;
                                 z = point.z;
+
                                 point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
                                 point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
                             }
@@ -346,20 +354,20 @@ void process_rs32obs(char * strdata,int nLen)
                                 point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
                                 point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
                             }
+                            Eigen::Vector3d new_point, old_point;
+                            old_point<<point.x, point.y, point.z;
+                            new_point = rotation_matrix * (old_point) + trans_matrix;
+                            point.x = new_point[0];
+                            point.y = new_point[1];
+                            point.z = new_point[2];
+
                             point.intensity = intensity;
                             point_cloud->points.push_back(point);
 
-
                             ++point_cloud->width;
                         }
                     }
 
-
-
-
-
-
-
                 }
             }
 

src/driver/driver_lidar_rs32/main.cpp

@@ -175,7 +175,7 @@ int main(int argc, char *argv[])
     showversion("driver_lidar_rs32");
     QCoreApplication a(argc, argv);
 
-    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_memname,255,"lidar_pc_in");
     snprintf(gstr_rollang,255,"-9.0");
     snprintf(gstr_inclinationang_xaxis,255,"0.0");
     snprintf(gstr_inclinationang_yaxis,255,"0");

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -91,7 +91,7 @@ void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::rad
 void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
 //    std::cout<< "    get   fusion  begin   "<<std::endl;
-    lidar_radar_fusion_object_array.Clear();
+//    lidar_radar_fusion_object_array.Clear();
     std::vector<match_index> match_idx;
     std::vector<int> radar_idx;
     Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
@@ -100,20 +100,25 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         iv::fusion::fusionobject fusion_object;
         fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
         fusion_object.set_sensor_type(0);
-        fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
+        float ob_size = lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() * lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y();
+        if(ob_size <=1){
+            fusion_object.set_type(2);
+        } else {
+            fusion_object.set_type(1);
+        }
         fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
         fusion_object.set_velocity_linear_x(-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x());
 
         if(match_idx.at(i).nradar == -1000)
         {
 //            std::cout<<"   fusion    is    ok  "<<std::endl;
-            if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
-                   lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
-            {
-                fusion_object.set_yaw(1.57);
-            } else {
+//            if( lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() *
+//                   lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y() > 18)
+//            {
+//                fusion_object.set_yaw(1.57);
+//            } else {
                 fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-            }
+//            }
 
             iv::fusion::VelXY vel_relative;
             iv::fusion::VelXY *vel_relative_;
@@ -212,51 +217,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;
-//    lidar_radar_fusion_object_array.Clear();
-    for(int j = 0; j< xobs_info.xobj_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_;
-        if(xobs_info.xobj(j).obstype() == iv::mobileye::obs_OBSTYPE::obs_OBSTYPE_PED)
-        {
-            fusion_obj.set_type(2);
-        }else{
-            fusion_obj.set_type(1);
-            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::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);
-#ifdef DEBUG_SHOW
-//        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
-#endif
-
-        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);
-        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
-        obj->CopyFrom(fusion_obj);
-    }
-
-}
-
 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++){

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -24,15 +24,12 @@ static QMutex gMutex;
 typedef  iv::radar::radarobjectarray RadarDataType;
 typedef  iv::lidar::objectarray LidarDataType;
 typedef  std::chrono::system_clock::time_point TimeType;
+typedef  iv::fusion::fusionobjectarray FusionDataType;
 
 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;
-iv::mobileye::lane lane_info;
-iv::mobileye::tsr tsr_info;
-
+FusionDataType fusion_info;
 
 QTime gTime;
 using namespace std;
@@ -117,27 +114,20 @@ void Listenradarback(const char * strdata,const unsigned int nSize,const unsigne
     gMutex.unlock();
 }
 
-void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+void Listenfusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-    iv::mobileye::mobileye mobileye;
+    FusionDataType fusion;
 
     if(nSize<1)return;
-    if(false == mobileye.ParseFromArray(strdata,nSize))
+    if(false == fusion.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);
+    fusion_info.CopyFrom(fusion);
     gMutex.unlock();
 }
 
@@ -146,6 +136,8 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 {
     //    gMutex.lock();
     //    Transformation::RadarPross(radarobjvec);
+    li_ra_fusion.Clear();
+    li_ra_fusion.CopyFrom(fusion_info);
 
     RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
 //    std::cout<<"   obj      size   "<<li_ra_fusion.obj_size()<<std::endl;
@@ -161,27 +153,27 @@ 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();
-      AddRadarBack(li_ra_fusion,radar_back);
-      radar_back.clear_obj();
+//      AddRadarBack(li_ra_fusion,radar_back);
+//      radar_back.clear_obj();
     //---------------------------------------------  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);
+//    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);
+//    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
+    ObsToNormal(li_ra_fusion);
     string out;
 
-    if(out_fusion.obj_size() == 0)
+    if(li_ra_fusion.obj_size() == 0)
     {
         //        std::cout<<"   fake   obj"<<std::endl;
 
@@ -195,19 +187,19 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
         fake_cen_ = fake_obj.mutable_centroid();
         fake_cen_ ->CopyFrom(fake_cen);
 
-        fake_obj_ = out_fusion.add_obj();
+        fake_obj_ = li_ra_fusion.add_obj();
         fake_obj_->CopyFrom(fake_obj);
-        out = out_fusion.SerializeAsString();
+        out = li_ra_fusion.SerializeAsString();
     }else
     {
-        out = out_fusion.SerializeAsString();
+        out = li_ra_fusion.SerializeAsString();
 
     }
 
-        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;
+        for (int k = 0; k<li_ra_fusion.obj_size();k++){
+            if(abs(li_ra_fusion.obj(k).centroid().x())<1 && li_ra_fusion.obj(k).centroid().y()>5 ){
+            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).velocity_linear_x()<<std::endl;
         }
         }
 
@@ -226,6 +218,6 @@ int main(int argc, char *argv[])
     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);
+    gpa = iv::modulecomm::RegisterRecv("grid_fusion",Listenfusion);
     return a.exec();
 }

src/tool/PerceptionShow/mainwindow.cpp

@@ -265,7 +265,7 @@ MainWindow::MainWindow(QWidget *parent) :
 
 //   pa = iv::modulecomm::RegisterRecv("can0",Listenesrfront);
 
-    pa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndectect);
+    pa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndectect);
 
     pa = iv::modulecomm::RegisterRecv("fusion",ListenFusion);