velocity_planning.hpp

Go to the documentation of this file.

#pragma once
 
#include <cmath>
 
#include "common_lib/structures/path_point.hpp"
#include "config/velocity_config.hpp"
 
using PathPoint = common_lib::structures::PathPoint;
 
class VelocityPlanning {
public:
  VelocityPlanning() = default;
  explicit VelocityPlanning(VelocityPlanningConfig config) : config_(config) {}
 
  void set_velocity(std::vector<PathPoint> &final_path);
 
  void trackdrive_velocity(std::vector<PathPoint> &final_path);
 
  void stop(std::vector<PathPoint> &final_path, double braking_distance);
 
private:
  VelocityPlanningConfig config_;
 
  static constexpr double epsilon = 1e-9;
 
  double find_curvature(const PathPoint &p1, const PathPoint &p2, const PathPoint &p3);
 
  void point_speed(const std::vector<double> &curvatures, std::vector<double> &velocities);
 
  void acceleration_limiter(const std::vector<PathPoint> &points, std::vector<double> &velocities,
                            const std::vector<double> &curvatures);
 
  void braking_limiter(std::vector<PathPoint> &points, std::vector<double> &velocities,
                       const std::vector<double> &curvatures);
};