（四）学习笔记autoware源码core_planning（waypoint_replanner)

1.waypoint_replanner_node.

#include <ros/ros.h>
#include <autoware_msgs/LaneArray.h>
#include "waypoint_replanner.h"

namespace waypoint_maker
{
class WaypointReplannerNode
{
public:
  WaypointReplannerNode();
private:
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Publisher lane_pub_;
  ros::Subscriber lane_sub_, config_sub_;
  bool replanning_mode_, realtime_tuning_mode_;
  bool is_first_publish_;
  WaypointReplanner replanner_;
  autoware_msgs::LaneArray lane_array_;
  void replan(autoware_msgs::LaneArray &lane_array);
  void publishLaneArray();
  void laneCallback(const autoware_msgs::LaneArray::ConstPtr& lane_array);
  void configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf);
  autoware_config_msgs::ConfigWaypointReplanner startup_config;
  bool use_decision_maker_;
};

WaypointReplannerNode::WaypointReplannerNode() : pnh_("~"), is_first_publish_(true)
{
  WaypointReplannerConfig temp_config;

  double velocity_max_kph, velocity_min_kph;

  pnh_.param<bool>("replanning_mode", replanning_mode_, false);
  pnh_.param<bool>("realtime_tuning_mode", realtime_tuning_mode_, true);
  pnh_.param<double>("velocity_max", velocity_max_kph, 0.0);
  pnh_.param<double>("velocity_min", velocity_min_kph, 0.0);
  pnh_.param<double>("accel_limit", temp_config.accel_limit, 0.0);
  pnh_.param<double>("decel_limit", temp_config.decel_limit, 0.0);
  pnh_.param<double>("radius_thresh", temp_config.radius_thresh, 0.0);
  pnh_.param<double>("radius_min", temp_config.radius_min, 0.0);
  pnh_.param<bool>("resample_mode", temp_config.resample_mode, false);
  pnh_.param<double>("resample_interval", temp_config.resample_interval, 0.0);
  pnh_.param<bool>("replan_curve_mode", temp_config.replan_curve_mode, false);
  pnh_.param<bool>("replan_endpoint_mode", temp_config.replan_endpoint_mode, false);
  pnh_.param<bool>("overwrite_vmax_mode", temp_config.overwrite_vmax_mode, false);
  pnh_.param<double>("velocity_offset", temp_config.velocity_offset, 0.0);
  pnh_.param<double>("end_point_offset", temp_config.end_point_offset, 0.0);
  pnh_.param<double>("braking_distance", temp_config.braking_distance, 0.0);
  pnh_.param<bool>("use_decision_maker", use_decision_maker_, false);

  temp_config.velocity_max = kmph2mps(velocity_max_kph);
  temp_config.velocity_min = kmph2mps(velocity_min_kph);

  replanner_.updateConfig(temp_config);

  if (use_decision_maker_)
  {
    lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_array", 10, true);
  }
  else
  {
    lane_pub_ = 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);
}

void WaypointReplannerNode::replan(autoware_msgs::LaneArray& lane_array)
{/*
replan函数内遍历lane_array.lanes。这里回顾一下，lane_array.lanes内的元素都是Lane类型，
其中储存了节点waypoint_loader 从本地文件内读取的轨迹点数据，每个轨迹点记录了世界坐标系，
一般为通用横轴墨卡托投影(universal transverseMercator，UTM)坐标系下的坐标x,y,z 以及航向角等必要信息供规划所用。
对每一条Lane，replanner_都调用replanLaneWaypointVel函数。

*/
  for (auto &el : lane_array.lanes)
  {
    replanner_.replanLaneWaypointVel(el);
  }
}

void WaypointReplannerNode::publishLaneArray()
{/*
如果replanning_mode_为 true，则调用replan函数重规划。
*/
  autoware_msgs::LaneArray array(lane_array_);

  if (replanning_mode_)
  {
    replan(array);
  }
//发布重规划过的路径集合至话题"/based/lane waypoints _array"
  lane_pub_.publish(array);
  is_first_publish_ = false;
}

void WaypointReplannerNode::laneCallback(const autoware_msgs::LaneArray::ConstPtr& lane_array)
{//laneCallback函数为话题"/based/lane_waypoints_raw"的回调函数，在更新lane_array_后调用publishLaneArray函数重规划路径并发布到对应话题。

  lane_array_ = *lane_array;
  publishLaneArray();
}

void WaypointReplannerNode::configCallback(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
  /*configCallback函数为话题"/config/waypoint_replanner"的回调函数，主要进行一些成员变量的初始化。*/

  replanning_mode_ = conf->replanning_mode;
  realtime_tuning_mode_ = conf->realtime_tuning_mode;
  use_decision_maker_ = conf->use_decision_maker;
  replanner_.initParameter(conf);
  if (!lane_array_.lanes.empty() && (is_first_publish_ || realtime_tuning_mode_))
  {
    publishLaneArray();
  }
}

}

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

  return 0;
  /*
  在 main函数中只做了一件事:新建waypoint_maker::WaypointReplannerNode对象 wr，
  因此转到 WaypointReplannerNode类的构造函数。构造函数内实现了话题的订阅和发布。
  其中订阅的"/based/lane_waypoints_raw"就是前面waypoint_loader 节点所发布的话题。
*/
}

2.waypoint_replanner.

#include "waypoint_replanner.h"

namespace waypoint_maker
{

WaypointReplanner::WaypointReplanner()
{
}

WaypointReplanner::~WaypointReplanner()
{
}

void WaypointReplanner::updateConfig(const WaypointReplannerConfig& config)
{
  config_ = config;
  config_.radius_inf = 10 * config_.radius_thresh;
  config_.velocity_param = calcVelParam(config_.velocity_max);
}

void WaypointReplanner::initParameter(const autoware_config_msgs::ConfigWaypointReplanner::ConstPtr& conf)
{
  WaypointReplannerConfig temp_config;

  temp_config.velocity_max = kmph2mps(conf->velocity_max);
  temp_config.velocity_min = kmph2mps(conf->velocity_min);
  temp_config.accel_limit = conf->accel_limit;
  temp_config.decel_limit = conf->decel_limit;
  temp_config.radius_thresh = conf->radius_thresh;
  temp_config.radius_min = conf->radius_min;
  temp_config.lookup_crv_width = 5;
  temp_config.resample_mode = conf->resample_mode;
  temp_config.resample_interval = conf->resample_interval;
  temp_config.replan_curve_mode = conf->replan_curve_mode;
  temp_config.replan_endpoint_mode = conf->replan_endpoint_mode;
  temp_config.overwrite_vmax_mode = conf->overwrite_vmax_mode;
  temp_config.velocity_offset = conf->velocity_offset;
  temp_config.end_point_offset = conf->end_point_offset;
  temp_config.braking_distance = conf->braking_distance;

  updateConfig(temp_config);
}

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);//fabs 取绝对值的函数。
  }
}

void WaypointReplanner::replanLaneWaypointVel(autoware_msgs::Lane& lane)
{                       //重规划轨迹点的速度。
  if (lane.waypoints.empty())//empty函数构造空对象。
  {
    return;
  }
  const LaneDirection dir = getLaneDirection(lane);//getDirection函数,确定车辆启动时的方向。
  unsigned long last = lane.waypoints.size() - 1;//size()获取字符串长度。
  changeVelSign(lane, true);//changeVelSign函数,改变轨迹点的速度符号。
  limitVelocityByRange(0, last, 0, config_.velocity_max, lane);
  if (config_.resample_mode)
  {
    resampleLaneWaypoint(config_.resample_interval, lane, dir);
    last = lane.waypoints.size() - 1;
  }
  if (config_.replan_curve_mode)
  {
    std::vector<double> curve_radius;
    KeyVal curve_list;
    createRadiusList(lane, curve_radius);
    createCurveList(curve_radius, curve_list);
    if (config_.overwrite_vmax_mode)
    {// set velocity_max for all_point  为所有轨迹点设置最大速度
      setVelocityByRange(0, last, 0, config_.velocity_max, lane);
    }
    // set velocity by curve    通过曲线设置速度
    for (const auto& el : curve_list)
    {
      const double& radius = el.second.second;
      double vmin = config_.velocity_max - config_.velocity_param * (config_.radius_thresh - radius);
      vmin = (vmin < config_.velocity_min) ? config_.velocity_min : vmin;
      limitVelocityByRange(el.first, el.second.first, config_.velocity_offset, vmin, lane);
    }
  }
  // set velocity on start & end of lane   设置Lane的起始和末尾点的速度

  if (config_.replan_endpoint_mode)
  {
    const unsigned long zerospeed_start = last - config_.end_point_offset;
    const unsigned long lowspeed_start = zerospeed_start - config_.braking_distance;
    raiseVelocityByRange(0, last, 0, config_.velocity_min, lane);
    limitVelocityByRange(0, 0, 0, config_.velocity_min, lane);
    limitVelocityByRange(lowspeed_start, last, 0, config_.velocity_min, lane);
    setVelocityByRange(zerospeed_start, last, 0, 0.0, lane);
  }
  if (dir == LaneDirection::Backward)
  {
    changeVelSign(lane, false);
  }
}

void WaypointReplanner::resampleLaneWaypoint(const double resample_interval, autoware_msgs::Lane& lane, LaneDirection dir)
{                       //重新采样轨迹点。
  if (lane.waypoints.size() < 2)
  {
    return;
  }
  autoware_msgs::Lane original_lane(lane);
  lane.waypoints.clear();//clear()清除所有元素
  lane.waypoints.emplace_back(original_lane.waypoints[0]);
  lane.waypoints.reserve(ceil(1.5 * calcPathLength(original_lane) / config_.resample_interval));
  //ceil()返回一个不小于给定数值的最小整数值。

  //reserve()是为容器预留空间，即为当前容器设定一个空间分配的阈值，
  //但是并不会为容器直接allocate具体的空间，具体空间的分配是在创建对象时候进行分配得

  //calcPathLength函数计算 original_lane中轨迹点之间总的距离
  //也就是整条轨迹的长度

  for (unsigned long i = 1; i < original_lane.waypoints.size(); i++)
  {     
    CbufGPoint curve_point = getCrvPointsOnResample(lane, original_lane, i);
    //getCrvPointsonResample函数采样三个轨迹点

    const std::vector<double> curve_param = calcCurveParam(curve_point);//根据三个轨迹点求轨迹曲线的圆心和半径
    lane.waypoints.back().twist.twist = original_lane.waypoints[i - 1].twist.twist;
    //back()返回当前vector容器中末尾元素的引用。end()返回一个当前vector容器中末尾元素的迭代器。
    //back()=end()-1。
    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, dir); //如果三点确定的轨迹是直线
    }
    // 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;
}

void WaypointReplanner::resampleOnStraight(const CbufGPoint& curve_point, autoware_msgs::Lane& lane, LaneDirection dir)
{
  if (curve_point.size() != 3)
  {
    return;
  }
  autoware_msgs::Waypoint wp(lane.waypoints.back());
  const geometry_msgs::Point& pt = wp.pose.pose.position;
  const int sgn = (dir == LaneDirection::Forward) ? 0 : 1;
  const double yaw = atan2(curve_point[2].y - curve_point[0].y, curve_point[2].x - curve_point[0].x) + sgn * M_PI;
  //atan2（y，x）求的是y/x的反正切,结果是一个弧度制的数。
  wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);

  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]);
  //sqrt()求平方根。
  std::vector<double> resample_vec = nvec;
  const double coeff = config_.resample_interval / dist;
  for (auto& el : resample_vec)
  {
    el *= coeff;
  }
  for (; dist > config_.resample_interval; dist -= config_.resample_interval)
  {
    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);
  }
}

void WaypointReplanner::resampleOnCurve(const geometry_msgs::Point& target_point,
                                        const std::vector<double>& curve_param, autoware_msgs::Lane& lane, LaneDirection 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 == LaneDirection::Backward) ? 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(a,b)求浮点数a除以b得到的余数。
  int sgn = (theta > 0.0) ? (1) : (-1);
  if (fabs(theta) > M_PI)
  {
    theta -= 2 * sgn * 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 = config_.resample_interval * (p1.z - p0.z) / dist;
  for (; dist > config_.resample_interval; dist -= config_.resample_interval)
  {
    if (lane.waypoints.size() == lane.waypoints.capacity())
    //capacity()容器能存储数据的个数;
    //size()容器目前存在的元素数;
    //reserve()指定容器能存储数据的个数;
    //resize()重新指定有效元素的个数，区别与reserve()指定容量的大小。
    {
      break;
    }
    t += sgn * config_.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);
  }
}

// Three points used for curve detection (the target point is the center)
// [0] = previous point, [1] = target point, [2] = next point
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 = (config_.lookup_crv_width - 1) / 2;
  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;
}

const CbufGPoint WaypointReplanner::getCrvPoints(const autoware_msgs::Lane& lane, unsigned long index) const
{
  CbufGPoint curve_point(3);
  const unsigned int n = (config_.lookup_crv_width - 1) / 2;
  const unsigned long curve_index[3] = { (index < n) ? 0 : (index - n), index, (index >= lane.waypoints.size() - n) ?
                                                                                   (lane.waypoints.size() - 1) :
                                                                                   (index + n) };
  for (int i = 0; i < 3; i++)
  {
    curve_point.push_back(lane.waypoints[curve_index[i]].pose.pose.position);
  }
  return curve_point;
}

void WaypointReplanner::createRadiusList(const autoware_msgs::Lane& lane, std::vector<double>& curve_radius)
{
  //createRadiusList函数(源码略)将每个轨迹点(除去第一个和最后一个轨迹点)的半径存入传入函数的变量curve_radius 中。
  if (lane.waypoints.empty())
  {
    return;
  }
  curve_radius.resize(lane.waypoints.size());
    //capacity()容器能存储数据的个数;
    //size()容器目前存在的元素数;
    //reserve()指定容器能存储数据的个数;
    //resize()重新指定有效元素的个数，区别与reserve()指定容量的大小。
  curve_radius.at(0) = curve_radius.back() = config_.radius_inf;
  //at(0)获取第一个字符。

  for (unsigned long i = 1; i < lane.waypoints.size() - 1; i++)
  {
    CbufGPoint curve_point(getCrvPoints(lane, i));
    const std::vector<double> curve_param(calcCurveParam(curve_point));

    // if going straight
    if (curve_param.empty())
    {
      curve_radius.at(i) = config_.radius_inf;
    }
    // else if turnning curve
    else
    {
      curve_radius.at(i) = (curve_param[2] > config_.radius_inf) ? config_.radius_inf : curve_param[2];
    }
  }
}

const double WaypointReplanner::calcVelParam(double vmax) const
{
  if (fabs(config_.radius_thresh - config_.radius_min) < 1e-8)
  {
    return DBL_MAX;  // error
  }
  return (vmax - config_.velocity_min) / (config_.radius_thresh - config_.radius_min);
}

void WaypointReplanner::createCurveList(const std::vector<double>& curve_radius, KeyVal& curve_list)
{
  unsigned long index = 0;
  bool on_curve = false;
  double radius_localmin = DBL_MAX;
  for (unsigned long i = 1; i < curve_radius.size(); i++)
  {
    if (!on_curve && curve_radius[i] <= config_.radius_thresh && curve_radius[i - 1] > config_.radius_thresh)
    {
      index = i;
      on_curve = true;
    }
    else if (on_curve && curve_radius[i - 1] <= config_.radius_thresh && curve_radius[i] > config_.radius_thresh)
    {
      on_curve = false;
      if (radius_localmin < config_.radius_min)
      {
        radius_localmin = config_.radius_min;
      }
      curve_list[index] = std::make_pair(i, radius_localmin);
      radius_localmin = DBL_MAX;
    }
    if (!on_curve)
    {
      continue;
    }
    if (radius_localmin > curve_radius[i])
    {
      radius_localmin = curve_radius[i];
    }
  }
}


void WaypointReplanner::setVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                             double vel, autoware_msgs::Lane& lane)
{//setVelocityByRange函数对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++)
  {
    lane.waypoints[idx].twist.twist.linear.x = vel;
  }
}

void WaypointReplanner::raiseVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                           double vmin, autoware_msgs::Lane& lane)
{//raiseVelocityByRange函数保证选定范围内轨迹点的速度不小于传入函数的vmin。

  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;
  }
}

void WaypointReplanner::limitVelocityByRange(unsigned long start_idx, unsigned long end_idx, unsigned int offset,
                                             double vmin, autoware_msgs::Lane& lane)
{/*
首先判断lane中储存的轨迹点是否为空，若不为空则接着判断offset(偏移)是否大于0，
如果大于О则进一步对start_idx 和 end_idx进行处理。前面replanLancWaypointVel函数调用时传入的 offset=0，
因此跳过。接着为了防止下标越界对end_idx进行检查。检查完毕后对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);
  //limitVelocityByRange函数中的limitAccelDecel函数(源码略)通过加速度与速度之间的运动学关系进一步对速度进行修正。
  limitAccelDecel(end_idx, lane);
}

void WaypointReplanner::limitAccelDecel(const unsigned long idx, autoware_msgs::Lane& lane)
{
  const double acc[2] = { config_.accel_limit, config_.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]: config_.accel_limitprocess, [j=1]: config_.decel_limitprocess
  {
    double v = lane.waypoints[idx].twist.twist.linear.x;
    unsigned long next = idx + sgn[j];
    for (unsigned long i = 1; i < end_idx[j]; i++, next += sgn[j])
    {
      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);
      //hypot(x,y)=sqrt(x * x + y * y),类似求直角三角形的斜边。
      v = sqrt(2 * acc[j] * dist + v * v);
      if (v > config_.velocity_max || v > lane.waypoints[next].twist.twist.linear.x)
      {
        break;
      }
      lane.waypoints[next].twist.twist.linear.x = v;
    }
  }
}

// get curve 3-Parameter [center_x, center_y, radius] with 3 point input. If error occured, return empty vector.
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
  {//front()与back()互反；begin()与end()互反。
    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
}

const double WaypointReplanner::calcPathLength(const autoware_msgs::Lane& lane) const
{
  double distance = 0.0;
  for (unsigned long i = 1; i < lane.waypoints.size(); i++)
  {
    const geometry_msgs::Point& p0 = lane.waypoints[i - 1].pose.pose.position;
    const geometry_msgs::Point& p1 = lane.waypoints[i].pose.pose.position;
    tf::Vector3 tf0(p0.x, p0.y, 0.0);
    tf::Vector3 tf1(p1.x, p1.y, 0.0);
    distance += tf::tfDistance(tf0, tf1);
    /*
    tf::tfDot(const Vector3 &v1, const Vector3 &v2)	计算两个向量的点积
    tf::length()	计算向量的模
    tf::tfAngle(const Vector3 &v1, const Vector3 &v2)	计算两个向量的夹角
    tf::tfDistance(const Vector3 &v1, const Vector3 &v2)	计算两个向量的距离
    tf::tfCross(const Vector3 &v1,const Vector3 &v2)	计算两个向量的乘积
    Vector3 &normalize()	求与已知向量同方向的单位向量
    */
  }
  return distance;
}

};

参考书目《Autoware与自动驾驶技术》