Main MRPT website > C++ reference for MRPT 1.5.7
CPTG_DiffDrive_CollisionGridBased.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 
13 #include <mrpt/math/CPolygon.h>
14 #include <mrpt/utils/TEnumType.h>
15 
16 namespace mrpt
17 {
18 namespace nav
19 {
20  /** \addtogroup nav_tpspace
21  * @{ */
22 
23  /** Trajectory points in C-Space for non-holonomic robots \sa CPTG_DiffDrive_CollisionGridBased */
25  {
26  TCPoint() {}
27  TCPoint(const float x_,const float y_,const float phi_,
28  const float t_,const float dist_,
29  const float v_,const float w_) :
30  x(x_), y(y_), phi(phi_), t(t_), dist(dist_), v(v_), w(w_)
31  {}
32  float x, y, phi,t, dist,v,w;
33  };
34  typedef std::vector<TCPoint> TCPointVector;
37 
38  /** Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles
39  * based on numerical integration of the trajectories and collision look-up-table.
40  * Regarding `initialize()`: in this this family of PTGs, the method builds the collision grid or load it from a cache file.
41  * Collision grids must be calculated before calling getTPObstacle(). Robot shape must be set before initializing with setRobotShape().
42  * The rest of PTG parameters should have been set at the constructor.
43  */
45  {
46  public:
47  /** The main method to be implemented in derived classes: it defines the differential-driven differential equation */
48  virtual void ptgDiffDriveSteeringFunction( float alpha, float t, float x, float y, float phi, float &v, float &w) const = 0;
49 
50  /** @name Virtual interface of each PTG implementation
51  * @{ */
52  // getDescription(): remains to be defined in derived classes.
53  // setParams() to be defined in derived classses.
54 
55  /** The default implementation in this class relies on a look-up-table. Derived classes may redefine this to closed-form expressions, when they exist.
56  * See full docs in base class CParameterizedTrajectoryGenerator::inverseMap_WS2TP() */
57  virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist = 0.10) const MRPT_OVERRIDE;
58 
59  /** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s).
60  * See more docs in CParameterizedTrajectoryGenerator::directionToMotionCommand() */
61  virtual mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand( uint16_t k) const MRPT_OVERRIDE;
62  virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand() const MRPT_OVERRIDE;
63 
64  /** Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change the reference distance
65  * after initialization. */
66  virtual void setRefDistance(const double refDist) MRPT_OVERRIDE;
67 
68  // Access to PTG paths (see docs in base class)
69  size_t getPathStepCount(uint16_t k) const MRPT_OVERRIDE;
70  void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const MRPT_OVERRIDE;
71  double getPathDist(uint16_t k, uint32_t step) const MRPT_OVERRIDE;
72  bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const MRPT_OVERRIDE;
73  double getPathStepDuration() const MRPT_OVERRIDE;
74  double getMaxLinVel() const MRPT_OVERRIDE { return V_MAX; }
75  double getMaxAngVel() const MRPT_OVERRIDE { return W_MAX; }
76 
77  void updateTPObstacle(double ox, double oy, std::vector<double> &tp_obstacles) const MRPT_OVERRIDE;
78  void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const MRPT_OVERRIDE;
79 
80  /** This family of PTGs ignores the dynamic states */
81  virtual void onNewNavDynamicState() MRPT_OVERRIDE {
82  // Do nothing.
83  }
84 
85  /** @} */ // --- end of virtual methods
86 
87  double getMax_V() const { return V_MAX; }
88  double getMax_W() const { return W_MAX; }
89 
90 protected:
92 
93  void internal_processNewRobotShape() MRPT_OVERRIDE;
94  void internal_initialize(const std::string & cacheFilename = std::string(), const bool verbose = true) MRPT_OVERRIDE;
95  void internal_deinitialize() MRPT_OVERRIDE;
96 
97  /** Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally, plus):
98  * - `${sKeyPrefix}resolution`: The cell size
99  * - `${sKeyPrefix}v_max`, ``${sKeyPrefix}w_max`: Maximum robot speeds.
100  * - `${sKeyPrefix}shape_x{0,1,2..}`, ``${sKeyPrefix}shape_y{0,1,2..}`: Polygonal robot shape [Optional, can be also set via `setRobotPolygonShape()`]
101  *
102  * See docs of derived classes for additional parameters in setParams()
103  */
104  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) MRPT_OVERRIDE;
105  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const MRPT_OVERRIDE;
106 
107  virtual void loadDefaultParams() MRPT_OVERRIDE;
108 
109  double V_MAX, W_MAX;
111  std::vector<TCPointVector> m_trajectory;
112  double m_resolution;
114 
115  void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE;
116  void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE;
117 
118  /** Numerically solve the diferential equations to generate a family of trajectories */
119  void simulateTrajectories(
120  float max_time,
121  float max_dist,
122  unsigned int max_n,
123  float diferencial_t,
124  float min_dist,
125  float *out_max_acc_v = NULL,
126  float *out_max_acc_w = NULL);
127 
128  /** A list of all the pairs (alpha,distance) such as the robot collides at that cell.
129  * - map key (uint16_t) -> alpha value (k)
130  * - map value (float) -> the MINIMUM distance (d), in meters, associated with that "k".
131  */
132  typedef std::vector<std::pair<uint16_t,float> > TCollisionCell;
133 
134  /** An internal class for storing the collision grid */
135  class NAV_IMPEXP CCollisionGrid : public mrpt::utils::CDynamicGrid<TCollisionCell>
136  {
137  private:
139 
140  public:
141  CCollisionGrid(float x_min, float x_max,float y_min, float y_max, float resolution, CPTG_DiffDrive_CollisionGridBased* parent )
142  : mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution),
143  m_parent(parent)
144  {
145  }
146  virtual ~CCollisionGrid() { }
147 
148  bool saveToFile( mrpt::utils::CStream* fil, const mrpt::math::CPolygon & computed_robotShape ) const; //!< Save to file, true = OK
149  bool loadFromFile( mrpt::utils::CStream* fil, const mrpt::math::CPolygon & current_robotShape ); //!< Load from file, true = OK
150 
151  /** For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides */
152  const TCollisionCell & getTPObstacle( const float obsX, const float obsY) const;
153 
154  /** Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than the previous value:
155  * \param cellInfo The index of the cell
156  * \param k The path index (alpha discreet value)
157  * \param d The distance (in TP-Space, range 0..1) to collision.
158  */
159  void updateCellInfo( const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist );
160 
161  }; // end of class CCollisionGrid
162 
163  // Save/Load from files.
164  bool saveColGridsToFile( const std::string &filename, const mrpt::math::CPolygon & computed_robotShape ) const; // true = OK
165  bool loadColGridsFromFile( const std::string &filename, const mrpt::math::CPolygon & current_robotShape ); // true = OK
166 
167  CCollisionGrid m_collisionGrid; //!< The collision grid
168 
169  /** Specifies the min/max values for "k" and "n", respectively.
170  * \sa m_lambdaFunctionOptimizer
171  */
173  {
175  k_min( std::numeric_limits<uint16_t>::max() ),
176  k_max( std::numeric_limits<uint16_t>::min() ),
177  n_min( std::numeric_limits<uint32_t>::max() ),
178  n_max( std::numeric_limits<uint32_t>::min() )
179  {}
180 
183 
184  bool isEmpty() const { return k_min==std::numeric_limits<uint16_t>::max(); }
185  };
186 
187  /** This grid will contain indexes data for speeding-up the default, brute-force lambda function */
189  };
190 
191  /** @} */
192 }
193  namespace utils
194  {
195  // Specialization must occur in the same namespace
197  }
198 }
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
Specifies the min/max values for "k" and "n", respectively.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
GLclampf GLclampf GLclampf alpha
Definition: glew.h:1425
Trajectory points in C-Space for non-holonomic robots.
double getMaxAngVel() const MRPT_OVERRIDE
Returns the maximum angular velocity expected from this PTG [rad/s].
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
GLdouble GLdouble t
Definition: glew.h:1303
double getMaxLinVel() const MRPT_OVERRIDE
Returns the maximum linear velocity expected from this PTG [m/s].
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
STL namespace.
std::vector< TCPoint > TCPointVector
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
TCPoint(const float x_, const float y_, const float phi_, const float t_, const float dist_, const float v_, const float w_)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
virtual void onNewNavDynamicState() MRPT_OVERRIDE
This family of PTGs ignores the dynamic states.
Base class for all PTGs using a 2D polygonal robot shape model.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
GLfloat GLfloat p
Definition: glew.h:10113
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell. ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Lightweight 2D pose.
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CAbstractHolonomicReactiveMethodPtr &pObj)
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
CCollisionGrid(float x_min, float x_max, float y_min, float y_max, float resolution, CPTG_DiffDrive_CollisionGridBased *parent)
mrpt::utils::CStream NAV_IMPEXP & operator<<(mrpt::utils::CStream &o, const mrpt::nav::TCPoint &p)
unsigned __int32 uint32_t
Definition: rptypes.h:49



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018