class mrpt::nav::CPTG_DiffDrive_C
A PTG for circular paths (“C” type PTG in papers).
Compatible kinematics : differential-driven / Ackermann steering
Compatible robot shape : Arbitrary 2D polygon
PTG parameters : Use the app
ptg-configurator
This PT generator functions are:
So, the radius of curvature of each trajectory is constant for each “alpha” value (the trajectory parameter):
from which a minimum radius of curvature can be set by selecting the appropriate values of V_MAX and W_MAX, knowning that \(\alpha \in (-\pi,\pi)\).
![C-PTG path examples](_images/PTG1_paths.png)
[Before MRPT 1.5.0 this was named CPTG1]
#include <mrpt/nav/tpspace/CPTG_DiffDrive_C.h> class CPTG_DiffDrive_C: public mrpt::nav::CPTG_DiffDrive_CollisionGridBased { public: // construction CPTG_DiffDrive_C(); CPTG_DiffDrive_C( 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 inverseMap_WS2TP( double x, double y, int& out_k, double& out_normalized_d, double tolerance_dist = 0.10 ) 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(); };
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(); virtual void setRefDistance(const double refDist); const mrpt::math::CPolygon& getRobotShape() const; 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)
Parameters accepted by this base class:
${sKeyPrefix}num_paths
: The number of different paths in this family (number of discretealpha
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 std::string getDescription() const
Gets a short textual description of the PTG and its parameters.
virtual bool inverseMap_WS2TP( double x, double y, int& out_k, double& out_normalized_d, double tolerance_dist = 0.10 ) const
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) Cartesian coordinates (x,y), relative to the current robot frame.
Parameters:
x |
X coordinate of the query point, relative to the robot frame. |
y |
Y coordinate of the query point, relative to the robot frame. |
out_k |
Trajectory parameter index (discretized alpha value, 0-based index). |
out_d |
Trajectory distance, normalized such that D_max becomes 1. |
Returns:
true if the distance between (x,y) and the actual trajectory point is below the given tolerance (in meters).
virtual bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.
The default implementation just calls inverseMap_WS2TP() and discards (k,d).
virtual void ptgDiffDriveSteeringFunction( float alpha, float t, float x, float y, float phi, float& v, float& w ) const
The main method to be implemented in derived classes: it defines the differential-driven differential equation.
virtual void loadDefaultParams()
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.