Main MRPT website > C++ reference for MRPT 1.5.6
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 */
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;
110  double turningRadiusReference;
111  std::vector<TCPointVector> m_trajectory;
112  double m_resolution;
113  double m_stepTimeDuration;
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
196  MRPT_DECLARE_TTYPENAME_NAMESPACE(TCPoint,mrpt::nav)
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
std::vector< TCPoint > TCPointVector
GLuint in
Definition: glew.h:7146
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. ...
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018