path_calculation.hpp

Go to the documentation of this file.

#ifndef SRC_PLANNING_INCLUDE_PLANNING_PATH_CALCULATION_HPP_
#define SRC_PLANNING_INCLUDE_PLANNING_PATH_CALCULATION_HPP_
 
#include <cmath>
#include <map>
#include <memory>
#include <queue>
#include <utility>
#include <vector>
 
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/midpoint.hpp"
#include "common_lib/structures/path_point.hpp"
#include "common_lib/structures/pose.hpp"
#include "config/path_calculation_config.hpp"
#include "planning/colorpoint.hpp"
#include "planning/midpoint_generator.hpp"
#include "rclcpp/rclcpp.hpp"
 
using Cone = common_lib::structures::Cone;
using PathPoint = common_lib::structures::PathPoint;
using Midpoint = common_lib::structures::Midpoint;
 
class PathCalculation {
public:
  PathCalculation() = default;
 
  explicit PathCalculation(const PathCalculationConfig& config)
      : config_(config), midpoint_generator_(config.midpoint_generator_) {}
 
  // ===================== Public Core Methods =====================
 
  std::vector<PathPoint> calculate_path(const std::vector<Cone>& cone_array);
 
  std::vector<PathPoint> calculate_trackdrive(const std::vector<Cone>& cone_array);
 
  void set_vehicle_pose(const common_lib::structures::Pose& vehicle_pose);
 
  bool is_map_closed(std::vector<PathPoint>& path) const;
 
  // ===================== Public Accessor Methods =====================
 
  std::vector<PathPoint> get_path_to_car() const;
 
  const std::vector<std::pair<Point, Point>>& get_triangulations() const;
 
  const std::vector<PathPoint>& get_yellow_cones() const;
 
  const std::vector<PathPoint>& get_blue_cones() const;
 
private:
  // ===================== Configuration and State =====================
 
  PathCalculationConfig config_;
  MidpointGenerator midpoint_generator_;
 
  // Vehicle and path state
  common_lib::structures::Pose initial_pose_;
  common_lib::structures::Pose vehicle_pose_;
  bool initial_pose_set_ = false;
 
  // Path storage
  std::vector<Colorpoint> path_to_car_;
  std::vector<Colorpoint> past_path_;
  int reset_path_counter_ = 0;
 
  // Midpoints for the current track
  std::vector<std::shared_ptr<Midpoint>> midpoints_;
 
  // Path construction state (temporary during calculation)
  std::vector<Colorpoint> current_path_;
  std::vector<PathPoint> yellow_cones_;
  std::vector<PathPoint> blue_cones_;
  std::unordered_map<Point, std::shared_ptr<Midpoint>> point_to_midpoint_;
  std::unordered_set<std::shared_ptr<Midpoint>> visited_midpoints_;
  std::unordered_set<std::shared_ptr<Cone>> discarded_cones_;
 
  // ===================== Path Calculation Methods =====================
 
  void initialize_path_from_initial_pose();
 
  void update_path_from_past_path();
 
  void extend_path(int max_points);
 
  void clear_path_state();
 
  int reset_path(bool should_reset);
 
  // ===================== Path Search and Optimization =====================
 
  std::pair<std::shared_ptr<Midpoint>, std::shared_ptr<Midpoint>> select_starting_midpoints();
 
  std::vector<std::shared_ptr<Midpoint>> select_candidate_midpoints(const Midpoint& anchor_pose,
                                                                    int num_candidates) const;
 
  std::pair<double, std::shared_ptr<Midpoint>> find_best_next_midpoint(
      int depth, const std::shared_ptr<Midpoint>& previous,
      const std::shared_ptr<Midpoint>& current, double max_cost) const;
 
  double calculate_cost(double previous_x, double previous_y, double current_x, double current_y,
                        double next_x, double next_y) const;
 
  // ===================== Cone Discarding Methods =====================
 
  void discard_cones_along_path();
 
  void discard_cone(const std::shared_ptr<Midpoint>& last_mp,
                    const std::shared_ptr<Midpoint>& current_mp);
 
  void invalidate_midpoints_with_discarded_cones();
 
  void remove_invalid_neighbors();
 
  // ===================== Utility Methods =====================
 
  std::shared_ptr<Midpoint> find_nearest_midpoint(const Point& target) const;
 
  std::pair<int, double> find_best_loop_closure(const std::vector<PathPoint>& path) const;
 
  std::vector<PathPoint> get_path_points_from_colorpoints(
      const std::vector<Colorpoint>& colorpoints) const;
};
 
#endif  // SRC_PLANNING_INCLUDE_PLANNING_PATH_CALCULATION_HPP_