TrajectoryFollowerTypes.hpp

Go to the documentation of this file.

#ifndef TRAJECTORY_FOLLOWER_TYPES_HPP
#define TRAJECTORY_FOLLOWER_TYPES_HPP

#include <base/Float.hpp>
#include <base/Pose.hpp>
#include <base/Time.hpp>
#include <base/commands/Motion2D.hpp>
#include <base/Trajectory.hpp>
#include <base/samples/RigidBodyState.hpp>

namespace trajectory_follower
{

enum FollowerStatus
{
    TRAJECTORY_FOLLOWING,
    TRAJECTORY_FINISHED,
    INITIAL_STABILITY_FAILED,
    EXEC_TURN_ON_SPOT,
    EXEC_LATERAL,
    SLAM_POSE_CHECK_FAILED
};

enum ControllerType
{
    CONTROLLER_UNKNOWN = -1,
    CONTROLLER_NO_ORIENTATION = 0,
    CONTROLLER_CHAINED = 1,
    CONTROLLER_SAMSON
};

struct NoOrientationControllerConfig
{
    double l1; 
    double K0; 
    bool useForwardAngleError, useForwardDistanceError;

    NoOrientationControllerConfig()
        : l1(base::unset< double >()),
          K0(base::unset< double >()),
          useForwardAngleError(false),
          useForwardDistanceError(false)
    {
    }
};

struct ChainedControllerConfig
{
    double K0; 
    double K2; 
    double K3; 

    ChainedControllerConfig()
        : K0(base::unset< double >()),
          K2(base::unset< double >()),
          K3(base::unset< double >())
    {
    }
};

struct SamsonControllerConfig
{
    double K2;
    double K3;

    SamsonControllerConfig()
        : K2(base::unset< double >()),
          K3(base::unset< double >())
    {
    }
};

struct FollowerConfig
{
    ControllerType controllerType; 
    NoOrientationControllerConfig noOrientationControllerConfig; 
    ChainedControllerConfig chainedControllerConfig; 
    SamsonControllerConfig samsonControllerConfig;
    
    double dampingAngleUpperLimit;
    double maxRotationalVelocity; 
    double pointTurnStart; 
    double pointTurnEnd;   
    double pointTurnVelocity; 
    double geometricResolution; 
    double trajectoryFinishDistance; 
    double splineReferenceError;
    double splineReferenceErrorMarginCoefficient;
    double maxForwardLenght, maxBackwardLenght;
    double slamPoseErrorCheckEllipseX, slamPoseErrorCheckEllipseY;
    bool usePoseErrorReachedEndCheck;

    FollowerConfig()
        : controllerType(CONTROLLER_UNKNOWN),
          dampingAngleUpperLimit(base::unset< double >()),
          maxRotationalVelocity(base::unset< double >()),
          pointTurnStart(base::unset< double >()),
          pointTurnEnd(base::unset< double >()),
          pointTurnVelocity(base::unset< double >()),
          geometricResolution(0.001),
          trajectoryFinishDistance(base::unset< double >()),
          splineReferenceError(base::unset< double >()),
          splineReferenceErrorMarginCoefficient(base::unset< double >()),
          maxForwardLenght(base::unset< double >()),
          maxBackwardLenght(base::unset< double >()),
          slamPoseErrorCheckEllipseX(base::unset< double >()),
          slamPoseErrorCheckEllipseY(base::unset< double >()),
          usePoseErrorReachedEndCheck(false)
    {
    }
};

struct FollowerData
{
    double distanceError;
    double angleError;
    base::samples::RigidBodyState splineReference;
    base::samples::RigidBodyState currentPose;
    std::vector< base::Trajectory > currentTrajectory;
    base::commands::Motion2D cmd;
    base::samples::RigidBodyState splineSegmentStart, splineSegmentEnd;
};

}

#endif