add sightseeing adapter

src/decition/decition_brain_sf/decition/adc_adapter/sightseeing_adapter.cpp

@@ -0,0 +1,351 @@
+#include <decition/adc_adapter/sightseeing_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::SightseeingAdapter::SightseeingAdapter(){
+    adapter_id=6;
+    adapter_name="sightseeing";
+}
+iv::decition::SightseeingAdapter::~SightseeingAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::SightseeingAdapter::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
+        }
+
+    }
+
+    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(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;
+    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6
+    (*decition)->torque=controlSpeed;
+    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);
+
+//    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::SightseeingAdapter::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::SightseeingAdapter::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/decition/adc_adapter/sightseeing_adapter.h

@@ -0,0 +1,43 @@
+#ifndef SIGHTSEEING_ADAPTER_H
+#define SIGHTSEEING_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 SightseeingAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        SightseeingAdapter();
+                        ~SightseeingAdapter();
+
+                        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/decition/decide_gps_00.cpp

@@ -2046,6 +2046,7 @@ void iv::decition::DecideGps00::initMethods(){
     zhongche_Adapter = new ZhongcheAdapter();
     bus_Adapter = new BusAdapter();
     hapo_Adapter = new HapoAdapter();
+    sightseeing_Adapter = new SightseeingAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
@@ -2055,6 +2056,7 @@ void iv::decition::DecideGps00::initMethods(){
     mzhongche_Adapter.reset(zhongche_Adapter);
     mbus_Adapter.reset(bus_Adapter);
     mhapo_Adapter.reset(hapo_Adapter);
+    msightseeing_Adapter.reset(sightseeing_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -2081,6 +2083,8 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
         bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
         hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="sightseeing"){
+        sightseeing_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
 }
 

src/decition/decition_brain_sf/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/sightseeing_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
@@ -126,6 +127,7 @@ public:
     BaseAdapter *zhongche_Adapter;
     BaseAdapter *bus_Adapter;
     BaseAdapter *hapo_Adapter;
+    BaseAdapter *sightseeing_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -134,6 +136,7 @@ public:
     std::shared_ptr<BaseAdapter> mzhongche_Adapter;
     std::shared_ptr<BaseAdapter> mbus_Adapter;
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
+    std::shared_ptr<BaseAdapter> msightseeing_Adapter;
 
 	FrenetPlanner *frenetPlanner;
 //    BasePlanner *laneChangePlanner;

src/decition/decition_brain_sf/decition/decition.pri

@@ -24,7 +24,8 @@ HEADERS += \
     $$PWD/adc_tools/dubins.h \
     $$PWD/adc_adapter/bus_adapter.h \
     $$PWD/fanyaapi.h \
-    $$PWD/adc_adapter/hapo_adapter.h
+    $$PWD/adc_adapter/hapo_adapter.h \
+    $$PWD/adc_adapter/sightseeing_adapter.h
 
 SOURCES += \
     $$PWD/decide_gps_00.cpp \
@@ -49,4 +50,5 @@ SOURCES += \
     $$PWD/adc_tools/dubins.cpp \
     $$PWD/adc_adapter/bus_adapter.cpp \
     $$PWD/fanyaapi.cpp \
-    $$PWD/adc_adapter/hapo_adapter.cpp
+    $$PWD/adc_adapter/hapo_adapter.cpp \
+    $$PWD/adc_adapter/sightseeing_adapter.cpp