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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019