2021-03-11-优快云博客

本文介绍了一种基于ROS的路径跟踪算法实现细节。该算法通过坐标转换将地图坐标系下的路径点转换到里程计坐标系下，并选取合适的路径点作为局部目标，以实现精确的路径跟踪。文中展示了具体的实现代码，包括路径点选择、坐标转换和局部目标确定等关键步骤。

https://robomaster.ones.ai/project/#/home
https://pinyin.sogou.com/linux/help.php
void Path_tracking::multi_select(const PoseSE2& currant_odom_pose)
{
int i=0,j=0;
for(geometry_msgs::PoseStamped each_path_point:map_path.poses)
{
geometry_msgs::PoseStamped path_point_odom;
try
{
tf_listener.transformPose(“odom”, ros::Time(0),each_path_point,“map” ,path_point_odom);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
break;
// ros::Duration(1.0).sleep();
}
if(i<100)
{
a[i][0] = path_point_odom.pose.position.x;
a[i][1] = path_point_odom.pose.position.y;
i++ ;
}
else{
i=0;
break;
}
}
for(geometry_msgs::PoseStamped each_path_point:map_path.poses)
{
geometry_msgs::PoseStamped path_point_odom;
try
{
tf_listener.transformPose(“odom”, ros::Time(0),each_path_point,“map” ,path_point_odom);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
break;
// ros::Duration(1.0).sleep();
}
if(j<100)
{
b[j][0] = path_point_odom.pose.position.x;
b[j][1] = path_point_odom.pose.position.y;
j++ ;
}
else{
j=0;
break;
}
}
double sumx=0,maxpoint=0,maxpointx,maxpointy;

for(int k=0;k<99;k++)
{
    double s = sqrt(pow((a[k][0] -b[k][0]),2)+pow((a[k][1] -b[k][1]),2));
    sumx+=s;
    if(sumx!=0 and sumx==sumx)
    {
 //std::cout<<"sumx"<<sumx<<std::endl;
    }

    if(s>maxpoint)
    {
        maxpoint = s;
        maxpointx = a[k][0];
        maxpointy = a[k][1];
    }
    if(sumx>2.2 && if_on==0)
    {   
    
        select=1;
    if_on =1;
        break;
    }
if(if_on==1 && sumx<0.8)
{
    select=0;
    if_on=0;
    ROS_INFO("guider off");
    break;
}
}
if(select)
{
    ROS_WARN("guiding");
    local_goal.x()= maxpointx;
    local_goal.y()= maxpointy;
}

}