Main MRPT website > C++ reference for MRPT 1.9.9
CParameterizedTrajectoryGenerator.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/wrap2pi.h>
13 #include <mrpt/core/round.h>
16 #include <mrpt/math/CPolygon.h>
17 #include <cstdint>
19 #include <mrpt/poses/CPose2D.h>
21 
22 namespace mrpt
23 {
24 namespace opengl
25 {
26 class CSetOfLines;
27 }
28 }
29 
30 namespace mrpt
31 {
32 namespace nav
33 {
34 /** Defines behaviors for where there is an obstacle *inside* the robot shape
35  *right at the beginning of a PTG trajectory.
36  *\ingroup nav_tpspace
37  * \sa Used in CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR
38  */
40 {
41  /** Favor getting back from too-close (almost collision) obstacles. */
43  /** Totally dissallow any movement if there is any too-close (almost
44  collision) obstacles. */
46 };
47 
48 /** \defgroup nav_tpspace TP-Space and PTG classes
49  * \ingroup mrpt_nav_grp
50  */
51 
52 /** This is the base class for any user-defined PTG.
53  * There is a class factory interface in
54  *CParameterizedTrajectoryGenerator::CreatePTG.
55  *
56  * Papers:
57  * - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Extending
58  *Obstacle Avoidance Methods through Multiple Parameter-Space Transformations",
59  *Autonomous Robots, vol. 24, no. 1, 2008.
60  *http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf
61  *
62  * Changes history:
63  * - 30/JUN/2004: Creation (JLBC)
64  * - 16/SEP/2004: Totally redesigned.
65  * - 15/SEP/2005: Totally rewritten again, for integration into MRPT
66  *Applications Repository.
67  * - 19/JUL/2009: Simplified to use only STL data types, and created the class
68  *factory interface.
69  * - MAY/2016: Refactored into CParameterizedTrajectoryGenerator,
70  *CPTG_DiffDrive_CollisionGridBased, PTG classes renamed.
71  * - 2016-2018: Many features added to support "PTG continuation", dynamic
72  *paths depending on vehicle speeds, etc.
73  *
74  * \ingroup nav_tpspace
75  */
79 {
81  public:
82  /** Default ctor. Must call `loadFromConfigFile()` before initialization */
84  /** Destructor */
86  /** The class factory for creating a PTG from a list of parameters in a
87  *section of a given config file (physical file or in memory).
88  * Possible parameters are:
89  * - Those explained in
90  *CParameterizedTrajectoryGenerator::loadFromConfigFile()
91  * - Those explained in the specific PTG being created (see list of
92  *derived classes)
93  *
94  * `ptgClassName` can be any PTG class name which has been registered as
95  *any other
96  * mrpt::serialization::CSerializable class.
97  *
98  * \exception std::logic_error On invalid or missing parameters.
99  */
101  const std::string& ptgClassName,
102  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection,
103  const std::string& sKeyPrefix);
104 
105  /** @name Virtual interface of each PTG implementation
106  * @{ */
107  /** Gets a short textual description of the PTG and its parameters */
108  virtual std::string getDescription() const = 0;
109 
110  protected:
111  /** Must be called after setting all PTG parameters and before requesting
112  * converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
113  virtual void internal_initialize(
114  const std::string& cacheFilename = std::string(),
115  const bool verbose = true) = 0;
116  /** This must be called to de-initialize the PTG if some parameter is to be
117  * changed. After changing it, call initialize again */
118  virtual void internal_deinitialize() = 0;
119 
120  public:
121  /** Computes the closest (alpha,d) TP coordinates of the trajectory point
122  * closest to the Workspace (WS)
123  * Cartesian coordinates (x,y), relative to the current robot frame.
124  * \param[in] x X coordinate of the query point, relative to the robot
125  * frame.
126  * \param[in] y Y coordinate of the query point, relative to the robot
127  * frame.
128  * \param[out] out_k Trajectory parameter index (discretized alpha value,
129  * 0-based index).
130  * \param[out] out_d Trajectory distance, normalized such that D_max
131  * becomes 1.
132  *
133  * \return true if the distance between (x,y) and the actual trajectory
134  * point is below the given tolerance (in meters).
135  */
136  virtual bool inverseMap_WS2TP(
137  double x, double y, int& out_k, double& out_normalized_d,
138  double tolerance_dist = 0.10) const = 0;
139 
140  /** Returns the same than inverseMap_WS2TP() but without any additional
141  * cost. The default implementation
142  * just calls inverseMap_WS2TP() and discards (k,d). */
143  virtual bool PTG_IsIntoDomain(double x, double y) const
144  {
145  int k;
146  double d;
147  return inverseMap_WS2TP(x, y, k, d);
148  }
149 
150  /** Returns true if a given TP-Space point maps to a unique point in
151  * Workspace, and viceversa. Default implementation returns `true`. */
152  virtual bool isBijectiveAt(uint16_t k, uint32_t step) const { return true; }
153  /** Converts a discretized "alpha" value into a feasible motion command or
154  * action. See derived classes for the meaning of these actions */
156  uint16_t k) const = 0;
157 
158  /** Returns an empty kinematic velocity command object of the type supported
159  * by this PTG.
160  * Can be queried to determine the expected kinematic interface of the PTG.
161  */
164 
165  /** Dynamic state that may affect the PTG path parameterization. \ingroup
166  * nav_reactive */
168  {
169  /** Current vehicle velocity (local frame of reference) */
171  /** Current relative target location */
173  /** Desired relative speed [0,1] at target. Default=0 */
175 
177  bool operator==(const TNavDynamicState& o) const;
178  inline bool operator!=(const TNavDynamicState& o) const
179  {
180  return !(*this == o);
181  }
184  };
185 
186  protected:
187  /** Invoked when `m_nav_dyn_state` has changed; gives the PTG the
188  * opportunity to react and parameterize paths depending on the
189  * instantaneous conditions */
190  virtual void onNewNavDynamicState() = 0;
191 
192  public:
193  virtual void setRefDistance(const double refDist) { refDistance = refDist; }
194  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps"
195  * along the trajectory.
196  * May be actual steps from a numerical integration or an arbitrary small
197  * length for analytical PTGs.
198  * \sa getAlphaValuesCount() */
199  virtual size_t getPathStepCount(uint16_t k) const = 0;
200 
201  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at
202  * discrete step `step`.
203  * \sa getPathStepCount(), getAlphaValuesCount() */
204  virtual void getPathPose(
205  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const = 0;
206 
207  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): traversed distance at
208  * discrete step `step`.
209  * \return Distance in pseudometers (real distance, NOT normalized to [0,1]
210  * for [0,refDist])
211  * \sa getPathStepCount(), getAlphaValuesCount() */
212  virtual double getPathDist(uint16_t k, uint32_t step) const = 0;
213 
214  /** Returns the duration (in seconds) of each "step"
215  * \sa getPathStepCount() */
216  virtual double getPathStepDuration() const = 0;
217 
218  /** Returns the maximum linear velocity expected from this PTG [m/s] */
219  virtual double getMaxLinVel() const = 0;
220  /** Returns the maximum angular velocity expected from this PTG [rad/s] */
221  virtual double getMaxAngVel() const = 0;
222 
223  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): largest step count for
224  * which the traversed distance is < `dist`
225  * \param[in] dist Distance in pseudometers (real distance, NOT normalized
226  * to [0,1] for [0,refDist])
227  * \return false if no step fulfills the condition for the given trajectory
228  * `k` (e.g. out of reference distance).
229  * Note that, anyway, the maximum distance (closest point) is returned in
230  * `out_step`.
231  * \sa getPathStepCount(), getAlphaValuesCount() */
232  virtual bool getPathStepForDist(
233  uint16_t k, double dist, uint32_t& out_step) const = 0;
234 
235  /** Updates the radial map of closest TP-Obstacles given a single obstacle
236  * point at (ox,oy)
237  * \param [in,out] tp_obstacles A vector of length `getAlphaValuesCount()`,
238  * initialized with `initTPObstacles()` (collision-free ranges, in
239  * "pseudometers", un-normalized).
240  * \param [in] ox Obstacle point (X), relative coordinates wrt origin of
241  * the PTG.
242  * \param [in] oy Obstacle point (Y), relative coordinates wrt origin of
243  * the PTG.
244  * \note The length of tp_obstacles is not checked for efficiency since
245  * this method is potentially called thousands of times per
246  * navigation timestap, so it is left to the user responsibility to
247  * provide a valid buffer.
248  * \note `tp_obstacles` must be initialized with initTPObstacle() before
249  * call.
250  */
251  virtual void updateTPObstacle(
252  double ox, double oy, std::vector<double>& tp_obstacles) const = 0;
253 
254  /** Like updateTPObstacle() but for one direction only (`k`) in TP-Space.
255  * `tp_obstacle_k` must be initialized with initTPObstacleSingle() before
256  * call (collision-free ranges, in "pseudometers", un-normalized). */
257  virtual void updateTPObstacleSingle(
258  double ox, double oy, uint16_t k, double& tp_obstacle_k) const = 0;
259 
260  /** Loads a set of default parameters into the PTG. Users normally will call
261  * `loadFromConfigFile()` instead, this method is provided
262  * exclusively for the PTG-configurator tool. */
263  virtual void loadDefaultParams();
264 
265  /** Returns true if it is possible to stop sending velocity commands to the
266  * robot and, still, the
267  * robot controller will be able to keep following the last sent trajectory
268  * ("NOP" velocity commands).
269  * Default implementation returns "false". */
270  virtual bool supportVelCmdNOP() const;
271 
272  /** Returns true if this PTG takes into account the desired velocity at
273  * target. \sa updateNavDynamicState() */
274  virtual bool supportSpeedAtTarget() const { return false; }
275  /** Only for PTGs supporting supportVelCmdNOP(): this is the maximum time
276  * (in seconds) for which the path
277  * can be followed without re-issuing a new velcmd. Note that this is only
278  * an absolute maximum duration,
279  * navigation implementations will check for many other conditions. Default
280  * method in the base virtual class returns 0.
281  * \param path_k Queried path `k` index [0,N-1] */
282  virtual double maxTimeInVelCmdNOP(int path_k) const;
283 
284  /** Returns the actual distance (in meters) of the path, discounting
285  * possible circular loops of the path (e.g. if it comes back to the
286  * origin).
287  * Default: refDistance */
288  virtual double getActualUnloopedPathLength(uint16_t k) const
289  {
290  return this->refDistance;
291  }
292 
293  /** Query the PTG for the relative priority factor (0,1) of this PTG, in
294  * comparison to others, if the k-th path is to be selected. */
295  virtual double evalPathRelativePriority(
296  uint16_t k, double target_distance) const
297  {
298  return 1.0;
299  }
300 
301  /** Returns an approximation of the robot radius. */
302  virtual double getMaxRobotRadius() const = 0;
303  /** Returns true if the point lies within the robot shape. */
304  virtual bool isPointInsideRobotShape(
305  const double x, const double y) const = 0;
306 
307  /** Evals the clearance from an obstacle (ox,oy) in coordinates relative to
308  * the robot center. Zero or negative means collision. */
309  virtual double evalClearanceToRobotShape(
310  const double ox, const double oy) const = 0;
311 
312  /** @} */ // --- end of virtual methods
313 
314  /** To be invoked by the navigator *before* each navigation step, to let the
315  * PTG to react to changing dynamic conditions. * \sa
316  * onNewNavDynamicState(), m_nav_dyn_state */
318  const TNavDynamicState& newState, const bool force_update = false);
320  {
321  return m_nav_dyn_state;
322  }
323 
324  /** The path used as defaul output in, for example, debugDumpInFiles.
325  * (Default="./reactivenav.logs/") */
327 
328  /** Must be called after setting all PTG parameters and before requesting
329  * converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
330  void initialize(
331  const std::string& cacheFilename = std::string(),
332  const bool verbose = true);
333  /** This must be called to de-initialize the PTG if some parameter is to be
334  * changed. After changing it, call initialize again */
335  void deinitialize();
336  /** Returns true if `initialize()` has been called and there was no errors,
337  * so the PTG is ready to be queried for paths, obstacles, etc. */
338  bool isInitialized() const;
339 
340  /** Get the number of different, discrete paths in this family */
342  /** Get the number of different, discrete paths in this family */
344  /** Alpha value for the discrete corresponding value \sa alpha2index */
345  double index2alpha(uint16_t k) const;
346  static double index2alpha(uint16_t k, const unsigned int num_paths);
347 
348  /** Discrete index value for the corresponding alpha value \sa index2alpha
349  */
350  uint16_t alpha2index(double alpha) const;
351  static uint16_t alpha2index(double alpha, const unsigned int num_paths);
352 
353  inline double getRefDistance() const { return refDistance; }
354  /** Resizes and populates the initial appropriate contents in a vector of
355  * tp-obstacles (collision-free ranges, in "pseudometers", un-normalized).
356  * \sa updateTPObstacle() */
357  void initTPObstacles(std::vector<double>& TP_Obstacles) const;
358  void initTPObstacleSingle(uint16_t k, double& TP_Obstacle_k) const;
359 
360  /** When used in path planning, a multiplying factor (default=1.0) for the
361  * scores for this PTG. Assign values <1 to PTGs with low priority. */
362  double getScorePriority() const { return m_score_priority; }
363  void setScorePriorty(double prior) { m_score_priority = prior; }
364  unsigned getClearanceStepCount() const { return m_clearance_num_points; }
365  void setClearanceStepCount(const unsigned res)
366  {
368  }
369 
370  unsigned getClearanceDecimatedPaths() const
371  {
373  }
374  void setClearanceDecimatedPaths(const unsigned num)
375  {
377  }
378 
379  /** Returns the representation of one trajectory of this PTG as a 3D OpenGL
380  * object (a simple curved line).
381  * \param[in] k The 0-based index of the selected trajectory (discrete
382  * "alpha" parameter).
383  * \param[out] gl_obj Output object.
384  * \param[in] decimate_distance Minimum distance between path points (in
385  * meters).
386  * \param[in] max_path_distance If >=0, cut the path at this distance (in
387  * meters).
388  */
389  virtual void renderPathAsSimpleLine(
390  const uint16_t k, mrpt::opengl::CSetOfLines& gl_obj,
391  const double decimate_distance = 0.1,
392  const double max_path_distance = -1.0) const;
393 
394  /** Dump PTG trajectories in four text files:
395  * `./reactivenav.logs/PTGs/PTG%i_{x,y,phi,d}.txt`
396  * Text files are loadable from MATLAB/Octave, and can be visualized with
397  * the script `[MRPT_DIR]/scripts/viewPTG.m`
398  * \note The directory "./reactivenav.logs/PTGs" will be created if doesn't
399  * exist.
400  * \return false on any error writing to disk.
401  * \sa OUTPUT_DEBUG_PATH_PREFIX
402  */
403  bool debugDumpInFiles(const std::string& ptg_name) const;
404 
405  /** Parameters accepted by this base class:
406  * - `${sKeyPrefix}num_paths`: The number of different paths in this
407  * family (number of discrete `alpha` values).
408  * - `${sKeyPrefix}ref_distance`: The maximum distance in PTGs [meters]
409  * - `${sKeyPrefix}score_priority`: When used in path planning, a
410  * multiplying factor (default=1.0) for the scores for this PTG. Assign
411  * values <1 to PTGs with low priority.
412  */
413  virtual void loadFromConfigFile(
415  const std::string& sSection) override;
416  virtual void saveToConfigFile(
418  const std::string& sSection) const override;
419 
420  /** Auxiliary function for rendering */
421  virtual void add_robotShape_to_setOfLines(
422  mrpt::opengl::CSetOfLines& gl_shape,
423  const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const = 0;
424 
425  /** Defines the behavior when there is an obstacle *inside* the robot shape
426  * right at the beginning of a PTG trajectory.
427  * Default value: COLL_BEH_BACK_AWAY
428  */
430 
431  /** Must be called to resize a CD to its correct size, before calling
432  * updateClearance() */
433  void initClearanceDiagram(ClearanceDiagram& cd) const;
434 
435  /** Updates the clearance diagram given one (ox,oy) obstacle point, in
436  * coordinates relative
437  * to the PTG path origin.
438  * \param[in,out] cd The clearance will be updated here.
439  * \sa m_clearance_dist_resolution
440  */
441  void updateClearance(
442  const double ox, const double oy, ClearanceDiagram& cd) const;
443  void updateClearancePost(
444  ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const;
445 
446  protected:
447  double refDistance;
448  /** The number of discrete values for "alpha" between -PI and +PI. */
451  /** Number of steps for the piecewise-constant approximation of clearance
452  * from TPS distances [0,1] (Default=5) \sa updateClearance() */
454  /** Number of paths for the decimated paths analysis of clearance */
456  /** Updated before each nav step by */
458  /** Update in updateNavDynamicState(), contains the path index (k) for the
459  * target. */
461 
462  static const uint16_t INVALID_PTG_PATH_INDEX = static_cast<uint16_t>(-1);
463 
465 
466  /** To be called by implementors of updateTPObstacle() and
467  * updateTPObstacleSingle() to
468  * honor the user settings regarding COLLISION_BEHAVIOR.
469  * \param new_tp_obs_dist The newly determiend collision-free ranges, in
470  * "pseudometers", un-normalized, for some "k" direction.
471  * \param inout_tp_obs The target where to store the new TP-Obs distance,
472  * if it fulfills the criteria determined by the collision behavior.
473  */
475  const double ox, const double oy, const double new_tp_obs_dist,
476  double& inout_tp_obs) const;
477 
479  virtual void internal_writeToStream(
480  mrpt::serialization::CArchive& out) const;
481 
482  public:
483  /** Evals the robot clearance for each robot pose along path `k`, for the
484  * real distances in
485  * the key of the map<>, then keep in the map value the minimum of its
486  * current stored clearance,
487  * or the computed clearance. In case of collision, clearance is zero.
488  * \param treat_as_obstacle true: normal use for obstacles; false: compute
489  * shortest distances to a target point (no collision)
490  */
491  virtual void evalClearanceSingleObstacle(
492  const double ox, const double oy, const uint16_t k,
493  ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
494  bool treat_as_obstacle = true) const;
495 
496 }; // end of class
497 
498 /** A list of PTGs (smart pointers) */
499 using TListPTGPtr =
500  std::vector<mrpt::nav::CParameterizedTrajectoryGenerator::Ptr>;
501 
502 /** Base class for all PTGs using a 2D polygonal robot shape model.
503  * \ingroup nav_tpspace
504  */
506 {
507  public:
509  virtual ~CPTG_RobotShape_Polygonal();
510 
511  /** @name Robot shape
512  * @{ **/
513  /** Robot shape must be set before initialization, either from ctor params
514  * or via this method. */
515  void setRobotShape(const mrpt::math::CPolygon& robotShape);
517  double getMaxRobotRadius() const override;
518  virtual double evalClearanceToRobotShape(
519  const double ox, const double oy) const override;
520  /** @} */
521  bool isPointInsideRobotShape(const double x, const double y) const override;
523  mrpt::opengl::CSetOfLines& gl_shape,
524  const mrpt::poses::CPose2D& origin =
525  mrpt::poses::CPose2D()) const override;
526 
527  protected:
528  /** Will be called whenever the robot shape is set / updated */
529  virtual void internal_processNewRobotShape() = 0;
534  const std::string& section);
535  void saveToConfigFile(
537  const std::string& sSection) const override;
540  /** Loads a set of default parameters; provided exclusively for the
541  * PTG-configurator tool. */
542  void loadDefaultParams() override;
543 };
544 
545 /** Base class for all PTGs using a 2D circular robot shape model.
546  * \ingroup nav_tpspace
547  */
549 {
550  public:
552  virtual ~CPTG_RobotShape_Circular();
553 
554  /** @name Robot shape
555  * @{ **/
556  /** Robot shape must be set before initialization, either from ctor params
557  * or via this method. */
558  void setRobotShapeRadius(const double robot_radius);
559  double getRobotShapeRadius() const { return m_robotRadius; }
560  double getMaxRobotRadius() const override;
561  virtual double evalClearanceToRobotShape(
562  const double ox, const double oy) const override;
563  /** @} */
565  mrpt::opengl::CSetOfLines& gl_shape,
566  const mrpt::poses::CPose2D& origin =
567  mrpt::poses::CPose2D()) const override;
568  bool isPointInsideRobotShape(const double x, const double y) const override;
569 
570  protected:
571  /** Will be called whenever the robot shape is set / updated */
572  virtual void internal_processNewRobotShape() = 0;
576  const std::string& section);
577  void saveToConfigFile(
579  const std::string& sSection) const override;
582  /** Loads a set of default parameters; provided exclusively for the
583  * PTG-configurator tool. */
584  void loadDefaultParams() override;
585 };
586 }
587 }
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
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...
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 < ...
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).
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
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...
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.
unsigned __int16 uint16_t
Definition: rptypes.h:44
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
virtual 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. ...
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
Base class for all PTGs using a 2D circular robot shape model.
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)...
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
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 void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
static CParameterizedTrajectoryGenerator * 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...
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Favor getting back from too-close (almost collision) obstacles.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
virtual double getMaxAngVel() const =0
Returns the maximum angular velocity expected from this PTG [rad/s].
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:21
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
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...
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
Clearance information for one particular PTG and one set of obstacles.
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.
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) 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 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 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.
GLuint GLuint num
Definition: glext.h:7278
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: `.
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
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...
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator::Ptr > TListPTGPtr
A list of PTGs (smart pointers)
const mrpt::math::CPolygon & getRobotShape() const
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
This is the base class for any user-defined PTG.
static std::string OUTPUT_DEBUG_PATH_PREFIX
The path used as defaul output in, for example, debugDumpInFiles.
This class allows loading and storing values and vectors of different types from a configuration text...
virtual 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...
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
void internal_shape_loadFromStream(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...
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
Base class for all PTGs using a 2D polygonal robot shape model.
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance() ...
mrpt::math::TPose2D relTarget
Current relative target location.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
GLsizei const GLchar ** string
Definition: glext.h:4101
virtual 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...
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0...
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
virtual double getActualUnloopedPathLength(uint16_t k) const
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path ...
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...
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
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 setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
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,oy)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
virtual double getMaxLinVel() const =0
Returns the maximum linear velocity expected from this PTG [m/s].
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 ...
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
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.
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. ...
GLenum GLint GLint y
Definition: glext.h:3538
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
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...
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
A set of independent lines (or segments), one line with its own start and end positions (X...
Definition: CSetOfLines.h:33
unsigned __int32 uint32_t
Definition: rptypes.h:47
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
Dynamic state that may affect the PTG path parameterization.
GLfloat GLfloat p
Definition: glext.h:6305
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
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 bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.
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...
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 alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019