Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/round.h>
16 #include <mrpt/math/CPolygon.h>
17 #include <mrpt/utils/mrpt_stdint.h> // compiler-independent version of "stdint.h"
18 #include <mrpt/nav/link_pragmas.h>
20 #include <mrpt/poses/CPose2D.h>
22 #include <mrpt/otherlibs/stlplus/smart_ptr.hpp> // STL+ library
23 
24 namespace mrpt { namespace opengl { class CSetOfLines; } }
25 
26 namespace mrpt
27 {
28 namespace nav
29 {
30 
31  /** Defines behaviors for where there is an obstacle *inside* the robot shape right at the beginning of a PTG trajectory.
32  *\ingroup nav_tpspace
33  * \sa Used in CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR
34  */
36  COLL_BEH_BACK_AWAY = 0, //!< Favor getting back from too-close (almost collision) obstacles.
37  COLL_BEH_STOP //!< Totally dissallow any movement if there is any too-close (almost collision) obstacles.
38  };
39 
40  /** \defgroup nav_tpspace TP-Space and PTG classes
41  * \ingroup mrpt_nav_grp
42  */
44 
45  /** This is the base class for any user-defined PTG.
46  * There is a class factory interface in CParameterizedTrajectoryGenerator::CreatePTG.
47  *
48  * Papers:
49  * - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Extending Obstacle Avoidance Methods through Multiple Parameter-Space Transformations", Autonomous Robots, vol. 24, no. 1, 2008. http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf
50  *
51  * Changes history:
52  * - 30/JUN/2004: Creation (JLBC)
53  * - 16/SEP/2004: Totally redesigned.
54  * - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
55  * - 19/JUL/2009: Simplified to use only STL data types, and created the class factory interface.
56  * - MAY/2016: Refactored into CParameterizedTrajectoryGenerator, CPTG_DiffDrive_CollisionGridBased, PTG classes renamed.
57  * - 2016-2017: Many features added to support "PTG continuation", dynamic paths depending on vehicle speeds, etc.
58  *
59  * \ingroup nav_tpspace
60  */
62  public mrpt::utils::CSerializable,
63  public mrpt::utils::CLoadableOptions
64  {
66  public:
67  CParameterizedTrajectoryGenerator(); //!< Default ctor. Must call `loadFromConfigFile()` before initialization
68  virtual ~CParameterizedTrajectoryGenerator() //!< Destructor
69  { }
70 
71  /** The class factory for creating a PTG from a list of parameters in a section of a given config file (physical file or in memory).
72  * Possible parameters are:
73  * - Those explained in CParameterizedTrajectoryGenerator::loadFromConfigFile()
74  * - Those explained in the specific PTG being created (see list of derived classes)
75  *
76  * `ptgClassName` can be any PTG class name which has been registered as any other
77  * mrpt::utils::CSerializable class.
78  *
79  * \exception std::logic_error On invalid or missing parameters.
80  */
81  static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection, const std::string &sKeyPrefix);
82 
83  /** @name Virtual interface of each PTG implementation
84  * @{ */
85  virtual std::string getDescription() const = 0 ; //!< Gets a short textual description of the PTG and its parameters
86 
87  protected:
88  /** Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
89  virtual void internal_initialize(const std::string & cacheFilename = std::string(), const bool verbose = true) = 0;
90  /** This must be called to de-initialize the PTG if some parameter is to be changed. After changing it, call initialize again */
91  virtual void internal_deinitialize() = 0;
92  public:
93 
94  /** Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS)
95  * Cartesian coordinates (x,y), relative to the current robot frame.
96  * \param[in] x X coordinate of the query point, relative to the robot frame.
97  * \param[in] y Y coordinate of the query point, relative to the robot frame.
98  * \param[out] out_k Trajectory parameter index (discretized alpha value, 0-based index).
99  * \param[out] out_d Trajectory distance, normalized such that D_max becomes 1.
100  *
101  * \return true if the distance between (x,y) and the actual trajectory point is below the given tolerance (in meters).
102  */
103  virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist = 0.10) const = 0;
104 
105  /** Returns the same than inverseMap_WS2TP() but without any additional cost. The default implementation
106  * just calls inverseMap_WS2TP() and discards (k,d). */
107  virtual bool PTG_IsIntoDomain(double x, double y ) const {
108  int k; double d;
109  return inverseMap_WS2TP(x,y,k,d);
110  }
111 
112  /** Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa. Default implementation returns `true`. */
113  virtual bool isBijectiveAt(uint16_t k, uint32_t step) const { return true; }
114 
115  /** Converts a discretized "alpha" value into a feasible motion command or action. See derived classes for the meaning of these actions */
116  virtual mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand( uint16_t k ) const = 0;
117 
118  /** Returns an empty kinematic velocity command object of the type supported by this PTG.
119  * Can be queried to determine the expected kinematic interface of the PTG. */
120  virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand() const = 0;
121 
122  /** Dynamic state that may affect the PTG path parameterization. \ingroup nav_reactive */
124  {
125  mrpt::math::TTwist2D curVelLocal; //!< Current vehicle velocity (local frame of reference)
126  mrpt::math::TPose2D relTarget; //!< Current relative target location
127  double targetRelSpeed; //!< Desired relative speed [0,1] at target. Default=0
128 
130  bool operator ==(const TNavDynamicState& o) const;
131  inline bool operator !=(const TNavDynamicState& o) const { return !(*this==o); }
132  void writeToStream(mrpt::utils::CStream &out) const;
133  void readFromStream(mrpt::utils::CStream &in);
134  };
135 
136  protected:
137  /** Invoked when `m_nav_dyn_state` has changed; gives the PTG the opportunity to react and parameterize paths depending on the instantaneous conditions */
138  virtual void onNewNavDynamicState() = 0;
139 
140  public:
141  virtual void setRefDistance(const double refDist) { refDistance=refDist; }
142 
143  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
144  * May be actual steps from a numerical integration or an arbitrary small length for analytical PTGs.
145  * \sa getAlphaValuesCount() */
146  virtual size_t getPathStepCount(uint16_t k) const = 0;
147 
148  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step `step`.
149  * \sa getPathStepCount(), getAlphaValuesCount() */
150  virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const = 0;
151 
152  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step `step`.
153  * \return Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])
154  * \sa getPathStepCount(), getAlphaValuesCount() */
155  virtual double getPathDist(uint16_t k, uint32_t step) const = 0;
156 
157  /** Returns the duration (in seconds) of each "step"
158  * \sa getPathStepCount() */
159  virtual double getPathStepDuration() const = 0;
160 
161  /** Returns the maximum linear velocity expected from this PTG [m/s] */
162  virtual double getMaxLinVel() const = 0;
163  /** Returns the maximum angular velocity expected from this PTG [rad/s] */
164  virtual double getMaxAngVel() const = 0;
165 
166  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < `dist`
167  * \param[in] dist Distance in pseudometers (real distance, NOT normalized to [0,1] for [0,refDist])
168  * \return false if no step fulfills the condition for the given trajectory `k` (e.g. out of reference distance).
169  * Note that, anyway, the maximum distance (closest point) is returned in `out_step`.
170  * \sa getPathStepCount(), getAlphaValuesCount() */
171  virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const = 0;
172 
173  /** Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
174  * \param [in,out] tp_obstacles A vector of length `getAlphaValuesCount()`, initialized with `initTPObstacles()` (collision-free ranges, in "pseudometers", un-normalized).
175  * \param [in] ox Obstacle point (X), relative coordinates wrt origin of the PTG.
176  * \param [in] oy Obstacle point (Y), relative coordinates wrt origin of the PTG.
177  * \note The length of tp_obstacles is not checked for efficiency since this method is potentially called thousands of times per
178  * navigation timestap, so it is left to the user responsibility to provide a valid buffer.
179  * \note `tp_obstacles` must be initialized with initTPObstacle() before call.
180  */
181  virtual void updateTPObstacle(double ox, double oy, std::vector<double> &tp_obstacles) const = 0;
182 
183  /** Like updateTPObstacle() but for one direction only (`k`) in TP-Space. `tp_obstacle_k` must be initialized with initTPObstacleSingle() before call (collision-free ranges, in "pseudometers", un-normalized). */
184  virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const = 0;
185 
186  /** Loads a set of default parameters into the PTG. Users normally will call `loadFromConfigFile()` instead, this method is provided
187  * exclusively for the PTG-configurator tool. */
188  virtual void loadDefaultParams();
189 
190  /** Returns true if it is possible to stop sending velocity commands to the robot and, still, the
191  * robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands).
192  * Default implementation returns "false". */
193  virtual bool supportVelCmdNOP() const;
194 
195  /** Returns true if this PTG takes into account the desired velocity at target. \sa updateNavDynamicState() */
196  virtual bool supportSpeedAtTarget() const {
197  return false;
198  }
199 
200  /** Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path
201  * can be followed without re-issuing a new velcmd. Note that this is only an absolute maximum duration,
202  * navigation implementations will check for many other conditions. Default method in the base virtual class returns 0.
203  * \param path_k Queried path `k` index [0,N-1] */
204  virtual double maxTimeInVelCmdNOP(int path_k) const;
205 
206  /** Returns the actual distance (in meters) of the path, discounting possible circular loops of the path (e.g. if it comes back to the origin).
207  * Default: refDistance */
208  virtual double getActualUnloopedPathLength(uint16_t k) const { return this->refDistance; }
209 
210  /** Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others, if the k-th path is to be selected. */
211  virtual double evalPathRelativePriority(uint16_t k, double target_distance) const { return 1.0; }
212 
213  /** Returns an approximation of the robot radius. */
214  virtual double getMaxRobotRadius() const = 0;
215  /** Returns true if the point lies within the robot shape. */
216  virtual bool isPointInsideRobotShape(const double x, const double y) const = 0;
217 
218  /** Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center. Zero or negative means collision. */
219  virtual double evalClearanceToRobotShape(const double ox, const double oy) const = 0;
220 
221  /** @} */ // --- end of virtual methods
222 
223  /** To be invoked by the navigator *before* each navigation step, to let the PTG to react to changing dynamic conditions. * \sa onNewNavDynamicState(), m_nav_dyn_state */
224  void updateNavDynamicState(const TNavDynamicState &newState, const bool force_update = false);
225  const TNavDynamicState & getCurrentNavDynamicState() const { return m_nav_dyn_state; }
226 
227  static std::string OUTPUT_DEBUG_PATH_PREFIX; //!< The path used as defaul output in, for example, debugDumpInFiles. (Default="./reactivenav.logs/")
228 
229  /** Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
230  void initialize(const std::string & cacheFilename = std::string(), const bool verbose = true);
231  /** This must be called to de-initialize the PTG if some parameter is to be changed. After changing it, call initialize again */
232  void deinitialize();
233  /** Returns true if `initialize()` has been called and there was no errors, so the PTG is ready to be queried for paths, obstacles, etc. */
234  bool isInitialized() const;
235 
236  /** Get the number of different, discrete paths in this family */
237  uint16_t getAlphaValuesCount() const { return m_alphaValuesCount; }
238  /** Get the number of different, discrete paths in this family */
239  uint16_t getPathCount() const { return m_alphaValuesCount; }
240 
241  /** Alpha value for the discrete corresponding value \sa alpha2index */
242  double index2alpha(uint16_t k) const;
243  static double index2alpha(uint16_t k, const unsigned int num_paths);
244 
245  /** Discrete index value for the corresponding alpha value \sa index2alpha */
246  uint16_t alpha2index( double alpha ) const;
247  static uint16_t alpha2index(double alpha, const unsigned int num_paths);
248 
249  inline double getRefDistance() const { return refDistance; }
250 
251  /** Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ranges, in "pseudometers", un-normalized). \sa updateTPObstacle() */
252  void initTPObstacles(std::vector<double> &TP_Obstacles) const;
253  void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const;
254 
255  /** When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. Assign values <1 to PTGs with low priority. */
256  double getScorePriority() const { return m_score_priority; }
257  void setScorePriorty(double prior) { m_score_priority = prior; }
258 
259  unsigned getClearanceStepCount() const { return m_clearance_num_points; }
260  void setClearanceStepCount(const unsigned res) { m_clearance_num_points = res; }
261 
262  unsigned getClearanceDecimatedPaths() const { return m_clearance_decimated_paths; }
263  void setClearanceDecimatedPaths(const unsigned num) { m_clearance_decimated_paths=num; }
264 
265  /** Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line).
266  * \param[in] k The 0-based index of the selected trajectory (discrete "alpha" parameter).
267  * \param[out] gl_obj Output object.
268  * \param[in] decimate_distance Minimum distance between path points (in meters).
269  * \param[in] max_path_distance If >=0, cut the path at this distance (in meters).
270  */
271  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;
272 
273  /** Dump PTG trajectories in four text files: `./reactivenav.logs/PTGs/PTG%i_{x,y,phi,d}.txt`
274  * Text files are loadable from MATLAB/Octave, and can be visualized with the script `[MRPT_DIR]/scripts/viewPTG.m`
275  * \note The directory "./reactivenav.logs/PTGs" will be created if doesn't exist.
276  * \return false on any error writing to disk.
277  * \sa OUTPUT_DEBUG_PATH_PREFIX
278  */
279  bool debugDumpInFiles(const std::string &ptg_name) const;
280 
281  /** Parameters accepted by this base class:
282  * - `${sKeyPrefix}num_paths`: The number of different paths in this family (number of discrete `alpha` values).
283  * - `${sKeyPrefix}ref_distance`: The maximum distance in PTGs [meters]
284  * - `${sKeyPrefix}score_priority`: When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. Assign values <1 to PTGs with low priority.
285  */
286  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) MRPT_OVERRIDE;
287  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const MRPT_OVERRIDE;
288 
289 
290  /** Auxiliary function for rendering */
292 
293  /** Defines the behavior when there is an obstacle *inside* the robot shape right at the beginning of a PTG trajectory.
294  * Default value: COLL_BEH_BACK_AWAY
295  */
297 
298  void initClearanceDiagram(ClearanceDiagram & cd) const; //!< Must be called to resize a CD to its correct size, before calling updateClearance()
299 
300  /** Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative
301  * to the PTG path origin.
302  * \param[in,out] cd The clearance will be updated here.
303  * \sa m_clearance_dist_resolution
304  */
305  void updateClearance(const double ox, const double oy, ClearanceDiagram & cd) const;
306  void updateClearancePost(ClearanceDiagram & cd, const std::vector<double> &TP_obstacles) const;
307 
308 protected:
309  double refDistance;
310  uint16_t m_alphaValuesCount; //!< The number of discrete values for "alpha" between -PI and +PI.
312  uint16_t m_clearance_num_points; //!< Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,1] (Default=5) \sa updateClearance()
313  uint16_t m_clearance_decimated_paths; //!< Number of paths for the decimated paths analysis of clearance
314  TNavDynamicState m_nav_dyn_state; //!< Updated before each nav step by
315  uint16_t m_nav_dyn_state_target_k; //!< Update in updateNavDynamicState(), contains the path index (k) for the target.
316 
317  static const uint16_t INVALID_PTG_PATH_INDEX = static_cast<uint16_t>(-1);
318 
320 
321  /** To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to
322  * honor the user settings regarding COLLISION_BEHAVIOR.
323  * \param new_tp_obs_dist The newly determiend collision-free ranges, in "pseudometers", un-normalized, for some "k" direction.
324  * \param inout_tp_obs The target where to store the new TP-Obs distance, if it fulfills the criteria determined by the collision behavior.
325  */
326  void internal_TPObsDistancePostprocess(const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const;
327 
328  virtual void internal_readFromStream(mrpt::utils::CStream &in);
329  virtual void internal_writeToStream(mrpt::utils::CStream &out) const;
330 
331  public:
332  /** Evals the robot clearance for each robot pose along path `k`, for the real distances in
333  * the key of the map<>, then keep in the map value the minimum of its current stored clearance,
334  * or the computed clearance. In case of collision, clearance is zero.
335  * \param treat_as_obstacle true: normal use for obstacles; false: compute shortest distances to a target point (no collision)
336  */
337  virtual void evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t & inout_realdist2clearance, bool treat_as_obstacle = true) const;
338 
339  }; // end of class
341 
342 
343  typedef std::vector<mrpt::nav::CParameterizedTrajectoryGenerator*> TListPTGs; //!< A list of PTGs (bare pointers)
344  typedef std::vector<mrpt::nav::CParameterizedTrajectoryGeneratorPtr> TListPTGPtr; //!< A list of PTGs (smart pointers)
345 
346 
347  /** Base class for all PTGs using a 2D polygonal robot shape model.
348  * \ingroup nav_tpspace
349  */
351  {
352  public:
354  virtual ~CPTG_RobotShape_Polygonal();
355 
356  /** @name Robot shape
357  * @{ **/
358  /** Robot shape must be set before initialization, either from ctor params or via this method. */
359  void setRobotShape(const mrpt::math::CPolygon & robotShape);
360  const mrpt::math::CPolygon & getRobotShape() const { return m_robotShape; }
361  double getMaxRobotRadius() const MRPT_OVERRIDE;
362  virtual double evalClearanceToRobotShape(const double ox, const double oy) const MRPT_OVERRIDE;
363  /** @} */
364  bool isPointInsideRobotShape(const double x, const double y) const MRPT_OVERRIDE;
365  void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D ()) const MRPT_OVERRIDE;
366  protected:
367  virtual void internal_processNewRobotShape() = 0; //!< Will be called whenever the robot shape is set / updated
368  mrpt::math::CPolygon m_robotShape;
369  double m_robotMaxRadius;
370  void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase & source,const std::string & section);
371  void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const MRPT_OVERRIDE;
372  void internal_shape_loadFromStream(mrpt::utils::CStream &in);
373  void internal_shape_saveToStream(mrpt::utils::CStream &out) const;
374  /** Loads a set of default parameters; provided exclusively for the PTG-configurator tool. */
375  void loadDefaultParams() MRPT_OVERRIDE;
376  };
377 
378  /** Base class for all PTGs using a 2D circular robot shape model.
379  * \ingroup nav_tpspace
380  */
382  {
383  public:
385  virtual ~CPTG_RobotShape_Circular();
386 
387  /** @name Robot shape
388  * @{ **/
389  /** Robot shape must be set before initialization, either from ctor params or via this method. */
390  void setRobotShapeRadius(const double robot_radius);
391  double getRobotShapeRadius() const { return m_robotRadius; }
392  double getMaxRobotRadius() const MRPT_OVERRIDE;
393  virtual double evalClearanceToRobotShape(const double ox, const double oy) const MRPT_OVERRIDE;
394  /** @} */
395  void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D ()) const MRPT_OVERRIDE;
396  bool isPointInsideRobotShape(const double x, const double y) const MRPT_OVERRIDE;
397  protected:
398  virtual void internal_processNewRobotShape() = 0; //!< Will be called whenever the robot shape is set / updated
399  double m_robotRadius;
400  void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase & source,const std::string & section);
401  void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const MRPT_OVERRIDE;
402  void internal_shape_loadFromStream(mrpt::utils::CStream &in);
403  void internal_shape_saveToStream(mrpt::utils::CStream &out) const;
404  /** Loads a set of default parameters; provided exclusively for the PTG-configurator tool. */
405  void loadDefaultParams() MRPT_OVERRIDE;
406  };
407 
408 }
409 }
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:26
Base class for all PTGs using a 2D circular robot shape model.
Base class for all PTGs using a 2D polygonal robot shape model.
const mrpt::math::CPolygon & getRobotShape() const
This is the base class for any user-defined PTG.
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
virtual double getMaxLinVel() const =0
Returns the maximum linear velocity expected from this PTG [m/s].
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.
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...
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
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...
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.
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0,...
static std::string OUTPUT_DEBUG_PATH_PREFIX
The path used as defaul output in, for example, debugDumpInFiles. (Default="./reactivenav....
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.
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
virtual void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const =0
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,...
virtual double getActualUnloopedPathLength(uint16_t k) const
Returns the actual distance (in meters) of the path, discounting possible circular loops of the path ...
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
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 internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
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 isBijectiveAt(uint16_t k, uint32_t step) const
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.
virtual bool PTG_IsIntoDomain(double x, double y) const
Returns the same than inverseMap_WS2TP() but without any additional cost.
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 mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
virtual double evalClearanceToRobotShape(const double ox, const double oy) const =0
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center.
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const =0
Like updateTPObstacle() but for one direction only (k) in TP-Space.
virtual double evalPathRelativePriority(uint16_t k, double target_distance) const
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others,...
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
virtual double getMaxAngVel() const =0
Returns the maximum angular velocity expected from this PTG [rad/s].
Clearance information for one particular PTG and one set of obstacles.
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose *‍/
A set of independent lines (or segments), one line with its own start and end positions (X,...
Definition: CSetOfLines.h:36
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:37
This class allows loading and storing values and vectors of different types from a configuration text...
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
GLuint res
Definition: glext.h:6298
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
GLenum GLint GLint y
Definition: glext.h:3516
GLuint in
Definition: glext.h:6301
GLenum GLint x
Definition: glext.h:3516
GLfloat GLfloat p
Definition: glext.h:5587
GLuint GLuint num
Definition: glext.h:6303
GLsizei const GLchar ** string
Definition: glext.h:3919
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
bool operator!=(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
@ COLL_BEH_STOP
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
@ COLL_BEH_BACK_AWAY
Favor getting back from too-close (almost collision) obstacles.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:58
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator * > TListPTGs
A list of PTGs (bare pointers)
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
bool NAV_IMPEXP operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned __int16 uint16_t
Definition: rptypes.h:46
unsigned __int32 uint32_t
Definition: rptypes.h:49
Lightweight 2D pose.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Dynamic state that may affect the PTG path parameterization.
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
double targetRelSpeed
Desired relative speed [0,1] at target. Default=0.



Page generated by Doxygen 1.9.1 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at mar 26 may 2026 13:06:43 CEST