colorpoint.hpp

Go to the documentation of this file.

#ifndef SRC_PLANNING_INCLUDE_PLANNING_COLORPOINT_HPP_
#define SRC_PLANNING_INCLUDE_PLANNING_COLORPOINT_HPP_
 
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <vector>
 
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/path_point.hpp"
 
using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Point = K::Point_2;
using Cone = common_lib::structures::Cone;
using Color = common_lib::competition_logic::Color;
using PathPoint = common_lib::structures::PathPoint;
 
class Colorpoint {
public:
  Colorpoint() = default;
 
  Colorpoint(const Point& pt, Cone c1, Cone c2) : point(pt), cone1(c1), cone2(c2) {}
 
  static void extract_cones(std::vector<Colorpoint>& colorpoints,
                            std::vector<PathPoint>& yellow_cones,
                            std::vector<PathPoint>& blue_cones);
 
  Point point;         
  Cone cone1;          
  Cone cone2;          
 
private:
  static void color_cones(Colorpoint& colorpoint, const Colorpoint& next_colorpoint,
                          std::vector<PathPoint>& yellow_cones, std::vector<PathPoint>& blue_cones);
 
  static void color_last_point(std::vector<Colorpoint>& colorpoints,
                               std::vector<PathPoint>& yellow_cones,
                               std::vector<PathPoint>& blue_cones);
  static void add_cones_by_reference(const Cone& reference_cone, Cone& matching_cone,
                                     Cone& other_cone, std::vector<PathPoint>& yellow_cones,
                                     std::vector<PathPoint>& blue_cones);
  static void color_pair_of_cones(Cone& yellow_cone, Cone& blue_cone,
                                  std::vector<PathPoint>& yellow_cones,
                                  std::vector<PathPoint>& blue_cones);
};
 
#endif  // SRC_PLANNING_INCLUDE_PLANNING_COLORPOINt_HPP_