MRPT  1.9.9
CPTG_DiffDrive_CollisionGridBased.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #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() = default;
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  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  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  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  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 
115  protected:
117 
118  void internal_processNewRobotShape() override;
119  void internal_initialize(
120  const std::string& cacheFilename = std::string(),
121  const bool verbose = true) override;
122  void internal_deinitialize() override;
123 
124  /** Possible values in "params" (those in CParameterizedTrajectoryGenerator,
125  * which is called internally, plus):
126  * - `${sKeyPrefix}resolution`: The cell size
127  * - `${sKeyPrefix}v_max`, ``${sKeyPrefix}w_max`: Maximum robot speeds.
128  * - `${sKeyPrefix}shape_x{0,1,2..}`, ``${sKeyPrefix}shape_y{0,1,2..}`:
129  * Polygonal robot shape [Optional, can be also set via
130  * `setRobotPolygonShape()`]
131  *
132  * See docs of derived classes for additional parameters in setParams()
133  */
134  void loadFromConfigFile(
136  const std::string& sSection) override;
137  void saveToConfigFile(
139  const std::string& sSection) const override;
140 
141  void loadDefaultParams() override;
142 
143  double V_MAX{.0}, W_MAX{.0};
145  std::vector<TCPointVector> m_trajectory;
146  double m_resolution{0.05};
147  double m_stepTimeDuration{0.01};
148 
151  mrpt::serialization::CArchive& out) const override;
152 
153  /** Numerically solve the diferential equations to generate a family of
154  * trajectories */
156  float max_time, float max_dist, unsigned int max_n, float diferencial_t,
157  float min_dist, float* out_max_acc_v = nullptr,
158  float* out_max_acc_w = nullptr);
159 
160  /** A list of all the pairs (alpha,distance) such as the robot collides at
161  *that cell.
162  * - map key (uint16_t) -> alpha value (k)
163  * - map value (float) -> the MINIMUM distance (d), in meters,
164  *associated with that "k".
165  */
166  using TCollisionCell = std::vector<std::pair<uint16_t, float>>;
167 
168  /** An internal class for storing the collision grid */
169  class CCollisionGrid : public mrpt::containers::CDynamicGrid<TCollisionCell>
170  {
171  private:
173 
174  public:
176  float x_min, float x_max, float y_min, float y_max,
177  float resolution, CPTG_DiffDrive_CollisionGridBased* parent)
178  : mrpt::containers::CDynamicGrid<TCollisionCell>(
179  x_min, x_max, y_min, y_max, resolution),
180  m_parent(parent)
181  {
182  }
183  ~CCollisionGrid() override = default;
184  /** Save to file, true = OK */
185  bool saveToFile(
187  const mrpt::math::CPolygon& computed_robotShape) const;
188  /** Load from file, true = OK */
189  bool loadFromFile(
191  const mrpt::math::CPolygon& current_robotShape);
192 
193  /** For an obstacle (x,y), returns a vector with all the pairs (a,d)
194  * such as the robot collides */
196  const float obsX, const float obsY) const;
197 
198  /** Updates the info into a cell: It updates the cell only if the
199  *distance d for the path k is lower than the previous value:
200  * \param cellInfo The index of the cell
201  * \param k The path index (alpha discreet value)
202  * \param d The distance (in TP-Space, range 0..1) to collision.
203  */
204  void updateCellInfo(
205  const unsigned int icx, const unsigned int icy, const uint16_t k,
206  const float dist);
207 
208  }; // end of class CCollisionGrid
209 
210  // Save/Load from files.
211  bool saveColGridsToFile(
212  const std::string& filename,
213  const mrpt::math::CPolygon& computed_robotShape) const; // true = OK
215  const std::string& filename,
216  const mrpt::math::CPolygon& current_robotShape); // true = OK
217 
218  /** The collision grid */
220 
221  /** Specifies the min/max values for "k" and "n", respectively.
222  * \sa m_lambdaFunctionOptimizer
223  */
225  {
227  : k_min(std::numeric_limits<uint16_t>::max()),
228  k_max(std::numeric_limits<uint16_t>::min()),
229  n_min(std::numeric_limits<uint32_t>::max()),
230  n_max(std::numeric_limits<uint32_t>::min())
231  {
232  }
233 
234  uint16_t k_min, k_max;
235  uint32_t n_min, n_max;
236 
237  bool isEmpty() const
238  {
239  return k_min == std::numeric_limits<uint16_t>::max();
240  }
241  };
242 
243  /** This grid will contain indexes data for speeding-up the default,
244  * brute-force lambda function */
247 };
248 
249 /** @} */
250 } // namespace nav
251 namespace typemeta
252 {
253 // Specialization must occur in the same namespace
255 } // namespace typemeta
256 } // namespace mrpt
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Declares a typename to be "namespace::type".
Definition: TTypeName.h:119
double getMaxAngVel() const override
Returns the maximum angular velocity expected from this PTG [rad/s].
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:3695
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
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.
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:4199
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.
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...
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.
CDynamicGrid(double x_min=-10., double x_max=10., double y_min=-10., double y_max=10., double resolution=0.1)
Constructor.
Definition: CDynamicGrid.h:55
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.
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:4116
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:3684
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...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
std::vector< TCPoint > TCPointVector
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
GLenum GLint GLint y
Definition: glext.h:3542
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...
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3542
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. ...
GLfloat GLfloat p
Definition: glext.h:6398
mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
void onNewNavDynamicState() override
This family of PTGs ignores the dynamic states.
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: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019