Autoware-Pure_pursuit代码解析_pure pursuit算法源码

// calculation relative coordinate of point from current_pose frame

计算全局坐标系相对于机器人坐标系中的位置,即point相对于current_pose坐标系的位置

公式参考，R=L^2/(2*y),论文--一种纯追踪模型改进算法

geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(current_pose, inverse);
  tf::Transform transform = inverse.inverse();

  tf::Point p;
  pointMsgToTF(point_msg, p);
  tf::Point tf_p = transform * p;
  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);

  return tf_point_msg;

}

例如：

    geometry_msgs::Point point_msg;

    point_msg.x=1;
    point_msg.y=2;
    point_msg.z=0;

    geometry_msgs::Pose current_pose;
    current_pose.position.x=4;
    current_pose.position.y=7;
    current_pose.position.z=0;
    current_pose.orientation.x=0;
    current_pose.orientation.y=0;
    current_pose.orientation.z=0;
    current_pose.orientation.w=1;

计算结果为

x: -3
y: -5i
z: 0

一、设置机器人的预瞄距离
 pp_.setLookaheadDista