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 #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 */
291  virtual void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D ()) const = 0;
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 }
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: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019