Autoware planning模块学习笔记（二）：路径规划（2）- 节点waypoint_replanner（上）

Autoware planning模块学习笔记（二）：路径规划（2）- 节点waypoint_replanner（上）

前面一篇博客Autoware planning模块学习笔记（二）：路径规划（1），介绍了“勾选waypoint_loader”这一操作时所启动的waypoint_loader节点，还剩下waypoint_replanner和waypoint_marker_publisher两个节点。这里帮大伙儿逐行分析节点waypoint_replanner的代码。

节点waypoint_replanner

节点waypoint_replanner的源文件是nodes/waypoint_replanner/waypoint_replanner.cpp和nodes/waypoint_replanner/waypoint_replanner_node.cpp

老规矩，先找到main函数。main函数在waypoint_replanner_node.cpp中，main函数中就做了一件事，新建waypoint_maker::WaypointReplannerNode对象wr，很容易就能猜测到waypoint_maker::WaypointReplannerNode的构造函数内执行了一些操作。

int main(int argc, char** argv)
{
  ros::init(argc, argv, "waypoint_replanner");
  waypoint_maker::WaypointReplannerNode wr;
  ros::spin();

  return 0;
}

接着我们查看waypoint_maker::WaypointReplannerNode的构造函数，可以发现replanning_mode_的初始值为false，构造函数内实现了topic的订阅和发布。其中订阅的“/based/lane_waypoints_raw”就是前面waypoint_loader节点所发布的消息。

WaypointReplannerNode::WaypointReplannerNode() : replanning_mode_(false)
{
  lane_pub_["with_decision"] = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_array", 10, true);
  lane_pub_["without_decision"] = nh_.advertise<autoware_msgs::LaneArray>("/lane_waypoints_array", 10, true);
  lane_sub_ = nh_.subscribe("/based/lane_waypoints_raw", 1, &WaypointReplannerNode::laneCallback, this);
  config_sub_ = nh_.subscribe("/config/waypoint_replanner", 1, &WaypointReplannerNode::configCallback, this);
}

紧接着分析构造函数内的两个回调函数laneCallback和configCallback，首先看回调函数configCallback，要注意的是subscribe的回调函数是接收到对应消息后才会被调用，因此在代码中的前后设定顺序跟实际执行顺序不是一样的，实际执行顺序是按照各自消息到达顺序确定，因此可以认为是随机的。我们先分析configCallback函数是因为有些参数需要通过它进行设置，如果参数未设置完毕，laneCallback函数被激发执行了呢？所以实际启动系统过程中（如下图），是在操作交互界面上设置相关参数完成后再勾选相关选项，再等一会全部初始化完成后切换至自动驾驶模式。

对应于topic "/config/waypoint_replanner"的回调函数configCallback，首先设置成员变量replanning_mode_和realtime_tuning_mode_，接着对replanner_进行初始化，replanner_的定义是“WaypointReplanner replanner_;”。接着（防盗标记：zhengkunxian）根据lane_array_.lanes（lane_array_定义为autoware_msgs::LaneArray lane_array_;）是否为空（lane_array_在另一个回调函数laneCallback中会被赋值）和realtime_tuning_mode_变量决定是否执行publishLaneArray函数。

void WaypointReplannerNode::configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
  replanning_mode_ = conf->replanning_mode;
  realtime_tuning_mode_ = conf->realtime_tuning_mode;
  replanner_.initParameter(conf);
  if (lane_array_.lanes.empty() || !realtime_tuning_mode_)
  {
    return;
  }
  publishLaneArray();
}

replanner_的参数初始化：

void WaypointReplanner::initParameter(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
  velocity_max_ = kmph2mps(conf->velocity_max);
  velocity_min_ = kmph2mps(conf->velocity_min);
  accel_limit_ = conf->accel_limit;
  decel_limit_ = conf->decel_limit;
  r_th_ = conf->radius_thresh;
  r_min_ = conf->radius_min;
  lookup_crv_width_ = 5;
  resample_mode_ = conf->resample_mode;
  resample_interval_ = conf->resample_interval;
  replan_curve_mode_ = conf->replan_curve_mode;
  replan_endpoint_mode_ = conf->replan_endpoint_mode;
  overwrite_vmax_mode_ = conf->overwrite_vmax_mode;
  velocity_offset_ = conf->velocity_offset;
  end_point_offset_ = conf->end_point_offset;
  braking_distance_ = conf->braking_distance;
  r_inf_ = 10 * r_th_;
  vel_param_ = calcVelParam(velocity_max_);
}

publishLaneArray函数内如果replanning_mode_为true，则调用replan函数重规划。

void WaypointReplannerNode::publishLaneArray()
{
  autoware_msgs::LaneArray array(lane_array_);
  if (replanning_mode_)
  {
    replan(array);
  }
  lane_pub_["with_decision"].publish(array);
  if (withoutDecisionMaker())
  {
    lane_pub_["without_decision"].publish(array);
  }
}

replan函数内遍历lane_array.lanes，这里回顾一下lane_array.lanes内的元素都是autoware_msgs::Lane类型，其中储存了从multi_lane_csv_文件内读取的路径点集合（std::vector），每个路径点记录了UWB坐标系下的坐标x,y,z以及航向角等必要信息供规划所用。对每一条lane，replanner_都调用replanLaneWaypointVel函数。

void WaypointReplannerNode::replan(autoware_msgs::LaneArray& lane_array)
{
  for (auto &el : lane_array.lanes)
  {
    replanner_.replanLaneWaypointVel(el);
  }
}

replanLaneWaypointVel函数：

void WaypointReplanner::replanLaneWaypointVel(autoware_msgs::Lane& lane)
{
  if (lane.waypoints.empty())
  {
    return;
  }
  const int dir = getDirection(lane);
  const unsigned long last = lane.waypoints.size() - 1;
  changeVelSign(lane, true);
  limitVelocityByRange(0, last, 0, velocity_max_, lane);
  if (resample_mode_)
  {
    resampleLaneWaypoint(resample_interval_, lane, dir);
  }
  if (replan_curve_mode_)
  {
    std::vector<double> curve_radius;
    KeyVal curve_list;
    createRadiusList(lane, curve_radius);
    createCurveList(curve_radius, curve_list);
    if (overwrite_vmax_mode_)
    {// set velocity_max for all_point
      setVelocityByRange(0, last, 0, velocity_max_, lane);
    }
    // set velocity by curve（防盗标记：zhengkunxian）
    for (const auto& el : curve_list)
    {
      const double& radius = el.second.second;
      double vmin = velocity_max_ - vel_param_ * (r_th_ - radius);
      vmin = (vmin < velocity_min_) ? velocity_min_ : vmin;
      limitVelocityByRange(el.first, el.second.first, velocity_offset_, vmin, lane);
    }
  }
  // set velocity on start & end of lane
  if (replan_endpoint_mode_)
  {
    const unsigned long zerospeed_start = last - end_point_offset_;
    const unsigned long lowspeed_start = zerospeed_start - braking_distance_;
    raiseVelocityByRange(0, last, 0, velocity_min_, lane);
    limitVelocityByRange(0, 0, 0, velocity_min_, lane);
    limitVelocityByRange(lowspeed_start, last, 0, velocity_min_, lane);
    setVelocityByRange(zerospeed_start, last, 0, 0.0, lane);
  }
  if (dir < 0)
  {
    changeVelSign(lane, false);
  }
}

getDirection函数：
calcRelativeCoordinate函数（防盗标记：zhengkunxian）位于src/autoware/common/waypoint_follower/lib/libwaypoint_follower.cpp，libwaypoint_follower.cpp被src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.h所引用。

int WaypointReplanner::getDirection(const autoware_msgs::Lane& lane) const
{
  if (lane.waypoints.size() < 2)
  {
    return 1;
  }
  const geometry_msgs::Pose& pose = lane.waypoints[0].pose.pose;
  const geometry_msgs::Point& point = lane.waypoints[1].pose.pose.position;
  const geometry_msgs::Point rlt_point(calcRelativeCoordinate(point, pose));
  return rlt_point.x < 0 ? -1 : 1;
}

calcRelativeCoordinate函数：
作用：计算全局坐标系中某点相对于机器人坐标系中的位置,即传入函数的point（全局坐标系下的坐标）相对于pose坐标系的位置。具体含义可见下面示意图，calcRelativeCoordinate函数返回的便是P点在车体坐标系O’x’y’中的坐标（x’, y’）。

实现：下面是Autoware的代码实现，调用了TF的一些函数进行处理。但是具体的内部机理，涉及到TF库的一些原理，坐标系的转换，四元数之类的，超出了博主的能力范围，也许以后有精力会去研究一下TF的一些原理，这里就直接用Autoware这个函数就好了。

// calculation relative coordinate of point from current_pose frame
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;
}

作用验证：我们设计一个小实验，验证一下calcRelativeCoordinate函数的作用，其中current_pose所对应的坐标系（也就是这个刚体对应的位置与方向，其中方向是用四元数表示的），其与世界坐标系之间的x,y,z轴方向是一致的，（防盗标记：zhengkunxian）只是其原点位于世界坐标系(4,7,0)处，很容易的我们知道point_msg(1,2,0)在current_pose所对应的坐标系中的坐标为(-3,-5,0)，而代码运行结果与我们的计算相符，因此得证代码的作用。

再回到getDirection函数，其根据返回的P点在车体坐标系O’x’y’中的坐标的x坐标，如果其小于0则返回-1，否则返回1。实际上getDirection函数内只分析了传入的lane中记录的lane.waypoints[0]所在坐标系和lane.waypoints[1]坐标点之间的关系。

再回到调用getDirection函数的replanLaneWaypointVel函数，接着分析changeVelSign函数的作用：顾名思义，改变路径点的速度符号。

void WaypointReplanner::changeVelSign(autoware_msgs::Lane& lane, bool positive) const
{
  const int sgn = positive ? 1 : -1;
  for (auto& el : lane.waypoints)
  {
    el.twist.twist.linear.x = sgn * fabs(el.twist.twist.linear.x);
  }
}

接着replanLaneWaypointVel函数继续执行，调用limitVelocityByRange函数：

limitVelocityByRange(0, last, 0, velocity_max_, lane);

其中last：const unsigned long last = lane.waypoints.size() - 1;
velocity_max_：velocity_max_ = kmph2mps(conf->velocity_max);

首先判断lane中储存的路径点是否为空，若不为空则接着判断offset（偏移）是否大于0，如果大于0则进一步对start_idx和end_idx进行处理。这里replanLaneWaypointVel函数调用时因为传入的offset=0，因此跳过。接着为了防止下标越界对end_idx进行检查。检查完毕后对lane中所有路径点的速度进行最大值的限制。

void WaypointReplanner::limitVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                             double vmin, autoware_msgs::Lane& lane)
{
  if (lane.waypoints.empty())
  {
    return;
  }
  if (offset > 0)
  {
    start_idx = (start_idx > offset) ? (start_idx - offset) : 0;
    end_idx = (end_idx > offset) ? (end_idx - offset) : 0;
  }
  end_idx = (end_idx >= lane.waypoints.size()) ? lane.waypoints.size() - 1 : end_idx;
  for (unsigned long idx = start_idx; idx <= end_idx; idx++)
  {
    if (lane.waypoints[idx].twist.twist.linear.x < vmin)
    {
      continue;
    }
    lane.waypoints[idx].twist.twist.linear.x = vmin;
  }
  limitAccelDecel(start_idx, lane);
  limitAccelDecel(end_idx, lane);
}

limitAccelDecel函数：
根据下式（高一物理哦～其实博主高中时候物理超强的，但是现在基本忘光了）对lane中所存储的下一路径点的速度进行修正
$$V_{next}^2-V_{now}^2=2ax\to V_{next}=\sqrt{2ax+V_{now}^2}.$$

其中 $a$ 是最大加速度，$x$ 是距离（根据前后两个路径点的位置算得）。由此可以计算出在最大加速度的限制条件下，下一路径点的速度最大值/最小值，据此进行修正。

void WaypointReplanner::limitAccelDecel(const unsigned long idx, autoware_msgs::Lane& lane)
{
  const double acc[2] = { accel_limit_, decel_limit_ };
  const unsigned long end_idx[2] = { lane.waypoints.size() - idx, idx + 1 };
  const int sgn[2] = { 1, -1 };
  for (int j = 0; j < 2; j++)  // [j=0]: accel_limit_process, [j=1]: decel_limit_process
  {
    double v = lane.waypoints[idx].twist.twist.linear.x;//v保持不变
    unsigned long next = idx + sgn[j];//[j=0]: next = idx + 1, [j=1]: next = idx - 1 
    for (unsigned long i = 1; i < end_idx[j]; i++, next += sgn[j])//[j=0]: end_idx[j] = lane.waypoints.size() - idx, [j=1]: end_idx[j] = idx + 1 
    {
      const geometry_msgs::Point& p0 = lane.waypoints[next - sgn[j]].pose.pose.position;
      const geometry_msgs::Point& p1 = lane.waypoints[next].pose.pose.position;
      const double dist = std::hypot(p0.x - p1.x, p0.y - p1.y);//计算距离
      v = sqrt(2 * acc[j] * dist + v * v);
      if (v > velocity_max_ || v > lane.waypoints[next].twist.twist.linear.x)
      {
        break;
      }
      lane.waypoints[next].twist.twist.linear.x = v;
    }
  }
}

再次回到replanLaneWaypointVel函数（博主把下一步代码贴出来，后面也这样，为大家节省往回翻的精力）：
根据设置的resample_mode_决定是否执行resampleLaneWaypoint函数。我们不管在Autoware交互界面是怎么设置的，所有的函数都会被分析讲解。

if (resample_mode_)
  {
    resampleLaneWaypoint(resample_interval_, lane, dir);
  }

resampleLaneWaypoint函数：
顾名思义，这个函数的作用是重新采样路径点。

void WaypointReplanner::resampleLaneWaypoint(const double resample_interval, autoware_msgs::Lane& lane, int dir)
{
  if (lane.waypoints.size() < 2)
  {
    return;
  }
  autoware_msgs::Lane original_lane(lane);
  lane.waypoints.clear();
  lane.waypoints.emplace_back(original_lane.waypoints[0]);
  lane.waypoints.reserve(ceil(1.5 * calcPathLength(original_lane) / resample_interval_));
  //reserve函数尝试为向量保留足够的内存以容纳指定数量的元素。
  //ceil(x)函数返回的是大于x的最小整数，例如ceil(10.5) == 11，ceil(-10.5) ==-10
  //calcPathLength函数计算original_lane中路径点之间总的距离，也就是整条轨迹的长度

  for (unsigned long i = 1; i < original_lane.waypoints.size(); i++)
  {
    CbufGPoint curve_point = getCrvPointsOnResample(lane, original_lane, i);//采三个轨迹点
    const std::vector<double> curve_param = calcCurveParam(curve_point);//根据三个轨迹点求轨迹曲线的圆心和半径
    lane.waypoints.back().twist.twist = original_lane.waypoints[i - 1].twist.twist;
    lane.waypoints.back().wpstate = original_lane.waypoints[i - 1].wpstate;
    lane.waypoints.back().change_flag = original_lane.waypoints[i - 1].change_flag;
    // if going straight
    if (curve_param.empty())
    {
      resampleOnStraight(curve_point, lane);//如果三点确定的轨迹是直线
    }
    // else if turnning curve
    else
    {
      resampleOnCurve(curve_point[1], curve_param, lane, dir);//如果三点确定的轨迹是曲线
    }
  }
  lane.waypoints[0].pose.pose.orientation = lane.waypoints[1].pose.pose.orientation;
  lane.waypoints.back().twist.twist = original_lane.waypoints.back().twist.twist;
  lane.waypoints.back().wpstate = original_lane.waypoints.back().wpstate;
  lane.waypoints.back().change_flag = original_lane.waypoints.back().change_flag;
}

对getCrvPointsOnResample采三个轨迹点$p_0$，$p_1$和$p_2$进行分析：

const CbufGPoint WaypointReplanner::getCrvPointsOnResample(
    const autoware_msgs::Lane& lane, const autoware_msgs::Lane& original_lane, unsigned long original_index) const
{
  unsigned long id = original_index;
  CbufGPoint curve_point(3);
  const unsigned int n = (lookup_crv_width_ - 1) / 2;//lookup_crv_width_此处被定义为5
  const autoware_msgs::Waypoint cp[3] = {
    (lane.waypoints.size() < n) ? lane.waypoints.front() : lane.waypoints[lane.waypoints.size() - n],
    original_lane.waypoints[id],
    (id < original_lane.waypoints.size() - n) ? original_lane.waypoints[id + n] : original_lane.waypoints.back()
  };
  for (int i = 0; i < 3; i++)
  {
    curve_point.push_back(cp[i].pose.pose.position);
  }
  return curve_point;
}

其中lookup_crv_width_此处被赋值为5，因此n=2。我们不妨进行假设，original_lane.waypoints.size()=5。因为lane.waypoints.clear()之后lane.waypoints.emplace_back(original_lane.waypoints[0])，可知lane在进入for循环被传入getCrvPointsOnResample函数时初始lane.waypoints.size()=1。每次for循环一次，会调用resampleOnStraight或者resampleOnCurve函数，往lane里面填入新的路径点，因此每次for循环结束自然而然lane.waypoints.size()++。基于上面的假设以及已知条件，我们开始进行分析。

calcCurveParam函数根据采样的三个点$p_0$，$p_1$和$p_2$首先计算：
$$\begin{gathered} d=2\{(p_{0y}-p_{2y})\times (p_{0x}-p_{1x})-(p_{0y}-p_{1y})\times (p_{0x}-p_{2x})\} \\ a=p_{0y}^2-p_{1y}^2+p_{0x}^2-p_{1x}^2 \\ b=p_{0y}^2-p_{2y}^2+p_{0x}^2-p_{2x}^2 \end{gathered}$$

由上面各式进一步计算得到曲线中心点和曲率：

$$\begin{gathered} c_x=\frac{(p_{0y}-p_{2y})\times a-(p_{0y}-p_{1y})\times b}{d} \\ {c}_y=\frac{(p_{0x}-p_{2x})\times a-(p_{0x}-p_{1x})\times b}{-d} \\ r=\frac{1}{\rho }=\sqrt{(c_x-p_{0x}{)}^2+(c_y-p_{0y}{)}^2} \end{gathered}$$
上面各式的原理是什么呢？上面的公式都是合并整理过的，我们看函数名猜测是用来根据圆上三点求圆心和半径的，而且最后一个求曲率的公式也确实是计算圆心到其中某一点的距离。“根据圆上三点求圆心和半径”这一问题有多种解法，主流的有两种：（1）分别通过其中两点的中垂线交点求圆心；（2）通过三个点到圆心距离相等联立方程求解。这两种方法的具体编程大家可以看圆上三点求圆心和半径，博主在这里就对第一种方法所得的公式与Autoware中的WaypointReplanner::calcCurveParam函数所使用的公式的最终结果进行对比，验证见下图，验证代码我也附上了，注意编译的时候要指明g++ -std=c++11，由此验证了WaypointReplanner::calcCurveParam函数确实是用来根据圆上三点求圆心和半径的，至于它的公式究竟是根据原理1整理而来，还是由原理2演化而来，都已经不重要了，这两个原理的求解公式必定是能合并整理成跟WaypointReplanner::calcCurveParam函数所使用的公式一样的（因为结果是一致的）。

const std::vector<double> WaypointReplanner::calcCurveParam(CbufGPoint p) const
{
  for (int i = 0; i < 3; i++, p.push_back(p.front()))  // if exception occured, change points order
  {
    const double d = 2 * ((p[0].y - p[2].y) * (p[0].x - p[1].x) - (p[0].y - p[1].y) * (p[0].x - p[2].x));
    if (fabs(d) < 1e-8)
    {
      continue;
    }
    const std::vector<double> x2 = { p[0].x * p[0].x, p[1].x * p[1].x, p[2].x * p[2].x };
    const std::vector<double> y2 = { p[0].y * p[0].y, p[1].y * p[1].y, p[2].y * p[2].y };
    const double a = y2[0] - y2[1] + x2[0] - x2[1];
    const double b = y2[0] - y2[2] + x2[0] - x2[2];
    std::vector<double> param(3);
    const double cx = param[0] = ((p[0].y - p[2].y) * a - (p[0].y - p[1].y) * b) / d;
    const double cy = param[1] = ((p[0].x - p[2].x) * a - (p[0].x - p[1].x) * b) / -d;
    param[2] = sqrt((cx - p[0].x) * (cx - p[0].x) + (cy - p[0].y) * (cy - p[0].y));
    return param;
  }
  return std::vector<double>();  // error
}

验证代码：

#include 
#include 
#include 
#include 
#include  

using namespace std;

class Point2f{
public: 
	double x,y,z;
	
public:
	Point2f(double X=0,double Y=0,double Z=0){
		this->x=X,this->y=Y,this->z=Z;
	}

	~Point2f(){}
};

struct CircleData
{
    Point2f center;
    double radius;
};

int main()  
{
	//随便定义三个点
	Point2f pt1, pt2, pt3;
	pt1.x = 150;
	pt1.y = 150;
	pt2.x = 200;
	pt2.y = 200;
	pt3.x = 250;
	pt3.y = 100;
	
    //定义两个点，分别表示两个中点
	Point2f midpt1, midpt2;
	//求出点1和点2的中点
	midpt1.x = (pt2.x + pt1.x) / 2;
	midpt1.y = (pt2.y + pt1.y) / 2;
	//求出点3和点1的中点
	midpt2.x = (pt3.x + pt1.x) / 2;
	midpt2.y = (pt3.y + pt1.y) / 2;
	//求出分别与直线pt1pt2，pt1pt3垂直的直线的斜率
	double k1 = -(pt2.x - pt1.x) / (pt2.y - pt1.y);
	double k2 = -(pt3.x - pt1.x) / (pt3.y - pt1.y);
	//然后求出过中点midpt1，斜率为k1的直线方程（既pt1pt2的中垂线）：y - midPt1.y = k1( x - midPt1.x)
	//以及过中点midpt2，斜率为k2的直线方程（既pt1pt3的中垂线）：y - midPt2.y = k2( x - midPt2.x)
	//定义一个圆的数据的结构体对象CD
	CircleData CD;
	//连立两条中垂线方程求解交点得到：
	CD.center.x = (midpt2.y - midpt1.y - k2* midpt2.x + k1*midpt1.x) / (k1 - k2);
	CD.center.y = midpt1.y + k1*(midpt2.y - midpt1.y - k2*midpt2.x + k2*midpt1.x) / (k1 - k2);
	//用圆心和其中一个点求距离得到半径：
	CD.radius = sqrt((CD.center.x - pt1.x)*(CD.center.x - pt1.x) + (CD.center.y - pt1.y)*(CD.center.y - pt1.y));
    cout << "CD.center.x = " << CD.center.x << endl;
    cout << "CD.center.y = " << CD.center.y << endl;
    cout << "CD.radius = " << CD.radius << endl;

    //随便定义三个点
	Point2f p0, p1, p2;
	p0.x = 150;
	p0.y = 150;
	p1.x = 200;
	p1.y = 200;
	p2.x = 250;
	p2.y = 100;
    const double d = 2 * ((p0.y - p2.y) * (p0.x - p1.x) - (p0.y - p1.y) * (p0.x - p2.x));
    const std::vector<double> x2 = { p0.x * p0.x, p1.x * p1.x, p2.x * p2.x };
    const std::vector<double> y2 = { p0.y * p0.y, p1.y * p1.y, p2.y * p2.y };
    const double a = y2[0] - y2[1] + x2[0] - x2[1];
    const double b = y2[0] - y2[2] + x2[0] - x2[2];
    std::vector<double> param(3);
    const double cx = ((p0.y - p2.y) * a - (p0.y - p1.y) * b) / d;
    const double cy = ((p0.x - p2.x) * a - (p0.x - p1.x) * b) / -d;
    const double r = sqrt((cx - p0.x) * (cx - p0.x) + (cy - p0.y) * (cy - p0.y));
    cout << "cx = " << cx << endl;
    cout << "cy = " << cy << endl;
    cout << "r = " << r << endl;

	return 0;
}

验证结果：
resampleOnStraight函数：

void WaypointReplanner::resampleOnStraight(const CbufGPoint& curve_point, autoware_msgs::Lane& lane)
{
  if (curve_point.size() != 3)
  {
    return;
  }
  autoware_msgs::Waypoint wp(lane.waypoints.back());
  const geometry_msgs::Point& pt = wp.pose.pose.position;
  const double yaw = atan2(curve_point[2].y - curve_point[0].y, curve_point[2].x - curve_point[0].x);
  wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
  //向量nvec由目标点和新lane的最后一个轨迹点确定
  const std::vector<double> nvec = { curve_point[1].x - pt.x, curve_point[1].y - pt.y, curve_point[1].z - pt.z };
  double dist = sqrt(nvec[0] * nvec[0] + nvec[1] * nvec[1]);//计算目标轨迹点和lane中最后一个轨迹点之间的距离
  std::vector<double> resample_vec = nvec;
  const double coeff = resample_interval_ / dist;
  for (auto& el : resample_vec)//根据重采样间隔resample_interval_矫正向量resample_vec的模
  {
    el *= coeff;
  }
  for (; dist > resample_interval_; dist -= resample_interval_)
  {//在lane的最后一个轨迹点的基础上根据向量resample_vec确定下一轨迹点，直到当前轨迹点离目标点的距离不足采样间隔
    wp.pose.pose.position.x += resample_vec[0];
    wp.pose.pose.position.y += resample_vec[1];
    wp.pose.pose.position.z += resample_vec[2];
    lane.waypoints.emplace_back(wp);
  }
}

resampleOnCurve函数：

void WaypointReplanner::resampleOnCurve(const geometry_msgs::Point& target_point,
                                        const std::vector<double>& curve_param, autoware_msgs::Lane& lane, int dir)
{
  if (curve_param.size() != 3)
  {
    return;
  }
  autoware_msgs::Waypoint wp(lane.waypoints.back());
  const double& cx = curve_param[0];
  const double& cy = curve_param[1];
  const double& radius = curve_param[2];
  const double reverse_angle = (dir < 0) ? M_PI : 0.0;

  const geometry_msgs::Point& p0 = wp.pose.pose.position;
  const geometry_msgs::Point& p1 = target_point;
  double theta = fmod(atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx), 2 * M_PI);//fmod取余，
  //保证atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx)的值在2 * M_PI范围内
  //atan2(p1.y - cy, p1.x - cx) - atan2(p0.y - cy, p0.x - cx)代表本次重采样的起始点和目标点与轨迹曲线圆心之间的夹角
  int sgn = (theta > 0.0) ? (1) : (-1);
  if (fabs(theta) > M_PI)
  {
    theta -= 2 * sgn * M_PI;//将theta的取值限定在-M_PI~M_PI
  }
  sgn = (theta > 0.0) ? (1) : (-1);
  // interport
  double t = atan2(p0.y - cy, p0.x - cx);
  double dist = radius * fabs(theta);//本次重采样的起始点和目标点之间的轨迹的弧长
  const double resample_dz = resample_interval_ * (p1.z - p0.z) / dist;//z坐标增长率，
  //在WaypointReplanner::resampleOnStraight函数中忘了z坐标的变化
  for (; dist > resample_interval_; dist -= resample_interval_)
  {
    if (lane.waypoints.size() == lane.waypoints.capacity())//这里检查了lane是否已经储存满，然而在
    //WaypointReplanner::resampleOnStraight函数中同样重采样轨迹点并存入lane，但是却忘了检查lane是否存满的问题
    {
      break;
    }
    t += sgn * resample_interval_ / radius;//resample_interval_ / radius->采样弧长/半径 得到 采样弧度
    const double yaw = fmod(t + sgn * M_PI / 2.0, 2 * M_PI) + reverse_angle;
    wp.pose.pose.position.x = cx + radius * cos(t);
    wp.pose.pose.position.y = cy + radius * sin(t);
    wp.pose.pose.position.z += resample_dz;
    wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
    lane.waypoints.emplace_back(wp);
  }
}

篇幅已经挺长的了，本篇就到此为止了。这一篇将节点waypoint_replanner中函数WaypointReplanner::replanLaneWaypointVel（位于src/autoware/core_planning/waypoint_maker/nodes/waypoint_replanner/waypoint_replanner.cpp）所调用的函数WaypointReplanner::resampleLaneWaypoint分析完成。后面一篇博客回到函数WaypointReplanner::replanLaneWaypointVel继续分析。