puresuit算法 C++实现

puresuit算法 C++实现（ROS RVIZ 环境下展示）

/*
*初次写代码时间：20211月18号
*编译环境：c++11
*系统环境：ubuntu16.04+ros-kinetic
*代码实现功能：puresuit 算法实现路径跟踪，现在只考虑位置点的变化，方位角和速度还没展示出来。    
*/
#include<iostream>
#include<cstring>
#include<stdlib.h>
#include<vector>
#include<math.h>
#include<cmath>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
using namespace std;
double dt=0.1;//时间间隔
double K=0.1;//前视距离系数
double Lfc=2.0;//前视距离
double Kp=1.0;//速度P控制系数
double L=2.9;//车辆轴距，单位：M
//创建类 vechicleState
struct vechicleState
{
    
    double x;
    double y;
    double yaw;
    double v;
};
vechicleState update(vechicleState state,double a,double delta)//定义更新函数
{
    state.x+=state.v*cos(state.yaw)*dt;
    state.y+=state.v*sin(state.yaw)*dt;
    state.yaw+=state.v/L*tan(delta)*dt;
    state.v+=a*dt;
    return state;
}
//转角控制器函数
double PControl(double target,double current)
{
    double a;
    a=Kp*(target-current);
    return a;
}
int calc_target_index(vechicleState state,vector<double> &cx,vector<double> &cy)//计算最短路径。
{
    
    double min=abs(sqrt(pow(state.x-cx[0],2)+pow(state.y-cy[0],2)));
    int index=0;
    for(int j=0;j<cx.size();j++)
    {
        double d=abs(sqrt(pow(state.x-cx[j],2)+pow(state.y-cy[j],2)));
        if(d<min)
        {
            min=d;
            index=j;
        }
    }
    double L=0.0;
    double Lf=K*state.v+2.0;
    while(Lf>L && (index+1)<50)
    {   
        double dx=cx[index+1]-cx[index];
        double dy=cy[index+1]-cy[index];
        L+=sqrt(pow(dx,2)+pow(dy,2));
        index+=1;
    }
    return index;
}
//用struct Result返回两个参数
struct Result
{
    double delta;
    int ind;
};
Result pure_pursuit_control(vechicleState state,vector<double> &cx,vector<double> &cy,int pind)//纯路径控制
{   
    Result ret;
    double tx;
    double ty;
    int ind=calc_target_index(state,cx,cy);//目标点的索引
    if(pind>=ind)
    {
        ind=pind;
    }
    if(ind<cx.size())
    {
        tx=cx[ind];
        ty=cy[ind];
    }
    else
    {
        tx=cx[cx.size()-1];//倒数第一个点
        ty=cy[cx.size()-1];
        ind=cx.size()-1;//最后一个点的索引
    }
    double pi=3.14;
    double alpha=atan2(ty-state.y,tx-state.x)-state.yaw;
    if (state.v<0)
    {
        alpha=pi-alpha;
    }
    //cout<<"alpha:"<<alpha<<endl;
    double Lf=K*state.v+Lfc;
    double delta=atan2(2.0*L*sin(alpha)/Lf,1.0);
    //cout<<"delta:"<<delta<<endl;
    ret.delta=delta;
    ret.ind=ind;
    return ret;//返回角度和最近点的索引。

}


int main(int argc,char **argv)
{   
    ros::init(argc,argv,"puresuit");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    visualization_msgs::Marker points;
    points.header.frame_id="odom";
    points.header.stamp =ros::Time::now();
    ros::Rate loop_rate(10);
    points.action =visualization_msgs::Marker::ADD;
    points.ns="puresuit";
    points.pose.orientation.w=1.0;
    //设置点的属性
    points.id = 0;
    points.type = visualization_msgs::Marker::POINTS;

    points.scale.x = 0.1;
    points.scale.y = 0.1;

    points.color.g = 1.0f;
    points.color.a = 1.0;
    vector<double> cx={};
    vector<double> cy={};
    for(int i=0;i<50;i++)
    {
        cx.push_back(i);
        cy.push_back(sin(i/5.0)*i/2.0);
        
    }
    
    double target_speed=10.0/3.6; //速度
    double T=100; //模拟最大时间
    //初始化state
    struct vechicleState state={0.0,-3.0,0.0,0.0};
    double lastIndex=cx.size()-1;
    double time =0.0;
    int target_ind=calc_target_index(state,cx,cy);
    Result res;
    while(T>=time && lastIndex>target_ind)
    {
        double ai =PControl(2.777778,state.v);
        
        //目标点的索引和航向角
        res=pure_pursuit_control(state,cx,cy,target_ind);
        double di=res.delta;
        int target_ind=res.ind;
        //更新车的位置
        state =update(state,ai,di);
        cout<<"ai:"<<ai<<" di:"<<di<<" target_ind:"<<target_ind<<" x:"<<state.x<<" v:"<<state.v<<endl;
        time=time+dt;
        geometry_msgs::Point p;
        p.x = state.x;
        p.y = state.y;
        p.z = 0;
        points.points.push_back(p);
        pub.publish(points);
        loop_rate.sleep();
        
        
        
    }
    

}

本次的实现环境为ROS-kinectic，用Marker几何信息发布。
以下为实现图

问题：算法实现了大半部分，但到最后一个点，不知是用了ROS原因还是最后while 条件没用正确，最后一个点过后，轨迹线又会绕回来。

图中x变为48了，说明已经往回走了。