path_calculation.cpp

Go to the documentation of this file.

#include "planning/path_calculation.hpp"
 
#include <algorithm>
#include <cmath>
#include <map>
#include <queue>
#include <set>
#include <utility>
#include <vector>
 
// ===================== Path Calculation (Core) =====================
 
std::vector<PathPoint> PathCalculation::calculate_path(const std::vector<Cone>& cone_array) {
  if (cone_array.size() < 4) {
    RCLCPP_ERROR(rclcpp::get_logger("planning"), "Not enough cones to create a path.");
    return {};
  }
 
  clear_path_state();
 
  // Determine if we should regenerate all midpoints (path reset)
  bool should_reset = config_.use_reset_path_ && reset_path_counter_ >= config_.close_cost_;
  bool rebuild_all_midpoints = should_reset || !config_.use_sliding_window_;
 
  // Generate midpoints using the generator
  midpoints_ = midpoint_generator_.generate_midpoints(cone_array, rebuild_all_midpoints);
 
  visited_midpoints_.reserve(midpoints_.size());
  // Map for quick access from Point to corresponding Midpoint
  point_to_midpoint_.reserve(midpoints_.size());
  for (const auto& mp : midpoints_) {
    point_to_midpoint_[mp->point] = mp;
  }
 
  Point car_point(vehicle_pose_.position.x, vehicle_pose_.position.y);
 
  // Find the point in the past_path_ closest to the car
  int cutoff_index = -1;
  double min_dist = std::numeric_limits<double>::max();
  for (size_t i = 0; i < past_path_.size(); ++i) {
    double dist = CGAL::squared_distance(past_path_[i].point, car_point);
    if (dist < min_dist) {
      min_dist = dist;
      cutoff_index = static_cast<int>(i);
    }
  }
 
  if (cutoff_index == -1) {
    RCLCPP_WARN(rclcpp::get_logger("planning"), "No valid path points found near the car.");
  }
 
  path_to_car_.reserve(past_path_.size());
  // Retain part of the existing path leading to the car
  if (cutoff_index != -1 && cutoff_index > config_.lookback_points_) {
    (void)path_to_car_.insert(path_to_car_.end(), past_path_.begin(),
                              past_path_.begin() + cutoff_index - config_.lookback_points_);
  }
 
  int max_points = reset_path(should_reset);
 
  // Build initial path segment
  if (path_to_car_.size() <= 2) {
    initialize_path_from_initial_pose();
  } else {
    update_path_from_past_path();
  }
 
  extend_path(max_points);
 
  yellow_cones_.reserve(current_path_.size());
  blue_cones_.reserve(current_path_.size());
 
  Colorpoint::extract_cones(current_path_, yellow_cones_, blue_cones_);
 
  past_path_ = current_path_;  // Update the path for next iteration
 
  return get_path_points_from_colorpoints(current_path_);
}
 
bool PathCalculation::is_map_closed(std::vector<PathPoint>& path) const {
  if (path.size() < 3) {
    return false;
  }
 
  auto [best_cutoff_index, combined_cost] = find_best_loop_closure(path);
 
  if (combined_cost < config_.close_cost_) {
    if (best_cutoff_index + 1 < static_cast<int>(path.size())) {
      (void)path.erase(path.begin() + best_cutoff_index + 1, path.end());
    }
    path.push_back(path[0]);
 
    RCLCPP_DEBUG(rclcpp::get_logger("planning"), "Loop closure cost: %.4f (threshold: %.4f)",
                combined_cost, config_.close_cost_);
    return true;
  }
 
  return false;
}
 
std::vector<PathPoint> PathCalculation::calculate_trackdrive(const std::vector<Cone>& cone_array) {
  config_.use_sliding_window_ = false;
  std::vector<PathPoint> result = calculate_path(cone_array);
 
  // Check if we have enough points to form a loop
  if (result.size() < 3) {
    RCLCPP_WARN(rclcpp::get_logger("planning"), "Not enough points to create trackdrive loop");
    return result;
  }
 
  // Find the best point to close the loop
  auto [best_cutoff_index, _] = find_best_loop_closure(result);
 
  if (result.begin() + best_cutoff_index + 1 != result.end()) {
    // Trim the path to the best cutoff point
    (void)result.erase(result.begin() + best_cutoff_index + 1, result.end());
    // If the path change we need to estimate the boundaries again
    RCLCPP_WARN(rclcpp::get_logger("planning"), "The path change to create trackdrive loop");
    yellow_cones_.clear();
    blue_cones_.clear();
    (void)current_path_.erase(current_path_.begin() + best_cutoff_index + 1, current_path_.end());
    Colorpoint::extract_cones(current_path_, yellow_cones_, blue_cones_);
  }
 
  // Close the loop by adding the first point again
  result.push_back(result[0]);
  yellow_cones_.push_back(yellow_cones_[0]);
  blue_cones_.push_back(blue_cones_[0]);
 
  return result;
}
 
// ===================== State Management =====================
 
void PathCalculation::clear_path_state() {
  current_path_.clear();
  path_to_car_.clear();
  point_to_midpoint_.clear();
  visited_midpoints_.clear();
  discarded_cones_.clear();
  yellow_cones_.clear();
  blue_cones_.clear();
}
 
int PathCalculation::reset_path(bool should_reset) {
  int max_points = config_.max_points_;
  reset_path_counter_++;
 
  if (should_reset) {
    max_points = static_cast<int>(path_to_car_.size()) + config_.max_points_;
    path_to_car_.clear();
    past_path_.clear();
    reset_path_counter_ = 0;
    RCLCPP_INFO(rclcpp::get_logger("planning"), "Global path reset");
  }
 
  return max_points;
}
 
void PathCalculation::set_vehicle_pose(const common_lib::structures::Pose& vehicle_pose) {
  midpoint_generator_.set_vehicle_pose(vehicle_pose);
  vehicle_pose_ = vehicle_pose;
}
 
// ===================== Path Initialization =====================
 
void PathCalculation::initialize_path_from_initial_pose() {
  if (!initial_pose_set_) {
    initial_pose_ = vehicle_pose_;
    initial_pose_set_ = true;
  }
 
  const auto [first_midpoint, second_midpoint] = select_starting_midpoints();
 
  if (first_midpoint && second_midpoint) {
    Colorpoint first_colorpoint =
        Colorpoint(first_midpoint->point, *first_midpoint->cone1, *first_midpoint->cone2);
    Colorpoint second_colorpoint =
        Colorpoint(second_midpoint->point, *second_midpoint->cone1, *second_midpoint->cone2);
    current_path_.push_back(first_colorpoint);
    current_path_.push_back(second_colorpoint);
    (void)visited_midpoints_.insert(first_midpoint);
    (void)visited_midpoints_.insert(second_midpoint);
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("planning"), "Failed to find valid starting points.");
  }
}
 
void PathCalculation::update_path_from_past_path() {
  RCLCPP_DEBUG(rclcpp::get_logger("planning"), "Selecting initial path from %zu points.",
               path_to_car_.size());
 
  current_path_.reserve(path_to_car_.size());
 
  Point last_added_point;
  bool first_point_added = false;
 
  for (const Colorpoint& path_to_car_colorpoint : path_to_car_) {
    // By default, use the original point
    Colorpoint candidate_colorpoint = path_to_car_colorpoint;
 
    // Snap to nearest valid midpoint
    std::shared_ptr<Midpoint> candidate_midpoint =
        find_nearest_midpoint(path_to_car_colorpoint.point);
 
    // If found a valid midpoint, use it
    if (candidate_midpoint) {
      candidate_colorpoint = Colorpoint(candidate_midpoint->point, *candidate_midpoint->cone1,
                                        *candidate_midpoint->cone2);
    }
 
    // Check if already visited
    if (candidate_midpoint && visited_midpoints_.count(candidate_midpoint) > 0) {
      RCLCPP_DEBUG(rclcpp::get_logger("planning"), "Skipping point: Already visited.");
      continue;
    }
 
    // For non-first points, check distance from last added point
    if (first_point_added) {
      double distance =
          std::sqrt(CGAL::squared_distance(last_added_point, candidate_colorpoint.point));
      if (distance <= config_.minimum_point_distance_) {
        RCLCPP_DEBUG(rclcpp::get_logger("planning"),
                     "Skipping point: Too close to last added point.");
        continue;
      }
    }
 
    // If the candidate_midpoint is valid, add it to visited set
    if (candidate_midpoint) {
      (void)visited_midpoints_.insert(candidate_midpoint);
    }
    
    current_path_.push_back(candidate_colorpoint);
    last_added_point = candidate_colorpoint.point;
    first_point_added = true;
 
  }
}
 
// ===================== Path Extension =====================
 
void PathCalculation::extend_path(int max_points) {
  int n_points = 0;
  // Define cost threshold for discarding poor path options
  const double worst_cost = config_.max_cost_ * config_.search_depth_;
  // Reserve space for expected path growth
  current_path_.reserve(current_path_.size() + max_points);
 
  while (true) {
    if (current_path_.size() < 2) {
      break;
    }
 
    const Colorpoint& prev = current_path_[current_path_.size() - 2];
    const Colorpoint& last = current_path_.back();
 
    std::shared_ptr<Midpoint> prev_mp = find_nearest_midpoint(prev.point);
    std::shared_ptr<Midpoint> last_mp = find_nearest_midpoint(last.point);
 
    // Abort if midpoints can't be matched
    if (!prev_mp || !last_mp) {
      RCLCPP_WARN(rclcpp::get_logger("planning"), "No valid midpoints found for path extension.");
      break;
    }
 
    // Search for the best continuation from the current midpoint
    auto [best_cost, best_midpoint] =
        find_best_next_midpoint(config_.search_depth_, prev_mp, last_mp, config_.max_cost_);
 
    // Stop if no good extension is found or point was already visited
    if (best_cost > worst_cost || !best_midpoint || visited_midpoints_.count(best_midpoint) > 0) {
      break;
    }
 
    current_path_.push_back(
        Colorpoint(best_midpoint->point, *best_midpoint->cone1, *best_midpoint->cone2));
    (void)visited_midpoints_.insert(best_midpoint);
    n_points++;
 
    if (n_points >= max_points) {
      break;
    }
 
    // Update midpoints validity and discard cones if needed
    if (current_path_.size() > 2) {
      discard_cones_along_path();
    }
  }
}
 
// ================================ Path Search ================================
 
std::pair<std::shared_ptr<Midpoint>, std::shared_ptr<Midpoint>>
PathCalculation::select_starting_midpoints() {
  std::pair<std::shared_ptr<Midpoint>, std::shared_ptr<Midpoint>> result{nullptr, nullptr};
 
  Midpoint anchor_pose_midpoint{Point(initial_pose_.position.x, initial_pose_.position.y), nullptr,
                                nullptr};
 
  // Get candidate midpoints
  anchor_pose_midpoint.close_points = select_candidate_midpoints(anchor_pose_midpoint, 8);
 
  double best_cost = std::numeric_limits<double>::max();
 
  for (const auto& first : anchor_pose_midpoint.close_points) {
    std::shared_ptr<Midpoint> anchor_mp = std::make_shared<Midpoint>(anchor_pose_midpoint);
 
    auto [cost, midpoint] = find_best_next_midpoint(config_.search_depth_, anchor_mp, first,
                                                    std::numeric_limits<double>::max());
 
    double dx = first->point.x() - anchor_pose_midpoint.point.x();
    double dy = first->point.y() - anchor_pose_midpoint.point.y();
    double distance = std::sqrt(dx * dx + dy * dy);
 
    // Calculate the deviation angle relative to the car’s heading
    double angle_to_point = std::atan2(dy, dx);
    double angle_deviation =
        std::abs(std::atan2(std::sin(angle_to_point - initial_pose_.orientation),
                            std::cos(angle_to_point - initial_pose_.orientation)));
 
    double initial_cost = std::pow(distance, config_.distance_exponent_) * config_.distance_gain_ +
                          std::pow(angle_deviation, config_.angle_exponent_) * config_.angle_gain_;
 
    cost += initial_cost;
    cost += initial_cost;
 
    if (cost < best_cost) {
      result.first = first;
      result.second = midpoint;
      best_cost = cost;
    }
  }
 
  return result;
}
 
std::vector<std::shared_ptr<Midpoint>> PathCalculation::select_candidate_midpoints(
    const Midpoint& anchor_pose, int num_candidates) const {
  auto cmp = [](const std::pair<double, std::shared_ptr<Midpoint>>& cost1,
                const std::pair<double, std::shared_ptr<Midpoint>>& cost2) {
    return cost1.first > cost2.first;
  };
 
  std::priority_queue<std::pair<double, std::shared_ptr<Midpoint>>,
                      std::vector<std::pair<double, std::shared_ptr<Midpoint>>>, decltype(cmp)>
      pq(cmp);
 
  double car_direction_x = std::cos(initial_pose_.orientation);
  double car_direction_y = std::sin(initial_pose_.orientation);
 
  // Find midpoints that are in front of the car
  for (const auto& mp : midpoints_) {
    double dx = mp->point.x() - anchor_pose.point.x();
    double dy = mp->point.y() - anchor_pose.point.y();
 
    // Skip points behind the car
    if ((dx * car_direction_x + dy * car_direction_y) <= 0.0) {
      continue;
    }
 
    double dist = std::sqrt(dx * dx + dy * dy);
 
    // Calculate the deviation angle relative to the car’s heading
    double angle_to_point = std::atan2(dy, dx);
    double angle_deviation =
        std::abs(std::atan2(std::sin(angle_to_point - initial_pose_.orientation),
                            std::cos(angle_to_point - initial_pose_.orientation)));
 
    double cost = std::pow(angle_deviation, config_.angle_exponent_) * config_.angle_gain_ +
                  std::pow(dist, config_.distance_exponent_) * config_.distance_gain_;
 
    pq.push({cost, mp});
  }
 
  // Get the closest midpoints in front of the car
  std::vector<std::shared_ptr<Midpoint>> candidate_points;
  int count = 0;
  while (count < num_candidates && !pq.empty()) {
    candidate_points.push_back(pq.top().second);
    pq.pop();
    count++;
  }
 
  return candidate_points;
}
 
std::pair<double, std::shared_ptr<Midpoint>> PathCalculation::find_best_next_midpoint(
    int depth, const std::shared_ptr<Midpoint>& previous, const std::shared_ptr<Midpoint>& current,
    double max_cost) const {
  if (depth == 0) {
    return {0.0, current};
  }
 
  double min_cost = config_.max_cost_ * config_.search_depth_;
  std::shared_ptr<Midpoint> min_point = current;
 
  for (const auto& next : current->close_points) {
    // Avoid revisiting the previous point
    if (next == previous) {
      continue;
    }
 
    double local_cost = calculate_cost(previous->point.x(), previous->point.y(), current->point.x(),
                                       current->point.y(), next->point.x(), next->point.y());
 
    // Skip if local cost exceeds maximum allowed cost
    if (local_cost > max_cost) {
      continue;
    }
 
    // Recursive cost calculation
    auto [best_cost, best_point] = find_best_next_midpoint(depth - 1, current, next, max_cost);
 
    double total_cost = local_cost + best_cost;
 
    // Update minimum cost and corresponding point
    if (total_cost < min_cost) {
      min_cost = total_cost;
      min_point = next;
    }
  }
 
  return {min_cost, min_point};
}
 
double PathCalculation::calculate_cost(double previous_x, double previous_y, double current_x,
                                       double current_y, double next_x, double next_y) const {
  double dx_dist = current_x - next_x;
  double dy_dist = current_y - next_y;
  double distance = std::sqrt(dx_dist * dx_dist + dy_dist * dy_dist);
 
  // Angle calculation
  double angle_with_previous = std::atan2(current_y - previous_y, current_x - previous_x);
  double angle_with_next = std::atan2(next_y - current_y, next_x - current_x);
 
  // Normalize angle to be between 0 and π
  double angle = abs(angle_with_next - angle_with_previous);
  if (angle > M_PI) {
    angle = 2 * M_PI - angle;
  }
 
  // Local cost calculation
  return std::pow(angle, config_.angle_exponent_) * config_.angle_gain_ +
         std::pow(distance, config_.distance_exponent_) * config_.distance_gain_;
}
 
// ===================== Cone Discarding =====================
 
void PathCalculation::discard_cones_along_path() {
  if (current_path_.size() < 2) {
    return;
  }
 
  const Colorpoint& last = current_path_[current_path_.size() - 2];
  const Colorpoint& current = current_path_.back();
 
  std::shared_ptr<Midpoint> last_mp = find_nearest_midpoint(last.point);
  std::shared_ptr<Midpoint> current_midpoint = find_nearest_midpoint(current.point);
 
  if (!last_mp || !current_midpoint) {
    RCLCPP_WARN(rclcpp::get_logger("planning"), "No valid midpoints found for discarding cones.");
    return;
  }
 
  // Discard a cone that was likely passed and should be discarded
  discard_cone(last_mp, current_midpoint);
 
  // Invalidate midpoints that rely on discarded cones
  invalidate_midpoints_with_discarded_cones();
 
  // Remove invalid neighbors from each midpoint's connections
  remove_invalid_neighbors();
}
 
void PathCalculation::discard_cone(const std::shared_ptr<Midpoint>& last_mp,
                                   const std::shared_ptr<Midpoint>& current_mp) {
  if (last_mp->cone1 == current_mp->cone1 || last_mp->cone1 == current_mp->cone2) {
    if (last_mp->cone2 != current_mp->cone1 && last_mp->cone2 != current_mp->cone2) {
      (void)discarded_cones_.insert(last_mp->cone2);
    }
  } else if (last_mp->cone2 == current_mp->cone1 || last_mp->cone2 == current_mp->cone2) {
    if (last_mp->cone1 != current_mp->cone1 && last_mp->cone1 != current_mp->cone2) {
      (void)discarded_cones_.insert(last_mp->cone1);
    }
  } else {
    RCLCPP_DEBUG(rclcpp::get_logger("planning"), "No valid cones found for discarding.");
  }
}
 
void PathCalculation::invalidate_midpoints_with_discarded_cones() {
  for (const auto& mp : midpoints_) {
    if (!mp->valid) {
      continue;
    }
    if (discarded_cones_.count(mp->cone1) > 0 || discarded_cones_.count(mp->cone2) > 0) {
      mp->valid = false;
    }
  }
}
 
void PathCalculation::remove_invalid_neighbors() {
  for (const auto& mp : midpoints_) {
    if (!mp->valid) {
      continue;
    }
    (void)mp->close_points.erase(
        remove_if(mp->close_points.begin(), mp->close_points.end(),
                  [](const std::shared_ptr<Midpoint>& neighbor) { return !neighbor->valid; }),
        mp->close_points.end());
  }
}
 
// ===================== Utility Methods =====================
 
std::shared_ptr<Midpoint> PathCalculation::find_nearest_midpoint(const Point& target) const {
  double min_dist_sq = config_.minimum_point_distance_ * config_.minimum_point_distance_;
  std::shared_ptr<Midpoint> nearest = nullptr;
 
  for (const auto& [pt, mp] : point_to_midpoint_) {
    double dx = pt.x() - target.x();
    double dy = pt.y() - target.y();
    double dist_sq = dx * dx + dy * dy;
 
    if (dist_sq < min_dist_sq) {
      min_dist_sq = dist_sq;
      nearest = mp;
    }
  }
 
  return nearest;
}
 
std::pair<int, double> PathCalculation::find_best_loop_closure(
    const std::vector<PathPoint>& path) const {
  if (path.size() < 3) {
    return {static_cast<int>(path.size()) - 1, std::numeric_limits<double>::max()};
  }
 
  const PathPoint& first = path[0];
  const PathPoint& second = path[1];
  double min_cost = std::numeric_limits<double>::max();
  int best_cutoff_index = static_cast<int>(path.size() - 1);
 
  for (int i = 2; i < static_cast<int>(path.size()); ++i) {
    const PathPoint& current = path[i];
    const PathPoint& previous = path[i - 1];
 
    double cost = calculate_cost(previous.position.x, previous.position.y, current.position.x,
                                 current.position.y, first.position.x, first.position.y);
 
    double cost_into_second =
        calculate_cost(current.position.x, current.position.y, first.position.x, first.position.y,
                       second.position.x, second.position.y);
 
    double combined_cost = cost + cost_into_second;
 
    if (combined_cost < min_cost) {
      min_cost = combined_cost;
      best_cutoff_index = i;
    }
  }
 
  return {best_cutoff_index, min_cost};
}
 
std::vector<PathPoint> PathCalculation::get_path_points_from_colorpoints(
    const std::vector<Colorpoint>& colorpoints) const {
  std::vector<PathPoint> path_points;
  path_points.reserve(colorpoints.size());
 
  for (const Colorpoint& pt : colorpoints) {
    (void)path_points.emplace_back(pt.point.x(), pt.point.y());
  }
 
  return path_points;
}
// ===================== Accessor Methods =====================
 
std::vector<PathPoint> PathCalculation::get_path_to_car() const {
  return get_path_points_from_colorpoints(path_to_car_);
}
 
const std::vector<std::pair<Point, Point>>& PathCalculation::get_triangulations() const {
  return midpoint_generator_.get_triangulations();
}
 
const std::vector<PathPoint>& PathCalculation::get_yellow_cones() const { return yellow_cones_; }
 
const std::vector<PathPoint>& PathCalculation::get_blue_cones() const { return blue_cones_; }