add yuhesen adapter

src/decition/common/common/car_status.h

@@ -163,7 +163,7 @@ namespace iv {
         iv::roadmode_vel mroadmode_vel;
         int vehGroupId;
         double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
+        float socfDis=5;   //停障触发距离
         float aocfDis=25;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离

src/decition/decition_brain_sf_jsguide/decition/adc_adapter/yuhesenfr07pro_adapter.cpp

@@ -0,0 +1,378 @@
+#include <decition/adc_adapter/yuhesenfr07pro_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::YuHeSenFR07proAdapter::YuHeSenFR07proAdapter(){
+    adapter_id=12;
+    adapter_name="yuhesenfr07pro";
+}
+iv::decition::YuHeSenFR07proAdapter::~YuHeSenFR07proAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::YuHeSenFR07proAdapter::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){
+//            controlBrake= u*1.0;     //1.5    zj-1.2
+//        }
+//        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+//            controlBrake=0;
+//            controlSpeed=max(0.0,ServiceCarStatus.torque_st-2.0);
+//        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+//                 && dSpeed>0 && lastBrake==0){
+//            controlBrake=0;
+//            controlSpeed=0;
+//        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+//            controlBrake=min(controlBrake,0.5f);
+//            controlSpeed=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(0.5f,controlBrake);
+//        }
+//        //斜坡刹车加大 end
+
+
+
+
+
+//    }
+//    else
+//    {
+//        controlBrake = 0;
+
+//        if(ServiceCarStatus.torque_st==0){
+//            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
+//            controlSpeed =min( controlSpeed,2.0f);
+
+//        }else{
+//            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
+//        }
+
+//        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+//        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+//        //            if(realSpeed<5  ){
+//        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+//        //            }else if(realSpeed<10){
+//        //                controlSpeed=min((float)25.0,controlSpeed);  //40
+//        //            }
+//        //        }
+
+
+//        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
+
+////        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
+////            if(controlSpeed>0){
+////                controlSpeed = ServiceCarStatus.torque_st;
+////            }
+////        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
+////            if(controlSpeed>0 ){
+////                controlSpeed = ServiceCarStatus.torque_st;
+////            }
+////        }
+
+
+//        if(controlSpeed>ServiceCarStatus.torque_st){
+//            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
+//        }else if(controlSpeed<ServiceCarStatus.torque_st){
+//            controlSpeed = ServiceCarStatus.torque_st-1.0;
+//        }
+
+
+
+
+//        //Seizing
+//        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
+//            seizingCount++;
+//        }else{
+//            seizingCount=0;
+//        }
+//        if(seizingCount>=10){
+//            controlSpeed=ServiceCarStatus.torque_st+2;
+//        }
+
+//        // 斜坡加大油门   0217 lsn
+
+//        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+//            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+//                && (dSpeed-realSpeed)>3.0){
+//            controlSpeed=max((float)30.0,controlSpeed);
+//            controlSpeed=min((float)40.0,controlSpeed);
+//        }
+//        // 斜坡加大油门  end
+
+
+//        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+//                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+//                && abs(realSpeed)<10.0){
+//            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+//        }
+
+//    }
+
+    controlSpeed=dSpeed;
+    controlSpeed = min(dSpeed,5.0f);
+//    if(controlSpeed>0){
+//        controlSpeed=max(controlSpeed,3.2f);
+//    }
+    static bool emergency_brake=false;
+    //0227 10m nei xianzhi shache
+//    if(obsDistance<8 &&obsDistance>0){
+//        emergency_brake=true;
+//    }
+//    if(emergency_brake==true){
+//        controlSpeed=0;
+//        if(realSpeed<6)
+//            controlBrake=max(controlBrake,0.5f);//0.8   zj-0.6
+//        else
+//            controlBrake=max(controlBrake,0.6f);
+//        if(obsDistance>12 || (obsDistance==-1)){
+//            emergency_brake=false;
+//        }
+//    }
+    if(obsDistance>0&&obsDistance<=3)
+    {
+       controlSpeed=0;
+       controlBrake=max(controlBrake,0.6f);
+    }
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+
+    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;
+//        accAim=0;
+//    }
+//    if(obsDistance>60){
+//        if(accAim<-0.5){
+//            accAim=max(accAim,-0.5f);
+//        }
+//    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6
+    (*decition)->torque=controlSpeed;
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+
+
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(IsINterpolationOK()))
+    {
+        double ftorque,fbrake;
+        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
+        if(accAim>0){
+            ftorque=max(ftorque,3.2);
+        }
+        (*decition)->brake = fbrake;
+
+        (*decition)->torque= ftorque;
+    }
+//    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+//                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
+
+//    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);
+    lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->home_light=0;
+    (*decition)->roof_light=0;
+
+     static int64_t DoorTimeStart=-1;
+     static int32_t door=0;
+     if(ServiceCarStatus.has_mbdoor){
+         if(ServiceCarStatus.mbdoor){
+             door=2;
+             //(*decition)->door=2;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }else{
+             door=3;
+            // (*decition)->door=3;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }
+     }
+
+     if((QDateTime::currentSecsSinceEpoch()-DoorTimeStart)>10)
+             (*decition)->door=0;
+     else
+              (*decition)->door=door;
+
+
+     if(ServiceCarStatus.has_mbjinguang){
+         if(ServiceCarStatus.mbjinguang){
+             (*decition)->nearLight=1;
+         }else{
+             (*decition)->nearLight=0;
+         }
+     }
+
+//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 ;
+    return *decition;
+}
+
+
+
+float iv::decition::YuHeSenFR07proAdapter::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::YuHeSenFR07proAdapter::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_jsguide/decition/adc_adapter/yuhesenfr07pro_adapter.h

@@ -0,0 +1,43 @@
+#ifndef YUHESENFR07PRO_ADAPTER_H
+#define YUHESENFR07PRO_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 YuHeSenFR07proAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        YuHeSenFR07proAdapter();
+                        ~YuHeSenFR07proAdapter();
+
+                        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 // HAPO_ADAPTER_H

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1326,19 +1326,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         controlAng=0-controlAng;
     }
 
-    //2020-0106
-    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+    //2020-0106//对于到缆车没用
+//    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
 
-        if(vehState==avoiding){
-            ServiceCarStatus.msysparam.vehWidth=2.4;
-            controlAng=max(-150.0,controlAng);//35  zj-150
-            controlAng=min(150.0,controlAng);//35   zj-150
-        }
-        if(vehState==backOri){
-            controlAng=max(-150.0,controlAng);//35   zj-150
-            controlAng=min(150.0,controlAng);//35     zj-150
-        }
-    }
+//        if(vehState==avoiding){
+//            ServiceCarStatus.msysparam.vehWidth=2.4;
+//            controlAng=max(-150.0,controlAng);//35  zj-150
+//            controlAng=min(150.0,controlAng);//35   zj-150
+//        }
+//        if(vehState==backOri){
+//            controlAng=max(-150.0,controlAng);//35   zj-150
+//            controlAng=min(150.0,controlAng);//35     zj-150
+//        }
+//    }
 
 //    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
 //            vehState,controlAng);
@@ -1844,8 +1844,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             road_permit_avoid=true;
     }
 
-    //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
+    //shiyanbizhang 0929                                      //导缆车不判定OBs速度
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)//&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
 //        ObsTimeStart=GetTickCount();
@@ -2276,8 +2276,24 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(obsDistance>0 && obsDistance<8){
                 dSpeed=0;
             }
-    }else if(obsDistance>0 && obsDistance<15){
-        dSpeed=0;
+    }else if(obsDistance>0 && obsDistance<15)
+    {
+//        dSpeed=0;
+        if(ServiceCarStatus.msysparam.mvehtype=="yuhesenfr07pro")
+        {
+            if(obsDistance>ServiceCarStatus.socfDis)
+            {
+                ;
+            }
+            else if((obsDistance>ServiceCarStatus.socfDis-4) && obsDistance<ServiceCarStatus.socfDis)
+            {
+               dSpeed=min(2.0,dSpeed) ;
+            }
+            else
+            {
+                dSpeed=0.0;
+            }
+        }
     }
 
     if(ServiceCarStatus.group_control){
@@ -2337,6 +2353,7 @@ void iv::decition::DecideGps00::initMethods(){
     bus_Adapter = new BusAdapter();
     hapo_Adapter = new HapoAdapter();
     sightseeing_Adapter = new SightseeingAdapter();
+    yuhesenfr07pro_Adapter = new YuHeSenFR07proAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
@@ -2347,6 +2364,7 @@ void iv::decition::DecideGps00::initMethods(){
     mbus_Adapter.reset(bus_Adapter);
     mhapo_Adapter.reset(hapo_Adapter);
     msightseeing_Adapter.reset(sightseeing_Adapter);
+    myuhesenfr07pro_Adapter.reset(yuhesenfr07pro_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -2384,6 +2402,9 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     }else if(ServiceCarStatus.msysparam.mvehtype=="sightseeing"){
         sightseeing_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
+    else if(ServiceCarStatus.msysparam.mvehtype=="yuhesenfr07pro"){
+            yuhesenfr07pro_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+        }
 }
 
 

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.h

@@ -15,6 +15,7 @@
 #include <decition/adc_adapter/zhongche_adapter.h>
 #include <decition/adc_adapter/bus_adapter.h>
 #include <decition/adc_adapter/hapo_adapter.h>
+#include <decition/adc_adapter/yuhesenfr07pro_adapter.h>
 #include <decition/adc_adapter/sightseeing_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
@@ -152,6 +153,7 @@ public:
     BaseAdapter *bus_Adapter;
     BaseAdapter *hapo_Adapter;
     BaseAdapter *sightseeing_Adapter;
+    BaseAdapter *yuhesenfr07pro_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -161,6 +163,7 @@ public:
     std::shared_ptr<BaseAdapter> mbus_Adapter;
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
     std::shared_ptr<BaseAdapter> msightseeing_Adapter;
+    std::shared_ptr<BaseAdapter> myuhesenfr07pro_Adapter;
 
 	FrenetPlanner *frenetPlanner;
 //    BasePlanner *laneChangePlanner;

src/decition/decition_brain_sf_jsguide/decition/decition.pri

@@ -26,7 +26,8 @@ HEADERS += \
     $$PWD/fanyaapi.h \
     $$PWD/adc_adapter/hapo_adapter.h \
     $$PWD/adc_adapter/sightseeing_adapter.h \
-    $$PWD/adc_adapter/interpolation_2d.h
+    $$PWD/adc_adapter/interpolation_2d.h \
+    $$PWD/adc_adapter/yuhesenfr07pro_adapter.h
 
 SOURCES += \
     $$PWD/decide_gps_00.cpp \
@@ -53,4 +54,5 @@ SOURCES += \
     $$PWD/fanyaapi.cpp \
     $$PWD/adc_adapter/hapo_adapter.cpp \
     $$PWD/adc_adapter/sightseeing_adapter.cpp \
-    $$PWD/adc_adapter/interpolation_2d.cc
+    $$PWD/adc_adapter/interpolation_2d.cc \
+    $$PWD/adc_adapter/yuhesenfr07pro_adapter.cpp