Method feedWaypoints

src/fiss_planner_node.cpp:478–571

Feed map waypoints into local map

// Feed map waypoints into local map
bool FissPlannerNode::feedWaypoints()
{
  if (lane_.points.empty())
  {
    ROS_WARN("Local Planner: Waiting for Lane Points");
    return false;
  }
  else if (lane_.points.size() < 5)
  {
    ROS_WARN("Local Planner: Global Path has less than 5 points, unable to plan");
    return false;
  }

  int start_id = lastWaypoint(current_state_, lane_);

  // if reached the end of the lane, stop
  if (start_id >= lane_.points.size() - 2)  // exclude last 2 waypoints for safety, and prevent code crashing
  {
    // ROS_WARN("Local Planner: Vehicle is at waypoint no.%d, with %d waypoints in total", start_id, int(lane_.points.size()));
    ROS_WARN("Local Planner: Vehicle has reached the destination");
    return false;
  }

  const double dist = distance(lane_.points[start_id].point.x, lane_.points[start_id].point.y, current_state_.x, current_state_.y);
  const double heading_diff = unifyAngleRange(current_state_.yaw - lane_.points[start_id].point.yaw);

  if (dist > MAX_DIST_FROM_PATH)
  {
    ROS_WARN("Local Planner: Vehicle's Location Is Too Far From The Target Lane");
    return false;
  }
  else if (std::abs(heading_diff) > HEADING_DIFF_THRESH)
  {
    ROS_WARN("Local Planner: Vehicle's Is Heading In A Different Direction");
    return false;
  }

  // clear the old waypoints
  local_lane_.clear(); 

  // Make sure there are at least 5 points in the remaining section of the global reference path
  if (start_id > lane_.points.size() - 5)
  {
    start_id = lane_.points.size() - 5;
  }

  // Check if the global waypoints need to be filtered
  const double ref_spline_length = SETTINGS.highest_speed*(SETTINGS.max_t);
  if ((lane_.points.back().point.s - lane_.points[start_id].point.s) >= ref_spline_length)
  {
    // Filter the waypoints to a uniform density
    double s_current = lane_.points[start_id].point.s;
    local_lane_.points.push_back(lane_.points[start_id]);
    for (size_t i = start_id + 1; i < lane_.points.size(); i++)
    {
      if (local_lane_.points.size() >= 5)
      {
        break;