change decition_brain_sf_changan_shenlan for hunter.

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.cpp

@@ -0,0 +1,304 @@
+#include <decition/adc_adapter/hunter_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+using namespace std;
+
+
+iv::decition::HunterAdapter::HunterAdapter(){
+    adapter_id=10;
+    adapter_name="hunter";
+}
+iv::decition::HunterAdapter::~HunterAdapter(){
+
+}
+
+//hunter 只用控制转角和速度，速度大于0前进，速度小于0后退，驻车是在完全停车的时候再用
+
+iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+//    if (accAim < 0)
+//    {
+//        controlSpeed=0;
+//        controlBrake=u; //102
+////        if(obsDistance>0 && obsDistance<10)
+//        if(obsDistance>0 && obsDistance<6)
+//        {
+//            controlBrake= u*1.0;     //1.5    zj-1.2
+//            controlSpeed=0;
+//        }
+////        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around)
+//        if(ttc<5)
+//        {
+//            controlBrake=0;
+//            controlSpeed=0;
+////            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+////        else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+////                 && dSpeed>0 && lastBrake==0)
+//        else if(ttc<10)
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-2.0);
+//        }
+////        else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around)
+//        else
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+
+////       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                  && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.3f);
+////            controlSpeed=0;
+////        }
+////        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                && dSpeed>0 && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.5f);
+////            controlSpeed=0;
+////        }
+
+
+//        //0110
+//        if(changingDangWei){
+//            controlBrake=max(0.5f,controlBrake);
+//        }
+
+//        //斜坡刹车加大 lsn 0217
+//        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+//            controlBrake=max(2.0f,controlBrake);
+//        }
+//        //斜坡刹车加大 end
+//    }
+//    else
+//    {
+//        controlBrake = 0;
+
+//        if(abs(dSpeed-realSpeed)<=2.0)
+//        {
+//            controlSpeed= dSpeed;
+//        }
+//        else if(abs(dSpeed-realSpeed)>2.0)
+//        {
+//             controlSpeed = min(realSpeed+1,dSpeed);
+//        }
+
+         controlSpeed= dSpeed;
+
+
+//        controlSpeed= dSpeed;
+
+        // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
+
+//    }
+
+    controlSpeed= dSpeed;
+//    controlSpeed=max(controlSpeed,5.0f); //对车子进行限速，车子最大速度是5km/h
+    if(controlSpeed>5.0)
+    {
+        controlSpeed=5.0;
+    }
+
+    //0227 10m nei xianzhi shache
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+    if(obsDistance<10 &&obsDistance>3){
+        controlSpeed=max(1.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+    else if(obsDistance<=3 &&obsDistance>0)
+    {
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+
+    if(DecideGps00::minDecelerate<0)  //在到达地图终点的时候，停车
+    {
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+//    if(DecideGps00::minDecelerate==-0.4){
+//        controlBrake =0.4;
+//    }
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+//    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+//        controlBrake =0;
+//        controlSpeed =0;
+//    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6//对于hunter车辆，brake 没有用
+    if(controlSpeed==0.0)
+    {
+        givlog->debug("brain_decition","controlSpeed: %f", controlSpeed);
+    }
+    (*decition)->torque =controlSpeed;//hunter目标速度大于0前进，小于0后退，hunter只有速度，角度和驻车控制
+
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed,ttc);
+
+    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
+                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
+
+
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)->brakeEnable=true;
+    (*decition)->air_enable = false;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+
+
+
+
+        //dangweiTrasfer
+//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
+//                 decition_gps->dangWei=2;
+//                 decition_gps->torque=0;
+//             }
+     lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->door=3;
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+
+   (*decition)->accelerator=  (*decition)->torque ;
+
+//将ttc时间和障碍物存储到log中方便后期数据分析
+
+
+    return *decition;
+}
+
+
+
+float iv::decition::HunterAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::HunterAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.h

@@ -0,0 +1,40 @@
+#ifndef HUNTER_ADAPTER_H
+#define HUNTER_ADAPTER_H
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class HunterAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        HunterAdapter();
+                        ~HunterAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+#endif // HUNTER_ADAPTER_H

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.cpp

@@ -119,7 +119,12 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
     }
 
-    bool bExtendPreview = false;
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        PreviewDistance = 3.0;
+    }
+
+    bool bExtendPreview = true;
     if(bExtendPreview)
     {
         ExtendPreviewDistance(trace,PreviewDistance);
@@ -168,6 +173,12 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     double wheel_base = 2.9;
     double wheelratio = 13.6;
 
+    if(ServiceCarStatus.msysparam.mvehtype == "hunter")
+    {
+        wheel_base = 0.6;
+        wheelratio = 21;
+    }
+
     if(bUseAutowareKappa)
     {
         double kappa = CalcKappa(trace,gpsIndex);

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -2786,6 +2786,14 @@ else
         }
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        if(dSpeed>5.0)
+        {
+            dSpeed=5.0;
+        }
+    }
+
     if(obsDistance == 0)obsDistance = -1;
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
@@ -3061,6 +3069,7 @@ void iv::decition::DecideGps00::initMethods(){
     hapo_Adapter = new HapoAdapter();
     sightseeing_Adapter = new SightseeingAdapter();
     changanshenlan_Adapter = new ChanganShenlanAdapter();
+    hunter_Adapter = new HunterAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
@@ -3072,6 +3081,7 @@ void iv::decition::DecideGps00::initMethods(){
     mhapo_Adapter.reset(hapo_Adapter);
     msightseeing_Adapter.reset(sightseeing_Adapter);
     mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
+    mhunter_Adapter.reset(hunter_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -3112,6 +3122,9 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
         changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
+    else if(ServiceCarStatus.msysparam.mvehtype=="hunter"){
+        hunter_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
 }
 
 
@@ -3131,6 +3144,11 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
         tracesize=800;
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        tracesize=100;//400;
+    }
+
     if (gpsMapLine.size() > tracesize && PathPoint >= 0)
     {
         int aimIndex;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -17,6 +17,7 @@
 #include <decition/adc_adapter/hapo_adapter.h>
 #include <decition/adc_adapter/changan_shenlan_adapter.h>
 #include <decition/adc_adapter/sightseeing_adapter.h>
+#include <decition/adc_adapter/hunter_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
@@ -157,6 +158,7 @@ public:
     BaseAdapter *hapo_Adapter;
     BaseAdapter *sightseeing_Adapter;
     BaseAdapter *changanshenlan_Adapter;
+    BaseAdapter *hunter_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -167,6 +169,7 @@ public:
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
     std::shared_ptr<BaseAdapter> msightseeing_Adapter;
     std::shared_ptr<BaseAdapter> mchanganshenlan_Adapter;
+    std::shared_ptr<BaseAdapter> mhunter_Adapter;
 
 	FrenetPlanner *frenetPlanner;
     SplinePlanner *splinePlanner;

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

@@ -30,7 +30,8 @@ HEADERS += \
     $$PWD/adc_adapter/interpolation_2d.h \
     $$PWD/adc_tools/PolynomialRegression.h \
     $$PWD/adc_tools/polyfit.h \
-    $$PWD/adc_planner/spline_planner.h
+    $$PWD/adc_planner/spline_planner.h \
+    $$PWD/adc_adapter/hunter_adapter.h
 
 SOURCES += \
     $$PWD/adc_adapter/changan_shenlan_adapter.cpp \
@@ -60,4 +61,5 @@ SOURCES += \
     $$PWD/adc_adapter/sightseeing_adapter.cpp \
     $$PWD/adc_adapter/interpolation_2d.cc \
     $$PWD/adc_tools/polyfit.cpp \
-    $$PWD/adc_planner/spline_planner.cpp
+    $$PWD/adc_planner/spline_planner.cpp \
+    $$PWD/adc_adapter/hunter_adapter.cpp

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -77,6 +77,7 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 #}
 
 INCLUDEPATH += /usr/include/opencv4/
-LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+#LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 
 
+LIBS += /usr/lib/libopencv*.so