trajectory_parameterizer.h

#pragma once
 
#include <cstdio>
#include <functional>
#include <memory>
#include <vector>
 
#include "core/math/trajectory/constraints/trajectory_constraint.h"
#include "core/math/trajectory/trajectory.h"
#include "core/math/units.h"
 
using namespace units::literals;

class TrajectoryParameterizer {
 public:
  static Trajectory time_parameterize_trajectory(
      const std::vector<PoseWithCurvature>& points,
      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
      units::Velocity start_velocity, units::Velocity end_velocity, units::Velocity max_velocity,
      units::Acceleration max_acceleration, bool reversed,
      std::function<void(const char*)> error_handler = [](const char* message) {
        std::fprintf(stderr, "%s\n", message);
      });
 
 private:
  constexpr static double kEpsilon = 1E-6;
 
  struct ConstrainedState {
    PoseWithCurvature pose = {Pose2d{}, 0_radpm};
    units::Length distance = 0_in;
    units::Velocity max_velocity = 0_inps;
    units::Acceleration min_acceleration = 0_inps2;
    units::Acceleration max_acceleration = 0_inps2;
  };
 
  static bool enforce_acceleration_limits(
      bool reverse, const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
      ConstrainedState* state, const std::function<void(const char*)>& error_handler);
};