Go to the documentation of this file.
103 const std::string& ptgClassName,
105 const std::string& sKeyPrefix);
116 const std::string& cacheFilename = std::string(),
117 const bool verbose =
true) = 0;
139 double x,
double y,
int& out_k,
double& out_normalized_d,
140 double tolerance_dist = 0.10)
const = 0;
154 virtual bool isBijectiveAt(uint16_t k, uint32_t step)
const {
return true; }
158 uint16_t k)
const = 0;
182 return !(*
this == o);
229 virtual double getPathDist(uint16_t k, uint32_t step)
const = 0;
250 uint16_t k,
double dist, uint32_t& out_step)
const = 0;
269 double ox,
double oy, std::vector<double>& tp_obstacles)
const = 0;
275 double ox,
double oy, uint16_t k,
double& tp_obstacle_k)
const = 0;
313 uint16_t k,
double target_distance)
const
322 const double x,
const double y)
const = 0;
327 const double ox,
const double oy)
const = 0;
335 const TNavDynamicState& newState,
const bool force_update =
false);
348 const std::string& cacheFilename = std::string(),
363 static double index2alpha(uint16_t k,
const unsigned int num_paths);
368 static uint16_t
alpha2index(
double alpha,
const unsigned int num_paths);
408 const double decimate_distance = 0.1,
409 const double max_path_distance = -1.0)
const;
432 const std::string& sSection)
override;
435 const std::string& sSection)
const override;
492 const double ox,
const double oy,
const double new_tp_obs_dist,
493 double& inout_tp_obs)
const;
509 const double ox,
const double oy,
const uint16_t k,
511 bool treat_as_obstacle =
true)
const;
517 std::vector<mrpt::nav::CParameterizedTrajectoryGenerator::Ptr>;
536 const double ox,
const double oy)
const override;
551 const std::string& section);
554 const std::string& sSection)
const override;
579 const double ox,
const double oy)
const override;
593 const std::string& section);
596 const std::string& sSection)
const override;
~CParameterizedTrajectoryGenerator() override=default
Destructor
Base class for all PTGs using a 2D polygonal robot shape model.
Base class for all PTGs using a 2D circular robot shape model.
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
~CPTG_RobotShape_Polygonal() override
void setClearanceStepCount(const unsigned res)
virtual void evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t &inout_realdist2clearance, bool treat_as_obstacle=true) const
Evals the robot clearance for each robot pose along path k, for the real distances in the key of the ...
void readFromStream(mrpt::serialization::CArchive &in)
bool operator==(const TNavDynamicState &o) const
void internal_TPObsDistancePostprocess(const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const
To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to honor the user set...
static std::string & OUTPUT_DEBUG_PATH_PREFIX()
The path used as defaul output in, for example, debugDumpInFiles.
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,...
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
double targetRelSpeed
Desired relative speed [0,1] at target.
double evalClearanceToRobotShape(const double ox, const double oy) const override
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.
virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual void setRefDistance(const double refDist)
void setScorePriorty(double prior)
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
bool operator!=(const TNavDynamicState &o) const
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method.
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
virtual double evalPathRelativePriority(uint16_t k, double target_distance) const
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others,...
virtual double getActualUnloopedPathLength(uint16_t k) const
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path ...
A set of independent lines (or segments), one line with its own start and end positions (X,...
virtual double evalClearanceToRobotShape(const double ox, const double oy) const =0
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
static const uint16_t INVALID_PTG_PATH_INDEX
mrpt::math::CPolygon m_robotShape
virtual double getMaxLinVel() const =0
Returns the maximum linear velocity expected from this PTG [m/s].
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator::Ptr > TListPTGPtr
A list of PTGs (smart pointers)
void updateClearance(const double ox, const double oy, ClearanceDiagram &cd) const
Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG pa...
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method.
virtual bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.
mrpt::vision::TStereoCalibResults out
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
virtual void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const =0
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,...
virtual void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const =0
Auxiliary function for rendering.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
void setClearanceDecimatedPaths(const unsigned num)
mrpt::math::TPose2D relTarget
Current relative target location.
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
double getRobotShapeRadius() const
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
Virtual base class for "archives": classes abstracting I/O streams.
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
TNavDynamicState m_nav_dyn_state
Updated before each nav step by
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
void initialize(const std::string &cacheFilename=std::string(), const bool verbose=true)
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
This class allows loading and storing values and vectors of different types from a configuration text...
TNavDynamicState()=default
virtual void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true)=0
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Clearance information for one particular PTG and one set of obstacles.
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
unsigned getClearanceDecimatedPaths() const
const mrpt::math::CPolygon & getRobotShape() const
CPTG_RobotShape_Polygonal()
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
double evalClearanceToRobotShape(const double ox, const double oy) const override
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Dynamic state that may affect the PTG path parameterization.
The virtual base class which provides a unified interface for all persistent objects in MRPT.
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and,...
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
void writeToStream(mrpt::serialization::CArchive &out) const
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
std::shared_ptr< CParameterizedTrajectoryGenerator > Ptr
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
static CParameterizedTrajectoryGenerator::Ptr CreatePTG(const std::string &ptgClassName, const mrpt::config::CConfigFileBase &cfg, const std::string &sSection, const std::string &sKeyPrefix)
The class factory for creating a PTG from a list of parameters in a section of a given config file (p...
@ COLL_BEH_BACK_AWAY
Favor getting back from too-close (almost collision) obstacles.
virtual bool isBijectiveAt(uint16_t k, uint32_t step) const
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.
virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
static PTG_collision_behavior_t & COLLISION_BEHAVIOR()
Defines the behavior when there is an obstacle inside the robot shape right at the beginning of a PTG...
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
std::shared_ptr< CVehicleVelCmd > Ptr
A wrapper of a TPolygon2D class, implementing CSerializable.
unsigned getClearanceStepCount() const
double getRefDistance() const
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const
Gets velocity ("twist") for path k ([0,N-1]=>[-pi,pi] in alpha), at vehicle discrete step step.
virtual size_t getPathStepCount(uint16_t k) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
CPTG_RobotShape_Circular()
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
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line)...
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const =0
Like updateTPObstacle() but for one direction only (k) in TP-Space.
~CPTG_RobotShape_Circular() override
void updateNavDynamicState(const TNavDynamicState &newState, const bool force_update=false)
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynam...
const TNavDynamicState & getCurrentNavDynamicState() const
@ COLL_BEH_STOP
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: .
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
virtual double getMaxAngVel() const =0
Returns the maximum angular velocity expected from this PTG [rad/s].
This is the base class for any user-defined PTG.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
CParameterizedTrajectoryGenerator()
Default ctor.
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance()
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |