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-2018, 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>
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 using TCPointVector = std::vector<TCPoint>;
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(
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 
150  mrpt::serialization::CArchive& out) const override;
151 
152  /** Numerically solve the diferential equations to generate a family of
153  * trajectories */
155  float max_time, float max_dist, unsigned int max_n, float diferencial_t,
156  float min_dist, float* out_max_acc_v = nullptr,
157  float* out_max_acc_w = nullptr);
158 
159  /** A list of all the pairs (alpha,distance) such as the robot collides at
160  *that cell.
161  * - map key (uint16_t) -> alpha value (k)
162  * - map value (float) -> the MINIMUM distance (d), in meters,
163  *associated with that "k".
164  */
165  using TCollisionCell = std::vector<std::pair<uint16_t, float>>;
166 
167  /** An internal class for storing the collision grid */
168  class CCollisionGrid : public mrpt::containers::CDynamicGrid<TCollisionCell>
169  {
170  private:
172 
173  public:
175  float x_min, float x_max, float y_min, float y_max,
176  float resolution, CPTG_DiffDrive_CollisionGridBased* parent)
177  : mrpt::containers::CDynamicGrid<TCollisionCell>(
178  x_min, x_max, y_min, y_max, resolution),
179  m_parent(parent)
180  {
181  }
182  virtual ~CCollisionGrid() {}
183  /** Save to file, true = OK */
184  bool saveToFile(
186  const mrpt::math::CPolygon& computed_robotShape) const;
187  /** Load from file, true = OK */
188  bool loadFromFile(
190  const mrpt::math::CPolygon& current_robotShape);
191 
192  /** For an obstacle (x,y), returns a vector with all the pairs (a,d)
193  * such as the robot collides */
195  const float obsX, const float obsY) const;
196 
197  /** Updates the info into a cell: It updates the cell only if the
198  *distance d for the path k is lower than the previous value:
199  * \param cellInfo The index of the cell
200  * \param k The path index (alpha discreet value)
201  * \param d The distance (in TP-Space, range 0..1) to collision.
202  */
203  void updateCellInfo(
204  const unsigned int icx, const unsigned int icy, const uint16_t k,
205  const float dist);
206 
207  }; // end of class CCollisionGrid
208 
209  // Save/Load from files.
210  bool saveColGridsToFile(
211  const std::string& filename,
212  const mrpt::math::CPolygon& computed_robotShape) const; // true = OK
214  const std::string& filename,
215  const mrpt::math::CPolygon& current_robotShape); // true = OK
216 
217  /** The collision grid */
219 
220  /** Specifies the min/max values for "k" and "n", respectively.
221  * \sa m_lambdaFunctionOptimizer
222  */
224  {
226  : k_min(std::numeric_limits<uint16_t>::max()),
227  k_max(std::numeric_limits<uint16_t>::min()),
228  n_min(std::numeric_limits<uint32_t>::max()),
229  n_max(std::numeric_limits<uint32_t>::min())
230  {
231  }
232 
235 
236  bool isEmpty() const
237  {
238  return k_min == std::numeric_limits<uint16_t>::max();
239  }
240  };
241 
242  /** This grid will contain indexes data for speeding-up the default,
243  * brute-force lambda function */
246 };
247 
248 /** @} */
249 } // namespace nav
250 namespace typemeta
251 {
252 // Specialization must occur in the same namespace
254 } // namespace typemeta
255 } // namespace mrpt
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Declares a typename to be "namespace::type".
Definition: TTypeName.h:115
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 ...
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &i, mrpt::nav::TCPoint &p)
GLdouble GLdouble t
Definition: glext.h:3689
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
#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.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
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.
bool loadFromFile(mrpt::serialization::CArchive *fil, const mrpt::math::CPolygon &current_robotShape)
Load from file, true = OK.
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
void internal_readFromStream(mrpt::serialization::CArchive &in) override
STL namespace.
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.
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_)
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &o, const mrpt::nav::TCPoint &p)
This class allows loading and storing values and vectors of different types from a configuration text...
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)
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 saveToFile(mrpt::serialization::CArchive *fil, const mrpt::math::CPolygon &computed_robotShape) const
Save to file, true = OK.
mrpt::containers::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function...
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:55
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
std::vector< TCPoint > TCPointVector
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
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)
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell. ...
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...
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3538
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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 internal_writeToStream(mrpt::serialization::CArchive &out) const override
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020