Auto Ware 代码解析系列 lane_select节点

/// get closest waypoint from current pose
int32_t getClosestWaypointNumber(const waypoint_follower::lane ¤t_lane, const geometry_msgs::Pose ¤t_pose,
                                 const geometry_msgs::Twist ¤t_velocity, const int32_t previous_number,
                                 const double distance_threshold)
{
  if (current_lane.waypoints.empty())
    return -1;

  std::vector 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(previous_number + dt) < current_lane.waypoints.size()
                         ? static_cast(previous_number + dt)
                         : current_lane.waypoints.size();
    for (uint32_t i = static_cast(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 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;
}