Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -20,34 +20,40 @@
 
 #include <thread>
 
-void * gpacansend;
-void * gpadecition;
-void * gpachassis;
+#ifdef Q_OS_LINUX
+#include <signal.h>
+#endif
+
+static void * gpacansend;
+static void * gpadecition;
+static void * gpachassis;
 
-std::string gstrmemdecition;
-std::string gstrmemcansend;
-std::string gstrmemchassis;
-bool gbSendRun = true;
+static std::string gstrmemdecition;
+static std::string gstrmemcansend;
+static std::string gstrmemchassis;
+static bool gbSendRun = true;
 
-bool gbChassisEPS = false;
+static bool gbChassisEPS = false;
 
-iv::brain::decition gdecition_def;
-iv::brain::decition gdecition;
+static iv::brain::decition gdecition_def;
+static iv::brain::decition gdecition;
 
-QTime gTime;
-int gnLastSendTime = 0;
-int gnLastRecvDecTime = -1000;
-int gnDecitionNum = 0; //when is zero,send default;
+static QTime gTime;
+static int gnLastSendTime = 0;
+static int gnLastRecvDecTime = -1000;
+static int gnDecitionNum = 0; //when is zero,send default;
 const int gnDecitionNumMax = 100;
-int gnIndex = 0;
+static int gnIndex = 0;
 
 static bool gbHaveVehSpd = false;
 static double gfVehSpd = 0.0;
 
-boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
+static boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 static QMutex gMutex;
 
+static std::thread * gpsendthread = NULL;
+
 
 // signal: @ACC_LatAngReq    //更改CANid
 #define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
@@ -325,7 +331,7 @@ void Activate()
 //    for(int j=0;j<100000;j++)
 //    {
         std::cout<<" run "<<std::endl;
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0.0);
         xdecition.set_angle_mode(0);
         xdecition.set_angle_active(0);
@@ -338,7 +344,7 @@ void Activate()
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0.0);
         xdecition.set_angle_mode(1);
         xdecition.set_angle_active(1);
@@ -357,7 +363,7 @@ void Activate()
 void UnAcitvate()
 {
     iv::brain::decition xdecition;
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0);
         xdecition.set_angle_mode(1);
         xdecition.set_angle_active(1);
@@ -370,7 +376,7 @@ void UnAcitvate()
         ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 3; i++){
         xdecition.set_wheelangle(0);
         xdecition.set_angle_mode(0);
         xdecition.set_angle_active(0);
@@ -563,8 +569,24 @@ void sendthread()
         if(gbChassisEPS == false) ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
+    UnAcitvate();
 }
 
+#ifdef Q_OS_LINUX
+void sig_int(int signo)
+{
+    gbSendRun = false;
+    gpsendthread->join();
+
+    iv::modulecomm::Unregister(gpacansend);
+    iv::modulecomm::Unregister(gpachassis);
+    iv::modulecomm::Unregister(gpadecition);
+
+    std::cout<<" controller exit."<<std::endl;
+    exit(0);
+}
+#endif
+
 int main(int argc, char *argv[])
 {
     RegisterIVBackTrace();
@@ -619,5 +641,12 @@ int main(int argc, char *argv[])
 
     std::thread xthread(sendthread);
 
+    gpsendthread = &xthread;
+
+#ifdef Q_OS_LINUX
+   signal(SIGINT, sig_int);
+   signal(SIGTERM,sig_int);
+#endif
+
     return a.exec();
 }

src/decition/decition_brain_sf/decition/adc_adapter/ge3_adapter.cpp

@@ -115,6 +115,8 @@ iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_g
     (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
 
 
+
+
     return *decition;
 }
 

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/changan_shenlan_adapter.cpp

@@ -126,8 +126,8 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     (*decition)->angle_mode = 2; //横向控制激活，和上一条同时满足才执行横向请求角度
     (*decition)->auto_mode = 3; //3为自动控制模式
 
-    (*decition)->wheel_angle=max((float)-500.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
-    (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
+        (*decition)->wheel_angle=max((float)-430.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
+        (*decition)->wheel_angle=min((float)430.0,(*decition)->wheel_angle);
 
 
     return *decition;

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -1010,6 +1010,7 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
 iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr) {
 
     iv::Point2D obsPoint(-1, -1);
+    obsPoint.s = -1;
     float xiuzheng=0;
     if(!ServiceCarStatus.useMobileEye){
         xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -933,32 +933,68 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double acc_end = 0.1;
     static double distoend=1000;
-    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+//    {
+//        if(PathPoint+1000>gpsMapLine.size())
+//        {
+//            distoend=computeDistToEnd(gpsMapLine,PathPoint);
+//        }
+//        else
+//        {
+//            distoend=1000;
+//        }
+//        if(!circleMode && distoend<50)
+//        {
+//            double nowspeed = realspeed/3.6;
+//            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
+//            {
+//                if(distoend == 0.0)
+//                    distoend = 0.09;
+//                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+//                if(acc_end<(-3.0))
+//                    acc_end = -3.0;
+//            }
+
+//            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
+//                acc_end = -0.5;
+//        }
+//    }
+    int useaccend = 1;
+    double mstopbrake=0.3;
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
     {
-        if(PathPoint+1000>gpsMapLine.size())
-        {
-            distoend=computeDistToEnd(gpsMapLine,PathPoint);
-        }
-        else
-        {
-            distoend=1000;
-        }
-        if(!circleMode && distoend<50)
-        {
-            double nowspeed = realspeed/3.6;
-            if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
-            {
-                if(distoend == 0.0)
-                    distoend = 0.09;
-                acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
-                if(acc_end<(-3.0))
-                    acc_end = -3.0;
-            }
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<100){
+                        double nowspeed = realspeed/3.6;
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                        }
 
-            if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))
-                acc_end = -0.5;
-        }
-    }else{
+                        if((distoend < 0.5)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
+                }
+    }
+    else{
         //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
         //                        minDecelerate=-0.5;
         //                        std::cout<<"map finish brake"<<std::endl;
@@ -969,6 +1005,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             static int BrakePoint=-1;
             static bool final_brake=false;
             static double distance_to_end=1000;
+            static bool enterTofinal=false;  //20230417,shenlan
+            static double enterTofinal_speed=200;
             if(PathPoint+forecast_final_point>gpsMapLine.size())
             {
                 distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
@@ -993,6 +1031,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 final_brake_lock=false;
                 brake_mode=false;
                 BrakePoint=-1;
+                enterTofinal=false;  //20230417,shenlan
+                enterTofinal_speed=200;
             }
             if(final_brake==true)
             {
@@ -1002,12 +1042,35 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 }
                 else
                 {
-                    dSpeed=min(dSpeed, 3.0);
+                    if(enterTofinal==false)  //20230417,shenlan)
+                    {
+                        enterTofinal_speed=realspeed;
+                        enterTofinal=true;
+                    }
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                       dSpeed=min(enterTofinal_speed, 3.0);
+                    }
+                    else
+                    {
+                       dSpeed=min(dSpeed, 3.0);
+                    }
                     final_brake_lock=true;
                     brake_mode=true;
-                    if(distance_to_end<0.8)
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
                     {
-                        minDecelerate=-0.7;
+                        if(distance_to_end<2.0)
+                        {
+                            minDecelerate=-0.7;
+                        }
+
+                    }
+                    else
+                    {
+                        if(distance_to_end<0.8)
+                        {
+                            minDecelerate=-0.7;
+                        }
                     }
                 }
             }
@@ -2487,6 +2550,7 @@ else
         }
     }
 
+    if(obsDistance == 0)obsDistance = -1;
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
     Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
@@ -3722,6 +3786,10 @@ void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::
         obsFrenet.s=obsFrenetMid.s-now_s_d.s;
         lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
         //    lidarDistance=-1;
+//        if(obsPoint.s < 0)
+//        {
+//           lidarDistance = -1;
+//        }
         if (lidarDistance<0)
         {
             lidarDistance = -1;
@@ -4213,7 +4281,7 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
     return  traffic_speed;
 }
 
-float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+float  iv::decition::DecideGps00:: ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                            int pathpoint,float secSpeed,float dSpeed){
     float traffic_speed=200;
     float traffic_dis=0;
@@ -4303,6 +4371,7 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
 
 
     default:
+        passEnable=true; //20230413
         break;
     }
 

src/detection/detection_lidar_centerpoint/detection_lidar_centerpoint.pro

@@ -83,9 +83,9 @@ LIBS += -L"/usr/local/lib" \
         -lcudart \
         -lcufft
 
-CUDA_SDK = "/usr/local/cuda-11.4/"   # cudaSDK路径
+CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
 
-CUDA_DIR = "/usr/local/cuda-11.4/"            # CUDA tookit路径
+CUDA_DIR = "/usr/local/cuda/"            # CUDA tookit路径
 
 SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
 

src/detection/detection_lidar_centerpoint/main.cpp

@@ -24,10 +24,10 @@ void init()
     std::vector<double> yaw_norm_thresholds ;
     const std::string densification_world_frame_id = "map";
     const int densification_num_past_frames = 1;
-    const std::string trt_precision = "fp32";
-    const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint_tiny.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
+    const std::string trt_precision = "fp16";
+    const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
     const std::string encoder_engine_path ="/home/nvidia/models/pts_voxel_encoder_centerpoint.eng";//this->declare_parameter<std::string>("encoder_engine_path");
-    const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint_tiny.onnx";//this->declare_parameter<std::string>("head_onnx_path");
+    const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint.onnx";//this->declare_parameter<std::string>("head_onnx_path");
     const std::string head_engine_path ="/home/nvidia/models/pts_backbone_neck_head_centerpoint.eng" ;//this->declare_parameter<std::string>("head_engine_path");
     const std::size_t point_feature_size =4;
     const std::size_t max_voxel_size =40000;

src/tool/IVSysMan/mainwindow.cpp

@@ -134,6 +134,15 @@ MainWindow::~MainWindow()
     {
         mPM->mvectorprog[i].mbautostart = false;
     }
+
+    for(i=0;i<mPM->mvectorprog.size();i++)
+    {
+        if(mPM->mvectorprog[i].mbRun)
+        {
+            mPM->mvectorprog[i].mProcess->terminate();
+        }
+    }
+
     for(i=0;i<mPM->mvectorprog.size();i++)
     {
         if(mPM->mvectorprog[i].mbRun)
@@ -146,6 +155,8 @@ MainWindow::~MainWindow()
         }
     }
 
+  //  QThread::msleep(300);
+
 #ifdef QT_DEBUG
     iv::ivexit::ExecIVExitCmd(mpivexit,"adcivexit-test");   // test iv exit code. can comment this line.
 #endif
@@ -154,7 +165,7 @@ MainWindow::~MainWindow()
 
     mPM->requestInterruption();
 
-    QThread::msleep(1000);
+//    QThread::msleep(1000);
 
     qint64 time;
     time = QDateTime::currentMSecsSinceEpoch();

src/tool/IVSysMan/progmon.cpp

@@ -33,6 +33,15 @@ ProgMon::~ProgMon()
     std::cout<<"~ProgMon"<<std::endl;
     unsigned int i;
     for(i=0;i<mvectorprog.size();i++)
+    {
+        if(mvectorprog[i].mProcess != 0)
+        {
+            mvectorprog[i].mProcess->terminate();
+  //          mvectorprog[i].mProcess->close();
+        }
+    }
+    QThread::msleep(300);
+    for(i=0;i<mvectorprog.size();i++)
     {
         if(mvectorprog[i].mProcess != 0)
         {
@@ -553,7 +562,10 @@ void ProgMon::StopProc(ProgUnit *pu)
         qDebug("process %s is not running. not need stop.",pu->strappname.data());
         return;
     }
+    pu->mProcess->terminate();
+    std::this_thread::sleep_for(std::chrono::milliseconds(300));
     pu->mProcess->kill();
+
     if(!checkStartState(pu)){
         return;
     }
@@ -651,7 +663,7 @@ void ProgMon::run()
         qint64 nNow = QDateTime::currentMSecsSinceEpoch();
         if((nNow - nLastUpdate)< interval)
         {
-            msleep(100);
+            msleep(10);
             continue;
         }
         QString strsysinfo;

src/tool/adcndtmultimapping/adcndtmultimapping.xml

@@ -1,5 +1,6 @@
 <xml>	
 	<node name="adcndtmultimapping">
 		<param name="msg" value="lidar_pc" />
+		<param name="oldlidar" value="false" />
 	</node>
 </xml>

src/tool/adcndtmultimapping/main.cpp

@@ -11,6 +11,8 @@ iv::Ivlog * givlog;
 std::string gstrlidarmsg;
 std::string gstrimumsg;
 
+bool gbOldLidar = false;
+
 
 int main(int argc, char *argv[])
 {
@@ -30,6 +32,11 @@ int main(int argc, char *argv[])
    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    gstrlidarmsg = xp.GetParam("msg","lidarpc_center");
    gstrimumsg = xp.GetParam("gpsmsg","ins550d_gpsimu");
+   std::string strboldlidar = xp.GetParam("oldlidar","false");
+   if(strboldlidar == "true")
+   {
+       gbOldLidar = true;
+   }
     MainWindow w;
     w.show();
 

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -442,7 +442,7 @@ void MainWindow::onTimer()
         //循环读取数据，直到读取到"lidar_pc"
         while((bx == true)&&(strcmp(strName,gstrlidarmsg.data()) != 0))
         {
-            if((strcmp(strName,gstrlidarmsg.data()) != 0))
+            if((strcmp(strName,gstrimumsg.data()) == 0))
             {
                                 iv::gps::gpsimu xgpsimu;
                                 bool bParse;

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -200,6 +200,8 @@ extern iv::Ivlog * givlog;
 
 bool gbManualSave = false;
 
+extern bool gbOldLidar;
+
 void setmethod(int nmethod)
 {
     _method_type = MethodType::PCL_GENERIC;
@@ -1152,7 +1154,9 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
   //显示打印的信息字符串
   gstr = g_ostr.str();
 
-  glidartime = current_scan_time;
+  if(gbOldLidar)gbOldLidar = current_scan_time;
+  else
+    glidartime = current_scan_time/1000;
 
   gstr = gstr.substr(0,g_ostr.pcount());
 //  std::cout<<g_ostr.str()<<std::endl;

src/tool/view_group_grpc/groupqueryclient.h

@@ -16,7 +16,7 @@
 
 #include <thread>
 
-#include "modulecommext.h"
+//#include "modulecommext.h"
 
 #include <iostream>
 #include <memory>