class mrpt::nav::CPTG_DiffDrive_alpha

Overview

The “a(symptotic)-alpha PTG”, as named in PTG papers.

Compatible kinematics : differential-driven / Ackermann steering
Compatible robot shape : Arbitrary 2D polygon
PTG parameters : Use the app ptg-configurator

This PT generator functions are:

\[v(\alpha) = V_{MAX} e^{ -\left( \dfrac{\alpha-\phi}{cte_{a0v}} \right)^2}\]

\[\omega(\alpha) = W_{MAX} \left( -\dfrac{1}{2} +\dfrac{1}{1+ e^{ - \dfrac{\alpha-\phi}{cte_{a0w}} } } \right)\]

So, the radius of curvature of each trajectory is NOT constant for each “alpha” value in this PTG:

[Before MRPT 1.5.0 this was named CPTG2]

#include <mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h>

class CPTG_DiffDrive_alpha: public mrpt::nav::CPTG_DiffDrive_CollisionGridBased
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::nav ::CPTG_DiffDrive_alpha> Ptr;
    typedef std::shared_ptr<const mrpt::nav ::CPTG_DiffDrive_alpha> ConstPtr;
    typedef std::unique_ptr<mrpt::nav ::CPTG_DiffDrive_alpha> UniquePtr;
    typedef std::unique_ptr<const mrpt::nav ::CPTG_DiffDrive_alpha> ConstUniquePtr;

    // fields

    static constexpr const char* className = "mrpt::nav" "::" "CPTG_DiffDrive_alpha";

    // construction

    CPTG_DiffDrive_alpha();

    CPTG_DiffDrive_alpha(
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& sSection
        );

    // methods

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() const;
    virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& cfg, const std::string& sSection);
    virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const;
    virtual std::string getDescription() const;

    virtual void ptgDiffDriveSteeringFunction(
        float alpha,
        float t,
        float x,
        float y,
        float phi,
        float& v,
        float& w
        ) const;

    virtual void loadDefaultParams();
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef std::shared_ptr<CParameterizedTrajectoryGenerator> Ptr;
    typedef std::shared_ptr<const CParameterizedTrajectoryGenerator> ConstPtr;

    // structs

    struct TNavDynamicState;
    struct FlatCollisionGrid;
    struct TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    CLoadableOptions& operator = (const CLoadableOptions&);
    CLoadableOptions& operator = (CLoadableOptions&&);
    virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& source, const std::string& section) = 0;
    void loadFromConfigFileName(const std::string& config_file, const std::string& section);
    virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const;
    void saveToConfigFileName(const std::string& config_file, const std::string& section) const;
    void dumpToConsole() const;
    virtual void dumpToTextStream(std::ostream& out) const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual std::string getDescription() const = 0;

    virtual std::optional<std::pair<int, double>> inverseMap_WS2TP(
        double x,
        double y,
        double tolerance_dist = 0.10
        ) const = 0;

    virtual bool PTG_IsIntoDomain(double x, double y) const;
    virtual bool isBijectiveAt(] uint16_t k, ] uint32_t step) const;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const = 0;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const = 0;
    virtual void setRefDistance(const double refDist);
    virtual size_t getPathStepCount(uint16_t k) const = 0;
    virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const = 0;
    virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const;
    virtual double getPathDist(uint16_t k, uint32_t step) const = 0;
    virtual double getPathStepDuration() const = 0;
    virtual double getMaxLinVel() const = 0;
    virtual double getMaxAngVel() const = 0;
    virtual std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist) const = 0;
    bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const;
    uint32_t getPathStepForDistClamped(uint16_t k, double dist) const;
    virtual void updateTPObstacle(double ox, double oy, std::vector<double>& tp_obstacles) const = 0;
    virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double& tp_obstacle_k) const = 0;

    virtual void updateTPObstacles(
        const float* xs,
        const float* ys,
        std::size_t n,
        std::vector<double>& tp_obstacles
        ) const;

    virtual void loadDefaultParams();
    virtual bool supportVelCmdNOP() const;
    virtual bool supportSpeedAtTarget() const;
    virtual double maxTimeInVelCmdNOP(int path_k) const;
    virtual double getActualUnloopedPathLength(] uint16_t k) const;
    virtual double evalPathRelativePriority(] uint16_t k, ] double target_distance) const;
    virtual double getMaxRobotRadius() const = 0;
    virtual bool isPointInsideRobotShape(const double x, const double y) const = 0;
    virtual double evalClearanceToRobotShape(const double ox, const double oy) const = 0;
    void updateNavDynamicState(const TNavDynamicState& newState, const bool force_update = false);
    const TNavDynamicState& getCurrentNavDynamicState() const;

    void initialize(
        const std::string& cacheFilename = std::string(),
        const bool verbose = true
        );

    void deinitialize();
    bool isInitialized() const;
    uint16_t getAlphaValuesCount() const;
    uint16_t getPathCount() const;
    double index2alpha(uint16_t k) const;
    uint16_t alpha2index(double alpha) const;
    double getRefDistance() const;
    void initTPObstacles(std::vector<double>& TP_Obstacles) const;
    void initTPObstacleSingle(uint16_t k, double& TP_Obstacle_k) const;
    double getScorePriority() const;
    void setScorePriority(double prior);
    void setScorePriorty(double prior);
    unsigned getClearanceStepCount() const;
    void setClearanceStepCount(const uint16_t res);
    unsigned getClearanceDecimatedPaths() const;
    void setClearanceDecimatedPaths(const uint16_t num);

    virtual void renderPathAsSimpleLine(
        const uint16_t k,
        mrpt::viz::CSetOfLines& gl_obj,
        const double decimate_distance = 0.1,
        const double max_path_distance = -1.0
        ) const;

    bool debugDumpInFiles(const std::string& ptg_name) const;
    virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& cfg, const std::string& sSection);
    virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const;
    virtual void add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const = 0;
    void initClearanceDiagram(ClearanceDiagram& cd) const;
    void updateClearance(const double ox, const double oy, ClearanceDiagram& cd) const;

    virtual void evalClearanceSingleObstacle(
        const double ox,
        const double oy,
        const uint16_t k,
        ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
        bool treat_as_obstacle = true
        ) const;

    static CParameterizedTrajectoryGenerator::Ptr CreatePTG(
        const std::string& ptgClassName,
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& sSection,
        const std::string& sKeyPrefix
        );

    static std::string getOutputDebugPathPrefix();
    static void setOutputDebugPathPrefix(const std::string& path);
    static std::string& OUTPUT_DEBUG_PATH_PREFIX();
    static double Index2alpha(uint16_t k, const unsigned int num_paths);
    static uint16_t Alpha2index(double alpha, const unsigned int num_paths);
    static PTGCollisionBehavior getCollisionBehavior();
    static void setCollisionBehavior(PTGCollisionBehavior behavior);
    static PTGCollisionBehavior& COLLISION_BEHAVIOR();
    void setRobotShape(const mrpt::math::CPolygon& robotShape);
    const mrpt::math::CPolygon& getRobotShape() const;
    virtual double getMaxRobotRadius() const;
    virtual double evalClearanceToRobotShape(const double ox, const double oy) const;
    virtual bool isPointInsideRobotShape(const double x, const double y) const;
    virtual void add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const;
    static void static_add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin, const mrpt::math::CPolygon& robotShape);

    virtual std::optional<std::pair<int, double>> inverseMap_WS2TP(
        double x,
        double y,
        double tolerance_dist = 0.10
        ) const;

    virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const;
    virtual void setRefDistance(const double refDist);
    virtual size_t getPathStepCount(uint16_t k) const;
    virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const;
    virtual double getPathDist(uint16_t k, uint32_t step) const;
    virtual std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist) const;
    virtual double getPathStepDuration() const;
    virtual double getMaxLinVel() const;
    virtual double getMaxAngVel() const;
    virtual void updateTPObstacle(double ox, double oy, std::vector<double>& tp_obstacles) const;

    virtual void updateTPObstacles(
        const float* xs,
        const float* ys,
        std::size_t n,
        std::vector<double>& tp_obstacles
        ) const;

    virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double& tp_obstacle_k) const;
    virtual void onNewNavDynamicState();
    std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist);
    bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step);

    virtual void ptgDiffDriveSteeringFunction(
        float alpha,
        float t,
        float x,
        float y,
        float phi,
        float& v,
        float& w
        ) const = 0;

    double getMax_V() const;
    double getMax_W() const;

Typedefs

typedef std::shared_ptr<mrpt::nav ::CPTG_DiffDrive_alpha> Ptr

A type for the associated smart pointer.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)

Parameters accepted by this base class:

${sKeyPrefix}num_paths : The number of different paths in this family (number of discrete alpha values).
${sKeyPrefix}ref_distance : The maximum distance in PTGs [meters]
${sKeyPrefix}score_priority : When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. Assign values <1 to PTGs with low priority.

virtual void saveToConfigFile(mrpt::config::CConfigFileBase& target, const std::string& section) const

This method saves the options to a “.ini”-like file or memory-stored string list.