class mrpt::nav::CPTG_DiffDrive_CS

Overview

A PTG for optimal paths of type “CS”, as named in PTG papers.

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

See “Obstacle Distance for Car-Like Robots”, IEEE Trans. Rob. And Autom, 1999. [Before MRPT 1.5.0 this was named CPTG5]

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

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

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

    // fields

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

    // construction

    CPTG_DiffDrive_CS();

    CPTG_DiffDrive_CS(
        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 bool PTG_IsIntoDomain(double x, double y) const;

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

    virtual void loadDefaultParams();
    double getMax_V() const;
    double getMax_W() const;
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    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 TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    // methods

    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    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;
    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 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 bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const = 0;
    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 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);

    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;
    void initTPObstacles(std::vector<double>& TP_Obstacles) const;
    double getScorePriority() const;

    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 PTGCollisionBehavior getCollisionBehavior();
    static void setCollisionBehavior(PTGCollisionBehavior behavior);
    static PTGCollisionBehavior& COLLISION_BEHAVIOR();
    const mrpt::math::CPolygon& getRobotShape() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual void setRefDistance(const double refDist);
    const TNavDynamicState& getCurrentNavDynamicState() const;
    double getRefDistance() const;
    void initTPObstacleSingle(uint16_t k, double& TP_Obstacle_k) const;
    void setScorePriorty(double prior);
    unsigned getClearanceStepCount() const;
    void setClearanceStepCount(const uint16_t res);
    unsigned getClearanceDecimatedPaths() const;
    void setClearanceDecimatedPaths(const uint16_t num);
    void updateClearancePost(ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const;
    static void static_add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin, const mrpt::math::CPolygon& robotShape);
    static double Index2alpha(uint16_t k, const unsigned int num_paths);
    static uint16_t Alpha2index(double alpha, const unsigned int num_paths);

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

Typedefs

typedef std::shared_ptr<mrpt::nav ::CPTG_DiffDrive_CS> 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)

Possible values in “params” (those in CParameterizedTrajectoryGenerator, which is called internally, plus):

${sKeyPrefix}resolution : The cell size
${sKeyPrefix}v_max, ${sKeyPrefix}w_max`: Maximum robot speeds.
`${sKeyPrefix}shape_x{0,1,2..}`, ${sKeyPrefix}shape_y{0,1,2..}`: Polygonal robot shape [Optional, can be also set via setRobotPolygonShape()]

See docs of derived classes for additional parameters in setParams()

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.