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:
    // construction

    CPTG_DiffDrive_CS();

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

    // methods

    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:
    // structs

    struct TNavDynamicState;
    struct TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    // methods

    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 std::string getDescription() const = 0;

    virtual bool inverseMap_WS2TP(
        double x,
        double y,
        int& out_k,
        double& out_normalized_d,
        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::opengl::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::opengl::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& OUTPUT_DEBUG_PATH_PREFIX();
    static PTG_collision_behavior_t& COLLISION_BEHAVIOR();
    const mrpt::math::CPolygon& getRobotShape() const;
    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 unsigned res);
    unsigned getClearanceDecimatedPaths() const;
    void setClearanceDecimatedPaths(const unsigned num);
    void updateClearancePost(ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const;
    static void static_add_robotShape_to_setOfLines(mrpt::opengl::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;

Methods

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.