Main MRPT website > C++ reference for MRPT 1.9.9
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
24  * CPTG_DiffDrive_CollisionGridBased */
25 struct TCPoint
26 {
27  TCPoint() {}
29  const float x_, const float y_, const float phi_, const float t_,
30  const float dist_, const float v_, const float w_)
31  : x(x_), y(y_), phi(phi_), t(t_), dist(dist_), v(v_), w(w_)
32  {
33  }
34  float x, y, phi, t, dist, v, w;
35 };
36 typedef std::vector<TCPoint> TCPointVector;
41 
42 /** Base class for all PTGs suitable to non-holonomic, differentially-driven (or
43  * Ackermann) vehicles
44  * based on numerical integration of the trajectories and collision
45  * look-up-table.
46  * Regarding `initialize()`: in this this family of PTGs, the method builds the
47  * collision grid or load it from a cache file.
48  * Collision grids must be calculated before calling getTPObstacle(). Robot
49  * shape must be set before initializing with setRobotShape().
50  * The rest of PTG parameters should have been set at the constructor.
51  */
53 {
54  public:
55  /** The main method to be implemented in derived classes: it defines the
56  * differential-driven differential equation */
57  virtual void ptgDiffDriveSteeringFunction(
58  float alpha, float t, float x, float y, float phi, float& v,
59  float& w) const = 0;
60 
61  /** @name Virtual interface of each PTG implementation
62  * @{ */
63  // getDescription(): remains to be defined in derived classes.
64  // setParams() to be defined in derived classses.
65 
66  /** The default implementation in this class relies on a look-up-table.
67  * Derived classes may redefine this to closed-form expressions, when they
68  * exist.
69  * See full docs in base class
70  * CParameterizedTrajectoryGenerator::inverseMap_WS2TP() */
71  virtual bool inverseMap_WS2TP(
72  double x, double y, int& out_k, double& out_d,
73  double tolerance_dist = 0.10) const override;
74 
75  /** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s),
76  * [1]: angular velocity (rad/s).
77  * See more docs in
78  * CParameterizedTrajectoryGenerator::directionToMotionCommand() */
80  uint16_t k) const override;
82  getSupportedKinematicVelocityCommand() const override;
83 
84  /** Launches an exception in this class: it is not allowed in numerical
85  * integration-based PTGs to change the reference distance
86  * after initialization. */
87  virtual void setRefDistance(const double refDist) override;
88 
89  // Access to PTG paths (see docs in base class)
90  size_t getPathStepCount(uint16_t k) const override;
91  void getPathPose(
92  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const override;
93  double getPathDist(uint16_t k, uint32_t step) const override;
94  bool getPathStepForDist(
95  uint16_t k, double dist, uint32_t& out_step) const override;
96  double getPathStepDuration() const override;
97  double getMaxLinVel() const override { return V_MAX; }
98  double getMaxAngVel() const override { return W_MAX; }
99  void updateTPObstacle(
100  double ox, double oy, std::vector<double>& tp_obstacles) const override;
102  double ox, double oy, uint16_t k, double& tp_obstacle_k) const override;
103 
104  /** This family of PTGs ignores the dynamic states */
105  virtual void onNewNavDynamicState() override
106  {
107  // Do nothing.
108  }
109 
110  /** @} */ // --- end of virtual methods
111 
112  double getMax_V() const { return V_MAX; }
113  double getMax_W() const { return W_MAX; }
114  protected:
116 
117  void internal_processNewRobotShape() override;
118  void internal_initialize(
119  const std::string& cacheFilename = std::string(),
120  const bool verbose = true) override;
121  void internal_deinitialize() override;
122 
123  /** Possible values in "params" (those in CParameterizedTrajectoryGenerator,
124  * which is called internally, plus):
125  * - `${sKeyPrefix}resolution`: The cell size
126  * - `${sKeyPrefix}v_max`, ``${sKeyPrefix}w_max`: Maximum robot speeds.
127  * - `${sKeyPrefix}shape_x{0,1,2..}`, ``${sKeyPrefix}shape_y{0,1,2..}`:
128  * Polygonal robot shape [Optional, can be also set via
129  * `setRobotPolygonShape()`]
130  *
131  * See docs of derived classes for additional parameters in setParams()
132  */
133  virtual void loadFromConfigFile(
134  const mrpt::utils::CConfigFileBase& cfg,
135  const std::string& sSection) override;
136  virtual void saveToConfigFile(
138  const std::string& sSection) const override;
139 
140  virtual void loadDefaultParams() override;
141 
142  double V_MAX, W_MAX;
144  std::vector<TCPointVector> m_trajectory;
145  double m_resolution;
147 
149  void internal_writeToStream(mrpt::utils::CStream& out) const override;
150 
151  /** Numerically solve the diferential equations to generate a family of
152  * trajectories */
154  float max_time, float max_dist, unsigned int max_n, float diferencial_t,
155  float min_dist, float* out_max_acc_v = nullptr,
156  float* out_max_acc_w = nullptr);
157 
158  /** A list of all the pairs (alpha,distance) such as the robot collides at
159  *that cell.
160  * - map key (uint16_t) -> alpha value (k)
161  * - map value (float) -> the MINIMUM distance (d), in meters,
162  *associated with that "k".
163  */
164  typedef std::vector<std::pair<uint16_t, float>> TCollisionCell;
165 
166  /** An internal class for storing the collision grid */
167  class CCollisionGrid : public mrpt::utils::CDynamicGrid<TCollisionCell>
168  {
169  private:
171 
172  public:
174  float x_min, float x_max, float y_min, float y_max,
175  float resolution, CPTG_DiffDrive_CollisionGridBased* parent)
176  : mrpt::utils::CDynamicGrid<TCollisionCell>(
177  x_min, x_max, y_min, y_max, resolution),
178  m_parent(parent)
179  {
180  }
181  virtual ~CCollisionGrid() {}
182  /** Save to file, true = OK */
183  bool saveToFile(
185  const mrpt::math::CPolygon& computed_robotShape) const;
186  /** Load from file, true = OK */
187  bool loadFromFile(
189  const mrpt::math::CPolygon& current_robotShape);
190 
191  /** For an obstacle (x,y), returns a vector with all the pairs (a,d)
192  * such as the robot collides */
194  const float obsX, const float obsY) const;
195 
196  /** Updates the info into a cell: It updates the cell only if the
197  *distance d for the path k is lower than the previous value:
198  * \param cellInfo The index of the cell
199  * \param k The path index (alpha discreet value)
200  * \param d The distance (in TP-Space, range 0..1) to collision.
201  */
202  void updateCellInfo(
203  const unsigned int icx, const unsigned int icy, const uint16_t k,
204  const float dist);
205 
206  }; // end of class CCollisionGrid
207 
208  // Save/Load from files.
209  bool saveColGridsToFile(
210  const std::string& filename,
211  const mrpt::math::CPolygon& computed_robotShape) const; // true = OK
213  const std::string& filename,
214  const mrpt::math::CPolygon& current_robotShape); // true = OK
215 
216  /** The collision grid */
218 
219  /** Specifies the min/max values for "k" and "n", respectively.
220  * \sa m_lambdaFunctionOptimizer
221  */
223  {
225  : k_min(std::numeric_limits<uint16_t>::max()),
226  k_max(std::numeric_limits<uint16_t>::min()),
227  n_min(std::numeric_limits<uint32_t>::max()),
228  n_max(std::numeric_limits<uint32_t>::min())
229  {
230  }
231 
234 
235  bool isEmpty() const
236  {
237  return k_min == std::numeric_limits<uint16_t>::max();
238  }
239  };
240 
241  /** This grid will contain indexes data for speeding-up the default,
242  * brute-force lambda function */
244 };
245 
246 /** @} */
247 }
248 namespace utils
249 {
250 // Specialization must occur in the same namespace
252 }
253 }
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:79
double getMaxAngVel() const override
Returns the maximum angular velocity expected from this PTG [rad/s].
virtual void setRefDistance(const double refDist) override
Launches an exception in this class: it is not allowed in numerical integration-based PTGs to change ...
GLdouble GLdouble t
Definition: glext.h:3689
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:44
bool saveColGridsToFile(const std::string &filename, const mrpt::math::CPolygon &computed_robotShape) const
bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true) override
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
Specifies the min/max values for "k" and "n", respectively.
const TCollisionCell & getTPObstacle(const float obsX, const float obsY) const
For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides...
Trajectory points in C-Space for non-holonomic robots.
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function...
void internal_readFromStream(mrpt::utils::CStream &in) override
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
mrpt::utils::CStream & operator>>(mrpt::utils::CStream &i, mrpt::nav::TCPoint &p)
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:22
STL namespace.
std::vector< TCPoint > TCPointVector
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const override
Like updateTPObstacle() but for one direction only (k) in TP-Space.
This class allows loading and storing values and vectors of different types from a configuration text...
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
In this class, out_action_cmd contains: [0]: linear velocity (m/s), [1]: angular velocity (rad/s)...
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:41
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const override
The default implementation in this class relies on a look-up-table.
double getMaxLinVel() const override
Returns the maximum linear velocity expected from this PTG [m/s].
double getPathDist(uint16_t k, uint32_t step) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual void onNewNavDynamicState() override
This family of PTGs ignores the dynamic states.
Base class for all PTGs using a 2D polygonal robot shape model.
CPTG_DiffDrive_CollisionGridBased()
Constructor: possible values in "params":
bool loadColGridsFromFile(const std::string &filename, const mrpt::math::CPolygon &current_robotShape)
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell. ...
CDynamicGrid(double x_min=-10.0, double x_max=10.0, double y_min=-10.0f, double y_max=10.0f, double resolution=0.10f)
Constructor.
Definition: CDynamicGrid.h:57
GLsizei const GLchar ** string
Definition: glext.h:4101
void updateCellInfo(const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist)
Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than...
size_t getPathStepCount(uint16_t k) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
const GLdouble * v
Definition: glext.h:3678
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool loadFromFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &current_robotShape)
Load from file, true = OK.
bool saveToFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &computed_robotShape) const
Save to file, true = OK.
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
void internal_writeToStream(mrpt::utils::CStream &out) const override
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
GLenum GLint GLint y
Definition: glext.h:3538
CCollisionGrid(float x_min, float x_max, float y_min, float y_max, float resolution, CPTG_DiffDrive_CollisionGridBased *parent)
void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const override
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
virtual void ptgDiffDriveSteeringFunction(float alpha, float t, float x, float y, float phi, float &v, float &w) const =0
The main method to be implemented in derived classes: it defines the differential-driven differential...
mrpt::utils::CStream & operator<<(mrpt::utils::CStream &o, const mrpt::nav::TCPoint &p)
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3538
std::shared_ptr< CVehicleVelCmd > Ptr
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLfloat GLfloat p
Definition: glext.h:6305
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
void simulateTrajectories(float max_time, float max_dist, unsigned int max_n, float diferencial_t, float min_dist, float *out_max_acc_v=nullptr, float *out_max_acc_w=nullptr)
Numerically solve the diferential equations to generate a family of trajectories. ...
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019