|
|
@@ -0,0 +1,5406 @@
|
|
|
+#include <decition/decide_gps_00.h>
|
|
|
+#include <decition/adc_tools/compute_00.h>
|
|
|
+#include <decition/adc_tools/gps_distance.h>
|
|
|
+#include <decition/decition_type.h>
|
|
|
+#include <decition/adc_tools/transfer.h>
|
|
|
+#include <decition/obs_predict.h>
|
|
|
+#include <common/constants.h>
|
|
|
+#include <common/car_status.h>
|
|
|
+#include <math.h>
|
|
|
+#include <iostream>
|
|
|
+#include <fstream>
|
|
|
+#include <control/can.h>
|
|
|
+#include <time.h>
|
|
|
+#include <decition/adc_controller/base_controller.h>
|
|
|
+#include <decition/adc_controller/pid_controller.h>
|
|
|
+#include <decition/adc_planner/lane_change_planner.h>
|
|
|
+#include <decition/adc_planner/frenet_planner.h>
|
|
|
+#include <QTime>
|
|
|
+#include <iomanip>
|
|
|
+
|
|
|
+using namespace std;
|
|
|
+
|
|
|
+
|
|
|
+#include "ivlog.h"
|
|
|
+extern iv::Ivlog * givlog;
|
|
|
+
|
|
|
+#define PI (3.1415926535897932384626433832795)
|
|
|
+#define AVOID_NEW 1
|
|
|
+iv::decition::DecideGps00::DecideGps00()
|
|
|
+{
|
|
|
+ std::cout<<" init gps00"<<std::endl;
|
|
|
+}
|
|
|
+iv::decition::DecideGps00::~DecideGps00()
|
|
|
+{
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::xiuzhengCs=0;
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::PathPoint = -1;
|
|
|
+double iv::decition::DecideGps00::minDis = iv::MaxValue;
|
|
|
+double iv::decition::DecideGps00::maxAngle = iv::MinValue;
|
|
|
+int iv::decition::DecideGps00::lastGpsIndex = 0;
|
|
|
+double iv::decition::DecideGps00::controlSpeed = 0;
|
|
|
+double iv::decition::DecideGps00::controlBrake = 0;
|
|
|
+double iv::decition::DecideGps00::controlAng = 0;
|
|
|
+double iv::decition::DecideGps00::Iv = 0;
|
|
|
+double iv::decition::DecideGps00::lastU = 0;
|
|
|
+double iv::decition::DecideGps00::lastVt = 0;
|
|
|
+double iv::decition::DecideGps00::lastEv = 0;
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::gpsLineParkIndex = 0;
|
|
|
+int iv::decition::DecideGps00::gpsMapParkIndex = 0;
|
|
|
+double iv::decition::DecideGps00::lastDistance = MaxValue;
|
|
|
+double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
|
|
|
+double iv::decition::DecideGps00::obsDistance = -1;
|
|
|
+double iv::decition::DecideGps00::obsSpeed=0;
|
|
|
+double iv::decition::DecideGps00::obsDistanceAvoid = -1;
|
|
|
+double iv::decition::DecideGps00::lastDistanceAvoid = -1;
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::lidarDistance = -1;
|
|
|
+double iv::decition::DecideGps00::myesrDistance = -1;
|
|
|
+double iv::decition::DecideGps00::lastLidarDis = -1;
|
|
|
+
|
|
|
+bool iv::decition::DecideGps00::parkMode = false;
|
|
|
+bool iv::decition::DecideGps00::readyParkMode = false;
|
|
|
+
|
|
|
+bool iv::decition::DecideGps00::zhucheMode = false;
|
|
|
+bool iv::decition::DecideGps00::readyZhucheMode = false;
|
|
|
+bool iv::decition::DecideGps00::hasZhuched = false;
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
|
|
|
+double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::finishPointNum = -1;
|
|
|
+int iv::decition::DecideGps00::zhuchePointNum = 0;
|
|
|
+
|
|
|
+///kkcai, 2020-01-03
|
|
|
+bool iv::decition::DecideGps00::isFirstRun = true;
|
|
|
+//////////////////////////////////////////////
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::minDecelerate=0;
|
|
|
+//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
|
|
|
+
|
|
|
+double offset_real = 0;
|
|
|
+double realspeed;
|
|
|
+double dSpeed;
|
|
|
+double dSecSpeed;
|
|
|
+double secSpeed;
|
|
|
+double ac;
|
|
|
+
|
|
|
+
|
|
|
+iv::Point2D obsPoint(-1, -1);
|
|
|
+iv::Point2D obsPointAvoid(-1, -1);
|
|
|
+
|
|
|
+
|
|
|
+int esrIndex = -1;
|
|
|
+int esrIndexAvoid = -1;
|
|
|
+double obsSpeedAvoid = 0;
|
|
|
+
|
|
|
+double esrDistance = -1;
|
|
|
+double esrDistanceAvoid = -1;
|
|
|
+int obsLostTime = 0;
|
|
|
+int obsLostTimeAvoid = 0;
|
|
|
+
|
|
|
+// double avoidTime = 0;
|
|
|
+
|
|
|
+double avoidXNewPre=0,avoidXNewPreFilter=0;//20230213,障碍物检测精度由1米换成0.5米
|
|
|
+//vector<int> avoidXNewPreVector;
|
|
|
+//int avoidXNew=0;
|
|
|
+//int avoidXNewLast=0;
|
|
|
+vector<double> avoidXNewPreVector;
|
|
|
+double avoidXNew=0;
|
|
|
+double avoidXNewLast=0; //20230213,障碍物检测精度由1米换成0.5米
|
|
|
+double avoidX =0;
|
|
|
+int turnLampFlag=0; // if <0:left , if >0:right
|
|
|
+float roadWidth = 3.5;
|
|
|
+int roadSum =10;
|
|
|
+int roadNow = 0;
|
|
|
+int roadOri =0;
|
|
|
+int roadPre = -1;
|
|
|
+int lastRoadOri = 0;
|
|
|
+
|
|
|
+int lightTimes = 0;
|
|
|
+
|
|
|
+
|
|
|
+int lastRoadNum=1;
|
|
|
+
|
|
|
+int stopCount = 0;
|
|
|
+
|
|
|
+int gpsMissCount = 0;
|
|
|
+
|
|
|
+bool rapidStop = false;
|
|
|
+
|
|
|
+bool hasTrumpeted = false;
|
|
|
+
|
|
|
+
|
|
|
+bool changeRoad=false;
|
|
|
+//实验手刹
|
|
|
+bool handFirst = true;
|
|
|
+double handTimeSpan = 0;
|
|
|
+double handStartTime = 0;
|
|
|
+double handLastTime = 0;
|
|
|
+bool handPark = false;
|
|
|
+long handParkTime=10000;
|
|
|
+
|
|
|
+//喇叭
|
|
|
+bool trumpetFirstCount = true;
|
|
|
+double trumpetTimeSpan = 0;
|
|
|
+double trumpetStartTime = 0;
|
|
|
+double trumpetLastTime = 0;
|
|
|
+
|
|
|
+//过渡
|
|
|
+bool transferFirstCount = true;
|
|
|
+double transferTimeSpan = 0;
|
|
|
+double transferStartTime = 0;
|
|
|
+double transferLastTime = 0;
|
|
|
+
|
|
|
+bool busMode = false;
|
|
|
+bool parkBesideRoad = false;
|
|
|
+
|
|
|
+bool chaocheMode = false;
|
|
|
+bool specialSignle = false;
|
|
|
+
|
|
|
+double offsetX = 0;
|
|
|
+
|
|
|
+double steerSpeed=9000;
|
|
|
+
|
|
|
+bool transferPieriod;
|
|
|
+bool transferPieriod2;
|
|
|
+double traceDevi;
|
|
|
+
|
|
|
+bool startingFlag = true; //起步标志，在起步时需要进行frenet规划。
|
|
|
+bool useFrenet = false; //标志是否启用frenet算法避障
|
|
|
+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,
|
|
|
+ waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
|
|
|
+ dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
|
|
|
+std::vector<iv::Point2D> gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
|
|
|
+
|
|
|
+std::vector<double> esrDisVector(roadSum, -1);
|
|
|
+std::vector<double> lidarDisVector(roadSum, -1);
|
|
|
+std::vector<double> obsDisVector(roadSum,-1);
|
|
|
+std::vector<double> obsSpeedVector(roadSum, 0);
|
|
|
+
|
|
|
+std::vector<int> obsLostTimeVector(roadSum, 0);
|
|
|
+
|
|
|
+std::vector<double> lastLidarDisVector(roadSum, -1);
|
|
|
+std::vector<double> lastDistanceVector(roadSum, -1);
|
|
|
+
|
|
|
+std::vector<iv::Point2D> traceOriLeft,traceOriRight;
|
|
|
+
|
|
|
+bool qiedianCount = false;
|
|
|
+
|
|
|
+static int obstacle_avoid_flag=0;
|
|
|
+static int front_car_id=-1;
|
|
|
+static int front_car_vector_id=-1;
|
|
|
+static bool final_brake_lock=false,brake_mode=false;
|
|
|
+std::vector<iv::Point2D> obsSpline;
|
|
|
+//日常展示
|
|
|
+
|
|
|
+#include <QDateTime>
|
|
|
+
|
|
|
+iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
|
|
|
+ const std::vector<GPSData> gpsMapLine,
|
|
|
+ iv::LidarGridPtr lidarGridPtr,
|
|
|
+ std::vector<iv::Perception::PerceptionOutput> lidar_per,
|
|
|
+ iv::TrafficLight trafficLight)
|
|
|
+{
|
|
|
+ Decition gps_decition(new DecitionBasic);
|
|
|
+
|
|
|
+ bool bgroupgrpc = false;
|
|
|
+ qint64 ngrpcvalid = 3000;
|
|
|
+ iv::group::groupinfo xgroupgrpcinfo;
|
|
|
+ if((QDateTime::currentMSecsSinceEpoch() - ServiceCarStatus.mgroupgrpcupdatetime ) < ngrpcvalid)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mMutexgroupgrpc.lock();
|
|
|
+ xgroupgrpcinfo.CopyFrom(ServiceCarStatus.mgroupgrpcinfo);
|
|
|
+ ServiceCarStatus.mMutexgroupgrpc.unlock();
|
|
|
+ bgroupgrpc = true;
|
|
|
+ }
|
|
|
+ if((bgroupgrpc==true)&&(ServiceCarStatus.curID>1)){
|
|
|
+ for(int i=0;i<xgroupgrpcinfo.mvehinfo_size();i++){
|
|
|
+ if((xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid()+1)==ServiceCarStatus.curID){
|
|
|
+ front_car_id=xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid();
|
|
|
+ front_car_vector_id=i;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ front_car_id=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front_car_id>0){
|
|
|
+ if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mgpsimu()){
|
|
|
+ front_car.front_car_ins.gps_lat=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lat();
|
|
|
+ front_car.front_car_ins.gps_lng=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lon();
|
|
|
+ front_car.front_car_ins.ins_heading_angle=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().heading();
|
|
|
+ front_car.front_car_ins.speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().speed();
|
|
|
+ GaussProjCal(front_car.front_car_ins.gps_lng,front_car.front_car_ins.gps_lat, &front_car.front_car_ins.gps_x, &front_car.front_car_ins.gps_y);
|
|
|
+ front_car.front_car_dis=GetDistance(now_gps_ins,front_car.front_car_ins);
|
|
|
+ }else{
|
|
|
+ front_car.front_car_dis=500;
|
|
|
+ }
|
|
|
+ if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mcarstate()){
|
|
|
+ front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mstate();
|
|
|
+ int avoidX_record=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mavoidx();
|
|
|
+ if(front_car.vehState!=0&&avoidX_record!=0)
|
|
|
+ front_car.avoidX=avoidX_record;
|
|
|
+ else
|
|
|
+ front_car.avoidX=0;
|
|
|
+ front_car.obs_distance=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsdistance();
|
|
|
+ front_car.obs_speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsspeed();
|
|
|
+ }
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ givlog->debug("decition_brain","front_car_id: %d,FrontState: %d,FrontAvoidX: %d,FrontDis: %f,FrontObs: %f",
|
|
|
+ front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(front_car_id>0){
|
|
|
+ if(front_car.front_car_dis>35){
|
|
|
+ front_car_decide_avoid=true;
|
|
|
+ }else if((front_car.front_car_dis<=35)&&(front_car.avoidX==0)){
|
|
|
+ front_car_decide_avoid=false;
|
|
|
+ }else if((front_car.front_car_dis<=35)&&(front_car.avoidX!=0)){
|
|
|
+ front_car_decide_avoid=true;
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ front_car_decide_avoid=true;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ static int file_num;//log文件存储计数
|
|
|
+ if (isFirstRun)
|
|
|
+ {
|
|
|
+ file_num=0;
|
|
|
+ initMethods();
|
|
|
+ vehState = normalRun;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ init();
|
|
|
+ PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
|
|
|
+ DecideGps00::lastGpsIndex,
|
|
|
+ DecideGps00::minDis,
|
|
|
+ DecideGps00::maxAngle);
|
|
|
+ DecideGps00::lastGpsIndex = PathPoint;
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.speed_control==true){
|
|
|
+ Compute00().getPlanSpeed(gpsMapLine);
|
|
|
+ }
|
|
|
+
|
|
|
+ double dis = GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
|
|
|
+ if(dis<1){
|
|
|
+ circleMode=true;
|
|
|
+ }else{
|
|
|
+ circleMode=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ getV2XTrafficPositionVector(gpsMapLine);
|
|
|
+
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ ServiceCarStatus.daocheMode=false;
|
|
|
+ parkMode=false;
|
|
|
+ readyParkMode=false;
|
|
|
+ finishPointNum=-1;
|
|
|
+ roadNowStart=true;
|
|
|
+ isFirstRun = false;
|
|
|
+ obstacle_avoid_flag=0;
|
|
|
+ avoidXNew=0;
|
|
|
+ avoidXNewLast=0;
|
|
|
+ avoidXNewPre=0;
|
|
|
+ avoidXNewPreFilter=0;
|
|
|
+ gpsMapLine[gpsMapLine.size()-1]->frenet_s=0;
|
|
|
+
|
|
|
+ if((ServiceCarStatus.txt_log_on==true)&&(ServiceCarStatus.msysparam.mvehtype=="hapo"))
|
|
|
+ {
|
|
|
+
|
|
|
+ givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",0,0);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.mvehState=vehState;
|
|
|
+ ServiceCarStatus.mavoidX=avoidXNew;
|
|
|
+
|
|
|
+ changingDangWei=false;
|
|
|
+ minDecelerate=0;
|
|
|
+
|
|
|
+ busMode = ServiceCarStatus.busmode;
|
|
|
+ nearStation=false;
|
|
|
+
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+
|
|
|
+ xiuzhengCs=0;
|
|
|
+ is_arrivaled = false;
|
|
|
+
|
|
|
+ realspeed = now_gps_ins.speed;//km/h
|
|
|
+ secSpeed = realspeed / 3.6;//m/s
|
|
|
+
|
|
|
+
|
|
|
+ if(front_car_decide_avoid == false) //geely logic yizhi
|
|
|
+ {
|
|
|
+ obstacle_avoid_flag = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
|
|
|
+ GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
|
|
|
+ now_gps_ins.gps_x=gpsOffset.gps_x;
|
|
|
+ now_gps_ins.gps_y=gpsOffset.gps_y;
|
|
|
+ GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90)
|
|
|
+ {
|
|
|
+ gps_decition->wheel_angle = 0.2;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+ gps_decition->accelerator = -0.5;
|
|
|
+ gps_decition->torque = 0.0;
|
|
|
+// gps_decition->brake=10;
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
|
|
|
+ {
|
|
|
+ gps_decition->brake=-10;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gps_decition->brake=10;
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","gps_lat is error,f%",now_gps_ins.gps_lat); //20230315
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.daocheMode){
|
|
|
+
|
|
|
+ now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
|
|
|
+ if( now_gps_ins.ins_heading_angle>=360)
|
|
|
+ now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
|
|
|
+ }
|
|
|
+ // qDebug("heading is %g",now_gps_ins.ins_heading_angle);
|
|
|
+
|
|
|
+/****************************self park logic begin************************************************/
|
|
|
+ aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
|
|
|
+ aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
|
|
|
+ aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+
|
|
|
+ //sidePark
|
|
|
+ if(ServiceCarStatus.mnParkType==1)
|
|
|
+ {
|
|
|
+ if( vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 && vehState!=dRever2
|
|
|
+ && vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr )
|
|
|
+ {
|
|
|
+ int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ if(bocheEN==1){
|
|
|
+ ServiceCarStatus.bocheEnable=1;
|
|
|
+ }else if(bocheEN==0){
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheEnable=2;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.bocheMode && vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 && vehState!=dRever2
|
|
|
+ && vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr )
|
|
|
+ {
|
|
|
+ if(abs(realspeed)>0.1)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ if(trumpet()>3000)
|
|
|
+ {
|
|
|
+ trumpetFirstCount=true;
|
|
|
+ vehState=dRever;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == dRever)
|
|
|
+ {
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ vehState = dRever0;
|
|
|
+ qiedianCount=true;
|
|
|
+ std::cout<<"enter side boche mode"<<std::endl;
|
|
|
+ }
|
|
|
+ if (vehState == dRever0)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
|
|
|
+ if (dis<2.0)
|
|
|
+ {
|
|
|
+ vehState = dRever1;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ controlAng = 0;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == dRever1)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
|
|
|
+ if(dis<2.0)
|
|
|
+ {
|
|
|
+ vehState = dRever2;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ controlAng = Compute00().dBocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng;
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever2)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
|
|
|
+ Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+ Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
|
|
|
+ double xx= (pt1.x-pt2.x);
|
|
|
+ // if(xx>0)
|
|
|
+ if(xx>-0.5)
|
|
|
+ {
|
|
|
+ GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+ double xxx=ptt.x;
|
|
|
+ double yyy =ptt.y;
|
|
|
+ vehState = dRever3;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
|
|
|
+ cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+ /* gps_decition->brake = 10;
|
|
|
+ gps_decition->torque = 0; */
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+
|
|
|
+ gps_decition->wheel_angle = 0 ;
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState == dRever3)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
|
|
|
+ double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
|
|
|
+ if((((angdis<5)||(angdis>355)))&&(dis<10.0))
|
|
|
+ {
|
|
|
+ vehState = dRever4;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+ controlAng = 0-Compute00().dBocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng*0.95;
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == dRever4)
|
|
|
+ {
|
|
|
+ // double dis = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+
|
|
|
+ if ((pt.y)<0.5)
|
|
|
+ {
|
|
|
+ qiedianCount=true;
|
|
|
+ vehState=reverseArr;
|
|
|
+ cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ // gps_decition->accelerator = -3;
|
|
|
+ // gps_decition->brake =10 ;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //if (qiedianCount && trumpet()<0)
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ controlAng = 0;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //chuizhiPark
|
|
|
+ if(ServiceCarStatus.mnParkType==0)
|
|
|
+ {
|
|
|
+ if( vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle && vehState!=reverseDirect && vehState!=reverseArr )
|
|
|
+ {
|
|
|
+ int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ if(bocheEN==1)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheEnable=1;
|
|
|
+ }
|
|
|
+ else if(bocheEN==0)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheEnable=2;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.bocheMode && vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle
|
|
|
+ && vehState!=reverseDirect&& vehState!=reverseArr )
|
|
|
+ {
|
|
|
+ if(abs(realspeed)>0.1)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(trumpet()>3000)
|
|
|
+ {
|
|
|
+ trumpetFirstCount=true;
|
|
|
+ vehState=reverseCar;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == reverseCar)
|
|
|
+ {
|
|
|
+ Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
|
|
|
+ vehState = reversing;
|
|
|
+ qiedianCount=true;
|
|
|
+ }
|
|
|
+ if (vehState == reversing)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
|
|
|
+ iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
|
|
|
+ iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
|
|
|
+
|
|
|
+ double fdistonear = fabs(pt.x - ptnear.x);
|
|
|
+ if(fdistonear<0.5) //将吉利项目中的停车逻辑移植过来
|
|
|
+ // if (dis<2.0)//0.5
|
|
|
+ {
|
|
|
+ vehState = reverseCircle;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ controlAng = 0;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == reverseCircle)
|
|
|
+ {
|
|
|
+ double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
|
|
|
+ double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
|
|
|
+ if((((angdis<5)||(angdis>355)))&&(dis<3.0))
|
|
|
+ // if((((angdis<4)||(angdis>356)))&&(dis<2.0))
|
|
|
+ {
|
|
|
+ vehState = reverseDirect;
|
|
|
+ qiedianCount = true;
|
|
|
+ cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (qiedianCount && trumpet()<0)
|
|
|
+ // if (qiedianCount && trumpet()<1500)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ controlAng = Compute00().bocheAngle*16.5;
|
|
|
+ gps_decition->wheel_angle = 0 - controlAng*1.05;
|
|
|
+
|
|
|
+ cout<<"farTpointDistance================"<<dis<<endl;
|
|
|
+
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState == reverseDirect)
|
|
|
+ {
|
|
|
+ // double dis = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+
|
|
|
+ if ((pt.y)<0.5)
|
|
|
+ {
|
|
|
+
|
|
|
+ qiedianCount=true;
|
|
|
+ vehState=reverseArr;
|
|
|
+ cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ // gps_decition->accelerator = -3;
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ gps_decition->wheel_angle=0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+
|
|
|
+ //if (qiedianCount && trumpet()<0)
|
|
|
+ if (qiedianCount && trumpet()<1000)
|
|
|
+ {
|
|
|
+
|
|
|
+ // gps_decition->brake = 10;
|
|
|
+ // gps_decition->torque = 0;
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ dSpeed = 1;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ }
|
|
|
+ controlAng = 0;
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.mnParkType==2)//guosai pingtai yanzheng 直接倒车
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheEnable=1;
|
|
|
+ if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
|
|
|
+ {
|
|
|
+ vehState = dReverTZD;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(vehState == dReverTZD)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
|
|
|
+ if(pt.y<0.5)
|
|
|
+ {
|
|
|
+ if(abs(realspeed)>0.1)
|
|
|
+ {
|
|
|
+ vehState = reverseArr;
|
|
|
+ }
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gps_decition->wheel_angle = 0;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ obsDistance=-1;
|
|
|
+ dSpeed = 2;
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ // gps_decition->accelerator = 0;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if ((vehState == reverseArr)&&((ServiceCarStatus.mnParkType==1)||(ServiceCarStatus.mnParkType==0)
|
|
|
+ ||(ServiceCarStatus.mnParkType==2))) //不论哪种泊车最后都会到这个状态
|
|
|
+ {
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ if (qiedianCount && trumpet()<1500)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ minDecelerate=min(minDecelerate,-0.5f);
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
|
|
|
+ ServiceCarStatus.bocheMode=false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ qiedianCount = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ ServiceCarStatus.bocheEnable=0;
|
|
|
+ vehState=normalRun;
|
|
|
+ ServiceCarStatus.mbRunPause=true;
|
|
|
+ ServiceCarStatus.mnBocheComplete=100;
|
|
|
+ cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ gps_decition->wheel_angle = 0 ;
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+/****************************self park logic end************************************************/
|
|
|
+
|
|
|
+ obsDistance = -1;
|
|
|
+ esrIndex = -1;
|
|
|
+ lidarDistance = -1;
|
|
|
+
|
|
|
+ obsDistanceAvoid = -1;
|
|
|
+ esrIndexAvoid = -1;
|
|
|
+ roadPre = -1;
|
|
|
+ avoidXNewPre=0;
|
|
|
+ //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;
|
|
|
+ int nmapsize = gpsMapLine.size();
|
|
|
+
|
|
|
+ gpsTraceOri.clear();
|
|
|
+ gpsTraceNow.clear();
|
|
|
+ gpsTraceAim.clear();
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ gpsTraceBack.clear();
|
|
|
+ gpsTraceNowLeft.clear();
|
|
|
+ gpsTraceNowRight.clear();
|
|
|
+
|
|
|
+ dSpeed =80;
|
|
|
+
|
|
|
+ if (!isFirstRun)
|
|
|
+ {
|
|
|
+ PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
|
|
|
+ if(PathPoint<0)
|
|
|
+ {
|
|
|
+ PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (PathPoint<0)
|
|
|
+ {
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
|
|
|
+
|
|
|
+ givlog->debug("decition_brain","PathPoint is smaller than 0, it is : %d",PathPoint);
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+ DecideGps00::lastGpsIndex = PathPoint;
|
|
|
+
|
|
|
+ if(!ServiceCarStatus.inRoadAvoid)
|
|
|
+ {
|
|
|
+ roadOri = gpsMapLine[PathPoint]->roadOri;
|
|
|
+ roadSum = gpsMapLine[PathPoint]->roadSum;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
|
|
|
+ roadSum = gpsMapLine[PathPoint]->roadSum*3;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadNowStart)
|
|
|
+ {
|
|
|
+ roadNow=roadOri;
|
|
|
+ roadNowStart=false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(ServiceCarStatus.avoidObs)
|
|
|
+ {
|
|
|
+ gpsMapLine[PathPoint]->runMode=1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gpsMapLine[PathPoint]->runMode=0;
|
|
|
+ }
|
|
|
+
|
|
|
+#ifdef AVOID_NEW
|
|
|
+ if(obstacle_avoid_flag==1)
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ avoidXNew=0;
|
|
|
+ roadNow = roadOri;
|
|
|
+ if(vehState==backOri)
|
|
|
+ {
|
|
|
+ vehState=normalRun;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","avoidXNew: %f",avoidXNew);
|
|
|
+#else
|
|
|
+ if(obstacle_avoid_flag==1)
|
|
|
+ {
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ avoidX=0;
|
|
|
+ roadNow = roadOri;
|
|
|
+ if(vehState==backOri)
|
|
|
+ {
|
|
|
+ vehState=normalRun;
|
|
|
+ }
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+ if ( gpsMapLine[PathPoint]->runMode == 0 && (vehState == avoidObs || vehState == stopObs || vehState == preAvoid
|
|
|
+ || vehState == avoiding || vehState == backOri || vehState ==preBack || vehState ==waitAvoid ) )
|
|
|
+ {
|
|
|
+ vehState = normalRun;
|
|
|
+ roadNow = roadOri;
|
|
|
+ }
|
|
|
+
|
|
|
+ gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
|
|
|
+ gpsTraceMapOri=gpsTraceOri;
|
|
|
+
|
|
|
+ if((vehState == avoiding)&&(obstacle_avoid_flag == 1))
|
|
|
+ {
|
|
|
+ iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
|
|
|
+ if((now_s_d.s+20>(gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s))&&((gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s)<((*gpsMapLine[gpsMapLine.size()-1]).frenet_s)))
|
|
|
+ {
|
|
|
+ double sPathFinal=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;
|
|
|
+ if(sPathFinal+20<=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s)
|
|
|
+ {
|
|
|
+ sPathFinal=sPathFinal+20;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ sPathFinal=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;
|
|
|
+ }
|
|
|
+ for(double s=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;s<=sPathFinal;s+=0.1)
|
|
|
+ {
|
|
|
+ iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s,-avoidXNew,PathPoint);
|
|
|
+ gpsTraceAvoidXY.push_back(gpsTracePoint);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if((vehState == avoiding || vehState == backOri) && (gpsTraceAvoidXY.size()>0) && (obstacle_avoid_flag==1))
|
|
|
+ {
|
|
|
+ gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
|
|
|
+ }
|
|
|
+
|
|
|
+ if(gpsTraceOri.empty())
|
|
|
+ {
|
|
|
+ gps_decition->wheel_angle = 0.2;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+ gps_decition->accelerator = -0.5;
|
|
|
+ gps_decition->torque = 0.0;
|
|
|
+ //gps_decition->brake=10; //
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
|
|
|
+ {
|
|
|
+ gps_decition->brake=-20;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gps_decition->brake=10;
|
|
|
+ }
|
|
|
+ givlog->warn("decition_brain","gpsTraceOri is empty");
|
|
|
+ return gps_decition;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+// traceDevi=gpsTraceOri[0].x;
|
|
|
+
|
|
|
+ //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
|
|
|
+ //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
|
|
|
+ // if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
|
|
|
+ // startingFlag = false;
|
|
|
+ // }
|
|
|
+// startingFlag = false;
|
|
|
+// if(startingFlag)
|
|
|
+// {
|
|
|
+// // gpsTraceAim = gpsTraceOri;
|
|
|
+// if(abs(gpsTraceOri[0].x)>1)
|
|
|
+// {
|
|
|
+// cout<<"frenetPlanner->getPath:pre"<<endl;
|
|
|
+// gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
|
|
|
+// cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
|
|
|
+// if(gpsTraceNow.size()==0)
|
|
|
+// {
|
|
|
+// gps_decition->accelerator = 0;
|
|
|
+// gps_decition->brake=10;
|
|
|
+// gps_decition->wheel_angle = lastAngle;
|
|
|
+// gps_decition->speed = 0;
|
|
|
+// return gps_decition;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// startingFlag = false;
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+// if(useFrenet){
|
|
|
+// //如果车辆在变道中，启用frenet规划局部路径
|
|
|
+// if(vehState == avoiding || vehState == backOri)
|
|
|
+// {
|
|
|
+// // avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+// avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
|
|
|
+// gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+// //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+// gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+// if(gpsTraceNow.size()==0)
|
|
|
+// {
|
|
|
+// gps_decition->accelerator = 0;
|
|
|
+// gps_decition->brake=10;
|
|
|
+// gps_decition->wheel_angle = lastAngle;
|
|
|
+// gps_decition->speed = 0;
|
|
|
+// return gps_decition;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+#ifdef AVOID_NEW
|
|
|
+ if(gpsTraceNow.size()==0)
|
|
|
+ {
|
|
|
+ if (avoidXNew==0.0)
|
|
|
+ {
|
|
|
+ if((vehState == backOri)||(vehState == avoiding))
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
|
|
|
+ gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if((vehState == avoiding)||(vehState == backOri))
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);//luojixuyao xiugai
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+#else
|
|
|
+ if(gpsTraceNow.size()==0){
|
|
|
+ if (roadNow==roadOri)
|
|
|
+ {
|
|
|
+ if(vehState == backOri)
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
|
|
|
+ gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
|
|
|
+ }
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ if((vehState == avoiding)||(vehState == backOri))
|
|
|
+ {
|
|
|
+ gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
|
|
|
+ // gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
|
|
|
+ // gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }else{
|
|
|
+ gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ // gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
|
|
|
+ // gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
|
|
|
+ getGpsTraceNowLeftRight(gpsTraceNow);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(vehState==normalRun)
|
|
|
+ {
|
|
|
+ if(gpsTraceNow.size()>200)
|
|
|
+ {
|
|
|
+ if(gpsTraceNow[200].x<-3)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if(gpsTraceNow[200].x>3)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // dSpeed = getSpeed(gpsTraceNow);
|
|
|
+
|
|
|
+
|
|
|
+ planTrace.clear();//Add By YuChuli, 2020.11.26
|
|
|
+ for(int i=0;i<gpsTraceNow.size();i++){
|
|
|
+ TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
|
|
|
+ planTrace.push_back(pt);
|
|
|
+ }
|
|
|
+
|
|
|
+ controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
|
|
|
+
|
|
|
+ /****************************terminal point stop logic begin************************************************/
|
|
|
+ //转角
|
|
|
+ if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
|
|
|
+ {
|
|
|
+ if(!circleMode && PathPoint>(gpsMapLine.size()-20))
|
|
|
+ {
|
|
|
+ controlAng=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(!circleMode && PathPoint>(gpsMapLine.size()-250))
|
|
|
+ {
|
|
|
+ if(realspeed<0.5)
|
|
|
+ controlAng=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //加速度
|
|
|
+ double acc_end = 0.1;
|
|
|
+ static double distoend=1000;
|
|
|
+
|
|
|
+ int useaccend = 1;
|
|
|
+ double mstopbrake=0.6;
|
|
|
+
|
|
|
+ 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<100){
|
|
|
+ if(distoend<3.0)
|
|
|
+ {
|
|
|
+ std::cout<<"distoend: "<<distoend<<std::endl;
|
|
|
+ }
|
|
|
+ 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 <= 0.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
|
|
|
+
|
|
|
+
|
|
|
+ if(acc_end<0)minDecelerate = acc_end;
|
|
|
+
|
|
|
+ givlog->debug("decition_brain","acc_end: %f",acc_end);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
|
|
|
+ // minDecelerate=-0.5;
|
|
|
+ // std::cout<<"map finish brake"<<std::endl;
|
|
|
+ // }
|
|
|
+ if(!circleMode){
|
|
|
+ double forecast_final=secSpeed*secSpeed+5;
|
|
|
+ int forecast_final_point=((int)forecast_final)*10+1500;
|
|
|
+ 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);
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ givlog->debug("decition_brain","distoend: %f",distance_to_end);
|
|
|
+ }
|
|
|
+ if((forecast_final>=distance_to_end)&&(realspeed>3))
|
|
|
+ {
|
|
|
+ final_brake=true;
|
|
|
+ if(BrakePoint==-1)
|
|
|
+ BrakePoint=PathPoint-10;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ distance_to_end=1000;
|
|
|
+ }
|
|
|
+ if(PathPoint<BrakePoint)
|
|
|
+ {
|
|
|
+ final_brake=false;
|
|
|
+ final_brake_lock=false;
|
|
|
+ brake_mode=false;
|
|
|
+ BrakePoint=-1;
|
|
|
+ enterTofinal=false; //20230417,shenlan
|
|
|
+ enterTofinal_speed=200;
|
|
|
+ }
|
|
|
+ if(final_brake==true)
|
|
|
+ {
|
|
|
+ if((realspeed>3)&&(final_brake_lock==false))
|
|
|
+ {
|
|
|
+ minDecelerate=-0.7;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ 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(ServiceCarStatus.msysparam.mvehtype=="shenlan")
|
|
|
+ {
|
|
|
+ if(distance_to_end<2.0)
|
|
|
+ {
|
|
|
+ minDecelerate=-0.7;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(distance_to_end<0.8)
|
|
|
+ {
|
|
|
+ minDecelerate=-0.7;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(brake_mode==true){
|
|
|
+ dSpeed=min(dSpeed, 3.0);
|
|
|
+ }
|
|
|
+ /****************************terminal point stop logic end************************************************/
|
|
|
+ //group map end park pro--------start
|
|
|
+ if(front_car_id>0){
|
|
|
+ static bool final_lock=false;
|
|
|
+ if((distoend<90)&&(front_car.front_car_dis<obsDistance+10)){
|
|
|
+ if((obsDistance>3)&&(obsDistance<20)){
|
|
|
+ if((realspeed>3)&&(final_lock==false)){
|
|
|
+ minDecelerate=-0.7;
|
|
|
+ }else{
|
|
|
+ dSpeed = min(3.0,dSpeed);
|
|
|
+ final_lock=true;
|
|
|
+ }
|
|
|
+ obsDistance=200;
|
|
|
+ }else if((obsDistance>1)&&(obsDistance<3)){
|
|
|
+ minDecelerate=-0.5;
|
|
|
+ obsDistance=200;
|
|
|
+ }else if(obsDistance<1){
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }
|
|
|
+ if(realspeed<1){
|
|
|
+ final_lock=false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //group map end park pro-----------end
|
|
|
+
|
|
|
+ if(front_car_id>0)
|
|
|
+ {
|
|
|
+ static bool brake_state=false;
|
|
|
+ if(((front_car.vehState!=0)||(front_car.avoidX!=0))&&(front_car.front_car_dis<15))
|
|
|
+ {
|
|
|
+ brake_state=true;
|
|
|
+ }
|
|
|
+ if(brake_state==true)
|
|
|
+ {
|
|
|
+ dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
|
|
|
+ }
|
|
|
+ if(((front_car.vehState==0)&&(front_car.avoidX==0))||(front_car.front_car_dis>25))
|
|
|
+ {
|
|
|
+ brake_state=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //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
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+// givlog->debug("brain_decition","vehState: %d,controlAng: %f",vehState,controlAng);
|
|
|
+
|
|
|
+ if(ServiceCarStatus.speed_control==true)
|
|
|
+ {
|
|
|
+ dSpeed = min(doubledata[PathPoint][4],dSpeed);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (gpsMapLine[PathPoint]->roadMode ==0)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 5)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 11)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 14)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 15)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 16)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 17)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 18)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 1)
|
|
|
+ {
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 2)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 7)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ xiuzhengCs=1.5;
|
|
|
+ }
|
|
|
+ else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
|
|
|
+ {
|
|
|
+ dSpeed = min(4.0,dSpeed);
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (gpsMapLine[PathPoint]->speed_mode == 2)
|
|
|
+ {
|
|
|
+ dSpeed = min(25.0,dSpeed);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if((gpsMapLine[PathPoint]->speed)>0.001)
|
|
|
+ {
|
|
|
+ dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
|
|
|
+ // if((gpsMapLine[PathPoint]->speed)>4.5)
|
|
|
+ // {
|
|
|
+ // dSpeed =gpsMapLine[PathPoint]->speed*3.6;
|
|
|
+ // }
|
|
|
+ }
|
|
|
+
|
|
|
+ //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
|
|
|
+
|
|
|
+#ifdef AVOID_NEW //20220223toggle
|
|
|
+ // if((vehState==avoiding)||(vehState==preAvoid))
|
|
|
+ // {
|
|
|
+ // dSpeed = min(8.0,dSpeed);
|
|
|
+ // }else if((vehState==backOri)||(avoidXNew!=0)){
|
|
|
+ // dSpeed = min(12.0,dSpeed);
|
|
|
+ // }
|
|
|
+#else
|
|
|
+ if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
|
|
|
+ {
|
|
|
+ dSpeed = min(8.0,dSpeed);
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = limitSpeed(controlAng, dSpeed);
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ // givlog->debug("brain_decition_speed","dspeed: %f",
|
|
|
+ // dSpeed);
|
|
|
+
|
|
|
+ if(vehState==changingRoad)
|
|
|
+ {
|
|
|
+ if(gpsTraceNow[0].x>1.0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if(gpsTraceNow[0].x<-1.0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ vehState=normalRun;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
|
|
|
+ static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0; //obs_speed_for_avoid: obstacle actual speed in km/h
|
|
|
+ if(!ServiceCarStatus.daocheMode)
|
|
|
+ {
|
|
|
+ //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
|
|
|
+ if(obs_filter_flag==0)
|
|
|
+ {
|
|
|
+ if(obsDistance>0&&obsDistance<60)
|
|
|
+ {
|
|
|
+ obs_filter++;
|
|
|
+ if(obs_filter<20) //80
|
|
|
+ {
|
|
|
+ obsDistance=-1;
|
|
|
+ obsSpeed=0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs_filter_flag=1;
|
|
|
+ obs_filter_dis_memory=obsDistance;
|
|
|
+ obs_filter_speed_memory=obsSpeed;
|
|
|
+ obs_filter=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs_filter=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(obsDistance<0||obsDistance>60)
|
|
|
+ {
|
|
|
+ obs_filter++;
|
|
|
+ if(obs_filter<20) //80
|
|
|
+ {
|
|
|
+ obsDistance=obs_filter_dis_memory;
|
|
|
+ obsSpeed=obs_filter_speed_memory;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs_filter_flag=0;
|
|
|
+ obs_filter=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs_filter=0;
|
|
|
+ obs_filter_dis_memory=obsDistance;
|
|
|
+ obs_filter_speed_memory=obsSpeed;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
|
|
|
+
|
|
|
+ givlog->debug("decition_brain","obs_filter: %d,obs_filter_flag: %d,obs_filter_dis_memory: %f,obsDistance: %f,obsSpeed: %f,obs_speed_for_avoid: %f",
|
|
|
+ obs_filter,obs_filter_flag,obs_filter_dis_memory,obsDistance,obsSpeed,obs_speed_for_avoid);
|
|
|
+
|
|
|
+ obs_speed_for_avoid=0;//shenlan guosai zhangaiwu sudu zhijiegei 0
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ gpsTraceRear.clear();
|
|
|
+ for(unsigned int i=0;i<gpsTraceNow.size();i++)
|
|
|
+ {
|
|
|
+ Point2D pt;
|
|
|
+ pt.x=0-gpsTraceNow[i].x;
|
|
|
+ pt.y=0-gpsTraceNow[i].y;
|
|
|
+ gpsTraceRear.push_back(pt);
|
|
|
+ }
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
|
|
|
+ // obsDistance=-1; //1227
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.mObsDistance=obsDistance;
|
|
|
+ ServiceCarStatus.mObsSpeed=obs_speed_for_avoid;
|
|
|
+
|
|
|
+ static bool avoid_speed_flag=false;
|
|
|
+ /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
|
|
|
+ (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
|
|
|
+ && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
|
|
|
+ {
|
|
|
+ minDecelerate=-0.4;
|
|
|
+ avoid_speed_flag=true;
|
|
|
+ }*/
|
|
|
+
|
|
|
+ road_permit_avoid=false;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //比赛总路线可能不会太长
|
|
|
+ {
|
|
|
+ if(PathPoint+200<gpsMapLine.size()){
|
|
|
+ int road_permit_sum=0;
|
|
|
+ for(int k=PathPoint;k<PathPoint+200;k++){
|
|
|
+ if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
|
|
|
+ road_permit_sum++;
|
|
|
+ }
|
|
|
+ if(road_permit_sum>=200)
|
|
|
+ road_permit_avoid=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(PathPoint+200<gpsMapLine.size()){ //400gaiwei 200
|
|
|
+ int road_permit_sum=0;
|
|
|
+ for(int k=PathPoint;k<PathPoint+200;k++){
|
|
|
+ if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
|
|
|
+ road_permit_sum++;
|
|
|
+ }
|
|
|
+ if(road_permit_sum>=200)
|
|
|
+ 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>0.01)//&& (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();
|
|
|
+ ObsTimeEnd+=1.0;
|
|
|
+ //dSpeed = min(6.0,dSpeed);
|
|
|
+ cout<<"\n====================preAvoid time count start==================\n"<<endl;
|
|
|
+ }
|
|
|
+ // if(ObsTimeStart!=-1){
|
|
|
+ // ObsTimeEnd=GetTickCount();
|
|
|
+ // }
|
|
|
+ // if(ObsTimeEnd!=-1){
|
|
|
+ // ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
|
|
|
+ // vehState=avoidObs;
|
|
|
+ // ObsTimeStart=-1;
|
|
|
+ // ObsTimeEnd=-1;
|
|
|
+ // ObsTimeSpan=-1;
|
|
|
+ // avoid_speed_flag=false;
|
|
|
+ // cout<<"\n====================preAvoid time count finish==================\n"<<endl;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
|
|
|
+ // ObsTimeEnd+=1.0;
|
|
|
+ // }
|
|
|
+
|
|
|
+ if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
|
|
|
+ vehState=avoidObs;
|
|
|
+ ObsTimeStart=-1;
|
|
|
+ ObsTimeEnd=-1;
|
|
|
+ ObsTimeSpan=-1;
|
|
|
+ avoid_speed_flag=false;
|
|
|
+ cout<<"\n====================preAvoid time count finish==================\n"<<endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){
|
|
|
+ ObsTimeStart=-1;
|
|
|
+ ObsTimeEnd=-1;
|
|
|
+ ObsTimeSpan=-1;
|
|
|
+ avoid_speed_flag=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ //避障模式
|
|
|
+ if (vehState == avoidObs || vehState==waitAvoid ) {
|
|
|
+
|
|
|
+ // if (obsDistance <20 && obsDistance >0)
|
|
|
+ if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
|
|
|
+ {
|
|
|
+ dSpeed = min(6.0,dSpeed);
|
|
|
+ avoidTimes++;
|
|
|
+ // if (avoidTimes>=6)
|
|
|
+ if (avoidTimes>=ServiceCarStatus.aocfTimes)
|
|
|
+ {
|
|
|
+ vehState = preAvoid;
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ // else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
|
|
|
+ // {
|
|
|
+ // dSpeed = 10;
|
|
|
+ // avoidTimes = 0;
|
|
|
+ // }
|
|
|
+ else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0,dSpeed);
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ avoidTimes = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if((vehState == preAvoid)||(avoidXNew!=0.0))//jiashang avoiding fangzhi avoid=0.0buneng lianbi
|
|
|
+ {
|
|
|
+ dSpeed = min(6.0,dSpeed);
|
|
|
+// int avoidLeft_value=0;
|
|
|
+// int avoidRight_value=0;
|
|
|
+// int* avoidLeft_p=&avoidLeft_value;
|
|
|
+// int* avoidRight_p=&avoidRight_value;
|
|
|
+ double avoidLeft_value=0;
|
|
|
+ double avoidRight_value=0;
|
|
|
+ double* avoidLeft_p=&avoidLeft_value;
|
|
|
+ double* avoidRight_p=&avoidRight_value;
|
|
|
+ computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
|
|
|
+ avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
|
|
|
+
|
|
|
+ givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
|
|
|
+
|
|
|
+ //const int nAvoidPreCount = 100;
|
|
|
+ if(avoidXNewPreVector.size()<5){
|
|
|
+ avoidXNewPreVector.push_back(avoidXNewPre);
|
|
|
+ }else{
|
|
|
+ if(avoidXNewPreVector[0]!=avoidXNewLast){
|
|
|
+ for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
|
|
|
+ if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
|
|
|
+ avoidXNewPreFilter=avoidXNewLast;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ if(i==avoidXNewPreVector.size()-1)
|
|
|
+ avoidXNewPreFilter=avoidXNewPreVector[0];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ avoidXNewPreVector.clear();
|
|
|
+ }
|
|
|
+ if(avoidXNewPreFilter!=avoidXNewLast){
|
|
|
+ avoidXNew=avoidXNewPre;
|
|
|
+ if(avoidXNew<0)
|
|
|
+ turnLampFlag=-1;
|
|
|
+ else if(avoidXNew>0)
|
|
|
+ turnLampFlag=1;
|
|
|
+ else
|
|
|
+ turnLampFlag=0;
|
|
|
+
|
|
|
+ gpsTraceNow.clear();
|
|
|
+ gpsTraceAvoidXY.clear();
|
|
|
+ givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
|
|
|
+ avoidXNew,avoidXNewLast);
|
|
|
+ //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
|
|
|
+ gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+ vehState = avoiding;
|
|
|
+ startAvoidGpsIns = now_gps_ins;
|
|
|
+ obstacle_avoid_flag=1;
|
|
|
+ hasCheckedAvoidLidar = false;
|
|
|
+ avoidXNewLast=avoidXNew;
|
|
|
+ avoidXNewPreFilter=avoidXNew;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+// if(avoidXNewPreVector.size()<nAvoidPreCount){
|
|
|
+// avoidXNewPreVector.push_back(avoidXNewPre);
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// avoidXNewPreVector.erase(avoidXNewPreVector.begin());
|
|
|
+// avoidXNewPreVector.push_back(avoidXNewPre);
|
|
|
+// }
|
|
|
+
|
|
|
+// if(vehState == preAvoid)
|
|
|
+// {
|
|
|
+// avoidXNew = avoidXNewPre;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// if(fabs(avoidXNewLast)>0.1)
|
|
|
+// {
|
|
|
+
|
|
|
+// bool bOriginSafe = true;
|
|
|
+
|
|
|
+// if(avoidXNewPreVector.size()<nAvoidPreCount)
|
|
|
+// {
|
|
|
+// bOriginSafe = false;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// unsigned int j;
|
|
|
+// unsigned int nSize = avoidXNewPreVector.size();
|
|
|
+// for(j=0;j<nSize;j++)
|
|
|
+// {
|
|
|
+// if(fabs(avoidXNewPreVector[j])>0.1)
|
|
|
+// {
|
|
|
+// bOriginSafe = false;
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// if(bOriginSafe)
|
|
|
+// {
|
|
|
+// avoidXNew = 0;
|
|
|
+// // avoidXNewPreVector.clear();
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// unsigned int j;
|
|
|
+// unsigned int nSize = avoidXNewPreVector.size();
|
|
|
+// if(avoidXNewPreVector.size()==nAvoidPreCount)
|
|
|
+// {
|
|
|
+// double filter = 0;
|
|
|
+// for(j=0;j<nSize;j++)
|
|
|
+// {
|
|
|
+// if(fabs(avoidXNewPreVector[j])>0.1)
|
|
|
+// {
|
|
|
+// if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
|
|
|
+// else
|
|
|
+// {
|
|
|
+// if(fabs(filter - avoidXNewPreVector[j])>0.1)
|
|
|
+// {
|
|
|
+// filter = 0;
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// if(fabs(filter)<0.1)
|
|
|
+// {
|
|
|
+// avoidXNew = avoidXNewLast;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// if(fabs(filter - avoidXNewLast)>=2.0)
|
|
|
+// {
|
|
|
+// avoidXNew = filter;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// avoidXNew = avoidXNewLast;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// avoidXNew = avoidXNewLast;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+
|
|
|
+
|
|
|
+// if(avoidXNew<0)
|
|
|
+// turnLampFlag=-1;
|
|
|
+// else if(avoidXNew>0)
|
|
|
+// turnLampFlag=1;
|
|
|
+// else
|
|
|
+// turnLampFlag=0;
|
|
|
+
|
|
|
+// if(fabs(avoidXNew - avoidXNewLast)>0.1)
|
|
|
+// {
|
|
|
+// gpsTraceNow.clear();
|
|
|
+// gpsTraceAvoidXY.clear();
|
|
|
+// givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
|
|
|
+// avoidXNew,avoidXNewLast);
|
|
|
+// //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
|
|
|
+// gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+// vehState = avoiding;
|
|
|
+// startAvoidGpsIns = now_gps_ins;
|
|
|
+// obstacle_avoid_flag=1;
|
|
|
+// hasCheckedAvoidLidar = false;
|
|
|
+// avoidXNewLast=avoidXNew;
|
|
|
+// }
|
|
|
+
|
|
|
+// if(avoidXNewPreVector.size()<5){
|
|
|
+// avoidXNewPreVector.push_back(avoidXNewPre);
|
|
|
+// }else{
|
|
|
+// if(avoidXNewPreVector[0]!=avoidXNewLast){
|
|
|
+// for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
|
|
|
+// if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
|
|
|
+// avoidXNewPreFilter=avoidXNewLast;
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// if(i==avoidXNewPreVector.size()-1)
|
|
|
+// avoidXNewPreFilter=avoidXNewPreVector[0];
|
|
|
+// }
|
|
|
+// }
|
|
|
+// avoidXNewPreVector.clear();
|
|
|
+// }
|
|
|
+// if(avoidXNewPreFilter!=avoidXNewLast){
|
|
|
+// avoidXNew=avoidXNewPre;
|
|
|
+// if(avoidXNew<0)
|
|
|
+// turnLampFlag=-1;
|
|
|
+// else if(avoidXNew>0)
|
|
|
+// turnLampFlag=1;
|
|
|
+// else
|
|
|
+// turnLampFlag=0;
|
|
|
+
|
|
|
+// gpsTraceNow.clear();
|
|
|
+// gpsTraceAvoidXY.clear();
|
|
|
+// givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
|
|
|
+// avoidXNew,avoidXNewLast);
|
|
|
+// //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
|
|
|
+// gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+// vehState = avoiding;
|
|
|
+// startAvoidGpsIns = now_gps_ins;
|
|
|
+// obstacle_avoid_flag=1;
|
|
|
+// hasCheckedAvoidLidar = false;
|
|
|
+// avoidXNewLast=avoidXNew;
|
|
|
+// avoidXNewPreFilter=avoidXNew;
|
|
|
+// }
|
|
|
+// }
|
|
|
+//20230407,
|
|
|
+// if((vehState == preAvoid)||((avoidXNew!=0)))
|
|
|
+// // if((vehState == preAvoid)||(avoidXNew!=0))
|
|
|
+// {
|
|
|
+// dSpeed = min(8.0,dSpeed);//6gaiwei 10
|
|
|
+// static int count_avoidx_0=0;
|
|
|
+// double avoidLeft_value=0;
|
|
|
+// double avoidRight_value=0;
|
|
|
+// double* avoidLeft_p=&avoidLeft_value;
|
|
|
+// double* avoidRight_p=&avoidRight_value;
|
|
|
+// computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
|
|
|
+// // avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
|
|
|
+// avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
|
|
|
+// if(avoidXNewPreVector.size()<20){
|
|
|
+// avoidXNewPreVector.push_back(avoidXNewPre);
|
|
|
+// }else{
|
|
|
+// if(avoidXNewPreVector[0]!=avoidXNewLast){
|
|
|
+// for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
|
|
|
+// if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
|
|
|
+// avoidXNewPreFilter=avoidXNewLast;
|
|
|
+// break;
|
|
|
+// }
|
|
|
+// if(i==avoidXNewPreVector.size()-1)
|
|
|
+// avoidXNewPreFilter=avoidXNewPreVector[0];
|
|
|
+// }
|
|
|
+// }
|
|
|
+// avoidXNewPreVector.clear();
|
|
|
+// }
|
|
|
+// if(avoidXNewPreFilter!=avoidXNewLast){
|
|
|
+// avoidXNew=avoidXNewPre;
|
|
|
+// //avoidXNew = avoidXNewPreFilter;
|
|
|
+// if(avoidXNew<0)
|
|
|
+// turnLampFlag=-1;
|
|
|
+// else if(avoidXNew>0)
|
|
|
+// turnLampFlag=1;
|
|
|
+// else
|
|
|
+// turnLampFlag=0;
|
|
|
+
|
|
|
+// // gpsTraceNow.clear();
|
|
|
+//// gpsTraceAvoidXY.clear();
|
|
|
+// givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
|
|
|
+// avoidXNew,avoidXNewLast);
|
|
|
+// //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
|
|
|
+// // gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+// if(vehState == preAvoid)
|
|
|
+// {
|
|
|
+// gpsTraceAvoidXY.clear();
|
|
|
+// gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+// }
|
|
|
+// else if(avoidXNew==0)
|
|
|
+// {
|
|
|
+// count_avoidx_0++;
|
|
|
+// if(count_avoidx_0>45)// you 60 gaicheng 30
|
|
|
+// {
|
|
|
+// gpsTraceAvoidXY.clear();
|
|
|
+// gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
|
|
|
+
|
|
|
+// }
|
|
|
+// }
|
|
|
+// if(avoidXNew!=0)
|
|
|
+// {
|
|
|
+// count_avoidx_0=0;
|
|
|
+// }
|
|
|
+// vehState = avoiding;
|
|
|
+// startAvoidGpsIns = now_gps_ins;
|
|
|
+// obstacle_avoid_flag=1;
|
|
|
+// hasCheckedAvoidLidar = false;
|
|
|
+// avoidXNewLast=avoidXNew;
|
|
|
+// avoidXNewPreFilter=avoidXNew;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// if (vehState == preAvoid)
|
|
|
+// {
|
|
|
+// //dSpeed = min(6.0,dSpeed);
|
|
|
+// iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
|
|
|
+// if ((obsDistance>ServiceCarStatus.aocfDis)&&(fabs(now_s_d.d)<0.05))
|
|
|
+// {
|
|
|
+// // vehState = avoidObs; 0929
|
|
|
+// vehState=normalRun;
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+
|
|
|
+ //old_bz--------------------------------------------------------------------------------------------------
|
|
|
+ if (vehState == avoiding)
|
|
|
+ {
|
|
|
+ // double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins); //Toggle 20220223
|
|
|
+ // //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
|
|
|
+ // if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
|
|
|
+ // vehState = normalRun;
|
|
|
+ // // useFrenet = false;
|
|
|
+ // // useOldAvoid = true;
|
|
|
+ // }
|
|
|
+ // else if (useOldAvoid && avoidDistance>35) { //zj 2021.06.21 gpsTraceNow[60].x)<0.02
|
|
|
+ // // vehState = avoidObs; 0929
|
|
|
+ // vehState = normalRun;
|
|
|
+ // turnLampFlag=0;
|
|
|
+ // }
|
|
|
+ // else if (turnLampFlag>0)
|
|
|
+ // {
|
|
|
+ // gps_decition->leftlamp = false;
|
|
|
+ // gps_decition->rightlamp = true;
|
|
|
+ // }
|
|
|
+ // else if(turnLampFlag<0)
|
|
|
+ // {
|
|
|
+ // gps_decition->leftlamp = true;
|
|
|
+ // gps_decition->rightlamp = false;
|
|
|
+ // }
|
|
|
+
|
|
|
+ dSpeed = min(6.0,dSpeed);
|
|
|
+ if (turnLampFlag>0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if(turnLampFlag<0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (vehState==preBack)
|
|
|
+ {
|
|
|
+ vehState = backOri;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (vehState==backOri)
|
|
|
+ {
|
|
|
+ double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
|
|
|
+ //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
|
|
|
+ if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
|
|
|
+ vehState = normalRun;
|
|
|
+ turnLampFlag=0;
|
|
|
+ // useFrenet = false;
|
|
|
+ // useOldAvoid = true;
|
|
|
+ }
|
|
|
+ else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
|
|
|
+ // vehState = avoidObs; 0929
|
|
|
+ vehState = normalRun;
|
|
|
+ obstacle_avoid_flag=0;
|
|
|
+ }
|
|
|
+ else if (turnLampFlag>0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = false;
|
|
|
+ gps_decition->rightlamp = true;
|
|
|
+ }
|
|
|
+ else if (turnLampFlag<0)
|
|
|
+ {
|
|
|
+ gps_decition->leftlamp = true;
|
|
|
+ gps_decition->rightlamp = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
|
|
|
+ // 计算回归原始路线
|
|
|
+#ifdef AVOID_NEW
|
|
|
+ if((avoidXNew == 0.0)&&(vehState == avoiding))
|
|
|
+ {
|
|
|
+ iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
|
|
|
+ if(fabs(now_s_d.d)<0.05)
|
|
|
+ {
|
|
|
+ vehState=normalRun;
|
|
|
+ obstacle_avoid_flag=0;
|
|
|
+ // avoidXNewPreVector.clear();//20230526,huifu
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+#else
|
|
|
+ if ((roadNow!=roadOri))
|
|
|
+ {
|
|
|
+ if(useFrenet){
|
|
|
+ if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
|
|
|
+ {
|
|
|
+ double offsetL = -(roadSum - 1)*roadWidth;
|
|
|
+ double offsetR = (roadOri - 0)*roadWidth;
|
|
|
+ roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(useOldAvoid){
|
|
|
+ roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth; //1012
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if ((roadNow != roadOri && roadPre!=-1))
|
|
|
+ {
|
|
|
+
|
|
|
+ roadNow = roadPre;
|
|
|
+ // avoidX = (roadOri - roadNow)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
|
|
|
+
|
|
|
+ if(avoidX<0)
|
|
|
+ turnLampFlag<0;
|
|
|
+ else if(avoidX>0)
|
|
|
+ turnLampFlag>0;
|
|
|
+ else
|
|
|
+ turnLampFlag=0;
|
|
|
+
|
|
|
+ if(useOldAvoid){
|
|
|
+ //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ gpsTraceAvoidXY.clear();
|
|
|
+ gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
|
|
|
+ startBackGpsIns = now_gps_ins;
|
|
|
+ }
|
|
|
+ else if(useFrenet){
|
|
|
+ gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
|
|
|
+ }
|
|
|
+
|
|
|
+ vehState = backOri;
|
|
|
+ hasCheckedBackLidar = false;
|
|
|
+ // checkBackObsTimes = 0;
|
|
|
+
|
|
|
+ cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+ if ( vehState==changingRoad || vehState==chaocheBack)
|
|
|
+ {
|
|
|
+ double lastAng = 0.0 - lastAngle;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
|
|
|
+
|
|
|
+ if (controlAng>40)
|
|
|
+ {
|
|
|
+ controlAng =40;
|
|
|
+ }
|
|
|
+ else if (controlAng<-40)
|
|
|
+ {
|
|
|
+ controlAng = - 40;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //速度结合角度的限制
|
|
|
+ controlAng = limitAngle(realspeed, controlAng);
|
|
|
+
|
|
|
+ //1220
|
|
|
+ if(PathPoint+80<gpsMapLine.size()-1){
|
|
|
+ if(gpsMapLine[PathPoint+80]->roadMode==4 && !ServiceCarStatus.daocheMode){
|
|
|
+ changingDangWei=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(changingDangWei){
|
|
|
+ if(abs(realspeed)>0.1){
|
|
|
+ dSpeed=0;
|
|
|
+ }else{
|
|
|
+ dSpeed=0;
|
|
|
+ gps_decition->dangWei=2;
|
|
|
+ ServiceCarStatus.daocheMode=true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.daocheMode)
|
|
|
+ {
|
|
|
+ controlAng=0-controlAng;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ dSecSpeed = dSpeed / 3.6;
|
|
|
+ gps_decition->speed = dSpeed;
|
|
|
+
|
|
|
+ if (gpsMapLine[PathPoint]->runMode == 2)
|
|
|
+ {
|
|
|
+ obsDistance = -1;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ //----------------------------------------shenlan 采集车车路协同,add---------------------------------------------
|
|
|
+ if(ServiceCarStatus.mRSUupdateTimer.elapsed()>10*1000)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.rsu_traffic_type=200;//lvdeng
|
|
|
+ ServiceCarStatus.rsu_warning_type=200;
|
|
|
+ // ServiceCarStatus.rsu_gps_lat = 0.0; //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
|
|
|
+ // ServiceCarStatus.rsu_gps_lng = 0.0;
|
|
|
+ // ServiceCarStatus.rsu_trafficelimit_spd=200;
|
|
|
+ }
|
|
|
+ if(ServiceCarStatus.mRSUTrafficUpdateTimer.elapsed()>10*1000)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.rsu_traffic_type=200;//lvdeng
|
|
|
+ ServiceCarStatus.rsu_trafficelimit_spd=200;
|
|
|
+
|
|
|
+// ServiceCarStatus.iTrafficeLightLon=0;//20230310
|
|
|
+// ServiceCarStatus.iTrafficeLightLat=0;//20230310
|
|
|
+
|
|
|
+ // ServiceCarStatus.rsu_warning_type=200;
|
|
|
+ // ServiceCarStatus.rsu_gps_lat = 0.0; //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
|
|
|
+ // ServiceCarStatus.rsu_gps_lng = 0.0;
|
|
|
+ // ServiceCarStatus.rsu_trafficelimit_spd=200;
|
|
|
+ }
|
|
|
+ if(ServiceCarStatus.mRSUWarnUpdateTimer.elapsed()>10*1000)
|
|
|
+ {
|
|
|
+ // ServiceCarStatus.rsu_traffic_type=200;//lvdeng
|
|
|
+ ServiceCarStatus.rsu_warning_type=200;
|
|
|
+ ServiceCarStatus.rsu_warnlimit_spd=200;
|
|
|
+ // ServiceCarStatus.rsu_gps_lat = 0.0; //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
|
|
|
+ // ServiceCarStatus.rsu_gps_lng = 0.0;
|
|
|
+ // ServiceCarStatus.rsu_trafficelimit_spd=200;
|
|
|
+ }
|
|
|
+ unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
|
|
|
+ unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
|
|
|
+ float distance_to_center=0;
|
|
|
+ GPS_INS traffic_center_gps;
|
|
|
+ traffic_center_gps.gps_lat=ServiceCarStatus.rsu_gps_lat;
|
|
|
+ traffic_center_gps.gps_lng=ServiceCarStatus.rsu_gps_lng;
|
|
|
+ float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
|
|
|
+ float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
|
|
|
+ float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
|
|
|
+ //以上变量信息都需要存储到log文件中
|
|
|
+ GaussProjCal(traffic_center_gps.gps_lng, traffic_center_gps.gps_lat, &traffic_center_gps.gps_x, &traffic_center_gps.gps_y);
|
|
|
+ distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
|
|
|
+
|
|
|
+ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
|
|
|
+ ||(warning_type==0x01)||(warning_type==0x02))
|
|
|
+{
|
|
|
+ if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
|
|
|
+ {
|
|
|
+ if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
|
|
|
+ {
|
|
|
+ //dSpeed = min(1.0,realspeed-0.2);
|
|
|
+ dSpeed = max(1.0,realspeed-2); //乘用车不同于底盘，速度是不稳定的，如果取最小一会就减成0了
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 1;
|
|
|
+ ServiceCarStatus.target_spd_1x = 1.0;
|
|
|
+ }
|
|
|
+ else if(distance_to_center<radiation_distance)
|
|
|
+ {
|
|
|
+ dSpeed=0.0;
|
|
|
+ minDecelerate=-2.0;
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 2;
|
|
|
+ ServiceCarStatus.target_spd_1x=0.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ givlog->debug("decition_brain","traffic_center_gps.gps_lat is : %f",traffic_center_gps.gps_lat);
|
|
|
+ givlog->debug("decition_brain","traffic_center_gps.gps_lng is : %f",traffic_center_gps.gps_lng);
|
|
|
+ givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
|
|
|
+ givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
|
|
|
+ givlog->debug("decition_brain","dSpeedd is : %f",dSpeed);
|
|
|
+ givlog->debug("decition_brain","minDecelerate is : %f",minDecelerate);
|
|
|
+ givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
|
|
|
+ {
|
|
|
+ if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
|
|
|
+ {
|
|
|
+// dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
|
|
|
+ if(realspeed-2>trafficlimit_spd)
|
|
|
+ {
|
|
|
+ dSpeed=realspeed-2;//先让速度稍微减少一点
|
|
|
+ }
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 1;
|
|
|
+ ServiceCarStatus.target_spd_1x = trafficlimit_spd;
|
|
|
+ }
|
|
|
+ else if(distance_to_center<radiation_distance)
|
|
|
+ {
|
|
|
+ //dSpeed=min((double)trafficlimit_spd,realspeed);
|
|
|
+ dSpeed=(double)trafficlimit_spd;
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 1;
|
|
|
+ ServiceCarStatus.target_spd_1x = trafficlimit_spd;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
|
|
|
+ givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
|
|
|
+ givlog->debug("decition_brain","dSpeed is : %f",dSpeed);
|
|
|
+ givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+//碰撞预警，1减速，2 停车
|
|
|
+ if(warning_type==0x01)
|
|
|
+ {
|
|
|
+ dSpeed=warnlimit_spd;//此处要判断限速值是不是在有效值范围以内
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 1;
|
|
|
+ ServiceCarStatus.target_spd_1x = warnlimit_spd;
|
|
|
+ }
|
|
|
+ else if(warning_type==0x02)
|
|
|
+ {
|
|
|
+ dSpeed=0.0;
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 2;
|
|
|
+ ServiceCarStatus.target_spd_1x = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+}
|
|
|
+else
|
|
|
+{
|
|
|
+ ServiceCarStatus.vehicle_state_1x = 0;
|
|
|
+ ServiceCarStatus.target_spd_1x = dSpeed;
|
|
|
+}
|
|
|
+
|
|
|
+ std::cout<<"juecesudu0="<<dSpeed<<std::endl;
|
|
|
+
|
|
|
+ //ServiceCarStatus.milightCheckTimer.elapsed();
|
|
|
+ if(ServiceCarStatus.milightCheckTimer.elapsed()>5*1000)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.iTrafficeLightColor=1;//lvdeng
|
|
|
+ ServiceCarStatus.iTrafficeLightTime=200;
|
|
|
+ }
|
|
|
+
|
|
|
+ //-------------------------------traffic light----------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ //1125 traficc
|
|
|
+ traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
|
|
|
+ traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
|
|
|
+ GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
|
|
|
+
|
|
|
+ if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
|
|
|
+ traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
|
|
|
+ // traffic_light_pathpoint=130;
|
|
|
+ //float traffic_speed = ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,gpsMapLine, traffic_light_pathpoint,PathPoint,secSpeed);
|
|
|
+ float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
|
|
|
+ traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
|
|
|
+
|
|
|
+ dSpeed = min((double)traffic_speed,dSpeed);
|
|
|
+ if(traffic_speed==0)
|
|
|
+ {
|
|
|
+ minDecelerate=-2.0; //-2.0 ,深蓝车速比较小将减速度改小一些
|
|
|
+ }
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ givlog->debug("decition_brain","traffic_light_pathpoint is : %d",traffic_light_pathpoint);
|
|
|
+ givlog->debug("decition_brain","traffic_speed is : %f",traffic_speed);
|
|
|
+ givlog->debug("decition_brain","traffic_light_gps.gps_lat is : %f",traffic_light_gps.gps_lat);
|
|
|
+ givlog->debug("decition_brain","traffic_light_gps.gps_lng is : %f",traffic_light_gps.gps_lng);
|
|
|
+ givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightColor is : %d",ServiceCarStatus.iTrafficeLightColor);
|
|
|
+ givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightTime is : %d",ServiceCarStatus.iTrafficeLightTime);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //-------------------------------traffic light end-----------------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+
|
|
|
+ double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight, gpsMapLine, v2xTrafficVector,
|
|
|
+ PathPoint, secSpeed, dSpeed, circleMode);
|
|
|
+
|
|
|
+
|
|
|
+ dSpeed = min(v2xTrafficSpeed,dSpeed);
|
|
|
+
|
|
|
+ //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ //20230310 add begin,shanlan 由决策决定什么时候开启红绿等检测
|
|
|
+ float distance_to_stopline;
|
|
|
+ distance_to_stopline=GetDistance(now_gps_ins,traffic_light_gps);
|
|
|
+ givlog->debug("decition_brain","distance_to_stopline is : %f",distance_to_stopline);
|
|
|
+ if(distance_to_stopline<20.0)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mLightStartSensorBtn=true;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mLightStartSensorBtn=false;
|
|
|
+ }
|
|
|
+ //20230310 add end,由决策决定什么时候开启红绿等检测
|
|
|
+
|
|
|
+ transferGpsMode2(gpsMapLine);
|
|
|
+/**************************zhuche/tingche logic begin**************************************************************/
|
|
|
+ //准备驻车
|
|
|
+ if (readyZhucheMode)
|
|
|
+ {
|
|
|
+ cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ cout<<"zhuche point : "<<zhuchePointNum<<endl;
|
|
|
+ double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
|
|
|
+ if (dis<35)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
|
|
|
+ double zhucheDistance = pt.y;
|
|
|
+#if 0
|
|
|
+ if (dSpeed > 15)
|
|
|
+ {
|
|
|
+ dSpeed = 15;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (zhucheDistance < 20 && dis < 25)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }
|
|
|
+#else
|
|
|
+ if (dSpeed > 12)
|
|
|
+ {
|
|
|
+ dSpeed = 12;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (zhucheDistance < 9 && dis < 9)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }
|
|
|
+#endif
|
|
|
+ if (zhucheDistance < 3.0 && dis < 3.5)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ zhucheMode = true;
|
|
|
+ readyZhucheMode = false;
|
|
|
+ cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //驻车
|
|
|
+ if (zhucheMode)
|
|
|
+ {
|
|
|
+ int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
|
|
|
+
|
|
|
+ // if(trumpet()<16000)
|
|
|
+ if (trumpet()<mzcTime)
|
|
|
+ {
|
|
|
+ // if (trumpet()<12000){
|
|
|
+ cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ if(abs(realspeed)<0.2){
|
|
|
+ controlAng=0; //tju
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ hasZhuched = true; //1125
|
|
|
+ zhucheMode = false;
|
|
|
+ trumpetFirstCount = true;
|
|
|
+ cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //判断驻车标志位
|
|
|
+ if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
|
|
|
+ {
|
|
|
+ hasZhuched = false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (readyParkMode)
|
|
|
+ {
|
|
|
+ double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
|
|
|
+ hasZhuched = true;
|
|
|
+
|
|
|
+ if (parkDis < 25)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
|
|
|
+ double parkDistance = pt.y;
|
|
|
+ if (dSpeed > 15)
|
|
|
+ {
|
|
|
+ dSpeed = 15;
|
|
|
+ }
|
|
|
+
|
|
|
+ //dSpeed = 15;//////////////////////////////
|
|
|
+
|
|
|
+ if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 8.0);
|
|
|
+ }
|
|
|
+ else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 5.0);
|
|
|
+ }
|
|
|
+ else if(parkDistance < 3.5 && parkDis<4.0)
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed, 3.0);
|
|
|
+ }
|
|
|
+
|
|
|
+ // float stopDis=2;
|
|
|
+ // if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
|
|
|
+ // stopDis=1.6;
|
|
|
+ // } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
|
|
|
+ // stopDis=1.8;
|
|
|
+ // }
|
|
|
+
|
|
|
+ if (parkDistance < 1.6 && parkDis<2.0)
|
|
|
+ {
|
|
|
+ // dSpeed = 0;
|
|
|
+ parkMode = true;
|
|
|
+ readyParkMode = false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (parkMode)
|
|
|
+ {
|
|
|
+ minDecelerate=-1.0;
|
|
|
+
|
|
|
+ if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
|
|
|
+ parkMode=false;
|
|
|
+ }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
|
|
|
+ parkMode=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+/**************************zhuche/tingche logic end**************************************************************/
|
|
|
+
|
|
|
+ for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsIns;
|
|
|
+ GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude, &gpsIns.gps_x, &gpsIns.gps_y);
|
|
|
+
|
|
|
+ double dis = GetDistance(gpsIns, now_gps_ins);
|
|
|
+ if(dis<20)
|
|
|
+ ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
|
|
|
+ }
|
|
|
+/**************************huche begin********************************************************************/
|
|
|
+ std::cout << "\n呼车指令状态:%d\n" << ServiceCarStatus.carState << std::endl;
|
|
|
+
|
|
|
+ if (ServiceCarStatus.carState == 0 && busMode)
|
|
|
+ {
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (ServiceCarStatus.carState == 3 && busMode)
|
|
|
+ {
|
|
|
+ if(realspeed<0.1){
|
|
|
+ ServiceCarStatus.carState=0;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }else{
|
|
|
+ nearStation=true;
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ ///kkcai, 2020-01-03
|
|
|
+ //if (ServiceCarStatus.carState == 2 && busMode)
|
|
|
+ if (ServiceCarStatus.carState == 2)
|
|
|
+ {
|
|
|
+ aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
|
|
|
+ aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
|
|
|
+ // gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
|
|
|
+ std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
|
|
|
+ qDebug("lat:%f", aim_gps_ins.gps_lat);
|
|
|
+ qDebug("lon:%f", aim_gps_ins.gps_lng);
|
|
|
+
|
|
|
+ std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
|
|
|
+ double dis = GetDistance(aim_gps_ins, now_gps_ins);
|
|
|
+ Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
|
|
|
+
|
|
|
+ std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
|
|
|
+ std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
|
|
|
+
|
|
|
+ // if (dis<20 && pt.y<8&& realspeed<0.1)
|
|
|
+ if (dis<20 && pt.y<5 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = 0;
|
|
|
+ nearStation=true;
|
|
|
+ //is_arrivaled = true;
|
|
|
+ //ServiceCarStatus.status[0] = true;
|
|
|
+ ServiceCarStatus.istostation=1;
|
|
|
+ minDecelerate=-1.0;
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (dis<20 && pt.y<15 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ nearStation=true;
|
|
|
+ dSpeed = min(8.0, dSpeed);
|
|
|
+ }
|
|
|
+ else if (dis<30 && pt.y<20 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = min(15.0, dSpeed);
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (dis<50 && abs(pt.x)<3)
|
|
|
+ {
|
|
|
+ dSpeed = min(20.0, dSpeed);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /**************************huche end********************************************************************/
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="hapo")
|
|
|
+ {
|
|
|
+ if(obsDistance>0 && obsDistance<8)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+// else if(obsDistance>0 && obsDistance<15)
|
|
|
+ else if(obsDistance>0 && obsDistance< ServiceCarStatus.socfDis)
|
|
|
+ {
|
|
|
+ dSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.group_control){
|
|
|
+ dSpeed = ServiceCarStatus.group_comm_speed;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(dSpeed==0){
|
|
|
+ if(realspeed<6)
|
|
|
+ minDecelerate=min(-0.5f,minDecelerate);
|
|
|
+ else
|
|
|
+ minDecelerate=min(-0.6f,minDecelerate); //1.0f zj-0.6
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
|
|
|
+ {
|
|
|
+ if(acc_end<0)
|
|
|
+ {
|
|
|
+ if(minDecelerate > acc_end) minDecelerate = acc_end;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
|
|
|
+ {
|
|
|
+ if(dSpeed>20.0)
|
|
|
+ {
|
|
|
+ dSpeed=20.0; //shenlan bisai xiansu 10
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout<<"juecesudu0="<<dSpeed<<std::endl;
|
|
|
+
|
|
|
+ if(obsDistance == 0)obsDistance = -1;
|
|
|
+
|
|
|
+ phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
|
|
|
+ gps_decition->wheel_angle = 0.0 - controlAng;
|
|
|
+
|
|
|
+ Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
|
|
|
+
|
|
|
+ givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
|
|
|
+ vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
|
|
|
+
|
|
|
+ if(ServiceCarStatus.txt_log_on==true){
|
|
|
+ QDateTime dt = QDateTime::currentDateTime();
|
|
|
+ //将数据写入文件开始
|
|
|
+ ofstream outfile;
|
|
|
+ outfile.open("control_log.txt", ostream::app);
|
|
|
+ outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
|
|
|
+ <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
|
|
|
+ <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
|
|
|
+ outfile.close();
|
|
|
+ //将数据写入文件结束
|
|
|
+ }
|
|
|
+ //shuju cunchu gognnenng add,20210624,cxw
|
|
|
+ if(ServiceCarStatus.txt_log_on==true)
|
|
|
+ {
|
|
|
+ if(first_start_en)
|
|
|
+ {
|
|
|
+ first_start_en=false;
|
|
|
+ start_tracepoint = PathPoint;
|
|
|
+ if(circleMode)
|
|
|
+ {
|
|
|
+ if(start_tracepoint==0)
|
|
|
+ {
|
|
|
+
|
|
|
+ end_tracepoint =gpsMapLine.size()-1; //这种计算终点坐标的序号只适合与闭环地图
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ end_tracepoint=start_tracepoint-1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ end_tracepoint =gpsMapLine.size()-1; //20210929,不是闭环地图的时候终点参考坐标就是地图数组的最大序号
|
|
|
+ }
|
|
|
+ }
|
|
|
+ double dis1 = GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
|
|
|
+ double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ if(brake_mode==true)
|
|
|
+ {
|
|
|
+ dis2=2;
|
|
|
+ }
|
|
|
+
|
|
|
+ //double dis2 = GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ if(circleMode)//闭环地图
|
|
|
+ {
|
|
|
+ if(dis1<3&&dis2<3&&circleMode) //距离不能太小，太小了可能会错过，所以适当将距离增大一些
|
|
|
+ // if(dis1<1&&dis2<1&&circleMode)
|
|
|
+ {
|
|
|
+ file_cnt_add_en=true; //201200102
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ file_cnt_add_en=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(dis1>10)
|
|
|
+ {
|
|
|
+ file_cnt_add=true;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(file_cnt_add_en&&file_num<256&&file_cnt_add)
|
|
|
+ {
|
|
|
+ file_num++;
|
|
|
+ file_cnt_add=false;
|
|
|
+ map_start_point=true;
|
|
|
+ }
|
|
|
+ if(file_num==256)
|
|
|
+ {
|
|
|
+ file_num=1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ }
|
|
|
+ QDateTime dt=QDateTime::currentDateTime();
|
|
|
+ QString date =dt.toString("yyyy_MM_dd_hhmm");
|
|
|
+ QString strsen = "1xdatalog_";
|
|
|
+ date.append(strsen);
|
|
|
+ QString s = QString::number(file_num);
|
|
|
+ date.append(s);
|
|
|
+ date.append(".txt");
|
|
|
+ string filename;
|
|
|
+ filename=date.toStdString();
|
|
|
+ ofstream outfile;
|
|
|
+ outfile.open(filename, ostream::app);
|
|
|
+ if((file_cnt_add==false)&&(map_start_point==true))
|
|
|
+ {
|
|
|
+ QDateTime dt=QDateTime::currentDateTime();
|
|
|
+ outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"
|
|
|
+ <<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
|
|
|
+ <<endl;
|
|
|
+ outfile<<"***********************the vehicle at map start point!*************************"<<endl;
|
|
|
+ outfile<<"the number of lap is "<<": "<<file_num<<""<<endl;
|
|
|
+ map_start_point=false;
|
|
|
+ }
|
|
|
+ QDateTime dt2=QDateTime::currentDateTime();
|
|
|
+ outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
|
|
|
+ <<"Decide_Dspeed" << "," <<setprecision(2)<<dSpeed<< ","
|
|
|
+ <<"Decide_ACC" << "," <<setprecision(2)<<gps_decition->torque<< ","
|
|
|
+ <<"Decide_Brake"<< "," <<gps_decition->brake<< ","
|
|
|
+ <<"Vehicle_RealSpd"<< "," <<setprecision(2)<<now_gps_ins.speed<< ","
|
|
|
+ <<"Decide_Angle"<< "," << setprecision(2)<<gps_decition->wheel_angle<< ","
|
|
|
+ <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
|
|
|
+ <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
|
|
|
+ <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
|
|
|
+ <<"Trace_Point"<< ","<<PathPoint<< ","
|
|
|
+ <<"OBS_Distance"<< ","<<obsDistance<< ","
|
|
|
+ <<"OBS_Speed"<< ","<<obsSpeed<< ","
|
|
|
+ <<endl;
|
|
|
+ outfile.close();
|
|
|
+ }
|
|
|
+ else //fei yuanxing luxian
|
|
|
+ {
|
|
|
+ if(dis1<3)
|
|
|
+ {
|
|
|
+ file_cnt_add_en=true; //201200102
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ file_cnt_add_en=false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(dis1>3)
|
|
|
+ {
|
|
|
+ file_cnt_add=true;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(file_cnt_add_en&&file_num<256&&file_cnt_add)
|
|
|
+ {
|
|
|
+ file_num++;
|
|
|
+ file_cnt_add=false;
|
|
|
+ map_start_point=true;
|
|
|
+ }
|
|
|
+ if(file_num==256)//20210712
|
|
|
+ {
|
|
|
+ file_num=1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ }
|
|
|
+ QDateTime dt=QDateTime::currentDateTime();
|
|
|
+ QString date =dt.toString("yyyy_MM_dd_hhmm");
|
|
|
+ QString strsen = "datalog_";
|
|
|
+ date.append(strsen);
|
|
|
+ QString s = QString::number(file_num);
|
|
|
+ date.append(s);
|
|
|
+ date.append(".txt");
|
|
|
+ string filename;
|
|
|
+ filename=date.toStdString();
|
|
|
+ ofstream outfile;
|
|
|
+ outfile.open(filename, ostream::app);
|
|
|
+ if((file_cnt_add==false)&&(map_start_point==true))
|
|
|
+ {
|
|
|
+ QDateTime dt=QDateTime::currentDateTime();
|
|
|
+ outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
|
|
|
+ <<endl;
|
|
|
+ outfile<<"********the vehicle near map start point!********"<<endl;
|
|
|
+ outfile<<"the number of lap is "<<":" <<file_num<<""<<endl;
|
|
|
+ map_start_point=false;
|
|
|
+ }
|
|
|
+ if(dis2<3)
|
|
|
+ {
|
|
|
+ outfile<<"********the vehicle near map end point!********" <<endl;
|
|
|
+ QDateTime dt=QDateTime::currentDateTime();
|
|
|
+ outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
|
|
|
+ <<endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ float ttc = 0-(obsDistance/obsSpeed);
|
|
|
+ double obsDistance_log=0;
|
|
|
+ if(obsDistance>500)
|
|
|
+ obsDistance_log=100;
|
|
|
+ else
|
|
|
+ obsDistance_log=obsDistance;
|
|
|
+ QDateTime dt2=QDateTime::currentDateTime();
|
|
|
+ double disToEndTrace = GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
|
|
|
+ double disToParkPoint = GetDistance(now_gps_ins, aim_gps_ins);
|
|
|
+ outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
|
|
|
+ <<"Decide_Dspeed" << "," <<setprecision(2)<<dSpeed<< ","
|
|
|
+ <<"Decide_ACC" << "," <<setprecision(2)<<gps_decition->torque<< ","
|
|
|
+ <<"Decide_Brake"<< "," <<gps_decition->brake<< ","
|
|
|
+ <<"Vehicle_RealSpd"<< "," <<setprecision(2)<<now_gps_ins.speed<< ","
|
|
|
+ // <<"readyParkMode"<< ","<<readyParkMode<< ","
|
|
|
+ // <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
|
|
|
+ <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
|
|
|
+ <<"disToParkPoint"<< ","<<disToParkPoint<< ","
|
|
|
+ <<"OBS_Distance"<< ","<<obsDistance_log<< ","
|
|
|
+ <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
|
|
|
+ <<"obsSpeed_fusion"<<","<<obsSpeed<<","
|
|
|
+ <<"SecSpeed"<<","<<secSpeed<<","
|
|
|
+ <<"Vehicle_state"<< ","<<vehState<< ","
|
|
|
+ <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
|
|
|
+ <<"avoidXNew"<<","<< setprecision(3)<<avoidXNew<<","
|
|
|
+ <<"avoidXNewPre"<<","<< setprecision(3)<<avoidXNewPre<<","
|
|
|
+ <<"Now_s_d_d"<< ","<< setprecision(3)<<now_s_d.d<< ","
|
|
|
+ <<"Now_s_d_s"<< ","<< setprecision(3)<<now_s_d.s<< ","
|
|
|
+ //<<"avoidXPre"<<","<<avoidXPre<<","
|
|
|
+ <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
|
|
|
+ <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
|
|
|
+ <<"Min_Decelation"","<<minDecelerate<< ","
|
|
|
+ <<"Decide_Angle"<< "," << setprecision(2)<<gps_decition->wheel_angle<< ","
|
|
|
+ <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
|
|
|
+ <<"Vehicle_GPS_lat"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
|
|
|
+ <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
|
|
|
+ <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
|
|
|
+ <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
|
|
|
+
|
|
|
+ <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
|
|
|
+ <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
|
|
|
+ <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
|
|
|
+ <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
|
|
|
+
|
|
|
+ <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
|
|
|
+ <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
|
|
|
+ <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
|
|
|
+ <<"Trace_Point"<< ","<<PathPoint<< ","
|
|
|
+ <<"changingDangWei"<< ","<<changingDangWei<< ","
|
|
|
+ <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
|
|
|
+ <<"TTC"<< ","<<ttc<< ","
|
|
|
+ <<"LightColor" << "," <<ServiceCarStatus.iTrafficeLightColor<< ","
|
|
|
+ <<"LightTime"<< "," <<ServiceCarStatus.iTrafficeLightTime<< ","
|
|
|
+ <<"DisTostopline"<< "," <<setprecision(2)<<distance_to_stopline<< ","
|
|
|
+ // <<"OBS_Distance"<< ","<<obsDistance_log<< ","
|
|
|
+ // <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
|
|
|
+ // <<"Vehicle_state"<< ","<<vehState<< ","
|
|
|
+ // <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
|
|
|
+ // <<"avoidXNew"<<","<<avoidXNew<<","
|
|
|
+ // <<"avoidXNewPre"<<","<<avoidXNewPre<<","
|
|
|
+ //// <<"avoidXPre"<<","<<avoidXPre<<","
|
|
|
+ // <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
|
|
|
+ <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
|
|
|
+ // <<"Min_Decelation"","<<minDecelerate<< ","
|
|
|
+ <<"Decide_Angle"<< "," << setprecision(2)<<gps_decition->wheel_angle<< ","
|
|
|
+ // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
|
|
|
+ // <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
|
|
|
+ // <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
|
|
|
+ // <<"Trace_Point"<< ","<<PathPoint<< ","
|
|
|
+ // <<"OBS_Speed"<< ","<<obsSpeed<< ","
|
|
|
+ // <<"OBS_Distance"<< ","<<obsDistance<< ","
|
|
|
+ // <<"TTC"<< ","<<ttc<< ","
|
|
|
+ <<endl;
|
|
|
+ outfile.close();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ lastAngle=gps_decition->wheel_angle;
|
|
|
+ lastVehState=vehState;
|
|
|
+
|
|
|
+ // qDebug("decide1 time is %d",xTime.elapsed());
|
|
|
+ return gps_decition;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::initMethods(){
|
|
|
+
|
|
|
+ pid_Controller= new PIDController();
|
|
|
+
|
|
|
+ ge3_Adapter = new Ge3Adapter();
|
|
|
+ qingyuan_Adapter = new QingYuanAdapter();
|
|
|
+ vv7_Adapter = new VV7Adapter();
|
|
|
+ zhongche_Adapter = new ZhongcheAdapter();
|
|
|
+ bus_Adapter = new BusAdapter();
|
|
|
+ hapo_Adapter = new HapoAdapter();
|
|
|
+ sightseeing_Adapter = new SightseeingAdapter();
|
|
|
+ changanshenlan_Adapter = new ChanganShenlanAdapter();
|
|
|
+
|
|
|
+
|
|
|
+ mpid_Controller.reset(pid_Controller);
|
|
|
+ mge3_Adapter.reset(ge3_Adapter);
|
|
|
+ mqingyuan_Adapter.reset(qingyuan_Adapter);
|
|
|
+ mvv7_Adapter.reset(vv7_Adapter);
|
|
|
+ mzhongche_Adapter.reset(zhongche_Adapter);
|
|
|
+ mbus_Adapter.reset(bus_Adapter);
|
|
|
+ mhapo_Adapter.reset(hapo_Adapter);
|
|
|
+ msightseeing_Adapter.reset(sightseeing_Adapter);
|
|
|
+ mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
|
|
|
+
|
|
|
+ frenetPlanner = new FrenetPlanner();
|
|
|
+ // laneChangePlanner = new LaneChangePlanner();
|
|
|
+
|
|
|
+ mfrenetPlanner.reset(frenetPlanner);
|
|
|
+ // mlaneChangePlanner.reset(laneChangePlanner);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
|
|
|
+
|
|
|
+ pid_Controller->getControlDecition( gpsIns, gpsTraceNow, dSpeed, obsDis, obsSpeed, false, true, &decition);
|
|
|
+
|
|
|
+ if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
|
|
|
+ {
|
|
|
+ if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
|
|
|
+ {
|
|
|
+ decition->accelerator = minDecelerate;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
|
|
|
+ ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
|
|
|
+ qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
|
|
|
+ vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
|
|
|
+ zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
|
|
|
+ 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);
|
|
|
+ }
|
|
|
+ else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
|
|
|
+ changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
|
|
|
+ vector<iv::Point2D> trace;
|
|
|
+
|
|
|
+ traceOriLeft.clear();
|
|
|
+ traceOriRight.clear();
|
|
|
+ int tracesize=800;
|
|
|
+ if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
|
|
|
+ {
|
|
|
+ tracesize=400;//400;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ tracesize=800;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (gpsMapLine.size() > tracesize && PathPoint >= 0)
|
|
|
+ {
|
|
|
+ int aimIndex;
|
|
|
+ if(circleMode){
|
|
|
+ aimIndex=PathPoint+tracesize;
|
|
|
+ }else{
|
|
|
+ aimIndex=min((PathPoint+tracesize),(int)gpsMapLine.size());
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = PathPoint; i < aimIndex; i++)
|
|
|
+ {
|
|
|
+ int index = i % gpsMapLine.size();
|
|
|
+ Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
|
|
|
+ pt.x += offset_real * 0.032;
|
|
|
+ pt.v1 = (*gpsMapLine[index]).speed_mode;
|
|
|
+ pt.v2 = (*gpsMapLine[index]).mode2;
|
|
|
+ pt.roadMode=(*gpsMapLine[index]).roadMode;
|
|
|
+ if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
|
|
|
+ {
|
|
|
+ readyZhucheMode = true;
|
|
|
+ DecideGps00::zhuchePointNum = index;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
|
|
|
+ {
|
|
|
+ readyZhucheMode = true;
|
|
|
+ DecideGps00::zhuchePointNum = index;
|
|
|
+ }
|
|
|
+
|
|
|
+ //csvv7
|
|
|
+ if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
|
|
|
+ {
|
|
|
+ readyParkMode = true;
|
|
|
+ DecideGps00::finishPointNum = index;
|
|
|
+ }
|
|
|
+ switch (pt.v1)
|
|
|
+ {
|
|
|
+ case 0:
|
|
|
+ pt.speed = 80;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ pt.speed = 25;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ pt.speed =25;
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ pt.speed = 20;
|
|
|
+ break;
|
|
|
+ case 4:
|
|
|
+ pt.speed =18;
|
|
|
+ break;
|
|
|
+ case 5:
|
|
|
+ pt.speed = 18;
|
|
|
+ break;
|
|
|
+ case 7:
|
|
|
+ pt.speed = 10;
|
|
|
+ break;
|
|
|
+ case 22:
|
|
|
+ pt.speed = 5;
|
|
|
+ break;
|
|
|
+ case 23:
|
|
|
+ pt.speed = 5;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ trace.push_back(pt);
|
|
|
+
|
|
|
+
|
|
|
+ double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
|
|
|
+ if(hdg < 0) hdg = hdg + 360;
|
|
|
+ if(hdg >= 360) hdg = hdg - 360;
|
|
|
+
|
|
|
+ double xyhdg = hdg/180.0*PI;
|
|
|
+ double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
|
|
|
+ double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
|
|
|
+ Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
|
|
|
+
|
|
|
+ double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
|
|
|
+ double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
|
|
|
+ Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
|
|
|
+
|
|
|
+ traceOriLeft.push_back(ptLeft);
|
|
|
+ traceOriRight.push_back(ptRight);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return trace;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
|
|
|
+ vector<iv::Point2D> trace;
|
|
|
+
|
|
|
+ int index = -1;
|
|
|
+ float minDis = 10;
|
|
|
+
|
|
|
+ for (unsigned int i = 0; i < gpsTrace.size(); i++)
|
|
|
+ {
|
|
|
+ double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));
|
|
|
+
|
|
|
+ if (tmpdis < minDis)
|
|
|
+ {
|
|
|
+ index = i;
|
|
|
+ minDis = tmpdis;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ trace.clear();
|
|
|
+ if (index >= 0) {
|
|
|
+ for (unsigned int i = index; i < gpsTrace.size(); i++)
|
|
|
+ {
|
|
|
+ Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
|
|
|
+ trace.push_back(pt);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return trace;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
|
|
|
+
|
|
|
+ if (offset==0)
|
|
|
+ {
|
|
|
+ return gpsTrace;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<iv::Point2D> trace;
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ double avoidLenth = abs(offset);
|
|
|
+ if (offset<0)
|
|
|
+ {
|
|
|
+ Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue + PI / 2));
|
|
|
+ ptLeft.speed = gpsTrace[j].speed;
|
|
|
+ ptLeft.v1 = gpsTrace[j].v1;
|
|
|
+ ptLeft.v2 = gpsTrace[j].v2;
|
|
|
+ trace.push_back(ptLeft);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue - PI / 2));
|
|
|
+ ptRight.speed = gpsTrace[j].speed;
|
|
|
+ ptRight.v1 = gpsTrace[j].v1;
|
|
|
+ ptRight.v2 = gpsTrace[j].v2;
|
|
|
+
|
|
|
+
|
|
|
+ trace.push_back(ptRight);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return trace;
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
|
|
|
+
|
|
|
+ Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
|
|
|
+
|
|
|
+ gpsTraceNowLeft.push_back(ptLeft);
|
|
|
+ gpsTraceNowRight.push_back(ptRight);
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
|
|
|
+
|
|
|
+ if (offset==0)
|
|
|
+ {
|
|
|
+ return gpsTrace;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<iv::Point2D> trace;
|
|
|
+ std::vector<iv::Point2D> traceXY;
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ double avoidLenth = abs(offset);
|
|
|
+ if (offset<0)
|
|
|
+ {
|
|
|
+ Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue + PI / 2));
|
|
|
+ ptLeft.speed = gpsTrace[j].speed;
|
|
|
+ ptLeft.v1 = gpsTrace[j].v1;
|
|
|
+ ptLeft.v2 = gpsTrace[j].v2;
|
|
|
+ trace.push_back(ptLeft);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue - PI / 2));
|
|
|
+ ptRight.speed = gpsTrace[j].speed;
|
|
|
+ ptRight.v1 = gpsTrace[j].v1;
|
|
|
+ ptRight.v2 = gpsTrace[j].v2;
|
|
|
+
|
|
|
+
|
|
|
+ trace.push_back(ptRight);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool use_new_method = true;
|
|
|
+ if (use_new_method)
|
|
|
+ {
|
|
|
+ const int val = 300;
|
|
|
+ if(trace.size()>val)
|
|
|
+ {
|
|
|
+ double V = trace[300].y;
|
|
|
+ for (int j = 0; j < val; j++)
|
|
|
+ {
|
|
|
+ double t = (double)j / val;
|
|
|
+ double s = t*t*(3.-2.*t);
|
|
|
+ double ox = s;
|
|
|
+ double oy = t *( V-3.0)+3.0;
|
|
|
+
|
|
|
+ trace[j].x=ox*trace[j].x;
|
|
|
+ trace[j].y=oy;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ traceXY.clear();
|
|
|
+ for(int j=0;j<30;j++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
|
|
|
+ Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
|
|
|
+ traceXY.push_back(gpsinxy);
|
|
|
+ }
|
|
|
+ for(int j=0;j<trace.size();j++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
|
|
|
+ Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
|
|
|
+ traceXY.push_back(gpsinxy);
|
|
|
+ }
|
|
|
+ return traceXY;
|
|
|
+}
|
|
|
+
|
|
|
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {
|
|
|
+
|
|
|
+
|
|
|
+ std::vector<iv::Point2D> trace;
|
|
|
+ std::vector<iv::Point2D> traceXY;
|
|
|
+
|
|
|
+ if (offset==0)
|
|
|
+ {
|
|
|
+ trace.assign(gpsTrace.begin(), gpsTrace.end());
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for (int j = 0; j < gpsTrace.size(); j++)
|
|
|
+ {
|
|
|
+ double sumx1 = 0, sumy1 = 0, count1 = 0;
|
|
|
+ double sumx2 = 0, sumy2 = 0, count2 = 0;
|
|
|
+ for (int k = max(0, j - 4); k <= j; k++)
|
|
|
+ {
|
|
|
+ count1 = count1 + 1;
|
|
|
+ sumx1 += gpsTrace[k].x;
|
|
|
+ sumy1 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
|
|
|
+ {
|
|
|
+ count2 = count2 + 1;
|
|
|
+ sumx2 += gpsTrace[k].x;
|
|
|
+ sumy2 += gpsTrace[k].y;
|
|
|
+ }
|
|
|
+ sumx1 /= count1; sumy1 /= count1;
|
|
|
+ sumx2 /= count2; sumy2 /= count2;
|
|
|
+
|
|
|
+ double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
|
|
|
+
|
|
|
+ double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
|
|
|
+ double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
|
|
|
+
|
|
|
+ double avoidLenth = abs(offset);
|
|
|
+ if (offset<0)
|
|
|
+ {
|
|
|
+ Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue + PI / 2));
|
|
|
+ ptLeft.speed = gpsTrace[j].speed;
|
|
|
+ ptLeft.v1 = gpsTrace[j].v1;
|
|
|
+ ptLeft.v2 = gpsTrace[j].v2;
|
|
|
+ trace.push_back(ptLeft);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - PI / 2),
|
|
|
+ carFronty + avoidLenth * sin(anglevalue - PI / 2));
|
|
|
+ ptRight.speed = gpsTrace[j].speed;
|
|
|
+ ptRight.v1 = gpsTrace[j].v1;
|
|
|
+ ptRight.v2 = gpsTrace[j].v2;
|
|
|
+
|
|
|
+
|
|
|
+ trace.push_back(ptRight);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ bool use_new_method = true;
|
|
|
+ if (use_new_method)
|
|
|
+ {
|
|
|
+ const int val = 300;
|
|
|
+ if(trace.size()>val)
|
|
|
+ {
|
|
|
+ double V = trace[300].y;
|
|
|
+ for (int j = 0; j < val; j++)
|
|
|
+ {
|
|
|
+ double t = (double)j / val;
|
|
|
+ double s = t*t*(3.-2.*t);
|
|
|
+ double ox = s;
|
|
|
+ double oy = t *( V-3.0)+3.0;
|
|
|
+
|
|
|
+ trace[j].x=ox*trace[j].x;
|
|
|
+ trace[j].y=oy;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ traceXY.clear();
|
|
|
+ for(int j=0;j<30;j++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
|
|
|
+ Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
|
|
|
+ traceXY.push_back(gpsinxy);
|
|
|
+ }
|
|
|
+ for(int j=0;j<trace.size();j++)
|
|
|
+ {
|
|
|
+ GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
|
|
|
+ Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
|
|
|
+ traceXY.push_back(gpsinxy);
|
|
|
+ }
|
|
|
+ return traceXY;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
|
|
|
+ double angle=0;
|
|
|
+ if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
|
|
|
+ {
|
|
|
+ // angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
|
|
|
+ pid_Controller->getControlDecition(gpsIns, gpsTrace, -1, -1, -1, true, false, &decition);
|
|
|
+ angle= decition->wheel_angle;
|
|
|
+ }
|
|
|
+ return angle;
|
|
|
+}
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
|
|
|
+ double speed=0;
|
|
|
+ int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
|
|
|
+ speed = gpsTrace[speedPoint].speed;
|
|
|
+ for (int i = 0; i < speedPoint; i++) {
|
|
|
+ speed = min(speed, gpsTrace[i].speed);
|
|
|
+ }
|
|
|
+ return speed;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
|
|
|
+//
|
|
|
+// if (!obsRadars.empty())
|
|
|
+// {
|
|
|
+// esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
|
|
|
+//
|
|
|
+// if (esrIndex != -1)
|
|
|
+// {
|
|
|
+// esrDistance = obsRadars[esrIndex].nomal_y;
|
|
|
+//
|
|
|
+//
|
|
|
+//
|
|
|
+// obsSpeed = obsRadars[esrIndex].speed_y;
|
|
|
+//
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// esrDistance = -1;
|
|
|
+// }
|
|
|
+//
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// esrIndex = -1;
|
|
|
+// esrDistance = -1;
|
|
|
+// }
|
|
|
+// if (esrDistance < 0) {
|
|
|
+// ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// ODS("\n------------------>ESR障碍物距离:%f ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
|
|
|
+// }
|
|
|
+//
|
|
|
+// ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
|
|
|
+//}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
|
|
|
+
|
|
|
+
|
|
|
+ int esrPathpoint;
|
|
|
+
|
|
|
+ esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
|
|
|
+
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ //优化
|
|
|
+ // double distance = 0.0;
|
|
|
+ // for(int i=0; i<esrPathpoint; ++i){
|
|
|
+ // distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
|
|
|
+ // }
|
|
|
+ // esrDistance = distance - Esr_Y_Offset;
|
|
|
+ // if(esrDistance<=0){
|
|
|
+ // esrDistance=1;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
|
|
|
+ obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
|
|
|
+
|
|
|
+ esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
|
|
|
+
|
|
|
+ if (esrIndexAvoid != -1)
|
|
|
+ {
|
|
|
+ esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ esrDistanceAvoid = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (esrDistanceAvoid < 0) {
|
|
|
+ std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ std::cout << "\nESR障碍物距离Avoid:%f %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
|
|
|
+ double preAngle = angle;
|
|
|
+
|
|
|
+ if (speed > 15)
|
|
|
+ {
|
|
|
+ if (preAngle > 350)
|
|
|
+ {
|
|
|
+ preAngle = 350;
|
|
|
+ }
|
|
|
+ if (preAngle < -350)
|
|
|
+ {
|
|
|
+ preAngle = -350;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 22)
|
|
|
+ {
|
|
|
+ if (preAngle > 200)
|
|
|
+ {
|
|
|
+ preAngle = 200;
|
|
|
+ }
|
|
|
+ if (preAngle < -200)
|
|
|
+ {
|
|
|
+ preAngle = -200;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 25)
|
|
|
+ {
|
|
|
+ if (preAngle > 150)
|
|
|
+ {
|
|
|
+ preAngle = 150;
|
|
|
+ }
|
|
|
+ if (preAngle < -150)
|
|
|
+ {
|
|
|
+ preAngle = -150;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 30)
|
|
|
+ {
|
|
|
+ if (preAngle > 70)
|
|
|
+ {
|
|
|
+ preAngle = 70;
|
|
|
+ }
|
|
|
+ if (preAngle < -70)
|
|
|
+ {
|
|
|
+ preAngle = -70;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (speed > 45) //20
|
|
|
+ {
|
|
|
+ if (preAngle > 15)
|
|
|
+ {
|
|
|
+ preAngle = 15;
|
|
|
+ }
|
|
|
+ if (preAngle < -15)
|
|
|
+ {
|
|
|
+ preAngle = -15;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return preAngle;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::limitSpeed(double angle, double speed)
|
|
|
+{
|
|
|
+ if (abs(angle) > 500 && speed > 8)
|
|
|
+ speed = 8;
|
|
|
+ else if (abs(angle) > 350 && speed > 14)
|
|
|
+ speed = 14;
|
|
|
+ else if (abs(angle) > 200 && speed > 21)
|
|
|
+ speed = 21;
|
|
|
+ else if (abs(angle) > 150 && speed > 24)
|
|
|
+ speed = 24;
|
|
|
+ else if (abs(angle) > 60 && speed > 29)
|
|
|
+ speed = 29;
|
|
|
+ else if (abs(angle) > 20 && speed > 34)
|
|
|
+ speed = 34;
|
|
|
+ return max(0.0, speed);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool iv::decition::DecideGps00::checkAvoidEnable(int roadNum)
|
|
|
+{
|
|
|
+ if ((obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth)
|
|
|
+ || (obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < 15))
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNum - roadNow > 1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow + 1; i < roadNum; i++)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow - roadNum > 1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow-1; i > roadNum; i--)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
|
|
|
+ //lsn
|
|
|
+ if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
|
|
|
+ (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (roadNum - roadNow>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow + 1; i < roadNum; i++)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow - roadNum>1)
|
|
|
+ {
|
|
|
+ for (int i = roadNow - 1; i >roadNum; i--)
|
|
|
+ {
|
|
|
+ if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
|
|
|
+ iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
|
|
|
+ }
|
|
|
+ std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
|
|
|
+ getEsrObsDistanceAvoid();
|
|
|
+
|
|
|
+ //lidarDistanceAvoid = -1; //20200103 apollo_fu
|
|
|
+
|
|
|
+ if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
|
|
|
+ {
|
|
|
+ if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
|
|
|
+ std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
|
|
|
+ std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistanceAvoid>0)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
|
|
|
+ }
|
|
|
+ else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
|
|
|
+ {
|
|
|
+ obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
|
|
|
+ obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
|
|
|
+ std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsDistanceAvoid = esrDistanceAvoid;
|
|
|
+ obsSpeedAvoid = 0 - secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
|
|
|
+ {
|
|
|
+ iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
|
|
|
+ obsLostTimeAvoid++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsLostTimeAvoid = 0;
|
|
|
+ iv::decition::DecideGps00::lastDistanceAvoid = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (obsDistanceAvoid>0)
|
|
|
+ {
|
|
|
+ iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::init() {
|
|
|
+ for (int i = 0; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ lastEsrIdVector.push_back(-1);
|
|
|
+ lastEsrCountVector.push_back(0);
|
|
|
+ GPS_INS gps_ins;
|
|
|
+ gps_ins.gps_x = 0;
|
|
|
+ gps_ins.gps_y = 0;
|
|
|
+ startAvoidGpsInsVector.push_back(gps_ins);
|
|
|
+ avoidMinDistanceVector.push_back(0);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+#include <QTime>
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+
|
|
|
+ // QTime xTime;
|
|
|
+ // xTime.start();
|
|
|
+ // qDebug("time 0 is %d ",xTime.elapsed());
|
|
|
+ double obs,obsSd;
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ float lidarXiuZheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
|
|
|
+ }
|
|
|
+ lidarDistance = obsPoint.y + lidarXiuZheng; //激光距离推到车头 gjw20191110 lidar
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance<0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+ obs = lidarDistance;
|
|
|
+ obsSd= obsPoint.obs_speed_y;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.useLidarPerPredict){
|
|
|
+ double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
|
|
|
+ if (preDis<obs){
|
|
|
+ obs = preDis;
|
|
|
+ if(abs(obs-preDis>0.5)){
|
|
|
+ obsSd = 0-realspeed;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(roadNum==roadNow){
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(obsDistance<500&&obsDistance>0){
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
|
|
|
+ }
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ //lsn
|
|
|
+ if(obs<0){
|
|
|
+ obsDisVector[roadNum]=500;
|
|
|
+ }else{
|
|
|
+ obsDisVector[roadNum]=obs;
|
|
|
+ }
|
|
|
+
|
|
|
+ // qDebug("time 3 is %d ",xTime.elapsed());
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
|
|
|
+// const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+
|
|
|
+ // QTime xTime;
|
|
|
+ // xTime.start();
|
|
|
+ // qDebug("time 0 is %d ",xTime.elapsed());
|
|
|
+ double obs,obsSd;
|
|
|
+ double obsCarCoordinateX,obsCarCoordinateY;
|
|
|
+ GPS_INS obsGeodetic;
|
|
|
+ Point2D obsFrenet,obsFrenetMid;
|
|
|
+ double obsCarCoordinateDistance=-1;
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+
|
|
|
+ obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
|
|
|
+ float lidarXiuZheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
|
|
|
+ }
|
|
|
+
|
|
|
+ obsCarCoordinateX=obsPoint.x;
|
|
|
+ obsCarCoordinateY=obsPoint.y;
|
|
|
+ obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins); //车体转大地
|
|
|
+ obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y); //大地转frenet
|
|
|
+ iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
|
|
|
+ givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+ obs = lidarDistance;
|
|
|
+
|
|
|
+ obsSd= obsPoint.obs_speed_y;
|
|
|
+
|
|
|
+ if(ServiceCarStatus.useLidarPerPredict){
|
|
|
+ double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
|
|
|
+ if (preDis<obs){
|
|
|
+ obs = preDis;
|
|
|
+ if(abs(obs-preDis>0.5)){
|
|
|
+ obsSd = 0-realspeed;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if(roadNum==roadNow){
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obsPoint.obs_type==2){
|
|
|
+ obsSpeed=-secSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obsDistance<500&&obsDistance>0){
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
|
|
|
+ }
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ //lsn
|
|
|
+ if(obs<0){
|
|
|
+ obsDisVector[roadNum]=500;
|
|
|
+ }else{
|
|
|
+ obsDisVector[roadNum]=obs;
|
|
|
+ }
|
|
|
+
|
|
|
+ // qDebug("time 3 is %d ",xTime.elapsed());
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//1220
|
|
|
+void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine) {
|
|
|
+
|
|
|
+ double obs,obsSd;
|
|
|
+
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ float lidarXiuZheng=0;
|
|
|
+ if(!ServiceCarStatus.useMobileEye){
|
|
|
+ lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
|
|
|
+ }
|
|
|
+ if(abs(obsPoint.y)>lidarXiuZheng)
|
|
|
+ lidarDistance = abs(obsPoint.y)-lidarXiuZheng; //激光距离推到车头 1220
|
|
|
+
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance<0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(lidarDistance<0){
|
|
|
+ lidarDistance=500;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+
|
|
|
+ if(lidarDistance==500){
|
|
|
+ lidarDistance=-1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+
|
|
|
+ obs=lidarDistance;
|
|
|
+ // obsSpeed = 0 - secSpeed;
|
|
|
+ obsSd = 0 -secSpeed;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(roadNum==roadNow){
|
|
|
+ obsDistance=obs;
|
|
|
+ obsSpeed=obsSd;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ if (obsDistance <0 && obsLostTime<4)
|
|
|
+ {
|
|
|
+ obsDistance = lastDistance;
|
|
|
+ obsLostTime++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsLostTime = 0;
|
|
|
+ lastDistance = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100){
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ //lsn
|
|
|
+ if(obs<0){
|
|
|
+ obsDisVector[roadNum]=500;
|
|
|
+ }else{
|
|
|
+ obsDisVector[roadNum]=obs;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
|
|
|
+ double preObsDis=500;
|
|
|
+
|
|
|
+ if(!lidar_per.empty()){
|
|
|
+ preObsDis=PredictObsDistance( realSpeed, gpsTrace, lidar_per);
|
|
|
+ lastPreObsDistance=preObsDis;
|
|
|
+
|
|
|
+ }else{
|
|
|
+ preObsDis=lastPreObsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.mfttc = preObsDis;
|
|
|
+ return preObsDis;
|
|
|
+ // if(preObsDis<obsDistance){
|
|
|
+ // obsDistance=preObsDis;
|
|
|
+ // lastDistance=obsDistance;
|
|
|
+ // }
|
|
|
+}
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
|
|
|
+ roadPre = -1;
|
|
|
+
|
|
|
+ for (int i = 0; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (lidarGridPtr!=NULL)
|
|
|
+ {
|
|
|
+ hasCheckedAvoidLidar = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ for(int i=0; i<roadSum;i++){
|
|
|
+ std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ checkAvoidObsTimes++;
|
|
|
+ if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
|
|
|
+ {
|
|
|
+ return - 1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 1; i < roadSum; i++)
|
|
|
+ {
|
|
|
+ if (roadNow + i < roadSum) {
|
|
|
+ // avoidX = (roadOri-roadNow-i)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ if (checkAvoidEnable(roadNow+i))
|
|
|
+ {
|
|
|
+ /*if (roadNow==roadOri)
|
|
|
+ {
|
|
|
+ avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ } */
|
|
|
+ avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGpsInsVector[roadNow] = now_gps_ins;
|
|
|
+ roadPre = roadNow + i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNow - i >= 0)
|
|
|
+ {
|
|
|
+ // avoidX = (roadOri - roadNow+i)*roadWidth;
|
|
|
+ avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ if (checkAvoidEnable(roadNow - i))
|
|
|
+ {
|
|
|
+ /*if (roadNow == roadOri)
|
|
|
+ {
|
|
|
+ avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoid_gps_ins = now_gps_ins;
|
|
|
+ }*/
|
|
|
+ avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGpsInsVector[roadNow] = now_gps_ins;
|
|
|
+ roadPre = roadNow - i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ return roadPre;
|
|
|
+}
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per)
|
|
|
+{
|
|
|
+ roadPre = -1;
|
|
|
+
|
|
|
+ for (int i = 0; i <roadSum; i++)
|
|
|
+ {
|
|
|
+ gpsTraceBack.clear();
|
|
|
+ avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
|
|
|
+ gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
|
|
|
+ computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (lidarGridPtr != NULL)
|
|
|
+ {
|
|
|
+ hasCheckedBackLidar = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ checkBackObsTimes++;
|
|
|
+ if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
|
|
|
+ {
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
|
|
|
+ (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
|
|
|
+ {
|
|
|
+ roadPre = roadOri;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (roadNow-roadOri>1)
|
|
|
+ {
|
|
|
+ for (int i = roadOri + 1;i < roadNow;i++) {
|
|
|
+ if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> avoidMinDistanceVector[i])&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
+ {
|
|
|
+ roadPre = i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ else if (roadNow <roadOri-1)
|
|
|
+ {
|
|
|
+ for (int i = roadOri - 1;i > roadNow;i--) {
|
|
|
+ if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
|
|
|
+ (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
|
|
|
+ {
|
|
|
+ roadPre = i;
|
|
|
+ return roadPre;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return roadPre;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::trumpet() {
|
|
|
+ if (trumpetFirstCount)
|
|
|
+ {
|
|
|
+ trumpetFirstCount = false;
|
|
|
+ trumpetLastTime= GetTickCount();
|
|
|
+ trumpetTimeSpan = 0.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ trumpetStartTime= GetTickCount();
|
|
|
+ trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
|
|
|
+ trumpetLastTime = trumpetStartTime;
|
|
|
+ }
|
|
|
+
|
|
|
+ return trumpetTimeSpan;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::transferP() {
|
|
|
+ if (transferFirstCount)
|
|
|
+ {
|
|
|
+ transferFirstCount = false;
|
|
|
+ transferLastTime= GetTickCount();
|
|
|
+ transferTimeSpan = 0.0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ transferStartTime= GetTickCount();
|
|
|
+ transferTimeSpan += transferStartTime - transferLastTime;
|
|
|
+ transferLastTime = transferStartTime;
|
|
|
+ }
|
|
|
+
|
|
|
+ return transferTimeSpan;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::handBrakePark(iv::decition::Decition decition, long duringTime, GPS_INS now_gps_ins)
|
|
|
+{
|
|
|
+ if (abs(now_gps_ins.speed)>0.1)
|
|
|
+ {
|
|
|
+ decition->accelerator = 0;
|
|
|
+ decition->brake = 20;
|
|
|
+ decition->wheel_angle = 0;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ decition->accelerator = 0;
|
|
|
+ decition->brake = 20;
|
|
|
+ decition->wheel_angle = 0;
|
|
|
+ handPark = true;
|
|
|
+ handParkTime = duringTime;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins)
|
|
|
+{
|
|
|
+ gmapsL.clear();
|
|
|
+ gmapsR.clear();
|
|
|
+ for (int i = 0; i < 31; i++)
|
|
|
+ {
|
|
|
+ std::vector<iv::GPSData> gpsMapLineBeside;
|
|
|
+ // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
|
|
|
+ gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
|
|
|
+ gmapsL.push_back(gpsMapLineBeside);
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < 31; i++)
|
|
|
+ {
|
|
|
+ std::vector<iv::GPSData> gpsMapLineBeside;
|
|
|
+ // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
|
|
|
+ gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
|
|
|
+ gmapsR.push_back(gpsMapLineBeside);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr)
|
|
|
+{
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
|
|
|
+ double lidarDistance = obsPoint.y - 2.5; //激光距离推到车头
|
|
|
+ // ODS("\n超车雷达距离:%f\n", lidarDistance);
|
|
|
+ if (lidarDistance >-20 && lidarDistance<35)
|
|
|
+ {
|
|
|
+ checkChaoCheBackCounts = 0;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ checkChaoCheBackCounts++;
|
|
|
+ }
|
|
|
+ if (checkChaoCheBackCounts>2)
|
|
|
+ {
|
|
|
+ checkChaoCheBackCounts = 0;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s)
|
|
|
+{
|
|
|
+ Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps); //大地转车体
|
|
|
+
|
|
|
+ ServiceCarStatus.group_x_local=pt.x;
|
|
|
+ // ServiceCarStatus.group_y_local=pt.y;
|
|
|
+ ServiceCarStatus.group_y_local=s;
|
|
|
+ if(realspeed < 0.36)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.group_velx_local = 0;
|
|
|
+ ServiceCarStatus.group_vely_local = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.group_velx_local = realspeed * sin(theta) / 3.6;
|
|
|
+ ServiceCarStatus.group_vely_local = realspeed * cos(theta) / 3.6;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.group_pathpoint = PathPoint;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, const std::vector<GPSData> gpsMapLine,
|
|
|
+ int traffic_light_pathpoint, int pathpoint,float secSpeed)
|
|
|
+{
|
|
|
+ float traffic_speed = 200;
|
|
|
+ float traffic_dis = 0;
|
|
|
+
|
|
|
+ if(abs(secSpeed)<0.1)
|
|
|
+ {
|
|
|
+ secSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(pathpoint <= traffic_light_pathpoint)
|
|
|
+ {
|
|
|
+ for(int i=pathpoint;i<traffic_light_pathpoint;i++)
|
|
|
+ {
|
|
|
+ traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for(int i = pathpoint; i < gpsMapLine.size()-1; i++)
|
|
|
+ {
|
|
|
+ traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ for(int i = 0; i < traffic_light_pathpoint; i++)
|
|
|
+ {
|
|
|
+ traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(traffic_light_color==0 && traffic_dis<10)
|
|
|
+ {
|
|
|
+ traffic_speed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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,
|
|
|
+ int pathpoint,float secSpeed,float dSpeed){
|
|
|
+ float traffic_speed=200;
|
|
|
+ float traffic_dis=0;
|
|
|
+ float passTime;
|
|
|
+ float passSpeed;
|
|
|
+ bool passEnable=false;
|
|
|
+
|
|
|
+ if(abs(secSpeed)<0.1){
|
|
|
+ secSpeed=0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if(pathpoint <= traffic_light_pathpoint){
|
|
|
+ for(int i=pathpoint;i<traffic_light_pathpoint;i++){
|
|
|
+ traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
|
|
|
+ traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ for(int i=0;i<traffic_light_pathpoint;i++){
|
|
|
+ traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // if(traffic_light_color != 0)
|
|
|
+ // {
|
|
|
+ // int a = 3;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+// if(traffic_light_color==0 && traffic_dis<10){
|
|
|
+// traffic_speed=0;
|
|
|
+// }
|
|
|
+
|
|
|
+ if(traffic_light_color==2 && traffic_dis<10){ //cxw,1xhonglvdeng gaibian
|
|
|
+ traffic_speed=0;
|
|
|
+// return traffic_speed;
|
|
|
+// if(traffic_dis>3.0)
|
|
|
+// {
|
|
|
+// traffic_speed=sqrt(traffic_dis*2.0*9.8*0.5);
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// //minDecelerate=-0.7;
|
|
|
+// traffic_speed=0;
|
|
|
+// }
|
|
|
+ return traffic_speed;
|
|
|
+ }
|
|
|
+ // else //20200108
|
|
|
+ // {
|
|
|
+ // traffic_speed=10;
|
|
|
+ // }
|
|
|
+// return traffic_speed;
|
|
|
+
|
|
|
+ passSpeed = min((float)(dSpeed/3.6),secSpeed);
|
|
|
+ passTime = traffic_dis/(dSpeed/3.6);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+//1+x V2R //1:绿灯 2：红灯 3：黄灯
|
|
|
+ //最开始0x0: 红色 0x1: 黄色 0x2: 绿色
|
|
|
+
|
|
|
+ switch(traffic_light_color){
|
|
|
+ case 2://case 0: //for 1+x修改
|
|
|
+ if(passTime>traffic_light_time+1 && traffic_dis>10){
|
|
|
+ passEnable=true;
|
|
|
+ }else{
|
|
|
+ passEnable=false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 3:// case 1:
|
|
|
+ if(passTime<traffic_light_time-1 && traffic_dis<10){
|
|
|
+ passEnable=true;
|
|
|
+ }else{
|
|
|
+ passEnable = false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 1://case 2:
|
|
|
+ if(passTime<traffic_light_time){
|
|
|
+ passEnable= true;
|
|
|
+ }else{
|
|
|
+ passEnable=false;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+
|
|
|
+
|
|
|
+ default:
|
|
|
+ passEnable=true; //20230413
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(!passEnable){
|
|
|
+ if(traffic_dis<5){
|
|
|
+ traffic_speed=0;
|
|
|
+ }else if(traffic_dis<10){
|
|
|
+ traffic_speed=5;
|
|
|
+ }else if(traffic_dis<20){
|
|
|
+ traffic_speed=15;
|
|
|
+ }else if(traffic_dis<30){
|
|
|
+ traffic_speed=25;
|
|
|
+ }else if(traffic_dis<50){
|
|
|
+ traffic_speed=30;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return traffic_speed;
|
|
|
+
|
|
|
+}
|
|
|
+void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs,
|
|
|
+ const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
|
|
|
+{
|
|
|
+ // Point2D obsCombinePoint = Point2D(-1,-1);
|
|
|
+ iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ double obsSd;
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ // lidarDistance = lastlidarDistance;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ // lidarDistance = obsPoint.y-2.5; //激光距离推到车头
|
|
|
+ iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;//激光距离推到车头 此处的参数要不要改改！！！！ 2022 apollo_fu
|
|
|
+
|
|
|
+ // lidarDistance=-1;
|
|
|
+ if (lidarDistance < 0)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ FrenetPoint esr_obs_frenet_point;
|
|
|
+ getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
|
|
|
+
|
|
|
+ if(lidarDistance < 0){
|
|
|
+ lidarDistance = 500;
|
|
|
+ }
|
|
|
+ if(esrDistance < 0){
|
|
|
+ esrDistance = 500;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "激光雷达距离:" << lidarDistance << std::endl;
|
|
|
+ std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "毫米波距离:" << esrDistance << std::endl;
|
|
|
+ myesrDistance = esrDistance;
|
|
|
+
|
|
|
+ if(lidarDistance == 500)
|
|
|
+ {
|
|
|
+ lidarDistance = -1;
|
|
|
+ }
|
|
|
+ if(esrDistance == 500)
|
|
|
+ {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ ServiceCarStatus.mRadarObs = esrDistance;
|
|
|
+ ServiceCarStatus.mLidarObs = lidarDistance;
|
|
|
+
|
|
|
+ if (esrDistance>0 && lidarDistance > 0)
|
|
|
+ {
|
|
|
+ if (lidarDistance >= esrDistance)
|
|
|
+ {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd = obsSpeed;
|
|
|
+ //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ }
|
|
|
+ else if (!ServiceCarStatus.obs_radar.empty())
|
|
|
+ {
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ // obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
|
|
|
+ obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs=lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ obsSd = 0 -secSpeed * cos(car_now_frenet_point.tangent_Ang-PI/2);
|
|
|
+ std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (esrDistance>0)
|
|
|
+ {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
|
|
|
+ obsSd = obsSpeed;
|
|
|
+ //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ }
|
|
|
+ else if (lidarDistance > 0)
|
|
|
+ {
|
|
|
+ obs = lidarDistance;
|
|
|
+ // obsCombinePoint = obsPoint;
|
|
|
+ obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
|
|
|
+ std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obs = esrDistance;
|
|
|
+ // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
|
|
|
+ obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
|
|
|
+ }
|
|
|
+
|
|
|
+ obsDistance = obs;
|
|
|
+ obsSpeed = obsSd;
|
|
|
+
|
|
|
+ std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
|
|
|
+
|
|
|
+ ServiceCarStatus.mObs = obsDistance;
|
|
|
+ if(ServiceCarStatus.mObs>100)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mObs =-1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsDistance>0)
|
|
|
+ {
|
|
|
+ lastDistance = obsDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obs<0)
|
|
|
+ {
|
|
|
+ obsDistance=500;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsDistance=obs;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum)
|
|
|
+{
|
|
|
+ esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
|
|
|
+
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
|
|
|
+ obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point,
|
|
|
+ FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
|
|
|
+{
|
|
|
+ esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
|
|
|
+ if (esrIndex != -1)
|
|
|
+ {
|
|
|
+ //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
|
|
|
+ //严格来说应是 esrDistance = 障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
|
|
|
+ esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset; //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
|
|
|
+
|
|
|
+ double speedx = ServiceCarStatus.obs_radar[esrIndex].speed_x; //障碍物相对于车辆x轴的速度
|
|
|
+ double speedy = ServiceCarStatus.obs_radar[esrIndex].speed_y; //障碍物相对于车辆y轴的速度
|
|
|
+ double speed_combine = sqrt(speedx * speedx + speedy * speedy); //将x、y轴两个方向的速度求矢量和
|
|
|
+ //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
|
|
|
+ //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
|
|
|
+ double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
|
|
|
+
|
|
|
+ obsSpeed = speed_combine * cos(Etheta); //由speed_combine分解的s轴方向上的速度
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ esrDistance = -1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine)
|
|
|
+{
|
|
|
+ v2xTrafficVector.clear();
|
|
|
+ for (int var = 0; var < gpsMapLine.size(); var++)
|
|
|
+ {
|
|
|
+ if(gpsMapLine[var]->roadMode == 6 || gpsMapLine[var]->mode2 == 1000001)
|
|
|
+ {
|
|
|
+ v2xTrafficVector.push_back(var);
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
|
|
|
+ int pathpoint,float secSpeed,float dSpeed, bool circleMode)
|
|
|
+{
|
|
|
+ float trafficSpeed = 200;
|
|
|
+ int nearTraffixPoint = -1;
|
|
|
+ float nearTrafficDis = 0;
|
|
|
+ int traffic_color = 0;
|
|
|
+ int traffic_time = 0;
|
|
|
+ bool passThrough = false;
|
|
|
+ float dSecSpeed = dSpeed / 3.6;
|
|
|
+
|
|
|
+ if(v2xTrafficVector.empty())
|
|
|
+ {
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+ if(!circleMode)
|
|
|
+ {
|
|
|
+ if(pathpoint > v2xTrafficVector.back())
|
|
|
+ {
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for(int i = 0; i < v2xTrafficVector.size();i++)
|
|
|
+ {
|
|
|
+ if (pathpoint <= v2xTrafficVector[i])
|
|
|
+ {
|
|
|
+ nearTraffixPoint = v2xTrafficVector[i];
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(circleMode)
|
|
|
+ {
|
|
|
+ if(pathpoint > v2xTrafficVector.back())
|
|
|
+ {
|
|
|
+ nearTraffixPoint = v2xTrafficVector[0];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for(int i=0; i < v2xTrafficVector.size();i++)
|
|
|
+ {
|
|
|
+ if (pathpoint <= v2xTrafficVector[i])
|
|
|
+ {
|
|
|
+ nearTraffixPoint = v2xTrafficVector[i];
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(nearTraffixPoint != -1)
|
|
|
+ {
|
|
|
+ for(int i = pathpoint;i < nearTraffixPoint;i++)
|
|
|
+ {
|
|
|
+ nearTrafficDis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(nearTrafficDis>50)
|
|
|
+ {
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ int roadMode = gpsMapLine[pathpoint]->roadMode;
|
|
|
+ if(roadMode == 14 || roadMode == 16)
|
|
|
+ {
|
|
|
+ traffic_color = trafficLight.leftColor;
|
|
|
+ traffic_time = trafficLight.leftTime;
|
|
|
+ }
|
|
|
+ else if(roadMode == 15 ||roadMode == 17)
|
|
|
+ {
|
|
|
+ traffic_color = trafficLight.rightColor;
|
|
|
+ traffic_time = trafficLight.rightTime;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ traffic_color = trafficLight.straightColor;
|
|
|
+ traffic_time = trafficLight.straightTime;
|
|
|
+ }
|
|
|
+
|
|
|
+// passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
|
|
|
+// if(passThrough)
|
|
|
+// {
|
|
|
+// return trafficSpeed;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
|
|
|
+// if(nearTrafficDis < 6)
|
|
|
+// {
|
|
|
+// float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
|
|
|
+// minDecelerate = min(minDecelerate,decelerate);
|
|
|
+// }
|
|
|
+// return trafficSpeed;
|
|
|
+// }
|
|
|
+ double fTrafficBrake = 1.0; //移植吉利项目的v2x
|
|
|
+ passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
|
|
|
+ if(passThrough){
|
|
|
+ return trafficSpeed;
|
|
|
+ }else{
|
|
|
+ trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
|
|
|
+ if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
|
|
|
+ if(fabs(nearTrafficDis)<1.0)
|
|
|
+ {
|
|
|
+ minDecelerate = -1.0 * fTrafficBrake;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
|
|
|
+ {
|
|
|
+ float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
|
|
|
+ minDecelerate=min(minDecelerate,decelerate);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return trafficSpeed;
|
|
|
+ }
|
|
|
+
|
|
|
+ return trafficSpeed;
|
|
|
+}
|
|
|
+
|
|
|
+//bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
|
|
|
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed)
|
|
|
+{
|
|
|
+ float passTime=0;
|
|
|
+ double fAcc = 0.6;
|
|
|
+ if(dSecSpeed == 0)return true;
|
|
|
+ if (trafficColor == 2 || trafficColor == 3)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ else if(trafficColor==0)
|
|
|
+ {
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ passTime=trafficDis/dSecSpeed;
|
|
|
+ if(passTime+1< trafficTime)
|
|
|
+ {
|
|
|
+ double passTime2= trafficTime - 0.1;
|
|
|
+ double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
|
|
|
+ if((disthru -1.0)< trafficDis )
|
|
|
+ {
|
|
|
+ if((realSecSpeed<3.0)&&(trafficDis>10))
|
|
|
+ {
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis)
|
|
|
+{
|
|
|
+ float limit=200;
|
|
|
+ double fBrake = 1.0;
|
|
|
+ if(trafficDis < 1.0)
|
|
|
+ {
|
|
|
+ limit =0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
|
|
|
+ }
|
|
|
+ return limit;
|
|
|
+ if(trafficDis<10)
|
|
|
+ {
|
|
|
+ limit = 0;
|
|
|
+ }
|
|
|
+ else if(trafficDis<15)
|
|
|
+ {
|
|
|
+ limit = 5;
|
|
|
+ }
|
|
|
+ else if(trafficDis<20)
|
|
|
+ {
|
|
|
+ limit=10;
|
|
|
+ }
|
|
|
+ else if(trafficDis<30)
|
|
|
+ {
|
|
|
+ limit=15;
|
|
|
+ }
|
|
|
+ return limit;
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine)
|
|
|
+{
|
|
|
+ static int obstacle_disable = 0;
|
|
|
+ static int speed_slowdown_flag = 0;
|
|
|
+ static bool lock_flag = false;
|
|
|
+ double forecast_distance = 0;
|
|
|
+ int forecast_point_num = 0;
|
|
|
+ bool cross = false;
|
|
|
+
|
|
|
+ double secLowSpeed = ServiceCarStatus.mroadmode_vel.mfmode18 / 3.6; //m/s
|
|
|
+ if(secSpeed > secLowSpeed)
|
|
|
+ {
|
|
|
+ forecast_distance = secSpeed * secSpeed - secLowSpeed * secLowSpeed + 5;
|
|
|
+ forecast_point_num = ((int)forecast_distance) * 10;
|
|
|
+ if((PathPoint + forecast_point_num + 2) > gpsMapLine.size())
|
|
|
+ forecast_point_num = 0;
|
|
|
+ }
|
|
|
+ if((PathPoint + forecast_point_num-8 > 0) && (PathPoint + forecast_point_num + 8 < gpsMapLine.size()))
|
|
|
+ {
|
|
|
+ for(int i = PathPoint + forecast_point_num - 8; i < PathPoint + forecast_point_num + 8; i++)
|
|
|
+ {
|
|
|
+ if(gpsMapLine[i]->mode2 == 5000)
|
|
|
+ cross = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d", PathPoint + forecast_point_num,forecast_point_num,cross);
|
|
|
+ if(gpsMapLine[PathPoint]->mode2 == 3000)
|
|
|
+ {
|
|
|
+ if(obsDistance>4)
|
|
|
+ { //7 zj-5
|
|
|
+ obsDistance = 200;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ lock_flag = false;
|
|
|
+ obsSpeed = -realspeed / 3.6;
|
|
|
+ }
|
|
|
+
|
|
|
+ if((realspeed > 3) && (lock_flag == false))
|
|
|
+ {
|
|
|
+ minDecelerate = -0.5;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed,3.0);
|
|
|
+ lock_flag = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(gpsMapLine[PathPoint]->mode2 == 3001)
|
|
|
+ {
|
|
|
+ obstacle_disable = 1;
|
|
|
+ }
|
|
|
+ else if(gpsMapLine[PathPoint]->mode2 == 3002)
|
|
|
+ {
|
|
|
+ obstacle_disable = 0;
|
|
|
+ }
|
|
|
+ else if(gpsMapLine[PathPoint]->mode2 == 4000)
|
|
|
+ {
|
|
|
+ //ServiceCarStatus.msysparam.vehWidth=5.6;
|
|
|
+ }
|
|
|
+ else if(cross == true)
|
|
|
+ {
|
|
|
+ speed_slowdown_flag =1;
|
|
|
+ lock_flag = false;
|
|
|
+ }
|
|
|
+ else if(gpsMapLine[PathPoint]->mode2==5001)
|
|
|
+ {
|
|
|
+ speed_slowdown_flag=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(obstacle_disable==1)
|
|
|
+ {
|
|
|
+ obsDistance=200;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(speed_slowdown_flag == 1)
|
|
|
+ {
|
|
|
+ if((realspeed > ServiceCarStatus.mroadmode_vel.mfmode18) && (lock_flag == false))
|
|
|
+ {
|
|
|
+ minDecelerate = -0.3;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ dSpeed = min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
|
|
|
+ lock_flag = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps)
|
|
|
+{
|
|
|
+ if(roadAim==roadOri)
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ float x = 0;
|
|
|
+ float veh_to_roadSide = (gps->mfLaneWidth - ServiceCarStatus.msysparam.vehWidth) * 0.5;
|
|
|
+ float roadSide_to_roadSide = ServiceCarStatus.msysparam.vehWidth;
|
|
|
+ if(!ServiceCarStatus.inRoadAvoid)
|
|
|
+ {
|
|
|
+ x = (roadOri-roadAim) * gps->mfLaneWidth;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ int num = roadOri - roadAim;
|
|
|
+ switch (abs(num) % 3)
|
|
|
+ {
|
|
|
+ case 0:
|
|
|
+ x = (num / 3) * gps->mfLaneWidth;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ if(num > 0)
|
|
|
+ {
|
|
|
+ x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ if(num > 0)
|
|
|
+ {
|
|
|
+ x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide + roadSide_to_roadSide;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide - roadSide_to_roadSide;
|
|
|
+ }
|
|
|
+
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return x;
|
|
|
+}
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint)
|
|
|
+{
|
|
|
+ double dist_to_end = 0;
|
|
|
+ for(int i = pathpoint;i < gpsMapLine.size() - 1; i++)
|
|
|
+ {
|
|
|
+ if(gpsMapLine[i]->mode2 != 23)
|
|
|
+ dist_to_end += GetDistance(*gpsMapLine[i+1], *gpsMapLine[i]);
|
|
|
+ }
|
|
|
+ return dist_to_end;
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight )
|
|
|
+{
|
|
|
+ double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
|
|
|
+ *avoidXRight = ((int)RightBoundary);
|
|
|
+ double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
|
|
|
+ *avoidXLeft = (-(int)LeftBoundary);
|
|
|
+}
|
|
|
+
|
|
|
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,double* avoidXLeft, double* avoidXRight )
|
|
|
+{
|
|
|
+ double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
|
|
|
+ *avoidXRight = ((int)RightBoundary);
|
|
|
+ double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
|
|
|
+ *avoidXLeft = (-(int)LeftBoundary);
|
|
|
+}
|
|
|
+
|
|
|
+int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
|
|
|
+ int avoidLeft,int avoidRight,int offsetLast)
|
|
|
+{
|
|
|
+ int signed_record_avoidX = offsetLast, record_avoidX = 20;
|
|
|
+ double obs_cur_distance =500, record_obstacle_distance;
|
|
|
+ iv::Point2D obs_spline_record;
|
|
|
+ double step = 0.5; //lyp
|
|
|
+
|
|
|
+ obs_property.clear();
|
|
|
+ for (double i = avoidLeft; i <= avoidRight; i=i+step) //由原来的1米改成0.5米，将i由int 转换成double
|
|
|
+ {
|
|
|
+ obsvalue x_value;
|
|
|
+ obsvalue *x = &x_value;
|
|
|
+ x_value.avoid_distance = i;
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
|
|
|
+ obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
|
|
|
+ obs_property.push_back(x_value);
|
|
|
+ if(i==offsetLast)
|
|
|
+ {
|
|
|
+ obs_cur_distance=x_value.obs_distance;
|
|
|
+ record_obstacle_distance=obs_cur_distance;
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
|
|
|
+
|
|
|
+ obsSpline.push_back(obs_spline_record);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (lidarGridPtr != NULL)
|
|
|
+ {
|
|
|
+ hasCheckedAvoidLidar = true;
|
|
|
+ }
|
|
|
+ if(offsetLast==0)
|
|
|
+ {
|
|
|
+ unsigned int vector_num_record=0;
|
|
|
+ for(int j = 0; j < obs_property.size(); j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > 100)
|
|
|
+ {
|
|
|
+ if(abs(obs_property[j].avoid_distance) < record_avoidX)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ vector_num_record = j;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
|
|
|
+ {
|
|
|
+ //int obs_test=abs(signed_record_avoidX)-1;
|
|
|
+ int obs_test = vector_num_record-1;
|
|
|
+ if(obs_property[obs_test].obs_distance > 100)
|
|
|
+ {
|
|
|
+ signed_record_avoidX -= 1;
|
|
|
+ givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //added by lyp
|
|
|
+ // if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
|
|
|
+ // {
|
|
|
+ // //int obs_test=abs(signed_record_avoidX)-1;
|
|
|
+ // int obs_test = vector_num_record-1;
|
|
|
+ // if(obs_property[obs_test].obs_distance > 100)
|
|
|
+ // {
|
|
|
+ // signed_record_avoidX -= 1;
|
|
|
+ // givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+
|
|
|
+ if(signed_record_avoidX == 0)
|
|
|
+ {
|
|
|
+ for(int j = 0; j < obs_property.size(); j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > obs_cur_distance+15)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > record_obstacle_distance)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ record_obstacle_distance = obs_property[j].obs_distance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for(int j = 0; j < obs_property.size();j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > 100)
|
|
|
+ {
|
|
|
+ if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
|
|
|
+ {
|
|
|
+ record_avoidX=abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX=obs_property[j].avoid_distance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(signed_record_avoidX==0)
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ else if(obs_cur_distance>30)
|
|
|
+ {
|
|
|
+ signed_record_avoidX = offsetLast;
|
|
|
+ }
|
|
|
+ else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
|
|
|
+ {
|
|
|
+ for(int j = 0;j<obs_property.size();j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance>obs_cur_distance+15)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance>record_obstacle_distance)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ record_obstacle_distance = obs_property[j].obs_distance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
|
|
|
+ {
|
|
|
+ avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGps = now_gps_ins;
|
|
|
+ return signed_record_avoidX;
|
|
|
+ }
|
|
|
+ return offsetLast;
|
|
|
+}
|
|
|
+
|
|
|
+double iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
|
|
|
+ double avoidLeft,double avoidRight,double offsetLast)
|
|
|
+{
|
|
|
+ double signed_record_avoidX = offsetLast, record_avoidX = 20;
|
|
|
+ double obs_cur_distance =500, record_obstacle_distance;
|
|
|
+ iv::Point2D obs_spline_record;
|
|
|
+ double step = 0.5;
|
|
|
+
|
|
|
+ obs_property.clear();
|
|
|
+ for (double i = avoidLeft; i <= avoidRight; i=i+step) //由原来的1米改成0.5米，将i由int 转换成double
|
|
|
+ {
|
|
|
+ obsvalue x_value;
|
|
|
+ obsvalue *x = &x_value;
|
|
|
+ x_value.avoid_distance = i;
|
|
|
+ gpsTraceAvoid.clear();
|
|
|
+ gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
|
|
|
+ obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
|
|
|
+ obs_property.push_back(x_value);
|
|
|
+ if(i==offsetLast)
|
|
|
+ {
|
|
|
+ obs_cur_distance=x_value.obs_distance;
|
|
|
+ record_obstacle_distance=obs_cur_distance;
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
|
|
|
+
|
|
|
+ obsSpline.push_back(obs_spline_record);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (lidarGridPtr != NULL)
|
|
|
+ {
|
|
|
+ hasCheckedAvoidLidar = true;
|
|
|
+ }
|
|
|
+ if(offsetLast==0)//上次的避障距离是0或者沿参考线行驶第一次避障
|
|
|
+ {
|
|
|
+ unsigned int vector_num_record=0;
|
|
|
+ for(int j = 0; j < obs_property.size(); j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > 100) //障碍物距离大于100米认为可以臂章
|
|
|
+ {
|
|
|
+ if(abs(obs_property[j].avoid_distance) < record_avoidX)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance); //找到距离车辆当前行驶的位置横向最近的臂章距离
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ vector_num_record = j;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //优先向左避障
|
|
|
+ if((signed_record_avoidX < 0) && (signed_record_avoidX-2*step>= avoidLeft)) //-0.5 -1 -1.5都可以避障的时候选则-1.5,
|
|
|
+ {
|
|
|
+ //int obs_test=abs(signed_record_avoidX)-1;
|
|
|
+ int obs_test = vector_num_record-1;
|
|
|
+ int obs_testneighbor = obs_test-1;
|
|
|
+ if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100))
|
|
|
+ {
|
|
|
+ signed_record_avoidX =signed_record_avoidX-2*step;
|
|
|
+ givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if((signed_record_avoidX < 0) && (signed_record_avoidX-1*step>= avoidLeft))//-0.5 -1 可以避障的时候选则-1,
|
|
|
+ {
|
|
|
+ //int obs_test=abs(signed_record_avoidX)-1;
|
|
|
+ int obs_test = vector_num_record-1;
|
|
|
+ if(obs_property[obs_test].obs_distance > 100)
|
|
|
+ {
|
|
|
+ signed_record_avoidX -= 1;
|
|
|
+ givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if((signed_record_avoidX > 0) && (signed_record_avoidX+2*step<= avoidRight)) //0.5 1 1.5都可以避障的时候选则1.5,
|
|
|
+ {
|
|
|
+ int obs_test = vector_num_record+1;
|
|
|
+ int obs_testneighbor = obs_test+1;
|
|
|
+ if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100)
|
|
|
+ )
|
|
|
+ {
|
|
|
+ signed_record_avoidX =signed_record_avoidX+2*step;
|
|
|
+ givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if((signed_record_avoidX>0) && (signed_record_avoidX+1*step<= avoidRight))//0.5 1 可以避障的时候选则1,
|
|
|
+ {
|
|
|
+ int obs_test = vector_num_record+1;
|
|
|
+ if(obs_property[obs_test].obs_distance > 100)
|
|
|
+ {
|
|
|
+ signed_record_avoidX -= 1;
|
|
|
+ givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (signed_record_avoidX < 0)
|
|
|
+ {
|
|
|
+ signed_record_avoidX=signed_record_avoidX; //只有-0.5 0.5 的时候就选则-0.5 0.5,优先选择-0.5
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(signed_record_avoidX == 0)
|
|
|
+ {
|
|
|
+ for(unsigned int j = 0; j < obs_property.size(); j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > obs_cur_distance+15)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > record_obstacle_distance)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ record_obstacle_distance = obs_property[j].obs_distance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","offsetLast==0,and signed_record_avoidX==0");//20230213
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ for(int j = 0; j < obs_property.size();j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance > 100)
|
|
|
+ {
|
|
|
+ if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
|
|
|
+ {
|
|
|
+ record_avoidX=abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX=obs_property[j].avoid_distance;//返回的是距离原始参考路线最近的路线
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(signed_record_avoidX==0) //优先返回原参考路线
|
|
|
+ {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ else if(obs_cur_distance>30)//如果当前障碍物距离大于30米，继续当前的避障距离
|
|
|
+ {
|
|
|
+ signed_record_avoidX = offsetLast;
|
|
|
+ }
|
|
|
+ else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
|
|
|
+ {
|
|
|
+ for(unsigned int j = 0;j<obs_property.size();j++)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance>obs_cur_distance+15)
|
|
|
+ {
|
|
|
+ if(obs_property[j].obs_distance>record_obstacle_distance)
|
|
|
+ {
|
|
|
+ record_avoidX = abs(obs_property[j].avoid_distance);
|
|
|
+ signed_record_avoidX = obs_property[j].avoid_distance;
|
|
|
+ record_obstacle_distance = obs_property[j].obs_distance;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ givlog->debug("decition_brain","obs_cur_distance<=30and signed_record_avoidX==offsetLast");//20230213
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
|
|
|
+ {
|
|
|
+ avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
|
|
|
+ startAvoidGps = now_gps_ins;
|
|
|
+ return signed_record_avoidX;
|
|
|
+ }
|
|
|
+ return offsetLast;
|
|
|
+}
|
|
|
+
|
|
|
+//iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
|
|
|
+// const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva)
|
|
|
+iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, double roadNum,
|
|
|
+ const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva) //20230213
|
|
|
+{
|
|
|
+ double obs=-1,obsSd=-1;
|
|
|
+ double obsCarCoordinateX,obsCarCoordinateY;
|
|
|
+ GPS_INS obsGeodetic;
|
|
|
+ Point2D obsFrenet,obsFrenetMid;
|
|
|
+ double obsCarCoordinateDistance=-1;
|
|
|
+
|
|
|
+ if (lidarGridPtr == NULL)
|
|
|
+ {
|
|
|
+ lidarDistance = lastLidarDis;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
|
|
|
+ obsCarCoordinateDistance=obsPoint.y;
|
|
|
+
|
|
|
+ if((obsCarCoordinateDistance > 0) && (obsCarCoordinateDistance < 100))
|
|
|
+ {
|
|
|
+ obsCarCoordinateX = obsPoint.x;
|
|
|
+ obsCarCoordinateY = obsPoint.y;
|
|
|
+ obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins); //车体转大地
|
|
|
+ obsFrenetMid = cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y); //大地转frenet
|
|
|
+ iv::Point2D now_s_d = cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
|
|
|
+ givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
|
|
|
+ obsFrenet.s=obsFrenetMid.s-now_s_d.s;
|
|
|
+ obsFrenet.d=roadNum;
|
|
|
+
|
|
|
+ //test
|
|
|
+ //obsFrenet.s=obsPoint.y;
|
|
|
+ //obsFrenet.d=roadNum;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ obsFrenet.s=500;
|
|
|
+ obsFrenet.d=roadNum;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (obsFrenet.s<0)
|
|
|
+ {
|
|
|
+ obsFrenet.s=0;
|
|
|
+ }
|
|
|
+ lidarDistance = obsFrenet.s;
|
|
|
+
|
|
|
+ lastLidarDis = lidarDistance;
|
|
|
+ }
|
|
|
+
|
|
|
+ obs = lidarDistance;
|
|
|
+ obsSd = obsPoint.obs_speed_y;
|
|
|
+ obsva->obs_distance = obs;
|
|
|
+ obsva->obs_speed = obsSd;
|
|
|
+
|
|
|
+ return obsFrenet;
|
|
|
+}
|
