jun Sheng runlegs adapter

src/decition/decition_brain_sf/decition/adc_adapter/haomo_adapter.cpp

@@ -0,0 +1,243 @@
+#include <decition/adc_adapter/haomo_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::HaoMoAdapter::HaoMoAdapter(){
+    adapter_id=11;
+    adapter_name="haomo";
+}
+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
+
+//        controlSpeed= dSpeed;
+
+        // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
+
+//    }
+
+    controlSpeed= dSpeed;
+    controlSpeed=max(controlSpeed,10.0f); //对车子进行限速，车子最大速度是,倒车是5*3.6km/h，前进是10*3.6
+
+    //0227 10m nei xianzhi shache
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+/*     if(obsDistance<=15 &&obsDistance>10){
+        controlSpeed=max(8.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);
+    }
+    else if(obsDistance<=10 &&obsDistance>5){
+        controlSpeed=max(5.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);
+    }
+    else if(obsDistance<=5 &&obsDistance>3)
+    {
+        controlSpeed=1;
+        controlBrake=max(controlBrake,0.6f);
+    } */
+    //else
+    if(obsDistance<=3 &&obsDistance>0) //障碍物距离小直接停车
+    {
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.6f);
+    }
+
+    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/decition/adc_adapter/haomo_adapter.h

@@ -0,0 +1,40 @@
+#ifndef HAOMO_ADAPTER_H
+#define HAOMO_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 HaoMoAdapter: 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/decition/decide_gps_00.cpp

@@ -170,6 +170,15 @@ bool useOldAvoid = true;   //标志是否用老方法避障
 bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
 bool road_permit_avoid=false;  //true: 道路允许避障，false:道路不允许避障
 
+//数据存储功能 ,20210903,cxw
+bool file_cnt_add_en =false;  //用于提示是否需要将文件计数值增加
+bool file_cnt_add=false;
+bool map_start_point = true;
+bool first_start_en=true;  //自主巡迹数据存储用
+int  start_tracepoint;
+int  end_tracepoint;
+//
+
 std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,

src/decition/decition_brain_sf/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/haomo_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/haomo_adapter.cpp

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

@@ -0,0 +1,243 @@
+#include <decition/adc_adapter/haomo_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::HaoMoAdapter::HaoMoAdapter(){
+    adapter_id=11;
+    adapter_name="haomo";
+}
+iv::decition::HaoMoAdapter::~HaoMoAdapter(){
+
+}
+
+//hunter 只用控制转角和速度，速度大于0前进，速度小于0后退，驻车是在完全停车的时候再用
+
+iv::decition::Decition iv::decition::HaoMoAdapter::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
+
+//        controlSpeed= dSpeed;
+
+        // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
+
+//    }
+
+    controlSpeed= dSpeed;
+    controlSpeed=max(controlSpeed,10.0f); //对车子进行限速，车子最大速度是,倒车是5*3.6km/h，前进是10*3.6
+
+    //0227 10m nei xianzhi shache
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+/*     if(obsDistance<=15 &&obsDistance>10){
+        controlSpeed=max(8.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);
+    }
+    else if(obsDistance<=10 &&obsDistance>5){
+        controlSpeed=max(5.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);
+    }
+    else if(obsDistance<=5 &&obsDistance>3)
+    {
+        controlSpeed=1;
+        controlBrake=max(controlBrake,0.6f);
+    } */
+    //else
+    if(obsDistance<=3 &&obsDistance>0) //障碍物距离小直接停车
+    {
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.6f);
+    }
+
+    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::HaoMoAdapter::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::HaoMoAdapter::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_jsrunlegs/decition/adc_adapter/haomo_adapter.h

@@ -0,0 +1,40 @@
+#ifndef HAOMO_ADAPTER_H
+#define HAOMO_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 HaoMoAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        HaoMoAdapter();
+                        ~HaoMoAdapter();
+
+                        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