Auto Ware 代码解析系列 lane_select节点

/// get closest waypoint from current poseint32_t getClosestWaypointNumber(const waypoint_follower::lane &current_lane, const geometry_msgs::Pose &current_pose,                                 const geometry_msgs::Twist &current_velocity, const int32_t previous_number,                                 const double distance_threshold){  if (current_lane.waypoints.empty())    return -1;  std::vector<uint32_t> idx_vec;  // if previous number is -1, search closest waypoint from waypoints in front of current pose  if (previous_number == -1)  {    idx_vec.reserve(current_lane.waypoints.size());    for (uint32_t i = 0; i < current_lane.waypoints.size(); i++)    {      geometry_msgs::Point converted_p =          convertPointIntoRelativeCoordinate(current_lane.waypoints.at(i).pose.pose.position, current_pose);      double angle = getRelativeAngle(current_lane.waypoints.at(i).pose.pose, current_pose);      if (converted_p.x > 0 && angle < 90)        idx_vec.push_back(i);    }  }  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;    }    double ratio = 3;    double minimum_dt = 2.0;    double dt = current_velocity.linear.x * ratio > minimum_dt ? current_velocity.linear.x * ratio : minimum_dt;    auto range_max = static_cast<uint32_t>(previous_number + dt) < current_lane.waypoints.size()                         ? static_cast<uint32_t>(previous_number + dt)                         : current_lane.waypoints.size();    for (uint32_t i = static_cast<uint32_t>(previous_number); i < range_max; i++)    {      geometry_msgs::Point converted_p =          convertPointIntoRelativeCoordinate(current_lane.waypoints.at(i).pose.pose.position, current_pose);      double angle = getRelativeAngle(current_lane.waypoints.at(i).pose.pose, current_pose);      if (converted_p.x > 0 && angle < 90)        idx_vec.push_back(i);    }  }  if (idx_vec.empty())    return -1;  std::vector<double> 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<double>::iterator itr = std::min_element(dist_vec.begin(), dist_vec.end());  int32_t found_number = idx_vec.at(static_cast<uint32_t>(std::distance(dist_vec.begin(), itr)));  return found_number;}