midpoint_generator.hpp

Go to the documentation of this file.

#ifndef SRC_PLANNING_INCLUDE_PLANNING_DELAUNY_MIDPOINT_GENERATOR_HPP_
#define SRC_PLANNING_INCLUDE_PLANNING_DELAUNY_MIDPOINT_GENERATOR_HPP_
 
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 
#include <rclcpp/rclcpp.hpp>
#include <vector>
 
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/midpoint.hpp"
#include "common_lib/structures/pose.hpp"
#include "config/midpoint_generator_config.hpp"
 
using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using DT = CGAL::Delaunay_triangulation_2<K>;
using Point = K::Point_2;
using Vertex_handle = DT::Vertex_handle;
 
using Pose = common_lib::structures::Pose;
using Cone = common_lib::structures::Cone;
using Midpoint = common_lib::structures::Midpoint;
 
class MidpointGenerator {
public:
  MidpointGenerator() = default;
 
  explicit MidpointGenerator(const MidpointGeneratorConfig& config) : config_(config) {}
 
  std::vector<std::shared_ptr<Midpoint>>& generate_midpoints(const std::vector<Cone>& cone_array,
                                                             bool rebuild_all_midpoints);
 
  const std::vector<std::pair<Point, Point>>& get_triangulations() const;
 
  void set_vehicle_pose(const Pose& vehicle_pose);
 
private:
  // Stores Delaunay triangulation edges for visualization and debugging.
  std::vector<std::pair<Point, Point>> triangulations_;
 
  // Current vehicle pose.
  Pose vehicle_pose_;
 
  // Vector containing all generated midpoints.
  std::vector<std::shared_ptr<Midpoint>> midpoints_;
 
  MidpointGeneratorConfig config_;
 
  void filter_cones(const std::vector<Cone>& cone_array,
                    std::vector<std::shared_ptr<Cone>>& filtered_cones, bool rebuild_all_midpoints);
 
  std::shared_ptr<Midpoint> process_triangle_edge(
      const Vertex_handle& va, const Vertex_handle& vb,
      std::vector<std::shared_ptr<Cone>>& filtered_cones,
      std::map<std::pair<int, int>, std::shared_ptr<Midpoint>>& segment_to_midpoint);
 
  void connect_triangle_midpoints(
      const std::array<std::shared_ptr<Midpoint>, 3>& triangle_midpoints);
 
  int find_cone(std::vector<std::shared_ptr<Cone>>& cones, double x, double y);
};
 
#endif  // SRC_PLANNING_INCLUDE_PLANNING_DELAUNY_MIDPOINT_GENERATOR_HPP_