#ifndef FRENET_PLANNER_H #define FRENET_PLANNER_H #include "base_planner.h" namespace iv{ namespace decition{ //x，y分别为frenet轨迹点相对于车辆的x，y轴坐标 //s，d分别为frenet轨迹点相对于frenet坐标系的s，d轴坐标值 //tangent_Ang为轨迹点在frenet坐标系的s轴的投影处的 切线 与车辆坐标系x轴之间的夹角。 //tangent_Ang用于分解计算车辆分别在s轴、d轴方向上的运动速度、加速度等。 struct FrenetPoint{ double x; double y; double s; double d; double tangent_Ang; }; class Quintic_polynomial { public: Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T); ~Quintic_polynomial(); private: // 计算五次多项式系数 double xs; //d0 double vxs; //d.0 double axs; //d..0 double xe; //d1 double vxe; //d.1 double axe; //d..1 double a0; double a1; double a2; double a3; double a4; double a5; public: double calc_point(double t); double calc_first_derivative(double t); double calc_second_derivative(double t); double calc_third_derivative(double t); }; class Quartic_polynomial { public: Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T); ~Quartic_polynomial(); private: //计算四次多项式系数 double xs; double vxs; double axs; double vxe; double axe; double a0; double a1; double a2; double a3; double a4; public: double calc_point(double t); double calc_first_derivative(double t); double calc_second_derivative(double t); double calc_third_derivative(double t); }; class Frenet_path { public: std::vector t; std::vector d; std::vector d_d; std::vector d_dd; std::vector d_ddd; std::vector s; std::vector s_d; std::vector s_dd; std::vector s_ddd; int roadflag = -1; //标记每一条frenet路径，是以哪一条道路为目标道路的 double cd = 0.0; double cv = 0.0; double cf = 0.0; std::vector x; std::vector y; std::vector yaw; std::vector ds; std::vector c; }; class FrenetPlanner : public BasePlanner{ public: int aimRoadByFrenet = -1; //记录由frenet规划选择出来的道路序号 int roadNow = -1; //记录避障时车辆当前道路序号,规划路径时，避开此条道路 double leftWidth = 0.0; //中心线左边偏移距离，为负值 double rightWidth = 0.0; //中心线右边偏移距离，为正值 GPS_INS now_gps_ins; //实时gps信息 std::vector gpsMapLine; //地图数据点 int PathPoint = 0; //地图路线中距离车辆位置最近的一个点的序号 iv::LidarGridPtr lidarGridPtr; //激光雷达信息网格 public: FrenetPlanner(); ~FrenetPlanner(); /** * @brief iv::decition::FrenetPlanner::getPath * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。 * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。 * * @param now_gps_ins 实时gps信息 * @param gpsMapLine 地图数据点 * @param PathPoint 地图路线中距离车辆位置最近的一个点的序号 * @param offset 在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。 * @param lidarGridPtr 激光雷达信息网格 * @return 返回一条车辆坐标系下的路径 */ std::vector getPath(GPS_INS now_gps_ins, const std::vector& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr); int chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow); static iv::decition::FrenetPoint XY2Frenet(double x, double y, const std::vector& gpsTracet); static void Frenet2XY(double *x, double *y, double s, double d, const std::vector& frenetTrace); static double distance(double x1, double y1, double x2, double y2); static int ClosestWaypoint(double x, double y, const std::vector& gpsTrace); static FrenetPoint getFrenetfromXY(double x, double y, const std::vector& gpsTrace,const std::vector& gpsMap,int pathpoint,GPS_INS nowGps); static void getXYfromFrenet(double *res_x, double *res_y, double s, double d, const std::vector& frenetTrace,const std::vector& gpsMap,int pathpoint,GPS_INS nowGps); static bool comp(const iv::decition::Frenet_path &a,const iv::decition::Frenet_path &b); private: std::vector getGpsTrace_AfterCalcFrenetTrace(const std::vector& gpsTrace,double realsecSpeed); std::vector frenet_optimal_planning(FrenetPoint car_now_frenet_point, const std::vector& frenet_s, double c_speed, double c_d_d, double c_d_dd); std::vector calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0); void calc_global_paths(std::vector& fplist,const std::vector& frenet_s); void calc_global_single_path(Frenet_path& fp,const std::vector& frenet_s); std::vector check_paths(const std::vector& fplist); bool check_single_path(const Frenet_path &fp); bool check_collision(const Frenet_path &frenet_path); std::vector frenet_path_to_gpsTrace(const Frenet_path& frenet_path); }; } } #endif // FRENET_PLANNER_H