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 FlatCollisionGrid;
    struct TCellForLambdaFunction;

    // classes

    class CCollisionGrid;

    // methods

    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;
};

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

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

Methods

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

The default implementation in this class relies on a look-up-table.

Derived classes may redefine this to closed-form expressions, when they exist. See full docs in base class CParameterizedTrajectoryGenerator::inverseMap_WS2TP()

virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const

In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s).

In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s)

See more docs in CParameterizedTrajectoryGenerator::directionToMotionCommand()

virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const

Returns an empty kinematic velocity command object of the type supported by this PTG.

Can be queried to determine the expected kinematic interface of the PTG.

virtual void setRefDistance(const double refDist)

Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change the reference distance after initialization.

virtual size_t getPathStepCount(uint16_t k) const

Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete “steps” along the trajectory.

May be actual steps from a numerical integration or an arbitrary small length for analytical PTGs.

See also:

getAlphaValuesCount()

virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const

Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.

See also:

getPathStepCount(), getAlphaValuesCount(), getPathTwist()

virtual double getPathDist(uint16_t k, uint32_t step) const

Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.

Returns:

Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])

See also:

getPathStepCount(), getAlphaValuesCount()

virtual std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist) const

Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is <dist

Parameters:

dist	Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])

Returns:

std::nullopt if no step fulfills the condition for the given trajectory k (e.g. out of reference distance).

See also:

getPathStepCount(), getAlphaValuesCount()

virtual double getPathStepDuration() const

Returns the duration (in seconds) of each “step”.

See also:

getPathStepCount()

virtual double getMaxLinVel() const

Returns the maximum linear velocity expected from this PTG [m/s].

virtual double getMaxAngVel() const

Returns the maximum angular velocity expected from this PTG [rad/s].

virtual void updateTPObstacle(
    double ox,
    double oy,
    std::vector<double>& tp_obstacles
    ) const

Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)

The length of tp_obstacles is not checked for efficiency since this method is potentially called thousands of times per navigation timestap, so it is left to the user responsibility to provide a valid buffer.

tp_obstacles must be initialized with initTPObstacle() before call.

Parameters:

tp_obstacles	A vector of length getAlphaValuesCount(), initialized with initTPObstacles() (collision-free ranges, in “pseudometers”, un-normalized).
ox	Obstacle point (X), relative coordinates wrt origin of the PTG.
oy	Obstacle point (Y), relative coordinates wrt origin of the PTG.

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

Same result as calling updateTPObstacle() for each point, but it reads a flattened copy of the collision grid and skips, without any memory access, the points that no grid cell entry can reach.

virtual void updateTPObstacleSingle(
    double ox,
    double oy,
    uint16_t k,
    double& tp_obstacle_k
    ) const

Like updateTPObstacle() but for one direction only (k) in TP-Space.

tp_obstacle_k must be initialized with initTPObstacleSingle() before call (collision-free ranges, in “pseudometers”, un-normalized).

virtual void onNewNavDynamicState()

This family of PTGs ignores the dynamic states.

std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist)

Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is <dist

Parameters:

dist	Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])

Returns:

std::nullopt if no step fulfills the condition for the given trajectory k (e.g. out of reference distance).

See also:

getPathStepCount(), getAlphaValuesCount()

bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step)

Deprecated Use the std::optional-returning overload.

Note that out_step is now left untouched when no step fulfills the condition.

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.