MRPT  2.0.4
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-2020, 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
mrpt::nav::CParameterizedTrajectoryGenerator::~CParameterizedTrajectoryGenerator
~CParameterizedTrajectoryGenerator() override=default
Destructor
mrpt::nav::CPTG_RobotShape_Polygonal
Base class for all PTGs using a 2D polygonal robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:522
mrpt::nav::CPTG_RobotShape_Circular
Base class for all PTGs using a 2D circular robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:565
mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
Definition: CParameterizedTrajectoryGenerator.cpp:45
mrpt::nav::CPTG_RobotShape_Polygonal::loadShapeFromConfigFile
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
Definition: CPTG_RobotShape_Polygonal.cpp:43
mrpt::nav::CPTG_RobotShape_Polygonal::~CPTG_RobotShape_Polygonal
~CPTG_RobotShape_Polygonal() override
ClearanceDiagram.h
mrpt::nav::CParameterizedTrajectoryGenerator::setClearanceStepCount
void setClearanceStepCount(const unsigned res)
Definition: CParameterizedTrajectoryGenerator.h:382
mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle
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 ...
Definition: CParameterizedTrajectoryGenerator.cpp:512
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::readFromStream
void readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:626
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator==
bool operator==(const TNavDynamicState &o) const
Definition: CParameterizedTrajectoryGenerator.cpp:609
mrpt::nav::CParameterizedTrajectoryGenerator::internal_TPObsDistancePostprocess
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...
Definition: CParameterizedTrajectoryGenerator.cpp:422
mrpt::nav::CPTG_RobotShape_Circular::m_robotRadius
double m_robotRadius
Definition: CParameterizedTrajectoryGenerator.h:590
mrpt::nav::CParameterizedTrajectoryGenerator::OUTPUT_DEBUG_PATH_PREFIX
static std::string & OUTPUT_DEBUG_PATH_PREFIX()
The path used as defaul output in, for example, debugDumpInFiles.
Definition: CParameterizedTrajectoryGenerator.cpp:25
mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_num_points
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,...
Definition: CParameterizedTrajectoryGenerator.h:470
mrpt::nav::CParameterizedTrajectoryGenerator::getSupportedKinematicVelocityCommand
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
mrpt::nav::CParameterizedTrajectoryGenerator::directionToMotionCommand
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed
double targetRelSpeed
Desired relative speed [0,1] at target.
Definition: CParameterizedTrajectoryGenerator.h:176
verbose
params verbose
Definition: chessboard_stereo_camera_calib_unittest.cpp:59
mrpt::nav::CPTG_RobotShape_Circular::evalClearanceToRobotShape
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.
Definition: CPTG_RobotShape_Circular.cpp:113
mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose
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...
Definition: CParameterizedTrajectoryGenerator.h:209
mrpt::nav::CParameterizedTrajectoryGenerator::setRefDistance
virtual void setRefDistance(const double refDist)
Definition: CParameterizedTrajectoryGenerator.h:195
mrpt::nav::CParameterizedTrajectoryGenerator::setScorePriorty
void setScorePriorty(double prior)
Definition: CParameterizedTrajectoryGenerator.h:380
mrpt::nav::CPTG_RobotShape_Circular::loadShapeFromConfigFile
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
Definition: CPTG_RobotShape_Circular.cpp:28
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::operator!=
bool operator!=(const TNavDynamicState &o) const
Definition: CParameterizedTrajectoryGenerator.h:180
mrpt::nav::CPTG_RobotShape_Circular::setRobotShapeRadius
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method.
Definition: CPTG_RobotShape_Circular.cpp:21
mrpt::nav::CPTG_RobotShape_Polygonal::internal_processNewRobotShape
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:155
mrpt::nav::CParameterizedTrajectoryGenerator::evalPathRelativePriority
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,...
Definition: CParameterizedTrajectoryGenerator.h:312
mrpt::nav::CParameterizedTrajectoryGenerator::getActualUnloopedPathLength
virtual double getActualUnloopedPathLength(uint16_t k) const
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path ...
Definition: CParameterizedTrajectoryGenerator.h:305
mrpt::opengl::CSetOfLines
A set of independent lines (or segments), one line with its own start and end positions (X,...
Definition: CSetOfLines.h:32
mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceToRobotShape
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.
mrpt::nav::CPTG_RobotShape_Circular::internal_processNewRobotShape
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
mrpt::nav::CParameterizedTrajectoryGenerator::INVALID_PTG_PATH_INDEX
static const uint16_t INVALID_PTG_PATH_INDEX
Definition: CParameterizedTrajectoryGenerator.h:479
mrpt::nav::CPTG_RobotShape_Polygonal::m_robotShape
mrpt::math::CPolygon m_robotShape
Definition: CParameterizedTrajectoryGenerator.h:547
mrpt::nav::CParameterizedTrajectoryGenerator::getMaxLinVel
virtual double getMaxLinVel() const =0
Returns the maximum linear velocity expected from this PTG [m/s].
mrpt::nav::TListPTGPtr
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator::Ptr > TListPTGPtr
A list of PTGs (smart pointers)
Definition: CParameterizedTrajectoryGenerator.h:517
TPose2D.h
mrpt::nav::CParameterizedTrajectoryGenerator::updateClearance
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...
Definition: CParameterizedTrajectoryGenerator.cpp:488
mrpt::nav::CParameterizedTrajectoryGenerator::deinitialize
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
Definition: CParameterizedTrajectoryGenerator.cpp:415
wrap2pi.h
mrpt::nav::CPTG_RobotShape_Polygonal::setRobotShape
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method.
Definition: CPTG_RobotShape_Polygonal.cpp:21
mrpt::nav::CParameterizedTrajectoryGenerator::PTG_IsIntoDomain
virtual bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.
Definition: CParameterizedTrajectoryGenerator.h:145
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::nav::CParameterizedTrajectoryGenerator::m_clearance_decimated_paths
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
Definition: CParameterizedTrajectoryGenerator.h:472
mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacle
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,...
mrpt::nav::CParameterizedTrajectoryGenerator::add_robotShape_to_setOfLines
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.
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::nav::CParameterizedTrajectoryGenerator::internal_deinitialize
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
mrpt::nav::CPTG_RobotShape_Polygonal::getMaxRobotRadius
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
Definition: CPTG_RobotShape_Polygonal.cpp:144
mrpt::nav::CParameterizedTrajectoryGenerator::setClearanceDecimatedPaths
void setClearanceDecimatedPaths(const unsigned num)
Definition: CParameterizedTrajectoryGenerator.h:391
CPose2D.h
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget
mrpt::math::TPose2D relTarget
Current relative target location.
Definition: CParameterizedTrajectoryGenerator.h:174
mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP
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...
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:208
mrpt::nav::CPTG_RobotShape_Circular::getRobotShapeRadius
double getRobotShapeRadius() const
Definition: CParameterizedTrajectoryGenerator.h:576
mrpt::nav::CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
Definition: CPTG_RobotShape_Polygonal.cpp:93
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
Definition: CParameterizedTrajectoryGenerator.cpp:223
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::curVelLocal
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
Definition: CParameterizedTrajectoryGenerator.h:172
mrpt::nav::CPTG_RobotShape_Circular::saveToConfigFile
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.
Definition: CPTG_RobotShape_Circular.cpp:37
mrpt::nav::CPTG_RobotShape_Polygonal::saveToConfigFile
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.
Definition: CPTG_RobotShape_Polygonal.cpp:74
mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state
TNavDynamicState m_nav_dyn_state
Updated before each nav step by
Definition: CParameterizedTrajectoryGenerator.h:474
CConfigFileBase.h
TTwist2D.h
mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
Definition: CParameterizedTrajectoryGenerator.h:291
mrpt::nav::CParameterizedTrajectoryGenerator::initialize
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...
Definition: CParameterizedTrajectoryGenerator.cpp:400
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Definition: CParameterizedTrajectoryGenerator.cpp:263
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::TNavDynamicState
TNavDynamicState()=default
mrpt::nav::CParameterizedTrajectoryGenerator::internal_initialize
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...
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::nav::ClearanceDiagram
Clearance information for one particular PTG and one set of obstacles.
Definition: ClearanceDiagram.h:28
mrpt::nav::CParameterizedTrajectoryGenerator::getMaxRobotRadius
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
Definition: CParameterizedTrajectoryGenerator.h:466
mrpt::nav::CParameterizedTrajectoryGenerator::getClearanceDecimatedPaths
unsigned getClearanceDecimatedPaths() const
Definition: CParameterizedTrajectoryGenerator.h:387
mrpt::nav::CPTG_RobotShape_Polygonal::getRobotShape
const mrpt::math::CPolygon & getRobotShape() const
Definition: CParameterizedTrajectoryGenerator.h:533
mrpt::nav::CPTG_RobotShape_Polygonal::CPTG_RobotShape_Polygonal
CPTG_RobotShape_Polygonal()
Definition: CPTG_RobotShape_Polygonal.cpp:19
mrpt::nav::ClearanceDiagram::dist2clearance_t
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
Definition: ClearanceDiagram.h:62
mrpt::nav::CParameterizedTrajectoryGenerator::getDescription
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
mrpt::nav::CPTG_RobotShape_Polygonal::evalClearanceToRobotShape
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.
Definition: CPTG_RobotShape_Polygonal.cpp:155
round.h
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:26
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState
Dynamic state that may affect the PTG path parameterization.
Definition: CParameterizedTrajectoryGenerator.h:169
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::serialization::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:30
mrpt::nav::CParameterizedTrajectoryGenerator::refDistance
double refDistance
Definition: CParameterizedTrajectoryGenerator.h:464
mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_loadFromStream
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
Definition: CPTG_RobotShape_Polygonal.cpp:119
mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:190
mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CParameterizedTrajectoryGenerator.cpp:54
mrpt::nav::CParameterizedTrajectoryGenerator::getPathCount
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
Definition: CParameterizedTrajectoryGenerator.h:360
mrpt::nav::CPTG_RobotShape_Polygonal::m_robotMaxRadius
double m_robotMaxRadius
Definition: CParameterizedTrajectoryGenerator.h:548
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
Definition: CParameterizedTrajectoryGenerator.cpp:270
mrpt::nav::CPTG_RobotShape_Circular::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_RobotShape_Circular.cpp:27
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepDuration
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
mrpt::nav::CPTG_RobotShape_Circular::isPointInsideRobotShape
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
Definition: CPTG_RobotShape_Circular.cpp:107
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::writeToStream
void writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:617
mrpt::nav::CPTG_RobotShape_Circular::getMaxRobotRadius
double getMaxRobotRadius() const override
Returns an approximation of the robot radius.
Definition: CPTG_RobotShape_Circular.cpp:102
mrpt::nav::CParameterizedTrajectoryGenerator::Ptr
std::shared_ptr< CParameterizedTrajectoryGenerator > Ptr
Definition: CParameterizedTrajectoryGenerator.h:82
DEFINE_VIRTUAL_SERIALIZABLE
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
Definition: CSerializable.h:171
mrpt::nav::CPTG_RobotShape_Polygonal::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_RobotShape_Polygonal.cpp:34
mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG
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...
Definition: CParameterizedTrajectoryGenerator_factory.cpp:22
mrpt::nav::COLL_BEH_BACK_AWAY
@ COLL_BEH_BACK_AWAY
Favor getting back from too-close (almost collision) obstacles.
Definition: CParameterizedTrajectoryGenerator.h:44
CPolygon.h
mrpt::nav::CParameterizedTrajectoryGenerator::isBijectiveAt
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.
Definition: CParameterizedTrajectoryGenerator.h:154
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepForDist
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 < ...
mrpt::nav::CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR
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...
Definition: CParameterizedTrajectoryGenerator.cpp:31
mrpt::nav::CPTG_RobotShape_Circular::internal_shape_saveToStream
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
Definition: CPTG_RobotShape_Circular.cpp:93
mrpt::kinematics::CVehicleVelCmd::Ptr
std::shared_ptr< CVehicleVelCmd > Ptr
Definition: CVehicleVelCmd.h:22
mrpt::math::CPolygon
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
mrpt::nav::CParameterizedTrajectoryGenerator::getClearanceStepCount
unsigned getClearanceStepCount() const
Definition: CParameterizedTrajectoryGenerator.h:381
mrpt::nav::CParameterizedTrajectoryGenerator::getRefDistance
double getRefDistance() const
Definition: CParameterizedTrajectoryGenerator.h:370
mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose
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.
mrpt::nav::CPTG_RobotShape_Polygonal::isPointInsideRobotShape
bool isPointInsideRobotShape(const double x, const double y) const override
Returns true if the point lies within the robot shape.
Definition: CPTG_RobotShape_Polygonal.cpp:149
mrpt::nav::CPTG_RobotShape_Circular::add_robotShape_to_setOfLines
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const override
Auxiliary function for rendering.
Definition: CPTG_RobotShape_Circular.cpp:46
mrpt::nav::CParameterizedTrajectoryGenerator::getPathTwist
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.
Definition: CParameterizedTrajectoryGenerator.cpp:584
CLoadableOptions.h
mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepCount
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.
mrpt::nav::CPTG_RobotShape_Circular::CPTG_RobotShape_Circular
CPTG_RobotShape_Circular()
mrpt::nav::CParameterizedTrajectoryGenerator::renderPathAsSimpleLine
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)...
Definition: CParameterizedTrajectoryGenerator.cpp:228
mrpt::nav::PTG_collision_behavior_t
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
Definition: CParameterizedTrajectoryGenerator.h:41
mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist
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.
mrpt::nav::CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
Definition: CParameterizedTrajectoryGenerator.cpp:58
mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile
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.
Definition: CParameterizedTrajectoryGenerator.cpp:101
mrpt::nav::CParameterizedTrajectoryGenerator::m_nav_dyn_state_target_k
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
Definition: CParameterizedTrajectoryGenerator.h:477
mrpt::nav::CParameterizedTrajectoryGenerator::updateClearancePost
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
Definition: CParameterizedTrajectoryGenerator.cpp:506
mrpt::nav::CParameterizedTrajectoryGenerator::isPointInsideRobotShape
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacleSingle
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.
mrpt::nav::CPTG_RobotShape_Circular::~CPTG_RobotShape_Circular
~CPTG_RobotShape_Circular() override
mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState
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...
Definition: CParameterizedTrajectoryGenerator.cpp:360
mrpt::nav::CParameterizedTrajectoryGenerator::getCurrentNavDynamicState
const TNavDynamicState & getCurrentNavDynamicState() const
Definition: CParameterizedTrajectoryGenerator.h:336
mrpt::nav::COLL_BEH_STOP
@ COLL_BEH_STOP
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
Definition: CParameterizedTrajectoryGenerator.h:47
mrpt::nav::CParameterizedTrajectoryGenerator::isInitialized
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
Definition: CParameterizedTrajectoryGenerator.cpp:355
CSerializable.h
mrpt::nav::CParameterizedTrajectoryGenerator::m_score_priority
double m_score_priority
Definition: CParameterizedTrajectoryGenerator.h:467
mrpt::nav::CPTG_RobotShape_Polygonal::internal_shape_saveToStream
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
Definition: CPTG_RobotShape_Polygonal.cpp:135
mrpt::nav::CParameterizedTrajectoryGenerator::debugDumpInFiles
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: .
Definition: CParameterizedTrajectoryGenerator.cpp:279
mrpt::nav::CPTG_RobotShape_Circular::internal_shape_loadFromStream
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
Definition: CPTG_RobotShape_Circular.cpp:77
mrpt::nav::CParameterizedTrajectoryGenerator::getMaxAngVel
virtual double getMaxAngVel() const =0
Returns the maximum angular velocity expected from this PTG [rad/s].
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:78
mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CParameterizedTrajectoryGenerator.cpp:63
mrpt::nav::CParameterizedTrajectoryGenerator::m_is_initialized
bool m_is_initialized
Definition: CParameterizedTrajectoryGenerator.h:481
mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
Definition: CParameterizedTrajectoryGenerator.h:358
mrpt::nav::CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator
CParameterizedTrajectoryGenerator()
Default ctor.
CVehicleVelCmd.h
mrpt::nav::CParameterizedTrajectoryGenerator::onNewNavDynamicState
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
mrpt::math::TTwist2D
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
mrpt::nav::CParameterizedTrajectoryGenerator::getScorePriority
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
Definition: CParameterizedTrajectoryGenerator.h:379
mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance()
Definition: CParameterizedTrajectoryGenerator.cpp:465



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020