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