#include "frenet_planner.h"
#include <adc_tools/transfer.h>
#include <common/car_status.h>
#include <adc_tools/parameter_status.h>
//#include <decide_gps_00.h>
#include <adc_planner/lane_change_planner.h>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
iv::decition::FrenetPlanner::FrenetPlanner(){
this->planner_id = 1;
this->planner_name = "Frenet";
this->lidarGridPtr = NULL;
}
iv::decition::FrenetPlanner::~FrenetPlanner(){
free(this->lidarGridPtr);
}
/**
* @brief iv::decition::FrenetPlanner::getPath
* 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
* 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
*
* @param now_gps_ins 实时gps信息
* @param gpsMapLine 地图数据点
* @param PathPoint 地图路线中距离车辆位置最近的一个点的序号
* @param offset 在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。offset = -(roadNow - roadOri)*roadWidth
* @param lidarGridPtr 激光雷达信息网格
* @return 返回一条车辆坐标系下的路径
*/
std::vector<iv::Point2D> iv::decition::FrenetPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
this->roadNow = -1;
this->now_gps_ins = now_gps_ins;
this->gpsMapLine = gpsMapLine;
this->PathPoint = PathPoint;
this->lidarGridPtr = lidarGridPtr;
LaneChangePlanner *lcp = new LaneChangePlanner();
std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
delete lcp;
double realSecSpeed = now_gps_ins.speed / 3.6;
leftWidth = offset;
rightWidth = offset;
std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
return trace;
}
/**
* @brief iv::decition::FrenetPlanner::chooseRoadByFrenet
* 用frenet的方法对每条道路考察，选择一个最优的道路
* @param now_gps_ins 实时gps信息
* @param gpsMapLine 地图数据点
* @param PathPoint 地图路线中距离车辆位置最近的一个点的序号
* @param offsetL 在选择道路中，地图源路线左边的最大宽度。可以为负值。offsetL = -(roadSum - 1)*roadWidth
* @param offsetR 在选择道路中，地图源路线右边的最大宽度。可以为非负值。offsetR = (roadOri - 0)*roadWidth
* @param lidarGridPtr 激光雷达信息网格
* @return
*/
int iv::decition::FrenetPlanner::chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow){
this->roadNow = roadNow;
this->aimRoadByFrenet = -1;
this->now_gps_ins = now_gps_ins;
this->gpsMapLine = gpsMapLine;
this->PathPoint = PathPoint;
this->lidarGridPtr = lidarGridPtr;
LaneChangePlanner *lcp = new LaneChangePlanner();
std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
delete lcp;
double realSecSpeed = now_gps_ins.speed / 3.6;
leftWidth = offsetL;
rightWidth = offsetR;
std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
return aimRoadByFrenet;
}
/**
* @brief iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace
* 进行frenet规划的总函数，还包括了规划前的一些准备工作，比如frenet坐标系建立、车辆起始状态等。
* @param gpsTrace 地图路线上从PathPoint开始的600个点
* @param realsecSpeed 实时车速 [m/s]
* @return 返回一条frenet规划后并转换到车辆坐标系下的路径
*/
std::vector<iv::Point2D> iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed){
vector<Point2D> trace;
vector<FrenetPoint> frenet_s;
double stp=0.0;
//把gpsTrace里的点全部转为frenet坐标系下的点，存储在frenet_s中。相当于frenet坐标系的S轴。
FrenetPoint tmp={.x=gpsTrace[0].x,.y=gpsTrace[0].y,.s=0.0,.d=0.0};
frenet_s.push_back(tmp);
for(int i=1; i<gpsTrace.size(); ++i){
stp+=iv::decition::GetDistance(gpsTrace[i-1],gpsTrace[i]);
tmp={.x=gpsTrace[i].x,.y=gpsTrace[i].y,.s=stp,.d=0.0};
frenet_s.push_back(tmp);
}
//求出车辆当前位置在frenet坐标系下的坐标
// FrenetPoint car_now_at_frenet = XY2Frenet(0,0,gpsTrace);
FrenetPoint car_now_at_frenet = getFrenetfromXY(0,0,gpsTrace,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
double c_s_speed = realsecSpeed * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
double c_d_speed = realsecSpeed * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
double realAcc = ServiceCarStatus.mfCalAcc;
double c_s_dd = realAcc * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
double c_d_dd = realAcc * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
vector<Point2D> path=frenet_optimal_planning(car_now_at_frenet,frenet_s,c_s_speed,c_d_speed,c_d_dd);
return path;
}
/**
* @brief iv::decition::FrenetPlanner::frenet_optimal_planning
* 对frenet规划出的路径进行后续操作，并选出损失最小的一条路径作为最优路径
* @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
* @param frenet_s frenet坐标系的s轴
* @param c_speed 车辆的纵向速度。沿s轴方向的速度。
* @param c_d_d 车辆沿d方向的速度。
* @param c_d_dd 车辆沿d方向的加速度。
* @return 返回一条frenet规划的最优路径
*/
vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_optimal_planning(iv::decition::FrenetPoint car_now_frenet_point,
const std::vector<FrenetPoint>& frenet_s, double c_speed, double c_d_d, double c_d_dd){
double s0 = car_now_frenet_point.s;
double c_d = car_now_frenet_point.d;
vector<iv::Point2D> gpsTrace;
vector<Frenet_path> fplist=calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0); //frenet规划
sort(fplist.begin(),fplist.end(),comp); //按损失度由小到大进行排序
for(int i=0; i<fplist.size(); ++i){
calc_global_single_path(fplist[i],frenet_s);
if(check_single_path(fplist[i])){
gpsTrace = frenet_path_to_gpsTrace(fplist[i]);
aimRoadByFrenet = fplist[i].roadflag;
return gpsTrace;
}
}
return gpsTrace;
/*
// calc_global_paths(fplist, frenet_s); //生成frenet路径中每个点在车辆坐标系下的坐标
// fplist = check_paths(fplist); //检验计算出的每条路径
// //找到损失最小的轨迹
// double min_cost = (numeric_limits<double>::max)();
// Frenet_path bestpath;
// for(int i=0; i<fplist.size(); ++i){
// if(min_cost > fplist[i].cf){
// min_cost = fplist[i].cf;
// bestpath = fplist[i];
// }
// }
// gpsTrace = frenet_path_to_gpsTrace(bestpath);
// aimRoadByFrenet = bestpath.roadflag;
// return gpsTrace;
*/
}
/**
* @brief iv::decition::FrenetPlanner::calc_frenet_paths
* 正式规划出不同的frenet路径，并统计相应的损失度。单从车辆行驶的平滑性计算而得，后续需检测是否有障碍物。
* @param c_speed 车辆的纵向速度。沿s轴方向的速度
* @param c_d 车辆的横向（d方向）偏移距离
* @param c_d_d 车辆沿d方向的速度
* @param c_d_dd 车辆沿d方向的加速度
* @param s0 车辆沿s方向的坐标
* @return 返回多条frenet路径
*/
vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0){
vector<iv::decition::Frenet_path> frenet_paths;
int roadNum = round(abs((leftWidth-rightWidth)/ServiceParameterStatus.D_ROAD_W)); //roadNum为frenet算法的起始道路序号
//采样，并对每一个目标配置生成轨迹
for(double di=leftWidth; di<=rightWidth; di+=ServiceParameterStatus.D_ROAD_W,roadNum--){
if(roadNum == this->roadNow)
continue;
//横向动作规划
for(double Ti=ServiceParameterStatus.MINT; Ti<ServiceParameterStatus.MAXT; Ti+=ServiceParameterStatus.DT){
Frenet_path fp;
fp.roadflag = roadNum;
//计算出关于目标配置di，Ti的横向多项式
Quintic_polynomial lat_qp = Quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti);
for(double i=0; i<Ti; i+=ServiceParameterStatus.D_POINT_T)
fp.t.push_back(i);
for(int i=0; i<fp.t.size(); ++i){
fp.d.push_back(lat_qp.calc_point(fp.t[i]));
fp.d_d.push_back(lat_qp.calc_first_derivative(fp.t[i]));
fp.d_dd.push_back(lat_qp.calc_second_derivative(fp.t[i]));
fp.d_ddd.push_back(lat_qp.calc_third_derivative(fp.t[i]));
}
//纵向速度规划 (速度保持)
for(double tv=ServiceParameterStatus.TARGET_SPEED - ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE;
tv<(ServiceParameterStatus.TARGET_SPEED + ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE);
tv+=ServiceParameterStatus.D_T_S){
Frenet_path tfp = fp;
Quartic_polynomial lon_qp = Quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti);
for(int i=0; i<fp.t.size(); ++i){
tfp.s.push_back(lon_qp.calc_point(fp.t[i]));
tfp.s_d.push_back(lon_qp.calc_first_derivative(fp.t[i]));
tfp.s_dd.push_back(lon_qp.calc_second_derivative(fp.t[i]));
tfp.s_ddd.push_back(lon_qp.calc_third_derivative(fp.t[i]));
}
//square of jerk
double Jp = inner_product(tfp.d_ddd.begin(), tfp.d_ddd.end(), tfp.d_ddd.begin(), 0);
//square of jerk
double Js = inner_product(tfp.s_ddd.begin(), tfp.s_ddd.end(), tfp.s_ddd.begin(), 0);
//square of diff from target speed
double ds = pow((ServiceParameterStatus.TARGET_SPEED - tfp.s_d.back()),2);
//横向的损失函数
tfp.cd = ServiceParameterStatus.KJ * Jp + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * tfp.d.back() * tfp.d.back();
//纵向的损失函数
tfp.cv = ServiceParameterStatus.KJ * Js + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * ds;
//总的损失函数为d 和 s方向的损失函数乘对应的系数相加
tfp.cf = ServiceParameterStatus.KLAT * tfp.cd + ServiceParameterStatus.KLON * tfp.cv;
frenet_paths.push_back(tfp);
}
}
}
return frenet_paths;
}
/**
* @brief iv::decition::FrenetPlanner::calc_global_paths
* 计算出每条frenet路径中轨迹点的x，y坐标。x，y坐标是轨迹点在车辆坐标系下的横纵坐标。
* 部分函数参数在第一种计算XY坐标的方法中没有使用，但在第二种方法中会用到。
* @param fplist 多条frenet路径
* @param frenet_s frenet坐标系的s轴
* @return 通过引用传递返回带有x，y值的多条frenet路径
*/
void iv::decition::FrenetPlanner::calc_global_paths(vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s){
for(auto& fp:fplist){
for(int i=0; i<fp.s.size(); ++i){
double ix,iy;
// Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s); //第一种方法
getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins); //第二种方法
fp.x.push_back(ix);
fp.y.push_back(iy);
}
for(int i=0; i<(fp.x.size()-1); ++i){
double dx = fp.x[i+1] - fp.x[i];
double dy = fp.y[i+1] - fp.y[i];
fp.yaw.push_back(atan2(dy,dx));
fp.ds.push_back(sqrt(dx*dx + dy*dy));
}
fp.ds.push_back(fp.ds.back());
fp.yaw.push_back(fp.yaw.back());
for(int i = 0; i < (fp.yaw.size() - 1); ++i){
fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
}
}
}
void iv::decition::FrenetPlanner::calc_global_single_path(Frenet_path &fp, const std::vector<FrenetPoint> &frenet_s){
for(int i=0; i<fp.s.size(); ++i){
double ix,iy;
// Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s); //第一种方法
getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins); //第二种方法
fp.x.push_back(ix);
fp.y.push_back(iy);
}
for(int i=0; i<(fp.x.size()-1); ++i){
double dx = fp.x[i+1] - fp.x[i];
double dy = fp.y[i+1] - fp.y[i];
fp.yaw.push_back(atan2(dy,dx));
fp.ds.push_back(sqrt(dx*dx + dy*dy));
}
fp.ds.push_back(fp.ds.back());
fp.yaw.push_back(fp.yaw.back());
for(int i = 0; i < (fp.yaw.size() - 1); ++i){
fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
}
}
/**
* @brief iv::decition::FrenetPlanner::check_paths
* 对多条frenet路径进行检验，排除不符合要求的路径。
* @param fplist 多条frenet路径
* @return 排除部分路径后的多条frenet路径
*/
vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::check_paths(const vector<Frenet_path>& fplist){
vector<Frenet_path> okind;
int i=0;
int j=0;
for(i=0; i<fplist.size(); ++i){
cout<<"&&&&&&&&&&fplist[i].roadflag: "<<fplist[i].roadflag<<endl;
for(j=0; j<fplist[i].s_d.size(); ++j){
if(fplist[i].s_d[j]>ServiceParameterStatus.MAX_SPEED) //最大速度检查
goto ContinueFlag;
}
for(j=0; j<fplist[i].s_dd.size(); ++j){
if(abs(fplist[i].s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
goto ContinueFlag;
}
for(j=30; j<fplist[i].c.size()-30; ++j){
if(abs(fplist[i].c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
goto ContinueFlag;
}
if(!check_collision(fplist[i]))
continue;
okind.push_back(fplist[i]);
ContinueFlag:continue;
}
return okind;
}
bool iv::decition::FrenetPlanner::check_single_path(const iv::decition::Frenet_path &fp){
int j=0;
cout<<"&&&&&&&&&&fp.roadflag: "<<fp.roadflag<<endl;
for(j=0; j<fp.s_d.size(); ++j){
if(fp.s_d[j]>ServiceParameterStatus.MAX_SPEED) //最大速度检查
return false;
}
for(j=0; j<fp.s_dd.size(); ++j){
if(abs(fp.s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
return false;
}
for(j=30; j<fp.c.size()-30; ++j){
if(abs(fp.c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
return false;
}
if(!check_collision(fp))
return false;
return true;
}
/**
* @brief iv::decition::FrenetPlanner::check_collision
* 检验frenet路径上是否有障碍物，调用的是改进的函数computeObsOnRoadByFrenet。
* @param frenet_path 一条frenet路径
* @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
* @return 在路径上有障碍物返回false，否则返回true。
*/
bool iv::decition::FrenetPlanner::check_collision(const iv::decition::Frenet_path &frenet_path){
std::vector<Point2D> gpsTrace = frenet_path_to_gpsTrace(frenet_path);
double obs=-1;
// iv::decition::DecideGps00().computeObsOnRoadByFrenet(this->lidarGridPtr,gpsTrace,obs,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
if(obs > 0 && obs < 30)
return false;
else
return true;
}
/**
* @brief iv::decition::FrenetPlanner::frenet_path_to_gpsTrace
* 将frenet路径转换到车辆坐标系下。
* @param frenet_path 一条frenet路径
* @return 一条车辆坐标系下的路径
*/
vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_path_to_gpsTrace(const Frenet_path& frenet_path){
vector<iv::Point2D> gpsTrace;
for (int i=0; i<frenet_path.x.size(); ++i) {
gpsTrace.push_back(Point2D(frenet_path.x[i],frenet_path.y[i]));
gpsTrace[i].speed = sqrt(frenet_path.d_d[i]*frenet_path.d_d[i]+frenet_path.s_d[i]*frenet_path.s_d[i])*3.6;
}
return gpsTrace;
}
double iv::decition::FrenetPlanner::distance(double x1, double y1, double x2, double y2)
{
return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}
//找出gpsTrace中距离（x，y）最近的一个点
int iv::decition::FrenetPlanner::ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace)
{
double closestLen = 100000; //large number
int closestWaypoint = 0;
for(int i = 1; i < gpsTrace.size(); i++)
{
double map_x = gpsTrace[i].x;
double map_y = gpsTrace[i].y;
double dist = distance(x,y,map_x,map_y);
if(dist < closestLen)
{
closestLen = dist;
closestWaypoint = i;
}
}
return closestWaypoint;
}
/* |==========================================================|
* | 车辆坐标系与 frenet 坐标系互转的第一种方法。 |
* |==========================================================| */
// 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
iv::decition::FrenetPoint iv::decition::FrenetPlanner::XY2Frenet(double x, double y, const std::vector<iv::Point2D>& gpsTrace){
int next_wp=ClosestWaypoint( x, y, gpsTrace);
int prev_wp = next_wp-1; //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
// if(next_wp == 0){
// prev_wp = gpsTrace.size()-1;
// }
double n_x = gpsTrace[next_wp].x-gpsTrace[prev_wp].x;
double n_y = gpsTrace[next_wp].y-gpsTrace[prev_wp].y;
double x_x = x - gpsTrace[prev_wp].x;
double x_y = y - gpsTrace[prev_wp].y;
// find the projection of x onto n
double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y);
double proj_x = proj_norm*n_x;
double proj_y = proj_norm*n_y; //proj_x、proj_y应该为垂足的坐标
double frenet_d = sqrt((x_x - proj_x) * (x_x - proj_x) + (x_y - proj_y) * (x_y - proj_y));
//经手动推算frenet_d = abs(n_x * x_y - n_y * x_x) / sqrt(n_x * n_x + n_y * n_y)。
//即frenet_d，等于点(x,y)到 过prev_wp、next_wp两点的直线 的垂直距离。
//see if d value is positive or negative by comparing it to a center point
double center_x = 1000-gpsTrace[prev_wp].x;
double center_y = 2000-gpsTrace[prev_wp].y;
double centerToPos = distance(center_x,center_y,x_x,x_y);
double centerToRef = distance(center_x,center_y,proj_x,proj_y);
if(centerToPos <= centerToRef){
frenet_d *= -1;
}
// calculate s value
double frenet_s = 0;
for(int i = 0; i < prev_wp; i++){
frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
}
if(prev_wp<=0){
frenet_s -= distance(0,0,proj_x,proj_y);
}else{
frenet_s += distance(0,0,proj_x,proj_y);
}
double tmp_Angle = atan2(n_y,n_x);
return {x:x,y:y,s:frenet_s,d:frenet_d,tangent_Ang:tmp_Angle};
}
// frenet坐标转车体坐标。
// Transform from Frenet s,d coordinates to Cartesian x,y
void iv::decition::FrenetPlanner::Frenet2XY(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace){
int prev_wp = 0;
//退出循环时，prev_wp 最大是等于 frenetTrace.size()-2。
while(s > frenetTrace[prev_wp+1].s && (prev_wp < (int)(frenetTrace.size()-2) )){
prev_wp++;
}
int wp2 = prev_wp+1;
double heading = atan2((frenetTrace[wp2].y-frenetTrace[prev_wp].y),(frenetTrace[wp2].x-frenetTrace[prev_wp].x));
double seg_s = (s-frenetTrace[prev_wp].s);
double seg_x = frenetTrace[prev_wp].x+seg_s*cos(heading);
double seg_y = frenetTrace[prev_wp].y+seg_s*sin(heading);
double perp_heading = heading-M_PI*0.5;
*res_x = seg_x + d*cos(perp_heading);
*res_y = seg_y + d*sin(perp_heading);
}
/* |==========================================================|
* | 车辆坐标系与 frenet 坐标系互转的第二种方法。 |
* |==========================================================| */
// 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
iv::decition::FrenetPoint iv::decition::FrenetPlanner::getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
int next_wp=ClosestWaypoint( x, y, gpsTrace);
int prev_wp = next_wp-1; //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
// if(next_wp == 0){
// prev_wp = gpsTrace.size()-1;
// }
GPS_INS gps = Coordinate_UnTransfer(x,y,nowGps);
Point2D pt = Coordinate_Transfer(gps.gps_x,gps.gps_y, *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
// calculate s value
double frenet_s = 0;
for(int i = 0; i < prev_wp; i++){
frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
}
frenet_s += pt.y;
//theta为prev_wp处车的航向角 与 nowGps处的车辆坐标系下x轴之间的夹角，逆时针为正。
double theta = (nowGps.ins_heading_angle - gpsMap[(pathpoint+prev_wp)%gpsMap.size()]->ins_heading_angle+90);
double tmp_Angle = theta * M_PI / 180;
return {x:x,y:y,s:frenet_s,d:pt.x,tangent_Ang:tmp_Angle};
}
void iv::decition::FrenetPlanner::getXYfromFrenet(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
{
int prev_wp = -1;
while((prev_wp < (int)(frenetTrace.size()-1) ) && s > frenetTrace[prev_wp+1].s)
{
prev_wp++;
}
if(prev_wp < 0){
prev_wp = 0;
}
// int wp2 =prev_wp+1;
GPS_INS gps= Coordinate_UnTransfer( d, s-frenetTrace[prev_wp].s , *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
Point2D pt = Coordinate_Transfer( gps.gps_x, gps.gps_y, nowGps);
*res_x = pt.x;
*res_y = pt.y;
}
iv::decition::Quintic_polynomial::Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T){
//计算五次多项式系数
this->xs = xs;
this->vxs = vxs;
this->axs = axs;
this->xe = xe;
this->vxe = vxe;
this->axe = axe;
this->a0 = xs;
this->a1 = vxs;
this->a2 = axs/ 2.0;
MatrixXd A(3, 3);
MatrixXd b(3, 1);
MatrixXd x(3, 1);
A << pow(T, 3), pow(T, 4), pow(T, 5),
3*pow(T, 2), 4*pow(T, 3), 5*pow(T, 4),
6*T, 12*T*T, 20*pow(T, 3);
b << xe - a0 - a1*T - a2*T*T,
vxe - a1 - 2*a2*T,
axe - 2*a2;
x=A.colPivHouseholderQr().solve(b);
this->a3 = x(0,0);
this->a4 = x(1,0);
this->a5 = x(2,0);
}
iv::decition::Quintic_polynomial::~Quintic_polynomial(){
}
double iv::decition::Quintic_polynomial::calc_point(double t){
double xt = this->a0 + this->a1 * t + this->a2 * t*t + this->a3 * t*t*t + this->a4 * t*t*t*t + this->a5 * t*t*t*t*t;
return xt;
}
double iv::decition::Quintic_polynomial::calc_first_derivative(double t){
double xt = this->a1 + 2*this->a2 * t + 3*this->a3 * t*t + 4*this->a4 * t*t*t + 5*this->a5 * t*t*t*t;
return xt;
}
double iv::decition::Quintic_polynomial::calc_second_derivative(double t){
double xt = 2*this->a2 + 6*this->a3 * t + 12*this->a4 * t*t + 20*this->a5 * t*t*t;
return xt;
}
double iv::decition::Quintic_polynomial::calc_third_derivative(double t){
double xt = 6*this->a3 + 24*this->a4 * t + 60*this->a5 * t*t;
return xt;
}
iv::decition::Quartic_polynomial::Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T){
//计算四次多项式系数
this->xs = xs;
this->vxs = vxs;
this->axs = axs;
this->vxe = vxe;
this->axe = axe;
this->a0 = xs;
this->a1 = vxs;
this->a2 = axs / 2.0;
MatrixXd A(2, 2);
MatrixXd b(2, 1);
MatrixXd x(2, 1);
A << 3*pow(T, 2), 4*pow(T, 3),
6*T, 12*T*T ;
b << vxe - a1 - 2*a2*T,
axe - 2*a2;
x=A.colPivHouseholderQr().solve(b);
this->a3 = x(0,0);
this->a4 = x(1,0);
}
iv::decition::Quartic_polynomial::~Quartic_polynomial(){
}
double iv::decition::Quartic_polynomial::calc_point(double t){
double xt = this->a0 + this->a1*t + this->a2*t*t + this->a3*t*t*t + this->a4*t*t*t*t;
return xt;
}
double iv::decition::Quartic_polynomial::calc_first_derivative(double t){
double xt = this->a1 + 2*this->a2*t + 3*this->a3*t*t + 4*this->a4*t*t*t;
return xt;
}
double iv::decition::Quartic_polynomial::calc_second_derivative(double t){
double xt = 2*this->a2 + 6*this->a3*t + 12*this->a4*t*t;
return xt;
}
double iv::decition::Quartic_polynomial::calc_third_derivative(double t){
double xt = 6*this->a3 + 24*this->a4*t;
return xt;
}
bool iv::decition::FrenetPlanner::comp(const iv::decition::Frenet_path &a, const iv::decition::Frenet_path &b){
return a.cf < b.cf;
}