skidpad.hpp

Go to the documentation of this file.

#ifndef SRC_PLANNING_INCLUDE_PLANNING_SKIDPAD_HPP_
#define SRC_PLANNING_INCLUDE_PLANNING_SKIDPAD_HPP_
 
#include <vector>
#include <utility>
#include <Eigen/Dense>
#include <rclcpp/rclcpp.hpp>
 
#include "common_lib/structures/path_point.hpp"
#include "common_lib/structures/pose.hpp"
#include "common_lib/structures/cone.hpp"
#include "config/skidpad_config.hpp"
 
 
using PathPoint = common_lib::structures::PathPoint;
using Pose = common_lib::structures::Pose;
using Cone = common_lib::structures::Cone;
 
class Skidpad {
public:
 
  Skidpad() = default;
 
  explicit Skidpad(SkidpadConfig config) : config_(config) {}
 
  std::vector<PathPoint> skidpad_path(const std::vector<Cone>& cone_array,
                                      common_lib::structures::Pose pose);
private:
 
  SkidpadConfig config_;
 
  bool skidpad_data_loaded_ = false;
  std::vector<std::pair<double, double>> reference_cones_;
  std::vector<PathPoint> hardcoded_path_;
  std::vector<PathPoint> predefined_path_;
 
  Eigen::Matrix4f align_cones_with_icp(const std::vector<Cone>& cone_array);
 
  size_t find_closest_path_index(
      const std::vector<PathPoint>& path,
      const common_lib::structures::Pose& pose);
};
 
#endif // SRC_PLANNING_INCLUDE_PLANNING_SKIDPAD_HPP_