smoothing.hpp

Go to the documentation of this file.

#ifndef SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_
#define SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_
 
#include <osqp.h>
 
#include <Eigen/Dense>
#include <cmath>
#include <functional>
#include <map>
#include <vector>
 
#include "common_lib/structures/path_point.hpp"
#include "config/smoothing_config.hpp"
#include "utils/splines.hpp"
 
using PathPoint = common_lib::structures::PathPoint;
 
class PathSmoothing {
public:
  PathSmoothing() = default;
 
  explicit PathSmoothing(PathSmoothingConfig config) : config_(config) {}
 
  std::vector<PathPoint> smooth_path(const std::vector<PathPoint>& path, bool is_path_closed) const;
 
  std::vector<PathPoint> optimize_path(const std::vector<PathPoint>& path,
                                       const std::vector<PathPoint>& yellow_cones,
                                       const std::vector<PathPoint>& blue_cones) const;
 
private:
  PathSmoothingConfig config_;
 
  std::vector<PathPoint> filter_path(const std::vector<PathPoint>& path) const;
 
  std::vector<PathPoint> osqp_optimization(const std::vector<PathPoint>& center,
                                           const std::vector<PathPoint>& left,
                                           const std::vector<PathPoint>& right) const;
 
  void add_curvature_terms(int num_path_points, const std::function<int(int)>& circular_index,
                           const std::function<void(int, int, double)>& add_coefficient) const;
 
  void add_slack_penalty_terms(int num_path_points,
                               const std::function<void(int, int, double)>& add_coefficient) const;
 
  void add_slack_penalty_terms(std::map<std::pair<int, int>, double>& quadratic_terms,
                               int num_path_points,
                               const std::function<void(int, int, double)>& add_coefficient) const;
 
  void add_boundary_constraints(std::vector<OSQPFloat>& constraint_values,
                                std::vector<OSQPInt>& constraint_row_indices,
                                std::vector<OSQPInt>& constraint_col_indices,
                                std::vector<OSQPFloat>& constraint_lower_bounds,
                                std::vector<OSQPFloat>& constraint_upper_bounds,
                                int& constraint_count, const std::vector<PathPoint>& left,
                                const std::vector<PathPoint>& right, int num_path_points,
                                double safety_margin) const;
 
  void add_slack_nonnegativity_constraints(std::vector<OSQPFloat>& constraint_values,
                                           std::vector<OSQPInt>& constraint_row_indices,
                                           std::vector<OSQPInt>& constraint_col_indices,
                                           std::vector<OSQPFloat>& constraint_lower_bounds,
                                           std::vector<OSQPFloat>& constraint_upper_bounds,
                                           int& constraint_count, int num_path_points) const;
 
  void convert_to_csc_format(const std::vector<OSQPFloat>& values,
                             const std::vector<OSQPInt>& row_indices,
                             const std::vector<OSQPInt>& col_indices, int total_variables,
                             std::vector<OSQPFloat>& csc_x, std::vector<OSQPInt>& csc_i,
                             std::vector<OSQPInt>& csc_p) const;
};
 
#endif  // SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_