SubTrajectory.hpp

Go to the documentation of this file.

#ifndef TRAJECTORY_FOLLOWER_SUBTRAJECTORY
#define TRAJECTORY_FOLLOWER_SUBTRAJECTORY

#include <base/Pose.hpp>
#include <base/geometry/Spline.hpp>
#include <vector>
#include <base/Trajectory.hpp>
#include <stdexcept>
#include "Motion2D.hpp"

namespace trajectory_follower {

class SubTrajectory
{
public:
    base::Pose2D startPose;
    base::Pose2D goalPose;
    double speed;
    base::geometry::Spline<3> posSpline;
    base::geometry::Spline<1> orientationSpline;
    DriveMode driveMode;

    SubTrajectory();

    SubTrajectory(const base::Trajectory &trajectory);
    
    static double angleLimit(double angle);
    
    base::Trajectory toBaseTrajectory();

    void interpolate(base::Pose2D start, const std::vector<base::Angle> &angles);

    void interpolate(const std::vector<base::Pose2D> &poses);

    void interpolate(const std::vector< base::Pose2D >& poses, const std::vector< double >& orientationDiff);

    base::Pose2D getIntermediatePoint(double d);

    double getClosestPoint(const base::Pose2D &pose, double guess) const;

    double getClosestPoint(const base::Pose2D &pose) const;

    base::Pose2D getIntermediatePointNormalized(double d);

    double getDist(double startParam, double endParam) const;

    double getClosestPoint(const base::Pose2D &pose, double guess, double start, double end);
    void setGeometricResolution(double geometricResolution);
    double getDistToGoal(double startParam) const;
    std::pair<double, double> error(const Eigen::Vector2d &pos, double currentHeading, double curveParam, double forwardDist);
    double advance(double curveParam, double length);
    double getCurvature(double param);
    double getCurvatureMax();
    double getVariationOfCurvature(double param);
    const base::Pose2D &getStartPose() const;
    const base::Pose2D &getGoalPose() const;
    const double &getSpeed() const;
    double getStartParam() const;
    double getEndParam() const;
    double getGeometricResolution() const;
    bool driveForward() const;
    void setSpeed(double speed);
    double splineHeading(double param);
};

class Lateral : public SubTrajectory {
public:
    Lateral();
    Lateral(const base::Pose2D &currentPose, const base::Position2D &end, double speed);
    Lateral(const base::Pose2D &currentPose, double angle, double length, double speed);
};

}

#endif