Merge branch 'JS_guide' of http://10.16.0.119:3000/adc_pilot/modularization into JS_guide

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/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1862,8 +1862,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     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) && (abs(gpsMapLine[k]->mfCurvature) < 0.03) )   //apollo_fu 20220622
+                road_permit_sum++;
         }
         if(road_permit_sum>=400)
             road_permit_avoid=true;
@@ -2362,24 +2363,28 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //        dSpeed=min(realspeed+0.5,dSpeed);
 //    }
     //超声波逻辑,begin
-     if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
+    if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
+    {
+       dSpeed=min(dSpeed,4.0);
+    }
+     if(ServiceCarStatus.mbUltraDis[0]>=1.5&&ServiceCarStatus.mbUltraDis[0]<=3.0)
      {
         dSpeed=min(dSpeed,3.0);
      }
-     else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
+     else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<1.5)
      {
-         minDecelerate=min(-1.0f,minDecelerate);
+         minDecelerate=min(-1.1f,minDecelerate);
      }
 
-     if((ServiceCarStatus.mbUltraDis[1]>=0.5&&ServiceCarStatus.mbUltraDis[1]<1.0)||
-        (ServiceCarStatus.mbUltraDis[3]>=0.5&&ServiceCarStatus.mbUltraDis[3]<1.0))
+     if((ServiceCarStatus.mbUltraDis[1]>=0.3&&ServiceCarStatus.mbUltraDis[1]<1.0)||
+        (ServiceCarStatus.mbUltraDis[3]>=0.3&&ServiceCarStatus.mbUltraDis[3]<1.0))
      {
         dSpeed=min(dSpeed,3.0);
      }
-     else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<0.5)||
-             (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<0.5))
+     else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<0.3)||
+             (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<0.3))
      {
-         minDecelerate=min(-0.5f,minDecelerate);
+         minDecelerate=min(-0.6f,minDecelerate);
      }
 
      if(ServiceCarStatus.daocheMode)
@@ -2524,12 +2529,12 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     traceOriLeft.clear();
     traceOriRight.clear();
 
-    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
+    if (gpsMapLine.size() > 700 && PathPoint >= 0) {
         int aimIndex;
         if(circleMode){
-            aimIndex=PathPoint+800;
+            aimIndex=PathPoint+700;
         }else{
-            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
+            aimIndex=min((PathPoint+700),(int)gpsMapLine.size());
         }
 
         for (int i = PathPoint; i < aimIndex; i++)
@@ -4359,9 +4364,13 @@ double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gp
 
 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);
+    //double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);
+    double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth);
+
     *avoidXRight=((int)RightBoundary);
-    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    //double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth);
+
     *avoidXLeft=(-(int)LeftBoundary);
 }
 

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 = 16;
+    double gpf_sensor_height = 1.35;
     // fitting multiple planes
     int gpf_num_segment;
     // number of iterations

src/detection/detection_ground_filter/main.cpp

@@ -2,7 +2,8 @@
 #include "modulecomm.h"
 #include "base_segmenter.hpp"
 #include "ground_plane_fitting_segmenter.hpp"
-void * gpoint_out= iv::modulecomm::RegisterSend("point_no_ground",20000000,1);
+#include <pcl/filters/radius_outlier_removal.h>
+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_;
 
@@ -34,6 +35,9 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
     int nNameSize;
     nNameSize = *pHeadSize - 4-4-8;
     char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
@@ -48,8 +52,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);
@@ -57,41 +61,36 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 
   //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
     }
+    pcl::RadiusOutlierRemoval<pcl::PointXYZI> pcFilter;
+    pcFilter.setInputCloud(point_cloud);
+    pcFilter.setRadiusSearch(0.8);
+    pcFilter.setMinNeighborsInRadius(10);
+    pcFilter.filter(*cloud_filtered);
+
     std::vector<lidar::segmenter::PointICloudPtr> cloud_clusters;
     lidar::segmenter::PointICloudPtr cloud_ground(new lidar::segmenter::PointICloud);
     lidar::segmenter::PointICloudPtr cloud_nonground(new lidar::segmenter::PointICloud);
-    ground_remover_->segment(*point_cloud, cloud_clusters);
+    ground_remover_->segment(*cloud_filtered, cloud_clusters);
     *cloud_ground = *cloud_clusters[0];
     *cloud_nonground = *cloud_clusters[1];
     PointShare(cloud_nonground);
 }
 
-
-
-
-
-
-
-
-
-
-
-
 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_height = 0.90;
+    params.gpf_sensor_model = 16;
+    params.gpf_sensor_height = 0.63;
     // fitting multiple planes
     params.gpf_num_segment = 1;
     // number of iterations
     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 +99,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/pointpillars.cc

@@ -517,6 +517,7 @@ void PointPillars::DoInference(const float* in_points_array,
 //        std::cout << setiosflags(ios::left) << setw(14) << Modules[i]  << setw(8)  << Times[i] * 1000 << " ms" << resetiosflags(ios::left) << std::endl;
 //    }
 //    std::cout << "------------------------------------" << std::endl;
+    GPU_CHECK(cudaFree(dev_points));
     cudaStreamDestroy(stream);
 
 }

src/detection/detection_lidar_PointPillars_MultiHead/postprocess.cu

@@ -512,5 +512,6 @@ void PostprocessCuda::DoPostprocessCuda(
 
         GPU_CHECK(cudaFree(dev_indexes));
         GPU_CHECK(cudaFree(dev_sorted_filtered_box));
+        GPU_CHECK(cudaFree(dev_sorted_filtered_scores));
     }
 }

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.35;
+static float VEHICLE_HEIGHT = 1.50;
 //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.4;
+static float skip_ymin = 0.0;
+static float skip_xmax = 0.4;
+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,42 @@ 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);
+
+//        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 +406,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 +477,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 +487,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 +691,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 +700,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.35");
+    snprintf(gstr_vehicleheight,255,"1.5");
     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,"li_ra_fusion");
+    snprintf(gstr_skipxmin,255,"-0.4");
+    snprintf(gstr_skipxmax,255,"0.4");
+    snprintf(gstr_skipymin,255,"0.0");
+    snprintf(gstr_skipymax,255,"0.3");
 
 
     givlog->verbose("yaml is %s ",gstr_yaml);

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -305,7 +305,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         break;
     case 2: ///< 2人工接管
         if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER)
-        {void sendPathPlanRequest(void);
+        {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
         emit setAllowPlan(false);
@@ -370,7 +370,7 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
         deltaY = localPositionNextY - localPositionY;
         distance = sqrt(deltaX*deltaX + deltaY*deltaY);
 
-        if(distance < 2.0 && currentSpeed < 0.1)
+        if(distance < 5.0 && currentSpeed < 0.1)
         {
             waitingStationArrived = true;
         }
@@ -397,7 +397,7 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
         deltaY = localPositionNextY - localPositionY;
         distance = sqrt(deltaX*deltaX + deltaY*deltaY);
 
-        if(distance < 2.0 && currentSpeed < 0.1)
+        if(distance < 5.0 && currentSpeed < 0.1)
         {
             maintainStationArrived = true;
         }

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -429,9 +429,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
+                        allowPlan = false;
                 }
             }
             lastTime = xTime.elapsed();

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -765,8 +765,8 @@ 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 < 20.0)
+//            std::cout<<"distance: "<<distance<<std::endl;
         if(distance < 1.5)
         {
             isArrived = 2;

src/driver/driver_lidar_rs16/lidar_driver_rs16.cpp

@@ -7,6 +7,7 @@
 #include <QMutex>
 #include <QDateTime>
 #include "math.h"
+#include "Eigen/Dense"
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -21,6 +22,10 @@
 #include "lidar_rs16_rawdata.h"
 
 
+Eigen::Matrix3d rotation_matrix;
+Eigen::Vector3d trans_matrix;
+
+
 #ifdef VV7_1
 
 int vv7;
@@ -265,6 +270,14 @@ void process_rs16obs(char * strdata,int nLen)
     int pointi = 0;
     float wt = 0;
     int wt1 = 0;
+
+    Eigen::AngleAxisd r_z ( -0.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+    Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+    Eigen::AngleAxisd r_x ( -0.0137, 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;
 //    int dx;
 //    int dy;
 //    float lidar_32_xdistance = 0.3;			//32线激光雷达X轴补偿
@@ -321,6 +334,7 @@ void process_rs16obs(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.0) continue;
                             if(binclix)
                             {
                                 double y,z;
@@ -337,10 +351,15 @@ void process_rs16obs(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;
                         }
                     }
@@ -378,6 +397,14 @@ void process_rs16obs(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);
 
@@ -388,22 +415,23 @@ void process_rs16obs(char * strdata,int nLen)
                 }
             }
 
-    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
 
-    int * pHeadSize = (int *)strOut;
-    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
-    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
-    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
-    *pu32 = point_cloud->header.seq;
-    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
-    pcl::PointXYZI * p;
-    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
-    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+            char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+            int * pHeadSize = (int *)strOut;
+            *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+            memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+            pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+            *pu32 = point_cloud->header.seq;
+            memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+            pcl::PointXYZI * p;
+            p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+            memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
 
-    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
-    delete strOut;
+            iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+            delete strOut;
 
-//            std::cout<<"point cloud width = "<<point_cloud->width<<"  size = "<<point_cloud->size()<<std::endl;
+            //            std::cout<<"point cloud width = "<<point_cloud->width<<"  size = "<<point_cloud->size()<<std::endl;
 }
 
 

src/driver/driver_lidar_rs16/main.cpp

@@ -179,7 +179,7 @@ int main(int argc, char *argv[])
     showversion("driver_lidar_rs16");
     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

@@ -287,6 +287,7 @@ void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array
         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);
 }