modified brain

src/decition/common/common/car_status.h

@@ -164,7 +164,7 @@ namespace iv {
         int vehGroupId;
         double mfEPSOff = 0.0;
         float socfDis=8;   //停障触发距离
-        float aocfDis=30;   //避障触发距离//20
+        float aocfDis=35;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90

src/decition/decition_brain_sf_jsrunlegs/decition/adc_adapter/haomo_adapter.cpp

@@ -50,8 +50,8 @@ iv::decition::Decition iv::decition::HaoMoAdapter::getAdapterDeciton(GPS_INS now
 //    }
 
     controlSpeed= dSpeed;
-    if(controlSpeed>8)
-        controlSpeed=8; //调试先限制下速度
+    if(controlSpeed>15)
+        controlSpeed=15; //调试先限制下速度
    // controlSpeed=min(controlSpeed,8.0f); //对车子进行限速，车子最大速度是,倒车是5*3.6km/h，前进是10*3.6
 
     //0227 10m nei xianzhi shache

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -208,6 +208,9 @@ static int front_car_id=-1;
 static int front_car_vector_id=-1;
 static bool final_brake_lock=false,brake_mode=false;
 
+bool gfinal_brake=false;
+double gdistance_to_end=1000;
+
 //日常展示
 
 #include <QDateTime>
@@ -1119,18 +1122,21 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         }
                         if(final_brake==true){
                                 if((realspeed>3)&&(final_brake_lock==false)){
-                                            minDecelerate=-0.7;
+                                            //minDecelerate=-0.7;
                                 }else{
-                                            dSpeed=min(dSpeed, 3.0);
+                                           dSpeed=min(dSpeed, 3.0);
                                             final_brake_lock=true;
                                             brake_mode=true;
-                                            if(distance_to_end<0.8){
+                                            //if(distance_to_end<0.8){
+                                             if(distance_to_end<0.4){
                                                 minDecelerate=-0.7;
                                                 ServiceCarStatus.mbFulfilTask = true;
                                             }
 
                                 }
                         }
+                        gfinal_brake=final_brake;
+                        gdistance_to_end = distance_to_end;
                 }
     }
 
@@ -1551,11 +1557,32 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     }
 #ifdef AVOID_NEW
+    if(vehState==avoidObs)
+    {
+        dSpeed=min(12.0,dSpeed);
+    }
+
     if((vehState==avoiding)||(vehState==preAvoid))
     {
-        dSpeed = min(8.0,dSpeed);
+        dSpeed = min(5.0,dSpeed);
+//        if(dSpeed>5)
+//        {
+//            dSpeed = max(5.0,realspeed-0.5);
+//        }
+//        else
+//        {
+//          dSpeed = min(8.0,dSpeed);
+//        }
     }else if((vehState==backOri)||(avoidXNew!=0)){
-        dSpeed = min(12.0,dSpeed);
+        dSpeed = min(8.0,dSpeed);
+//         if(dSpeed>5)
+//         {
+//            dSpeed = min(7.0,realspeed+0.5);
+//         }
+//         else
+//         {
+//           dSpeed = min(12.0,dSpeed);
+//         }
     }
 #else
     if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
@@ -1573,9 +1600,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //        }
     }
     //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
-    if(brake_mode==true){
-        dSpeed=min(dSpeed, 3.0);
-    }
+//    if(brake_mode==true){
+//            dSpeed=min(dSpeed, 3.0);
+//       // dSpeed=min(dSpeed, 3.0);
+//    }
 
     if(front_car_id>0){
         static bool brake_state=false;
@@ -2353,7 +2381,22 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
        ServiceCarStatus.mbNoPath=false;
        ServiceCarStatus.mbNoPathCnt=0;
     }
-//
+//junsheng ditu zhongdian
+
+    if((realspeed>3.5)&&(dSpeed>3.5)&&(gfinal_brake==true)&&(final_brake_lock==false))
+    {
+        dSpeed=realspeed-0.5;
+    }
+
+    if((dSpeed-realspeed>1.0)&&(final_brake_lock==false))
+    {
+        dSpeed = min(realspeed+0.5,dSpeed) ;
+    }
+
+    if(brake_mode==true){
+           // dSpeed=min(dSpeed, realspeed);
+       dSpeed=min(dSpeed, 3.0);
+    }
 
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;
@@ -2548,7 +2591,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                            <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
                            <<"Vehicle_state"<< ","<<vehState<< ","
                            <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
-                           <<"avoid_cnt"<<","<<ObsTimeSpan<<","
+                           <<"avoidXNew"<<","<<avoidXNew<<","
+                           <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+                           <<"avoidXPre"<<","<<avoidXPre<<","
+                           <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
                            <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
                            <<"Min_Decelation"","<<minDecelerate<< ","
                            <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","

src/detection/detection_lidar_PointPillars_MultiHead/cfgs/cbgs_pp_multihead.yaml

@@ -1,9 +1,11 @@
-CLASS_NAMES: ['car','truck', 'construction_vehicle', 'bus', 'trailer',
-              'barrier', 'motorcycle', 'bicycle', 'pedestrian', 'traffic_cone']
+#CLASS_NAMES: ['轿车', '卡车', '施工车辆', '公交车', '拖车/挂车',
+#              '路碍', '摩托车', '自行车', '行人', '锥形筒']
 
+CLASS_NAMES: ['car', 'truck', 'bus', 'barrier', 'bicycle', 'pedestrian', 'traffic_cone']
 DATA_CONFIG:
-    _BASE_CONFIG_: cfgs/dataset_configs/nuscenes_dataset.yaml
+    _BASE_CONFIG_: /home/a/tian/OpenPCDet/tools/cfgs/dataset_configs/nuscenes_dataset_new.yaml
 
+    #POINT_CLOUD_RANGE: [-38.4, -38.4, -5.0, 38.4, 38.4, 3.0]
     POINT_CLOUD_RANGE: [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0]
     DATA_PROCESSOR:
         -   NAME: mask_points_and_boxes_outside_range
@@ -58,9 +60,9 @@ MODEL:
         ANCHOR_GENERATOR_CONFIG: [
             {
                 'class_name': car,
-                'anchor_sizes': [[4.63, 1.97, 1.74]],
+                'anchor_sizes': [[4.62, 1.95, 1.73]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.95],
+                'anchor_bottom_heights': [-0.85],
                 'align_center': False,
                 'feature_map_stride': 4,
                 'matched_threshold': 0.6,
@@ -68,79 +70,49 @@ MODEL:
             },
             {
                 'class_name': truck,
-                'anchor_sizes': [[6.93, 2.51, 2.84]],
+                'anchor_sizes': [[6.85, 2.56, 2.89]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.6],
+                'anchor_bottom_heights': [-0.23],
                 'align_center': False,
                 'feature_map_stride': 4,
-                'matched_threshold': 0.55,
-                'unmatched_threshold': 0.4
-            },
-            {
-                'class_name': construction_vehicle,
-                'anchor_sizes': [[6.37, 2.85, 3.19]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.225],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.35
+                'matched_threshold': 0.525,
+                'unmatched_threshold': 0.375
             },
             {
                 'class_name': bus,
-                'anchor_sizes': [[10.5, 2.94, 3.47]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.085],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.55,
-                'unmatched_threshold': 0.4
-            },
-            {
-                'class_name': trailer,
-                'anchor_sizes': [[12.29, 2.90, 3.87]],
+                'anchor_sizes': [[11.81, 2.91, 3.71]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [0.115],
+                'anchor_bottom_heights': [0.24],
                 'align_center': False,
                 'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.35
+                'matched_threshold': 0.525,
+                'unmatched_threshold': 0.375
             },
             {
                 'class_name': barrier,
                 'anchor_sizes': [[0.50, 2.53, 0.98]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-1.33],
+                'anchor_bottom_heights': [-1.18],
                 'align_center': False,
                 'feature_map_stride': 4,
                 'matched_threshold': 0.55,
                 'unmatched_threshold': 0.4
             },
-            {
-                'class_name': motorcycle,
-                'anchor_sizes': [[2.11, 0.77, 1.47]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-1.085],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.3
-            },
             {
                 'class_name': bicycle,
-                'anchor_sizes': [[1.70, 0.60, 1.28]],
+                'anchor_sizes': [[1.91, 0.69, 1.38]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-1.18],
+                'anchor_bottom_heights': [-0.96],
                 'align_center': False,
                 'feature_map_stride': 4,
                 'matched_threshold': 0.5,
-                'unmatched_threshold': 0.35
+                'unmatched_threshold': 0.325
             },
             {
                 'class_name': pedestrian,
-                'anchor_sizes': [[0.73, 0.67, 1.77]],
+                'anchor_sizes': [[0.72, 0.67, 1.77]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.935],
+                'anchor_bottom_heights': [-0.67],
                 'align_center': False,
                 'feature_map_stride': 4,
                 'matched_threshold': 0.6,
@@ -150,36 +122,13 @@ MODEL:
                 'class_name': traffic_cone,
                 'anchor_sizes': [[0.41, 0.41, 1.07]],
                 'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-1.285],
+                'anchor_bottom_heights': [-1.23],
                 'align_center': False,
                 'feature_map_stride': 4,
                 'matched_threshold': 0.6,
                 'unmatched_threshold': 0.4
             },
         ]
-
-        SHARED_CONV_NUM_FILTER: 64
-
-        RPN_HEAD_CFGS: [
-            {
-                'HEAD_CLS_NAME': ['car'],
-            },
-            {
-                'HEAD_CLS_NAME': ['truck', 'construction_vehicle'],
-            },
-            {
-                'HEAD_CLS_NAME': ['bus', 'trailer'],
-            },
-            {
-                'HEAD_CLS_NAME': ['barrier'],
-            },
-            {
-                'HEAD_CLS_NAME': ['motorcycle', 'bicycle'],
-            },
-            {
-                'HEAD_CLS_NAME': ['pedestrian', 'traffic_cone'],
-            },
-        ]
         SEPARATE_REG_CONFIG: 
             NUM_MIDDLE_CONV: 1
             NUM_MIDDLE_FILTER: 64
@@ -226,7 +175,7 @@ MODEL:
 
 OPTIMIZATION:
     BATCH_SIZE_PER_GPU: 4
-    NUM_EPOCHS: 20
+    NUM_EPOCHS: 30
 
     OPTIMIZER: adam_onecycle
     LR: 0.001
@@ -235,7 +184,7 @@ OPTIMIZATION:
 
     MOMS: [0.95, 0.85]
     PCT_START: 0.4
-    DIV_FACTOR: 10
+    DIV_FACTOR: 20
     DECAY_STEP_LIST: [35, 45]
     LR_DECAY: 0.1
     LR_CLIP: 0.0000001
@@ -244,3 +193,4 @@ OPTIMIZATION:
     WARMUP_EPOCH: 1
 
     GRAD_NORM_CLIP: 10
+

src/detection/detection_lidar_PointPillars_MultiHead/detection_lidar_PointPillars_MultiHead.pro

@@ -143,7 +143,7 @@ LIBS += -lcudart -lcufft -lyaml-cpp
 
 #LIBS += -L/home/adc/soft/cudnn-10.2-linux-x64-v7.6.5.32/cuda/lib64 -lcudnn
 
-LIBS += -lnvinfer -lnvonnxparser -lnvcaffe_parser
+LIBS +=  -lmyelin -lnvinfer -lnvonnxparser -lnvcaffe_parser
 
 #LIBS += -L/home/nvidia/git/libtorch_gpu-1.6.0-linux-aarch64/lib -ltorch_cuda  -ltorch -lc10 -ltorch_cpu
 

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -563,6 +563,7 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
 
     int ntlen;
     std::string out = lidarobjvec.SerializeAsString();
+
     iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
 
     //    givlog->verbose("lenth is %d",out.length());
@@ -745,9 +746,9 @@ int main(int argc, char *argv[])
                     );
     }
     std::cout<<"PointPillars Init OK."<<std::endl;
-    Eigen::AngleAxisd r_z ( -0.0274, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
-    Eigen::AngleAxisd r_y ( -0.0426, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
-    Eigen::AngleAxisd r_x ( -0.0059, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
+    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();

src/detection/detection_lidar_PointPillars_MultiHead/pointpillars.cc

@@ -224,13 +224,28 @@ void PointPillars::DeviceMemoryMalloc() {
 
     GPU_CHECK(cudaMalloc(&rpn_buffers_[0],  kRpnInputSize * sizeof(float)));
 
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    
+
+    if(kNumClass == 10)
+    {
+////class = 10
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    }
+    else
+    {
+////class = 7
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    }
+
     GPU_CHECK(cudaMalloc(&rpn_buffers_[7],  kNumAnchorPerCls * kNumClass * kNumOutputBoxFeature * sizeof(float))); //boxes
 
     // for scatter kernel
@@ -283,33 +298,13 @@ void PointPillars::SetDeviceMemoryToZero() {
     GPU_CHECK(cudaMemset(dev_filter_count_,     0, kNumClass * sizeof(int)));
 }
 
-
-#include <fstream>
-#include <iostream>
-#
-
-void PointPillars::SaveEngine(nvinfer1::ICudaEngine* pengine,std::string strpath)
-{
-    nvinfer1::IHostMemory* data = pengine->serialize();
-    std::ofstream file;
-    file.open(strpath,std::ios::binary | std::ios::out);
-    if(!file.is_open())
-    {
-        std::cout << "read create engine file" << strpath <<" failed" << std::endl;
-        return;
-    }
-
-    file.write((const char*)data->data(), data->size());
-    file.close();
-}
-
 void PointPillars::InitTRT(const bool use_onnx) {
   if (use_onnx_) {
     // create a TensorRT model from the onnx model and load it into an engine
     OnnxToTRTModel(pfe_file_, &pfe_engine_);
-    SaveEngine(pfe_engine_,"/home/nvidia/models/lidar/cbgs_pp_multihead_pfe.trt");
+    SaveEngine(pfe_engine_, pfe_file_.substr(0, pfe_file_.find(".")) + ".trt");
     OnnxToTRTModel(backbone_file_, &backbone_engine_);
-    SaveEngine(backbone_engine_,"/home/nvidia/models/lidar/cbgs_pp_multihead_backbone.trt");
+    SaveEngine(backbone_engine_, backbone_file_.substr(0, backbone_file_.find(".")) + ".trt");
   }else {
     EngineToTRTModel(pfe_file_, &pfe_engine_);
     EngineToTRTModel(backbone_file_, &backbone_engine_);
@@ -350,7 +345,7 @@ void PointPillars::OnnxToTRTModel(
 
     // Build the engine
     builder->setMaxBatchSize(kBatchSize);
-    //builder->setHalf2Mode(true);
+    builder->setHalf2Mode(true);
     nvinfer1::IBuilderConfig* config = builder->createBuilderConfig();
     config->setMaxWorkspaceSize(1 << 25);
     nvinfer1::ICudaEngine* engine =
@@ -363,58 +358,19 @@ void PointPillars::OnnxToTRTModel(
     builder->destroy();
 }
 
-void PointPillars::LoadEngineModel(const string &engine_file, nvinfer1::ICudaEngine **engine_ptr)
+void PointPillars::SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath)
 {
-//    using namespace std;
-//    fstream file;
-
-//    file.open(engine_file,ios::binary | ios::in);
-//    if(!file.is_open())
-//    {
-//        cout << "read engine file" << engine_file <<" failed" << endl;
-//        return;
-//    }
-//    file.seekg(0, ios::end);
-//    int length = file.tellg();
-//    file.seekg(0, ios::beg);
-//    std::unique_ptr<char[]> data(new char[length]);
-//    file.read(data.get(), length);
-
-//    file.close();
-
-//    std::cout << "deserializing" << std::endl;
-//    mTrtRunTime = createInferRuntime(gLogger);
-//    assert(mTrtRunTime != nullptr);
-//    mTrtEngine= mTrtRunTime->deserializeCudaEngine(data.get(), length, &mTrtPluginFactory);
-//    assert(mTrtEngine != nullptr);
-
-//    nvinfer1::IRuntime* runtime = nvinfer1::createInferRuntime(g_logger_);
-
-//    if (runtime == nullptr) {
-//        std::string msg("failed to build runtime parser");
-//        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
-//        exit(EXIT_FAILURE);
-//    }
-
-
-
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << "------------------------------------------------------------------"<< std::endl;
-//    std::cout << ">>>>                                                          >>>>"<< std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << "Input filename:   " << engine_file << std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << ">>>>                                                          >>>>"<< std::endl;
-//    std::cout << "------------------------------------------------------------------"<< std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-
-//    nvinfer1::ICudaEngine* engine = runtime->deserializeCudaEngine(modelMem, modelSize, NULL);
-//    if (engine == nullptr) {
-//        std::string msg("failed to build engine parser");
-//        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
-//        exit(EXIT_FAILURE);
-//    }
-//    *engine_ptr = engine;
+    // serialize the engine, then close everything down
+    nvinfer1::IHostMemory& trtModelStream = *(engine->serialize());
+    std::ofstream file;
+    file.open(engine_filepath, std::ios::binary | std::ios::out);
+    if(!file.is_open())
+    {
+        std::cout << "read create engine file" << engine_filepath <<" failed" << std::endl;
+        return;
+    }
+    file.write((const char*)trtModelStream.data(), std::streamsize(trtModelStream.size()));
+    file.close();
 }
 
 void PointPillars::EngineToTRTModel(
@@ -561,8 +517,6 @@ 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/pointpillars.h

@@ -76,7 +76,7 @@ class Logger : public nvinfer1::ILogger {
   explicit Logger(Severity severity = Severity::kWARNING)
       : reportable_severity(severity) {}
 
-  void log(Severity severity, nvinfer1::AsciiChar const* msg) noexcept {
+  void log(Severity severity, const char* msg) override {
     // suppress messages with severity enum value greater than the reportable
     if (severity > reportable_severity) return;
 
@@ -179,8 +179,8 @@ class PointPillars {
     void* pfe_buffers_[2];
     //variable for doPostprocessCudaMultiHead
     void* rpn_buffers_[8];
-
-    std::vector<float*> rpn_box_output_;
+    
+    std::vector<float*> rpn_box_output_; 
     std::vector<float*> rpn_cls_output_;
 
     float* dev_scattered_feature_;
@@ -221,10 +221,11 @@ class PointPillars {
     void InitParams();
     /**
      * @brief Initializing TensorRT instances
-     * @param[in] usr_onnx_ if true, parse ONNX
+     * @param[in] usr_onnx_ if true, parse ONNX 
      * @details Called in the constructor
      */
     void InitTRT(const bool use_onnx);
+    void SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath);
     /**
      * @brief Convert ONNX to TensorRT model
      * @param[in] model_file ONNX model file path
@@ -237,15 +238,12 @@ class PointPillars {
     /**
      * @brief Convert Engine to TensorRT model
      * @param[in] model_file Engine(TensorRT) model file path
-     * @param[out] engine_ptr TensorRT model engine made
+     * @param[out] engine_ptr TensorRT model engine made 
      * @details Load Engine model, and convert it to TensorRT model
      */
-    void EngineToTRTModel(const std::string &engine_file ,
+    void EngineToTRTModel(const std::string &engine_file ,     
                         nvinfer1::ICudaEngine** engine_ptr) ;
 
-    void LoadEngineModel(const std::string &engine_file ,
-                         nvinfer1::ICudaEngine** engine_ptr) ;
-
     /**
      * @brief Preproces points
      * @param[in] in_points_array Point cloud array
@@ -272,8 +270,6 @@ class PointPillars {
                 const std::string pp_config);
     ~PointPillars();
 
-    void SaveEngine(nvinfer1::ICudaEngine* pengine,std::string strpath);
-
     /**
      * @brief Call PointPillars for the inference
      * @param[in] in_points_array Point cloud array
@@ -288,3 +284,4 @@ class PointPillars {
                     std::vector<int>* out_labels,
                     std::vector<float>* out_scores);
 };
+

src/detection/detection_lidar_ukf_pda/imm_ukf_pda.cpp

@@ -543,7 +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 = 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_)
     {

src/driver/driver_gps_hcp2/main.cpp

@@ -24,10 +24,11 @@ int main(int argc, char *argv[])
     std::cout<<strpath.toStdString()<<std::endl;
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
-    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB0");
+    std::string strdevname = xp.GetParam("devname","/dev/ttysWK0");
     std::string strgpsimumemname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
     std::string strgpsmemname = xp.GetParam("msg_gps","hcp2_gps");
     std::string strimumemname = xp.GetParam("msg_imu","hcp2_imu");
+    std::cout<<strdevname.data()<<std::endl ;
     hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data());
     x.start();
     return a.exec();

src/driver/driver_lidar_rs32/lidar_driver_rs32.cpp

@@ -266,16 +266,28 @@ void process_rs32obs(char * strdata,int nLen)
 //    float lidar_32_ydistance = -2.3;			//32线激光雷达Y轴补偿
 
 
+//    float H_BETA[32] = {
+//        -8,-8,-8,-8,-8,-2.672,2.672,8,
+//        -8,-2.672,2.672,8,-8,-2.672,2.672,8,
+//        8,8,8,-8,8,-8,8,-8,
+//        -8,-2.672,2.672,8,-8,-2.672,2.672,8
+//    };
+
     float H_BETA[32] = {
-        -8,-8,-8,-8,-8,-2.672,2.672,8,
-        -8,-2.672,2.672,8,-8,-2.672,2.672,8,
-        8,8,8,-8,8,-8,8,-8,
-        -8,-2.672,2.672,8,-8,-2.672,2.672,8
+        -4.11,3.67,-4.07,3.64,-4.03,3.62,-4.02,3.61,
+                                   -4.01,3.61,-4.01,3.61,-4.02,3.62,-4.03,3.63,
+                                  -4.06,3.71,-4.09,3.75,-4.23,3.92,-4.30,4.01,
+                                  -4.56,4.18,-4.80,4.42,-5.07,4.66,-5.36,4.84
     };
 
 //    float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,15,13,11,9,7,5,3,1};
-    float V_theta[32] = {-25,-14.638,-7.91,-5.407,-3.667,-4,-4.333,-4.667,-2.333,-2.667,-3,-3.333,-1,-1.333,-1.667,-2,
-                        -10.281,-6.424,2.333,3.333,4.667,7,10.333,15,0.333,0,-0.333,-0.667,1.667,1.333,1.0,0.667};
+//    float V_theta[32] = {-25,-14.638,-7.91,-5.407,-3.667,-4,-4.333,-4.667,-2.333,-2.667,-3,-3.333,-1,-1.333,-1.667,-2,
+//                        -10.281,-6.424,2.333,3.333,4.667,7,10.333,15,0.333,0,-0.333,-0.667,1.667,1.333,1.0,0.667};
+    float V_theta[32] = {14.94,13.03,10.92,8.9,6.96,5.46,
+                         3.98,2.67,1.33,0,-1.33,-2.67,-3.96,
+                         -5.23,-6.63,-8.01,-9.94,-15.97,-12.90,
+                         -18.98,-21.92,-27.87,-24.96,-30.95,-33.89,
+                         -37.09,-39.95,-43.11,-46.10,-49.13,-51.90,-53.70};
 //    float V_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};
 //    float T[32] = {    0,  3.125,  1.5625,   4.6875,  6.25,  9.375,  7.8125,  10.9375,
 //                    12.5,  15.625, 14.0625, 17.1875, 18.75, 21.875,  20.3125, 23.4375,
@@ -310,7 +322,7 @@ void process_rs32obs(char * strdata,int nLen)
                         Ang = (0 - wt-H_BETA[pointi]) / 180.0 * Lidar_Pi;
                         Range = ((pstr[bag*1206 + Group * 100 + 4 + 3 * pointi] << 8) + pstr[bag*1206+Group * 100 + 5 + 3 * pointi]);
                         unsigned char intensity = pstr[bag*1206 + Group * 100 + 6 + 3 * pointi];
-                        Range=Range* 5.0/1000.0;
+                        Range=Range* 5.0/2000.0;
 
                         if(Range<150)
                         {

src/driver/driver_map_xodrload/driver_map_xodrload.xml

@@ -1,6 +1,7 @@
 <xml>	
 	<node name="driver_map_xodrload">
 		<param name="extendmap" value="true" />
+                <param name="sideenable" value="true" />
 	</node>
 </xml>
 

src/driver/driver_map_xodrload/main.cpp

@@ -1093,6 +1093,7 @@ int main(int argc, char *argv[])
         strparapath = argv[2];
     }
 
+    std::cout<<strparapath<<std::endl;
 
     iv::xmlparam::Xmlparam xp(strparapath);
     xp.GetParam(std::string("he"),std::string("1"));
@@ -1108,7 +1109,7 @@ int main(int argc, char *argv[])
 
     std::string strextendmap = xp.GetParam("extendmap","false");
 
-    std::string strsideenable = xp.GetParam("sideenable","true");
+    std::string strsideenable = xp.GetParam("sideenable","false");
 
 
     glat0 = atof(strlat0.data());

src/tool/bqev_pcdview/main.cpp

@@ -79,8 +79,8 @@ int main(int argc, char *argv[])
     showversion("bqev_pcdview");
     QApplication a(argc, argv);
 
-//    snprintf(gstr_memname,255,"lidar_pc");
-    snprintf(gstr_memname,255,"lidarpc_center");
+    snprintf(gstr_memname,255,"lidar_pc");
+//    snprintf(gstr_memname,255,"lidarpc_center");
 
     int nRtn = GetOptLong(argc,argv);
     if(nRtn == 1)  //show help,so exit.