modularization/src/decition/decition_brain/decision/decide_gps_00.h

#pragma once
#ifndef _IV_DECITION__DECIDE_GPS_00_
#define _IV_DECITION__DECIDE_GPS_00_

#include <common/gps_type.h>
#include <decision/decition_type.h>
#include <common/obstacle_type.h>
#include <vector>
#include <decision/adc_tools/gnss_coordinate_convert.h>
#include <decision/adc_planner/frenet_planner.h>
#include <decision/adc_controller/pid_controller.h>
#include <decision/adc_adapter/ge3_adapter.h>
#include <decision/adc_adapter/qingyuan_adapter.h>
#include <decision/adc_adapter/vv7_adapter.h>
#include <decision/adc_adapter/zhongche_adapter.h>
#include <decision/adc_adapter/bus_adapter.h>
#include <decision/adc_adapter/hapo_adapter.h>
#include <decision/adc_adapter/zhongche_adapter.h>
#include <decision/adc_adapter/yuhesen_adapter.h>
#include "perception/perceptionoutput.h"
#include "ivlog.h"
#include <memory>

namespace iv {
namespace decision {
//根据传感器传输来的信息作出决策
class DecideGps00 {
public:
    DecideGps00();
    ~DecideGps00();

    static double minDis;
    static double maxAngle;
    static int lastGpsIndex;
    static double lidarDistance;
    static double myesrDistance;
    static double lastLidarDis;
    static double lastLidarDisAvoid;

    static double lastPreObsDistance;

    static int gpsLineParkIndex;
    static int gpsMapParkIndex;

    static double lastDistance;

    static float xiuzhengCs;

    static double controlAng;
    double laneAng;
    static double controlSpeed;
    static double controlBrake;

    static bool parkMode;
    static bool readyParkMode;

    static bool zhucheMode;
    static bool readyZhucheMode;
    static bool hasZhuched;
    static double Iv;
    static double lastEv;
    static double lastVt;
    static double lastU;
    static double obsDistance;
    static double obsSpeed;
    static double lidarDistanceAvoid;
    static double obsDistanceAvoid;
    static double lastDistanceAvoid;

    GPS_INS group_ori_gps;
    GPS_INS startAvoid_gps_ins;
    static int finishPointNum;
    static int zhuchePointNum;
    double avoidRunDistance=0;

    std::vector<iv::GPS_INS> startAvoidGpsInsVector;
    std::vector<double> avoidMinDistanceVector;
    std::vector<int> v2xTrafficVector;

    ///kkcai, 2020-01-03
    //bool isFirstRun = true;
    static bool isFirstRun;
    /////////////////////////////////////

//    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
    static float minDecelerate;


    double startTime = 0;
    double firstTime = 0;

    bool circleMode=false;

    int avoidTimes = 0;
    int backTimes = 0;

    int checkAvoidObsTimes = 0;
    int checkBackObsTimes = 0;

    bool hasCheckedAvoidLidar=false;
    bool hasCheckedBackLidar=false;

    bool personWaited = false;
    bool roadLightWaited = false;

    double decision_Angle = 0;
    double lastAngle=0;

    iv::Point2D obsPoint;

    int checkAvoidTimes=0;
    int checkBackTimes=0;
    int chaocheCounts=0;

    static int PathPoint;
    float lastGroupOffsetX=0.0;

    GPS_INS traffic_light_gps;
    int traffic_light_pathpoint;

    bool changingDangWei=false;

    BaseController *pid_Controller;
    BaseAdapter *ge3_Adapter;
    BaseAdapter *qingyuan_Adapter;
    BaseAdapter *vv7_Adapter;
    BaseAdapter *zhongche_Adapter;
    BaseAdapter *bus_Adapter;
    BaseAdapter *hapo_Adapter;
    BaseAdapter *yuhesen_Adapter;

    std::shared_ptr<BaseController> mpid_Controller;
    std::shared_ptr<BaseAdapter> mge3_Adapter;
    std::shared_ptr<BaseAdapter> mqingyuan_Adapter;
    std::shared_ptr<BaseAdapter> mvv7_Adapter;
    std::shared_ptr<BaseAdapter> mzhongche_Adapter;
    std::shared_ptr<BaseAdapter> mbus_Adapter;
    std::shared_ptr<BaseAdapter> mhapo_Adapter;
    std::shared_ptr<BaseAdapter> myuhesen_Adapter;

	FrenetPlanner *frenetPlanner;
//    BasePlanner *laneChangePlanner;
	
	std::shared_ptr<FrenetPlanner> mfrenetPlanner;
//    std::shared_ptr<BasePlanner> mlaneChangePlanner;


    Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
                              std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);

    void initMethods();
    static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);


    static double getAngle(std::vector<Point2D> gpsTrace);
    double getAngle(std::vector<Point2D> gpsTrace, GPS_INS gpsIns,iv::decision::Decition decision);
    static double getSpeed(std::vector<Point2D> gpsTrace);

    static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
    static void getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
    static void getEsrObsDistanceAvoid();

    void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
    void phaseSpeedDecition(iv::decision::Decition decision, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns );

    double limitAngle(double speed, double angle);
    double limitSpeed(double angle, double speed);

    bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
    bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);

    void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
    static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decision::FrenetPoint car_now_frenet_point,iv::decision::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);

    void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
    void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);

    void predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);

    int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
    int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);

    void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);

    void  handBrakePark(iv::decision::Decition  decision, long duringTime, GPS_INS now_gps_ins);

    bool checkChaoCheBack(iv::LidarGridPtr);
    double trumpet();
    double transferP();

    bool nearStation;

    void updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s);
    void init();

    std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);

    std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);

    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                   int pathpoint,float secSpeed,float dSpeed);
    void getV2XTrafficPositionVector( const std::vector<GPSData> gpsMapLine);
    float ComputeV2XTrafficLightSpeed(iv::TrafficLight TrafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
                                   int pathpoint,float secSpeed,float dSpeed,bool circleMode);
    float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);

    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
    float computeTrafficSpeedLimt(float trafficDis);

    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);

    bool adjuseultra();

    iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);

    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
    GPS_INS aim_gps_ins;
    GPS_INS chaoche_start_gps;
    bool is_arrivaled=false;

    double chaocheObsDis = 0;
    double preChaocheDis = 0;
    double aimObsSpeed = 0;
    double followSpeed = 30;
    double chaocheSpeed = 50;
    int checkChaoCheBackCounts = 0;

    float lastTorque=0;
    float splimit=-1;

    float  ObsTimeSpan=-1;
    double ObsTimeStart=-1;
    double ObsTimeEnd=-1;
    float ObsTimeWidth=1500;
    std::vector<iv::TracePoint> planTrace;
    bool roadNowStart=true;
private:
    //  void changeRoad(int roadNum);


};

}
}

extern bool handPark;
extern long handParkTime;
extern bool rapidStop;
extern int gpsMissCount;
extern bool changeRoad;
extern double avoidX;
extern bool parkBesideRoad;
extern double steerSpeed;
extern bool transferPieriod;
extern bool transferPieriod2;
extern double traceDevi;
#endif // !  _IV_DECITION__DECIDE_GPS_00_