class mrpt::nav::CPTG_Holo_Blend

Overview

A PTG for circular-shaped robots with holonomic kinematics.

• Compatible kinematics : Holonomic robot capable of velocity commands with a linear interpolation (“ramp “or “blending”) time. See mrpt::kinematics::CVehicleSimul_Holo

• Compatible robot shape : Circular robots

• PTG parameters : Use the app ptg-configurator

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

class CPTG_Holo_Blend: public mrpt::nav::CPTG_RobotShape_Circular
{
public:
    // typedefs

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

    // fields

    static constexpr const char* className = "mrpt::nav" "::" "CPTG_Holo_Blend";
    static constexpr double EPSILON = 1e-4;
    static constexpr double DEFAULT_PATH_TIME_STEP = 10e-3;

    // construction

    CPTG_Holo_Blend();

    CPTG_Holo_Blend(
        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;
    void setRobotShapeRadius(const double robot_radius);
    virtual double getMaxRobotRadius() const;
    virtual double evalClearanceToRobotShape(const double ox, const double oy) const;
    static double calc_trans_distance_t_below_Tramp(double k2, double k4, double vxi, double vyi, double t);
    static double calc_trans_distance_t_below_Tramp_abc(double t, double a, double b, double c);
    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 loadDefaultParams();
    virtual bool supportVelCmdNOP() const;
    virtual double maxTimeInVelCmdNOP(int path_k) const;
    virtual std::string getDescription() const;

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

    virtual bool PTG_IsIntoDomain(double x, double y) const;
    virtual void onNewNavDynamicState();
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const;
    virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const;
    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 updateTPObstacleSingle(double ox, double oy, uint16_t k, double& tp_obstacle_k) const;
    void setPathTimeStep(double dt);
    std::optional<uint32_t> getPathStepForDist(uint16_t k, double dist);
    bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step);
    virtual void add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const;
    virtual bool isPointInsideRobotShape(const double x, const double y) const;
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;

    // structs

    struct TNavDynamicState;

    // methods

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

    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;
    void setScorePriorty(double prior);

    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();
    double getRobotShapeRadius() const;
    static void static_add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin, const double robotRadius);

Typedefs

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

A type for the associated smart pointer.

Fields

static constexpr double EPSILON = 1e-4

Mathematical “epsilon”, to detect ill-conditioned situations (e.g.

1/0)

static constexpr double DEFAULT_PATH_TIME_STEP = 10e-3

Default value of the per-instance path time step [s].

See also:

setPathTimeStep

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.

void setRobotShapeRadius(const double robot_radius)

Robot shape must be set before initialization, either from ctor params or via this method.

virtual double getMaxRobotRadius() const

Returns an approximation of the robot radius.

virtual double evalClearanceToRobotShape(const double ox, const double oy) const

Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.

Zero or negative means collision.

static double calc_trans_distance_t_below_Tramp(
    double k2,
    double k4,
    double vxi,
    double vyi,
    double t
    )

Axiliary function for computing the line-integral distance along the trajectory, handling special cases of 1/0:

static double calc_trans_distance_t_below_Tramp_abc(
    double t,
    double a,
    double b,
    double c
    )

Axiliary function for calc_trans_distance_t_below_Tramp() and others.

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.

See also:

loadFromConfigFile, saveToConfigFileName

virtual void loadDefaultParams()

Loads a set of default parameters; provided exclusively for the PTG-configurator tool.

virtual bool supportVelCmdNOP() const

Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory (“NOP” velocity commands).

Default implementation returns “false”.

virtual double maxTimeInVelCmdNOP(int path_k) const

Only for PTGs supporting supportVelCmdNOP() : this is the maximum time (in seconds) for which the path can be followed without re-issuing a new velcmd.

Note that this is only an absolute maximum duration, navigation implementations will check for many other conditions. Default method in the base virtual class returns 0.

Parameters:

path_k

Queried path k index [0,N-1]

virtual std::string getDescription() const

Gets a short textual description of the PTG and its parameters.

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

Maps a Workspace point (x,y), in the robot frame, to the TP-Space coordinates of the closest point of the closest trajectory.

Parameters:

x
y	Query point coordinates, relative to the robot frame [m].
tolerance_dist	How far the closest trajectory point may lie from (x,y) for the mapping to be accepted [m].

Returns:

The pair (k path index, distance normalized so that getRefDistance() maps to 1), or std::nullopt if (x,y) is out of the PTG domain within that tolerance.

virtual bool PTG_IsIntoDomain(double x, double y) const

Returns true if (x,y) is within the PTG domain.

The default implementation just calls inverseMap_WS2TP().

virtual void onNewNavDynamicState()

Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize paths depending on the instantaneous conditions.

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

Converts a discretized “alpha” value into a feasible motion command or action.

See derived classes for the meaning of these actions

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 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 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).

void setPathTimeStep(double dt)

Duration of each PTG “step” [s] (config key: path_time_step).

Shorter steps mean a finer path discretization at a proportionally higher CPU and memory cost.

See also:

getPathStepDuration()

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 add_robotShape_to_setOfLines(mrpt::viz::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const

Auxiliary function for rendering.

virtual bool isPointInsideRobotShape(const double x, const double y) const

Returns true if the point lies within the robot shape.