CscSequenceMovements.h

Go to the documentation of this file.

#ifndef CscSequenceMovements_H_
#define CscSequenceMovements_H_
 
#include "CscCommon.h"
#include "Axiomes/Csc3dTypes.h"
#include "CoreObjects/CscSequencePositions.h"
 
using namespace conscience_utils::logging;
using namespace conscience_core::axiomes;
using std::min, std::max;
 
enum class MovementType {
    ROTATION = 0,
    POINT = 1,
    LINE = 2,
    MULTI_LINE = 3,
    ARC = 4
};
 
enum class MovementStatesType {
    CONTINU = 0,
    LOOP_DISTANCE = 1,
    LOOP_TIME = 2
};
 
typedef uint8_t MovementOptionalAttributType;
enum MovementOptionalAttribut : MovementOptionalAttributType {
    STATES_TYPE = 1 << 0,
    ALIGNMENT = 1 << 1,
    SPEED = 1 << 2
};
 
enum EnslavementAlignementType {
    ALIGNEMENT_IN_OF_LINE,
    ALIGNEMENT_OUT_OF_LINE,
    NO_ALIGNMENT
};
 
struct MovementStatesParameters {
    MovementStatesType type;               
    int firstStateId;                      
    int secondStateId;                     
    optional<double> loopFirstStateValue;  
    optional<double> loopSecondStateValue; 
 
    MovementStatesParameters(MovementStatesType type, int firstStateId, int secondStateId, optional<double> loopFirstStateValue, optional<double> loopSecondStateValue)
        : type(type),
          firstStateId(firstStateId),
          secondStateId(secondStateId),
          loopFirstStateValue(loopFirstStateValue),
          loopSecondStateValue(loopSecondStateValue) {}
};
 
struct MovementNavigationInformations {
    double translationSpeedTarget; 
    double rotationSpeedTarget;    
    double positionError;          
    double orientationError;       
 
    MovementNavigationInformations(double translationSpeed, double rotationSpeed, double posError, double orientError)
        : translationSpeedTarget(translationSpeed),
          rotationSpeedTarget(rotationSpeed),
          positionError(posError),
          orientationError(orientError) {}
};
 
struct RobotNavigationParameters {
    optional<double> translationSpeedMaxInCmBySecond;
    optional<double> rotationSpeedMaxInCmBySecond;
    optional<double> wheelSpacing; // Spacing between wheels in centimeters
 
    RobotNavigationParameters(
        optional<double> translationSpeedMax = {},
        optional<double> rotationSpeedMax = {},
        optional<double> wheelSpacing = {})
        : translationSpeedMaxInCmBySecond(translationSpeedMax),
          rotationSpeedMaxInCmBySecond(rotationSpeedMax),
          wheelSpacing(wheelSpacing) {}
};
 
struct CurrentRobotInformations {
    CscPoint3d *currentPosition;
    double currentYaw;
 
    CurrentRobotInformations(
        CscPoint3d *currentPosition,
        double currentYaw)
        : currentPosition(currentPosition),
          currentYaw(currentYaw) {}
    ~CurrentRobotInformations();
};
 
class CscMovement {
protected:
    MovementType type;                                   // Type of movement (rotation, line, etc)
    optional<MovementStatesParameters> statesParameters; // State parameters (optional)
    optional<EnslavementAlignementType> alignement;      // Alignment type if needed (optional)
    optional<double> speed;                              // Optional target speed
    int movementDirection;                               // 1 (forward) or -1 (backward)
    CscSequencePositionType positionType;                // Coordinate type (XYZ, LNG_LAT)
    bool requiertAlignement = false;                     // True if force alignment step is needed
    optional<string> projection = {};                    // Current map projection used if LNG_LAT positon type
 
public:
    CscMovement(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, std::optional<EnslavementAlignementType> alignement, CscSequencePositionType positionType);
 
    virtual ~CscMovement() {
    };
 
    MovementType getType() const;
    optional<EnslavementAlignementType> getAlignmentType() const;
    optional<double> getSpeed() const;
    int getMovementDirection() const;
    CscSequencePositionType getPositionType() const;
 
    const optional<MovementStatesParameters> getStatesParameters() const;
};
 
class CscMovementRotation : public CscMovement {
protected:
    double angleTarget;             // Target yaw angle
    optional<double> startYaw = {}; // Initial yaw at start (for progression)
public:
    CscMovementRotation(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, double angleTarget, std::optional<EnslavementAlignementType> alignement, CscSequencePositionType positionType);
 
    double getAngleTarget() const;
};
 
class CscMovementLine : public CscMovement {
protected:
    CscPoint3d *pointTarget = nullptr;
    CscPoint3d *pointSource = nullptr;
 
public:
    CscMovementLine(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, std::optional<EnslavementAlignementType> alignement, CscPoint3d *pointTarget, CscPoint3d *pointSource, CscSequencePositionType positionType);
    virtual ~CscMovementLine();
 
    CscPoint3d *getPointSource() const;
    CscPoint3d *getPointTarget() const;
};
 
class CscMovementPoint : public CscMovementLine {
protected:
public:
    CscMovementPoint(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, std::optional<EnslavementAlignementType> alignement, CscPoint3d *pointTarget, CscSequencePositionType positionType);
    virtual ~CscMovementPoint();
};
 
class CscMovementMultiLine : public CscMovement {
protected:
    const double MAX_DISTANCE_LOOK_HEAD_CENTIMETERS = 25;
    const double MIN_DISTANCE_LOOK_HEAD_CENTIMETERS = 10;
    const double MAX_DERIVE_ANGLE_IN_DEGREES = 15.0;
 
    vector<CscPoint3d *> *points; // List of path points
 
public:
    CscMovementMultiLine(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, std::optional<EnslavementAlignementType> alignement, vector<CscPoint3d *> *points, CscSequencePositionType positionType);
    virtual ~CscMovementMultiLine();
 
    CscPoint3d *getPoint(int index) const;
    vector<CscPoint3d *> *getPoints() const;
    int countPoint() const;
};
 
class CscMovementArc : public CscMovementLine {
protected:
    const double MINIMUM_RADIUS_FOR_SPEED_IN_CENTIMETERS = 30.0;
    CscPoint3d *center; // Center of arc
    double radius;      // Arc radius in centimeters
    int arcDirection;   // Arc direction: 1 = counterclockwise (trigonometric), -1 = clockwise (anti-trigonometric)
 
public:
    CscMovementArc(optional<double> speed, int movementDirection, optional<MovementStatesParameters> statesParameters, std::optional<EnslavementAlignementType> alignement, CscPoint3d *pointTarget, CscPoint3d *pointSource, CscPoint3d *center, double radius, int arcDirection, CscSequencePositionType positionType);
    virtual ~CscMovementArc();
 
    CscPoint3d *getPointCenter() const;
    double getRadius() const;
    int getArcDirection() const;
};
 
class CscSequenceMovements : virtual public CscSequence {
public:
    CscSequenceMovements(vector<CscMovement *> *movements);
    CscSequenceMovements();
 
    ~CscSequenceMovements();
 
    int countMovements() const;
 
    CscMovement *getMovement(int index) const;
 
    void addMovement(CscMovement *movement);
 
    vector<CscMovement *> *getMovements() const;
 
    void setMovements(vector<CscMovement *> *movements);
 
protected:
    vector<CscMovement *> *movements;
};
 
#endif