（九）学习笔记autoware源码core_planning（lane_select)

1.lane_select_node

就一个主函数。

#include 

#include "lane_select_core.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "lane_select");
  lane_planner::LaneSelectNode lsn;
  lsn.run();

  return 0;
}

2.hermite_curve

#include "hermite_curve.h"

namespace lane_planner
{

  /*
  crcateVectorFromPosc函数实际上得到的是世界坐标系下位置为(1,0,0)的点经过跟
  当前车体坐标系(位姿p 所对应的车体坐标系)一样的x,y,z轴的旋转变换后在世界坐标系中新的位置。
  current_pose所对应的坐标系是沿着z轴旋转90度得到的，因此向量(1,0,0)同样绕z轴旋转
  90度，其终点在世界坐标系下的新坐标为(0,1,0)，符合createVectorFromPose函数输出的结果。
  */
void createVectorFromPose(const geometry_msgs::Pose &p, tf::Vector3 *v)
{
  tf::Transform pose;
  tf::poseMsgToTF(p, pose);
  tf::Vector3 x_axis(1, 0, 0);

  //getBasis 函数返回旋转的基础矩阵乘以x_axis后得到矩阵的第一列
  *v = pose.getBasis() * x_axis;
}


/*
getPointAndVectorFromPose函数赋值传入函数的point和 vector。其中，point为位姿pose在
世界坐标系下的位置坐标，vector为位姿pose对应的车体坐标系的x轴正方向在世界坐标系下的方位(用二维单位向量表示)。
*/
void getPointAndVectorFromPose(const geometry_msgs::Pose &pose, Element2D *point, Element2D *vector)
{
  point->set(pose.position.x, pose.position.y);

  tf::Vector3 tmp_tf_vevtor;
  createVectorFromPose(pose, &tmp_tf_vevtor);
  vector->set(tmp_tf_vevtor.getX(), tmp_tf_vevtor.getY());
}


//generateHermiteCurveForROS函数确定从start 到end的具体行车轨迹。
std::vector generateHermiteCurveForROS(const geometry_msgs::Pose &start,
                                                                const geometry_msgs::Pose &end,
                                                                const double velocity_mps, const double vlength)
{
  std::vector wps;
  Element2D p0(0, 0), v0(0, 0), p1(0, 0), v1(0, 0);
  getPointAndVectorFromPose(start, &p0, &v0);
  getPointAndVectorFromPose(end, &p1, &v1);


//已知曲线的两个端点坐标p0、p1和端点处的切线v0、v1,确定Hermite(埃尔米特)曲线，获得采样点序列形式的曲线表达结果
  std::vector result = generateHermiteCurve(p0, v0, p1, v1, vlength);

  double height_d = fabs(start.position.z - end.position.z);
  for (uint32_t i = 0; i < result.size(); i++)
  {
    autoware_msgs::Waypoint wp;
    wp.pose.pose.position.x = result.at(i).x;
    wp.pose.pose.position.y = result.at(i).y;
    wp.twist.twist.linear.x = velocity_mps;

    // height
    wp.pose.pose.position.z =
        (i == 0) ? start.position.z : (i == result.size() - 1) ? end.position.z : start.position.z < end.position.z ?
                                                                 start.position.z + height_d * i / result.size() :
                                                                 start.position.z - height_d * i / result.size();

    // orientation
    if (i != result.size() - 1)
    {
      double radian = atan2(result.at(i + 1).y - result.at(i).y, result.at(i + 1).x - result.at(i).x);
      wp.pose.pose.orientation = tf::createQuaternionMsgFromYaw(radian);
    }
    else
    {
      wp.pose.pose.orientation = wps.at(wps.size() - 1).pose.pose.orientation;
    }

    wps.push_back(wp);
  }
  return wps;
}

//generateHermiteCurve函数根据曲线端点坐标和切线返回对应Hermite曲线上的点序列。
std::vector generateHermiteCurve(const Element2D &p0, const Element2D &v0, const Element2D &p1,
                                            const Element2D &v1, const double vlength)
{
  std::vector result;
  const double interval = 1.0;
  int32_t divide = 2;
  const int32_t loop = 100;
  while (divide < loop)
  {
    result.reserve(divide);
    for (int32_t i = 0; i < divide; i++)
    {
      double u = i * 1.0 / (divide - 1);
      double u_square = pow(u, 2);
      double u_cube = pow(u, 3);
      double coeff_p0 = 2 * u_cube - 3 * u_square + 1;
      double coeff_v0 = u_cube - 2 * u_square + u;
      double coeff_p1 = (-1) * 2 * u_cube + 3 * u_square;
      double coeff_v1 = u_cube - u_square;
      // printf("u: %lf, u^2: %lf, u^3: %lf, coeff_p0: %lf, coeff_v0: %lf, coeff_p1: %lf, coeff_v1: %lf\n", u, u_square,
      // u_cube, coeff_p0, coeff_p1, coeff_v0, coeff_v1);
      result.push_back(
          Element2D((p0.x * coeff_p0 + vlength * v0.x * coeff_v0 + p1.x * coeff_p1 + vlength * v1.x * coeff_v1),
                    (p0.y * coeff_p0 + vlength * v0.y * coeff_v0 + p1.y * coeff_p1 + vlength * v1.y * coeff_v1)));
    }

    double dt = sqrt(pow((result.at(divide / 2 - 1).x - result.at(divide / 2).x), 2) +
                     pow((result.at(divide / 2 - 1).y - result.at(divide / 2).y), 2));
    std::cout << "interval : " << dt << std::endl;
    if (interval > dt || divide == loop - 1)
      return result;
    else
    {
      result.clear();
      result.shrink_to_fit();
      divide++;
    }
  }
  return result;
}
}  // namespace

3.lane_select_core

#include "lane_select_core.h"

#include 

namespace lane_planner
{
// Constructor

/*
节点lane_select启动时新建LaneSelectNode对象，查看LaneSelectNode 唯一的构造函数LaneSelectNode()。
LaneSelectNode函数中首先对一些成员变量进行赋值,接着调用initForROS函数。
*/
LaneSelectNode::LaneSelectNode()
  : private_nh_("~")
  , lane_array_id_(-1)
  , current_lane_idx_(-1)
  , right_lane_idx_(-1)
  , left_lane_idx_(-1)
  , is_lane_array_subscribed_(false)
  , is_current_pose_subscribed_(false)
  , is_current_velocity_subscribed_(false)
  , is_current_state_subscribed_(false)
  , is_config_subscribed_(false)
  , distance_threshold_(3.0)
  , lane_change_interval_(10.0)
  , lane_change_target_ratio_(2.0)
  , lane_change_target_minimum_(5.0)
  , vlength_hermite_curve_(10)
  , search_closest_waypoint_minimum_dt_(5)
  , current_state_("UNKNOWN")
{
  initForROS();
}

// Destructor
LaneSelectNode::~LaneSelectNode()
{
}


//initForROS函数的内容跟前面分析的一些ROS节点的main 函数中的内容类似:设置订阅者/发布者，设置参数。
void LaneSelectNode::initForROS()
{
  // setup subscriber  设置订阅
  sub1_ = nh_.subscribe("traffic_waypoints_array", 1, &LaneSelectNode::callbackFromLaneArray, this);
  sub2_ = nh_.subscribe("current_pose", 1, &LaneSelectNode::callbackFromPoseStamped, this);
  sub3_ = nh_.subscribe("current_velocity", 1, &LaneSelectNode::callbackFromTwistStamped, this);
  sub4_ = nh_.subscribe("state", 1, &LaneSelectNode::callbackFromState, this);
  sub5_ = nh_.subscribe("/config/lane_select", 1, &LaneSelectNode::callbackFromConfig, this);
  sub6_ = nh_.subscribe("/decision_maker/state", 1, &LaneSelectNode::callbackFromDecisionMakerState, this);

  // setup publisher  设置发布
  pub1_ = nh_.advertise("base_waypoints", 1);
  pub2_ = nh_.advertise("closest_waypoint", 1);
  pub3_ = nh_.advertise("change_flag", 1);
  pub4_ = nh_.advertise("/current_lane_id", 1);
  pub5_ = nh_.advertise("vehicle_location", 1);

  vis_pub1_ = nh_.advertise("lane_select_marker", 1);

  // get from rosparam  设置参数
  private_nh_.param("lane_change_interval", lane_change_interval_, double(2));
  private_nh_.param("distance_threshold", distance_threshold_, double(3.0));
  private_nh_.param("search_closest_waypoint_minimum_dt", search_closest_waypoint_minimum_dt_, int(5));
  private_nh_.param("lane_change_target_ratio", lane_change_target_ratio_, double(2.0));
  private_nh_.param("lane_change_target_minimum", lane_change_target_minimum_, double(5.0));
  private_nh_.param("vector_length_hermite_curve", vlength_hermite_curve_, double(10.0));
}

//isAl1TopicsSubscribed 函数判断节点lane_select是否订阅了所有所需的话题。
bool LaneSelectNode::isAllTopicsSubscribed()
{
  //查看各个标志确定是否此节点所需的所有话题都被订阅
  if (!is_current_pose_subscribed_ || !is_lane_array_subscribed_ || !is_current_velocity_subscribed_)
  {
    ROS_WARN("Necessary topics are not subscribed yet. Waiting...");
    return false;
  }
  return true;
}


//对于initForLaneSelect函数的代码注释中 tuple_vec_的详细分析位于后文“getCloscstWaypointNumberForEachLanes函数”中。
void LaneSelectNode::initForLaneSelect()
{
  //isAllTopicsSubscribed函数判断节点lane_select是否订阅了所有所需的话题
  if (!isAllTopicsSubscribed())
    return;

  // search closest waypoint number for each lanes

/*
getClosestwaypointNumberForEachLanes函数搜索每条Lane内的"距离最近轨迹点"下标距离最近轨迹点: 
Lane内在期望行车轨迹上的且距离当前位置current_pose_距离最近的轨迹点
*/
  if (!getClosestWaypointNumberForEachLanes())
  {
    //如果搜索失败发布消息"-1"到话题"closest__waypoint""
    publishClosestWaypoint(-1);

    //lane_array_id_在话题"traffic_waypoints_array"的回调函数callbackFromLaneArray中被更新向话题"vehicle_location"发布消息
    publishVehicleLocation(-1, lane_array_id_);

    //重置current_lane_idx_, right_lane_idx_和left_lane_idx_,并且可视化各类轨迹点
    resetLaneIdx();
    return;
  }

/*
更新current_lane_idx_, current_lane_idx_是"全局距离最近点"所在的元组在tuple_vec_中的下标
全局距离最近点:所有Lane中与当前位置的"距离最近轨迹点”形成的集合中距离当前位置最近的轨迹点
*/
  findCurrentLane();

/*
分别在"全局距离最近点"所在车体位姿左侧和右侧的所有"距离最近轨迹点中找到距离“全局距离最近点”最近的轨迹点
所在的元组下标，并对应更新left_lane_idx_和 right_lane_idx_
*/
  findNeighborLanes();

  //更新tuple_vec_中所有元组的changeFlag
  updateChangeFlag();

  //更新lane_for_change_
  createLaneForChange();

//根据元组tuple_vec_.at(current_lane_idx_)中的信息发布消息到各个话题
  publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
  publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
  publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
  publishVehicleLocation(std::get<1>(tuple_vec_.at(current_lane_idx_)), lane_array_id_);

  //可视化各类轨迹点组成的行车路径
  publishVisualizer();

//重置各种作为标志的变量
  resetSubscriptionFlag();
  return;
}

//重置current_lane_idx_, right_lane_idx_和 left_lane_idx_。
void LaneSelectNode::resetLaneIdx()
{
  current_lane_idx_ = -1;
  right_lane_idx_ = -1;
  left_lane_idx_ = -1;
  publishVisualizer();
}

//重置各种作为标志的变量。
void LaneSelectNode::resetSubscriptionFlag()
{
  is_current_pose_subscribed_ = false;
  is_current_velocity_subscribed_ = false;
  is_current_state_subscribed_ = false;
}


void LaneSelectNode::processing()
{
  if (!isAllTopicsSubscribed())
    return;

  // search closest waypoint number for each lanes
  if (!getClosestWaypointNumberForEachLanes())
  {
    publishClosestWaypoint(-1);
    publishVehicleLocation(-1, lane_array_id_);
    resetLaneIdx();
    return;
  }

  // if closest waypoint on current lane is -1,
  if (std::get<1>(tuple_vec_.at(current_lane_idx_)) == -1)
  {
    publishClosestWaypoint(-1);
    publishVehicleLocation(-1, lane_array_id_);
    resetLaneIdx();
    return;
  }

  findNeighborLanes();
  ROS_INFO("current_lane_idx: %d", current_lane_idx_);
  ROS_INFO("right_lane_idx: %d", right_lane_idx_);
  ROS_INFO("left_lane_idx: %d", left_lane_idx_);

  if (current_state_ == "LANE_CHANGE")
  {
    try
    {
      //依次更新current_lane_idx_, left_lane_idx_和right_lane_idx.changeLane();
      changeLane();

      //得到Lane std: :get<0>(lane_for_change_)中"距离最近轨迹点"下标，并更新std: : get<1>(lane_for_change_)
      std::get<1>(lane_for_change_) =
          getClosestWaypointNumber(std::get<0>(lane_for_change_), current_pose_.pose, current_velocity_.twist,
                                   std::get<1>(lane_for_change_), distance_threshold_, search_closest_waypoint_minimum_dt_);
      std::get<2>(lane_for_change_) = static_cast(
          std::get<0>(lane_for_change_).waypoints.at(std::get<1>(lane_for_change_)).change_flag);
      ROS_INFO("closest: %d", std::get<1>(lane_for_change_));

      //根据元组lane_for_change_中的信息发布消息到各个话题
      publishLane(std::get<0>(lane_for_change_));
      publishLaneID(std::get<0>(lane_for_change_));
      publishClosestWaypoint(std::get<1>(lane_for_change_));
      publishChangeFlag(std::get<2>(lane_for_change_));
      publishVehicleLocation(std::get<1>(lane_for_change_), lane_array_id_);
    }
    catch (std::out_of_range)
    {
      ROS_WARN("Failed to get closest waypoint num\n");
    }
  }
  else
  {
    //更新tuple_vec_中每个元组内的changeFlag数据
    updateChangeFlag();
    //更新lane_for_change_
    createLaneForChange();


    //根据元组 tuple_vec_.at(current_lane_idx_)中的信息发布消息到各个话题
    publishLane(std::get<0>(tuple_vec_.at(current_lane_idx_)));
    publishClosestWaypoint(std::get<1>(tuple_vec_.at(current_lane_idx_)));
    publishChangeFlag(std::get<2>(tuple_vec_.at(current_lane_idx_)));
    publishVehicleLocation(std::get<1>(tuple_vec_.at(current_lane_idx_)), lane_array_id_);
  }
  publishVisualizer();
  resetSubscriptionFlag();
}


//从下标 cl_wp开始查找传入函数的轨迹点序列wps中第一个change_flag 为ChangeFlag:right或ChangeFlag::left的轨迹点的下标。
int32_t LaneSelectNode::getClosestLaneChangeWaypointNumber(const std::vector &wps,
                                                           int32_t cl_wp)
{
  for (uint32_t i = cl_wp; i < wps.size(); i++)
  {
    if (static_cast(wps.at(i).change_flag) == ChangeFlag::right ||
        static_cast(wps.at(i).change_flag) == ChangeFlag::left)
    {
      return i;
    }
  }
  return -1;
}

/*
生成换道轨迹，并更新lane_for_change_，lane_for_change_的数据类型是std:tuple,可以发现lane_for_change_与tuple_vec_中元组的数据类型是一样的。
*/
void LaneSelectNode::createLaneForChange()
{
  std::get<0>(lane_for_change_).waypoints.clear();
  std::get<0>(lane_for_change_).waypoints.shrink_to_fit();
  std::get<1>(lane_for_change_) = -1;

/*
current_lane_idx_是”"全局距离最近点"所在的Lane(同时也是"全局距离最近点”所在的元组)
在tuple_vec_中的下标此处得到”全局距离最近点”所在的Lane
*/
  const autoware_msgs::Lane &cur_lane = std::get<0>(tuple_vec_.at(current_lane_idx_));

  //此处得到"全局距离最近点"在cur_lane的轨迹点序列中的下标
  const int32_t &clst_wp = std::get<1>(tuple_vec_.at(current_lane_idx_));

/*
从下标clst_wp开始查找轨迹点序列cur_lane.waypoints中第一个change_flag为 
ChangeFlag : :right或ChangeFlag : : left 的轨迹点的下标(代表从当前位置继续行车，
在当前Lane遇到的第一个可以换道的轨迹点)
*/
  int32_t num_lane_change = getClosestLaneChangeWaypointNumber(cur_lane.waypoints, clst_wp);
  ROS_INFO("num_lane_change: %d", num_lane_change);

  检查num_lane_change是否正确
  if (num_lane_change < 0 || num_lane_change >= static_cast(cur_lane.waypoints.size()))
  {
    ROS_WARN("current lane doesn't have change flag");
    return;
  }
/*
如果不存在"全局距离最近点”所在车体位姿 左侧/右侧 的所有"距离最近轨迹点”中有距离"全局距离最近点"
最近的轨迹点的情况,然而此时当前Lane却有换道 左侧/右侧 的轨迹点，则是不合理的
*/
  if ((static_cast(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right &&
       right_lane_idx_ < 0) ||
      (static_cast(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::left &&
       left_lane_idx_ < 0))
  {
    ROS_WARN("current lane doesn't have the lane for lane change");
    return;
  }

//"换道轨迹点”与"全局距离最近点"之间的距离(这两个轨迹点都在同一 Lane 上，即 cur_lane)
  double dt = getTwoDimensionalDistance(cur_lane.waypoints.at(num_lane_change).pose.pose.position,
                                        cur_lane.waypoints.at(clst_wp).pose.pose.position);

  //dt_by_vel最小值为lane_change_target_minimum_                                     
  double dt_by_vel = std::max(fabs(current_velocity_.twist.linear.x * lane_change_target_ratio_), lane_change_target_minimum_);
  ROS_INFO("dt : %lf, dt_by_vel : %lf", dt, dt_by_vel);

  //查找"换道轨迹点"对应的目标车道
  autoware_msgs::Lane &nghbr_lane =
      static_cast(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right ?
          std::get<0>(tuple_vec_.at(right_lane_idx_)) :
          std::get<0>(tuple_vec_.at(left_lane_idx_));

  //目标车道中的”距离最近轨迹点"在其车道轨迹点序列中的下标        
  const int32_t &nghbr_clst_wp =
      static_cast(cur_lane.waypoints.at(num_lane_change).change_flag) == ChangeFlag::right ?
          std::get<1>(tuple_vec_.at(right_lane_idx_)) :
          std::get<1>(tuple_vec_.at(left_lane_idx_));

//查找在目标车道内与其中的"距离最近轨迹点"的距离刚刚超过(dt + dt_by_vel)的轨迹点下标
  int32_t target_num = -1;
  for (uint32_t i = nghbr_clst_wp; i < nghbr_lane.waypoints.size(); i++)
  {
    if (i == nghbr_lane.waypoints.size() - 1 ||
        dt + dt_by_vel < getTwoDimensionalDistance(nghbr_lane.waypoints.at(nghbr_clst_wp).pose.pose.position,
                                                   nghbr_lane.waypoints.at(i).pose.pose.position))
    {
      target_num = i;
      break;
    }
  }

  ROS_INFO("target_num : %d", target_num);
  if (target_num < 0)
    return;
//更新lane_for_change_
  std::get<0>(lane_for_change_).header.stamp = nghbr_lane.header.stamp;

  /*
  hermite_wps是从换道起始点cur_lane.waypoints.at(num_lane_change).pose.pose到
  换道目标点nghbr_lane.waypoints.at(target_num).pose.pose 的具体行车轨迹
  */
  std::vector hermite_wps = generateHermiteCurveForROS(
      cur_lane.waypoints.at(num_lane_change).pose.pose, nghbr_lane.waypoints.at(target_num).pose.pose,
      cur_lane.waypoints.at(num_lane_change).twist.twist.linear.x, vlength_hermite_curve_);

  for (auto &&el : hermite_wps)
    el.change_flag = cur_lane.waypoints.at(num_lane_change).change_flag;

  std::get<0>(lane_for_change_).waypoints.reserve(nghbr_lane.waypoints.size() + hermite_wps.size());

  /*
  std: :move将范围[ hermite_wps.begin(，hermite_wps.end()内的数据移入std : : back_inserter
  (std: :get<0>(lane_for_change_).waypoints)std : : move将对象的状态/所有权从一个对象转移到另一个对象，
  只是转移，没有内存的搬迁/拷贝所以可以提高利用效率,改善性能
  */
  std::move(hermite_wps.begin(), hermite_wps.end(), std::back_inserter(std::get<0>(lane_for_change_).waypoints));
  auto itr = nghbr_lane.waypoints.begin();

  //令原本指向起始位置的迭代器itr指向下标为target_num的位置
  std::advance(itr, target_num);

  //换道后，目标点之后的一段距离内不应再次换道，因此修正后面轨迹点的change_flag为changeFlag : : straight
  for (auto i = itr; i != nghbr_lane.waypoints.end(); i++)
  {
    if (getTwoDimensionalDistance(itr->pose.pose.position, i->pose.pose.position) < lane_change_interval_)
      i->change_flag = enumToInteger(ChangeFlag::straight);
    else
      break;
  }

  //将换道后的目标点拷贝至lane_for_change_中
  std::copy(itr, nghbr_lane.waypoints.end(), std::back_inserter(std::get<0>(lane_for_change_).waypoints));
}


//updateChangeFlag 函数更新tuple_vec_中每个元组内的 ChangeFlag数据。

void LaneSelectNode::updateChangeFlag()
{
  for (auto &el : tuple_vec_)//一般情况下按照 tuple_vec_中各个元组中的“距离最近轨迹点”对应的change_flag 更新其所在元组的第三项。
  {
    std::get<2>(el) = (std::get<1>(el) != -1) ?
                          static_cast(std::get<0>(el).waypoints.at(std::get<1>(el)).change_flag) :
                          ChangeFlag::unknown;

//修正std: : get<2>(el)
    if (std::get<2>(el) == ChangeFlag::right && right_lane_idx_ == -1)
      std::get<2>(el) = ChangeFlag::unknown;
    else if (std::get<2>(el) == ChangeFlag::left && left_lane_idx_ == -1)
      std::get<2>(el) = ChangeFlag::unknown;

    ROS_INFO("change_flag: %d", enumToInteger(std::get<2>(el)));
  }
}

//changeLane函数依次更新current_lane_idx_, left_lane_idx_和right_lane_idx_。
void LaneSelectNode::changeLane()
{
  //根据换道的目标Lane 在tuple_vec_中的下标更新current_lane_idx_
  if (std::get<2>(tuple_vec_.at(current_lane_idx_)) == ChangeFlag::right && right_lane_idx_ != -1 &&
      std::get<1>(tuple_vec_.at(right_lane_idx_)) != -1)
  {
    current_lane_idx_ = right_lane_idx_;
  }
  else if (std::get<2>(tuple_vec_.at(current_lane_idx_)) == ChangeFlag::left && left_lane_idx_ != -1 &&
           std::get<1>(tuple_vec_.at(left_lane_idx_)) != -1)
  {
    current_lane_idx_ = left_lane_idx_;
  }

//更新left_lane_idx_和 right_1ane_idx_
  findNeighborLanes();
  return;
}

bool LaneSelectNode::getClosestWaypointNumberForEachLanes()
{
  //el为元组(车道，本车道"距离最近轨迹点"下标，change_flag)更新各条Lane内"距离最近轨迹点"下标
  for (auto &el : tuple_vec_)
  {
    //得到Lane std: : get<0>(el)中"距离最近轨迹点"下标并更新std: : get<1>(el)
    std::get<1>(el) = getClosestWaypointNumber(std::get<0>(el), current_pose_.pose, current_velocity_.twist,
                                               std::get<1>(el), distance_threshold_, search_closest_waypoint_minimum_dt_);
    ROS_INFO("closest: %d", std::get<1>(el));
  }

  // confirm if all closest waypoint numbers are -1. If so, output warning
  int32_t accum = 0;
  for (const auto &el : tuple_vec_)
  {
    accum += std::get<1>(el);
  }
  if (accum == (-1) * static_cast(tuple_vec_.size()))
  {
    ROS_WARN("Cannot get closest waypoints. All closest waypoints are changed to -1...");
    return false;
  }

  return true;
  /*
  tuple_vec_的数据类型是std::vector>，
  向量tuple_vec_中的元素是元组(车道，本车道“距离最近轨迹点”下标,change_flag)。 
  tuple_vec_在话题“traffic_waypoints_array”的回调函数callbackFrom-LaneArray中被更新。
  */
}


void LaneSelectNode::findCurrentLane()
{
  std::vector idx_vec;
  idx_vec.reserve(tuple_vec_.size());

  //遍历tuple_vec_，将具有“距离最近轨迹点”的tuple_vec_中的对应元组下标存入idx_vec
  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
  {
    if (std::get<1>(tuple_vec_.at(i)) == -1)
      continue;
    idx_vec.push_back(i);
  }
  current_lane_idx_ = findMostClosestLane(idx_vec, current_pose_.pose.position);
}


/*
findMostClosestLane函数进行分析可知,其主要作用是根据传入函数的下标列表idx_vec查找
tuple_vec_中对应下标的元组中距离传入函数的Point p最近的元组，返回其下标。
通俗而言，findMostClosestLane函数被findCurrentLane函数调用时的
作用是:返回“全局距离最近点”(在已经筛选过的各条Lane的“距离最近轨迹点”集合中进一步筛选)在 tuple_vec_中的下标，
“全局距离最近点”在 tuple_vec_中的下标也是其所在的Lane在 tuple_vec_中的下标，即寻找“全局距离最近点”所在的 Lanc。
*/
int32_t LaneSelectNode::findMostClosestLane(const std::vector idx_vec, const geometry_msgs::Point p)
{
  std::vector dist_vec;
  dist_vec.reserve(idx_vec.size());
  for (const auto &el : idx_vec)
  {
    int32_t closest_number = std::get<1>(tuple_vec_.at(el));

    /*
    tuple_vec_.at(el)指tuple_vec_内下标为el的元素，即元组(车道，本车道”距离最近轨迹点"下标，change_flag)
    std: : get<0>(tuple_vec_.at(el))对应的是下标为el的元组的第一项，即车道
    */
    dist_vec.push_back(
        getTwoDimensionalDistance(p, std::get<0>(tuple_vec_.at(el)).waypoints.at(closest_number).pose.pose.position));
  }
  std::vector::iterator itr = std::min_element(dist_vec.begin(), dist_vec.end());

//返回的是下标列表idx_vec对应的所有Lane中"距离最近轨迹点”集合中距离Point p最近的轨迹点所对应的元组在tuple_vec_的下标
  return idx_vec.at(std::distance(dist_vec.begin(), itr));
}


/*
分别在“全局距离最近点”所在车体位姿左侧和右侧的所有“距离最近轨迹点”中找到距离“全局距离最近点”
最近的轨迹点所在的元组下标，并对应更新left_lane_idx_和 right_lane_idx_。
*/
void LaneSelectNode::findNeighborLanes()
{
  int32_t current_closest_num = std::get<1>(tuple_vec_.at(current_lane_idx_));
  const geometry_msgs::Pose ¤t_closest_pose =
      std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.at(current_closest_num).pose.pose;

  std::vector left_lane_idx_vec;
  left_lane_idx_vec.reserve(tuple_vec_.size());
  std::vector right_lane_idx_vec;
  right_lane_idx_vec.reserve(tuple_vec_.size());
  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
  {
    if (i == static_cast(current_lane_idx_) || std::get<1>(tuple_vec_.at(i)) == -1)
      continue;

    int32_t target_num = std::get<1>(tuple_vec_.at(i));
    const geometry_msgs::Point &target_p = std::get<0>(tuple_vec_.at(i)).waypoints.at(target_num).pose.pose.position;

    geometry_msgs::Point converted_p = convertPointIntoRelativeCoordinate(target_p, current_closest_pose);

    ROS_INFO("distance: %lf", converted_p.y);
    if (fabs(converted_p.y) > distance_threshold_)
    {
      ROS_INFO("%d lane is far from current lane...", i);
      continue;
    }

    if (converted_p.y > 0)
      left_lane_idx_vec.push_back(i);
    else
      right_lane_idx_vec.push_back(i);
  }

  if (!left_lane_idx_vec.empty())
    left_lane_idx_ = findMostClosestLane(left_lane_idx_vec, current_closest_pose.position);
  else
    left_lane_idx_ = -1;

  if (!right_lane_idx_vec.empty())
    right_lane_idx_ = findMostClosestLane(right_lane_idx_vec, current_closest_pose.position);
  else
    right_lane_idx_ = -1;
}
visualization_msgs::Marker LaneSelectNode::createCurrentLaneMarker()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "current_lane_marker";

  if (current_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
  {
    marker.action = visualization_msgs::Marker::DELETE;
    return marker;
  }

  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.05;

  std_msgs::ColorRGBA color_current;
  color_current.b = 1.0;
  color_current.g = 0.7;
  color_current.a = 1.0;
  marker.color = color_current;

  for (const auto &em : std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints)
    marker.points.push_back(em.pose.pose.position);

  return marker;
}

visualization_msgs::Marker LaneSelectNode::createRightLaneMarker()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "right_lane_marker";

  if (right_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
  {
    marker.action = visualization_msgs::Marker::DELETE;
    return marker;
  }

  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.05;

  std_msgs::ColorRGBA color_neighbor;
  color_neighbor.r = 0.5;
  color_neighbor.b = 0.5;
  color_neighbor.g = 0.5;
  color_neighbor.a = 1.0;

  std_msgs::ColorRGBA color_neighbor_change;
  color_neighbor_change.b = 0.7;
  color_neighbor_change.g = 1.0;
  color_neighbor_change.a = 1.0;

  const ChangeFlag &change_flag = std::get<2>(tuple_vec_.at(current_lane_idx_));
  marker.color = change_flag == ChangeFlag::right ? color_neighbor_change : color_neighbor;

  for (const auto &em : std::get<0>(tuple_vec_.at(right_lane_idx_)).waypoints)
    marker.points.push_back(em.pose.pose.position);

  return marker;
}

visualization_msgs::Marker LaneSelectNode::createLeftLaneMarker()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "left_lane_marker";

  if (left_lane_idx_ == -1 || std::get<0>(tuple_vec_.at(current_lane_idx_)).waypoints.empty())
  {
    marker.action = visualization_msgs::Marker::DELETE;
    return marker;
  }

  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.05;

  std_msgs::ColorRGBA color_neighbor;
  color_neighbor.r = 0.5;
  color_neighbor.b = 0.5;
  color_neighbor.g = 0.5;
  color_neighbor.a = 1.0;

  std_msgs::ColorRGBA color_neighbor_change;
  color_neighbor_change.b = 0.7;
  color_neighbor_change.g = 1.0;
  color_neighbor_change.a = 1.0;

  const ChangeFlag &change_flag = std::get<2>(tuple_vec_.at(current_lane_idx_));
  marker.color = change_flag == ChangeFlag::left ? color_neighbor_change : color_neighbor;

  for (const auto &em : std::get<0>(tuple_vec_.at((left_lane_idx_))).waypoints)
    marker.points.push_back(em.pose.pose.position);

  return marker;
}

visualization_msgs::Marker LaneSelectNode::createChangeLaneMarker()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "change_lane_marker";

  if (std::get<0>(lane_for_change_).waypoints.empty())
  {
    marker.action = visualization_msgs::Marker::DELETE;
    return marker;
  }

  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.05;

  std_msgs::ColorRGBA color;
  color.r = 1.0;
  color.a = 1.0;

  std_msgs::ColorRGBA color_current;
  color_current.b = 1.0;
  color_current.g = 0.7;
  color_current.a = 1.0;

  marker.color = current_state_ == "LANE_CHANGE" ? color_current : color;
  for (const auto &em : std::get<0>(lane_for_change_).waypoints)
    marker.points.push_back(em.pose.pose.position);

  return marker;
}

visualization_msgs::Marker LaneSelectNode::createClosestWaypointsMarker()
{
  visualization_msgs::Marker marker;
  std_msgs::ColorRGBA color_closest_wp;
  color_closest_wp.r = 1.0;
  color_closest_wp.b = 1.0;
  color_closest_wp.g = 1.0;
  color_closest_wp.a = 1.0;

  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "closest_waypoints_marker";
  marker.type = visualization_msgs::Marker::POINTS;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.5;
  marker.color = color_closest_wp;

  marker.points.reserve(tuple_vec_.size());
  for (uint32_t i = 0; i < tuple_vec_.size(); i++)
  {
    if (std::get<1>(tuple_vec_.at(i)) == -1)
      continue;

    marker.points.push_back(
        std::get<0>(tuple_vec_.at(i)).waypoints.at(std::get<1>(tuple_vec_.at(i))).pose.pose.position);
  }

  return marker;
}

void LaneSelectNode::publishVisualizer()
{
  /*
  createChangeLaneMarker ，
  createCurrentLaneMarker，
  createRightLaneMarker ,
  createLeftLaneMarker和 
  createClosestWaypointsMarker函数的作用都是类似的，
  即以某种Marker(形状/颜色不同)在rviz中标记出不同含义的轨迹点,具体的实现流程也是高度类似的。
  createChangeLaneMarker函数以线条(点的连线)的形式标记出 lane_for_change_中的轨迹点。
  有关`visualization_msgs::Marker的知识点在前面分析节点waypoint_marker_publisher 的时候已经介绍了，在此不再赘述。
  */
  visualization_msgs::MarkerArray marker_array;
  marker_array.markers.push_back(createChangeLaneMarker());
  marker_array.markers.push_back(createCurrentLaneMarker());
  marker_array.markers.push_back(createRightLaneMarker());
  marker_array.markers.push_back(createLeftLaneMarker());
  marker_array.markers.push_back(createClosestWaypointsMarker());

  vis_pub1_.publish(marker_array);
}

//发布传入函数的消息到对应的话题。
void LaneSelectNode::publishLane(const autoware_msgs::Lane &lane)
{
  // publish global lane
  pub1_.publish(lane);
}

void LaneSelectNode::publishLaneID(const autoware_msgs::Lane &lane)
{
  std_msgs::Int32 msg;
  msg.data = lane.lane_id;
  pub4_.publish(msg);
}

//向话题"closest_waypoint"发布距离当前位置current_pose_最近的轨迹点的下标
void LaneSelectNode::publishClosestWaypoint(const int32_t clst_wp)
{
  // publish closest waypoint
  std_msgs::Int32 closest_waypoint;
  closest_waypoint.data = clst_wp;
  pub2_.publish(closest_waypoint);
}

//发布传入函数的消息到对应的话题。
void LaneSelectNode::publishChangeFlag(const ChangeFlag flag)
{
  std_msgs::Int32 change_flag;
  change_flag.data = enumToInteger(flag);
  pub3_.publish(change_flag);
}

//向话题"vehicle_location"发布消息
void LaneSelectNode::publishVehicleLocation(const int32_t clst_wp, const int32_t larray_id)
{
  autoware_msgs::VehicleLocation vehicle_location;
  vehicle_location.header.stamp = ros::Time::now();
  vehicle_location.waypoint_index = clst_wp;
  vehicle_location.lane_array_id = larray_id;
  pub5_.publish(vehicle_location);
}

void LaneSelectNode::callbackFromLaneArray(const autoware_msgs::LaneArrayConstPtr &msg)
{
  tuple_vec_.clear();
  tuple_vec_.shrink_to_fit();
  tuple_vec_.reserve(msg->lanes.size());
  for (const auto &el : msg->lanes)
  {
    auto t = std::make_tuple(el, -1, ChangeFlag::unknown);
    tuple_vec_.push_back(t);
  }

  lane_array_id_ = msg->id;
  current_lane_idx_ = -1;
  right_lane_idx_ = -1;
  left_lane_idx_ = -1;
  is_lane_array_subscribed_ = true;

  if (current_lane_idx_ == -1)
    initForLaneSelect();
  else
    processing();
}

void LaneSelectNode::callbackFromPoseStamped(const geometry_msgs::PoseStampedConstPtr &msg)
{
  current_pose_ = *msg;
  is_current_pose_subscribed_ = true;

  if (current_lane_idx_ == -1)
    initForLaneSelect();
  else
    processing();
}

void LaneSelectNode::callbackFromTwistStamped(const geometry_msgs::TwistStampedConstPtr &msg)
{
  current_velocity_ = *msg;
  is_current_velocity_subscribed_ = true;

  if (current_lane_idx_ == -1)
    initForLaneSelect();
  else
    processing();
}

void LaneSelectNode::callbackFromState(const std_msgs::StringConstPtr &msg)
{
  current_state_ = msg->data;
  is_current_state_subscribed_ = true;

  if (current_lane_idx_ == -1)
    initForLaneSelect();
  else
    processing();
}
void LaneSelectNode::callbackFromDecisionMakerState(const std_msgs::StringConstPtr &msg)
{
  is_current_state_subscribed_ = true;

  if (msg->data.find("ChangeTo") != std::string::npos)
  {
    current_state_ = std::string("LANE_CHANGE");
    ;
  }
  else
  {
    current_state_ = msg->data;
  }

  if (current_lane_idx_ == -1)
    initForLaneSelect();
  else
    processing();
}


//callbackFromConfig函数是话题"/config/lane_select"的回调函数，对节点lane_select初始化。
void LaneSelectNode::callbackFromConfig(const autoware_config_msgs::ConfigLaneSelectConstPtr &msg)
{
  //配置参数
  distance_threshold_ = msg->distance_threshold_neighbor_lanes;
  lane_change_interval_ = msg->lane_change_interval;
  lane_change_target_ratio_ = msg->lane_change_target_ratio;
  lane_change_target_minimum_ = msg->lane_change_target_minimum;
  vlength_hermite_curve_ = msg->vector_length_hermite_curve;
  is_config_subscribed_ = true;

  if (current_lane_idx_ == -1)//current_lane_idx_在构造函数中被赋默认值-1,因此一开始会执行initForLaneselect函数
    initForLaneSelect();
  else
    processing();
}

void LaneSelectNode::run()
{
  ros::spin();
}

// distance between target 1 and target2 in 2-D
//getTwoDimensionalDistance函数表示得到两个Point targetl和 target2之间的平面距离。
double getTwoDimensionalDistance(const geometry_msgs::Point &target1, const geometry_msgs::Point &target2)
{
  double distance = sqrt(pow(target1.x - target2.x, 2) + pow(target1.y - target2.y, 2));
  return distance;
}

geometry_msgs::Point convertPointIntoRelativeCoordinate(const geometry_msgs::Point &input_point,
                                                        const geometry_msgs::Pose &pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(pose, inverse);
  tf::Transform transform = inverse.inverse();

  tf::Point p;
  pointMsgToTF(input_point, 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 convertPointIntoWorldCoordinate(const geometry_msgs::Point &input_point,
                                                     const geometry_msgs::Pose &pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(pose, inverse);

  tf::Point p;
  pointMsgToTF(input_point, p);
  tf::Point tf_p = inverse * p;

  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);
  return tf_point_msg;
}

//getRelativeAngle函数得到传入函数的两个坐标系之间的夹角。
double getRelativeAngle(const geometry_msgs::Pose &waypoint_pose, const geometry_msgs::Pose ¤t_pose)
{
  //tf: : vector3三维向量
  tf::Vector3 x_axis(1, 0, 0);
  tf::Transform waypoint_tfpose;
  tf::poseMsgToTF(waypoint_pose, waypoint_tfpose);

  //getBasis()用于获取旋转矩阵m_basis，其用3×3的矩阵表示旋转
  tf::Vector3 waypoint_v = waypoint_tfpose.getBasis() * x_axis;
  tf::Transform current_tfpose;
  tf::poseMsgToTF(current_pose, current_tfpose);
  tf::Vector3 current_v = current_tfpose.getBasis() * x_axis;

//angle()返回向量current_v与向量waypoint_v之间的角度
  return current_v.angle(waypoint_v) * 180 / M_PI;
}

// get closest waypoint from current pose

/*
对上面函数中的 getClosestWaypointNumber函数进行分析，可知其主要作用是基于当前自动驾驶车辆的
位置姿态获取当前车道上“距离最近轨迹点”，并返回其在当前Lane的轨迹点序列current_lane.waypoints中的下标。
*/
int32_t getClosestWaypointNumber(const autoware_msgs::Lane ¤t_lane, const geometry_msgs::Pose ¤t_pose,
                                 const geometry_msgs::Twist ¤t_velocity, const int32_t previous_number,
                                 const double distance_threshold, const int search_closest_waypoint_minimum_dt)
{
  if (current_lane.waypoints.size() < 2)
    return -1;

  std::vector idx_vec;
  // if previous number is -1, search closest waypoint from waypoints in front of current pose
  uint32_t range_min = 0;
  uint32_t range_max = current_lane.waypoints.size();

  //如果先前的轨迹点编号为-1，则从当前车道的轨迹点集合current_lane.waypoints中搜索当前车辆位姿前面最近的轨迹点
  if (previous_number == -1)
  {
    idx_vec.reserve(current_lane.waypoints.size());
  }
  else
  {
    if (distance_threshold <
        getTwoDimensionalDistance(current_lane.waypoints.at(previous_number).pose.pose.position, current_pose.position))
    {
      ROS_WARN("Current_pose is far away from previous closest waypoint. Initilized...");
      return -1;
    }
    range_min = static_cast(previous_number);
    double ratio = 3;
    double dt = std::max(current_velocity.linear.x * ratio, static_cast(search_closest_waypoint_minimum_dt));
    if (static_cast(previous_number + dt) < current_lane.waypoints.size())
    {
      range_max = static_cast(previous_number + dt);
    }
  }
  const LaneDirection dir = getLaneDirection(current_lane);
  const int sgn = (dir == LaneDirection::Forward) ? 1 : (dir == LaneDirection::Backward) ? -1 : 0;
  for (uint32_t i = range_min; i < range_max; i++)
  {
    /*
    convertPointIntoRelativeCoordinate函数计算得到传入函数的current_lane.waypoints.at(i).pose.pose.position
    (全局坐标系坐标)相对于current _pose坐标系的位置
    */   
    //注：convertPointIntoRelativeCoordinate函数的代码与节点waypointreplanner 中的calcRelativeCoordinate函数是一样的，用于计算全局坐标系中某点相对于机器人坐标系中的位置。
    geometry_msgs::Point converted_p =
      convertPointIntoRelativeCoordinate(current_lane.waypoints.at(i).pose.pose.position, current_pose);

    //getRelativeAngle函数得到传入函数的两个坐标系之间的夹角
    double angle = getRelativeAngle(current_lane.waypoints.at(i).pose.pose, current_pose);

    /*
    如果遍历到的轨迹点:
      ①在自动驾驶车前进方向上;
      ②所在的坐标系跟当前位置对应的坐标系夹角小于90度满足以上两点，也称作遍历到的轨迹点在期望行车轨迹上
    */
    if (converted_p.x * sgn > 0 && angle < 90)
    {
      idx_vec.push_back(i);
    }
  }

  if (idx_vec.empty())
    return -1;

  std::vector dist_vec;
  dist_vec.reserve(idx_vec.size());
  for (const auto &el : idx_vec)
  {
    double dt = getTwoDimensionalDistance(current_pose.position, current_lane.waypoints.at(el).pose.pose.position);
    dist_vec.push_back(dt);
  }
  std::vector::iterator itr = std::min_element(dist_vec.begin(), dist_vec.end());
  int32_t found_number = idx_vec.at(static_cast(std::distance(dist_vec.begin(), itr)));
  return found_number;
}

// let the linear equation be "ax + by + c = 0"
// if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double *a, double *b, double *c)
{
  //(x1, y1) = (start.x, star.y), (x2, y2) = (end.x, end.y)
  double sub_x = fabs(start.x - end.x);
  double sub_y = fabs(start.y - end.y);
  double error = pow(10, -5);  // 0.00001

  if (sub_x < error && sub_y < error)
  {
    ROS_INFO("two points are the same point!!");
    return false;
  }

  *a = end.y - start.y;
  *b = (-1) * (end.x - start.x);
  *c = (-1) * (end.y - start.y) * start.x + (end.x - start.x) * start.y;

  return true;
}
double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double a, double b, double c)
{
  double d = fabs(a * point.x + b * point.y + c) / sqrt(pow(a, 2) + pow(b, 2));

  return d;
}

}  // lane_planner

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

注：可以下载Visual Studio Coded代码编辑软件，展示性好点。