class mrpt::nav::CPTG_DiffDrive_CollisionGridBased

Overview

Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles based on numerical integration of the trajectories and collision look-up-table.

Regarding initialize() : in this this family of PTGs, the method builds the collision grid or load it from a cache file. Collision grids must be calculated before calling getTPObstacle(). Robot shape must be set before initializing with setRobotShape(). The rest of PTG parameters should have been set at the constructor.

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

class CPTG_DiffDrive_CollisionGridBased: public mrpt::nav::CPTG_RobotShape_Polygonal
{
public:
    // structs

    struct TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    // methods

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

// direct descendants

class CPTG_DiffDrive_C;
class CPTG_DiffDrive_CC;
class CPTG_DiffDrive_CCS;
class CPTG_DiffDrive_CS;
class CPTG_DiffDrive_alpha;

Inherited Members

public:
    // structs

    struct TNavDynamicState;

    // 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);

Methods

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

The main method to be implemented in derived classes: it defines the differential-driven differential equation.