Main MRPT website > C++ reference for MRPT 1.4.0
CHolonomicND.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-2016, 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 #ifndef CHolonomicND_H
10 #define CHolonomicND_H
11 
14 
15 namespace mrpt
16 {
17  namespace nav
18  {
20 
21  /** \addtogroup nav_holo Holonomic navigation methods
22  * \ingroup mrpt_nav_grp
23  * @{ */
24 
25  /** An implementation of the holonomic reactive navigation method "Nearness-Diagram".
26  * The algorithm "Nearness-Diagram" was proposed in:
27  *
28  * Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on
29  * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.
30  *
31  * These are the optional parameters of the method which can be set by means of a configuration file passed to the constructor or to CHolonomicND::initialize() or directly in \a CHolonomicND::options
32  *
33  * \code
34  * [ND_CONFIG]
35  * factorWeights=1.0 0.5 2.0 0.4
36  * // 1: Free space
37  * // 2: Dist. in sectors
38  * // 3: Closer to target (euclidean)
39  * // 4: Hysteresis
40  * WIDE_GAP_SIZE_PERCENT = 0.25
41  * MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25
42  * RISK_EVALUATION_SECTORS_PERCENT = 0.25
43  * RISK_EVALUATION_DISTANCE = 0.15 // In normalized ps-meters [0,1]
44  * TARGET_SLOW_APPROACHING_DISTANCE = 0.60 // For stopping gradually
45  * TOO_CLOSE_OBSTACLE = 0.02 // In normalized ps-meters
46  * \endcode
47  *
48  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
49  * \ingroup
50  */
52  {
53  public:
55  public:
56  /** Initialize the parameters of the navigator, from some configuration file, or default values if set to NULL.
57  */
58  CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
59 
60  /** This method performs the holonomic navigation itself.
61  * \param target [IN] The relative location (x,y) of target point.
62  * \param obstacles [IN] Distance to obstacles from robot location (0,0). First index refers to -PI direction, and last one to +PI direction. Distances can be dealed as "meters", although they are "pseudometers", see note below.
63  * \param maxRobotSpeed [IN] Maximum robot speed, in "pseudometers/sec". See note below.
64  * \param desiredDirection [OUT] The desired motion direction, in the range [-PI,PI]
65  * \param desiredSpeed [OUT] The desired motion speed in that direction, in "pseudometers"/sec. (See note below)
66  * \param logRecord [IN/OUT] A placeholder for a pointer to a log record with extra info about the execution. Set to NULL if not required. User <b>must free memory</b> using "delete logRecord" after using it.
67  *
68  * NOTE: With "pseudometers" we refer to the distance unit in TP-Space, thus:
69  * <br><center><code>pseudometer<sup>2</sup>= meter<sup>2</sup> + (rad * r)<sup>2</sup></code><br></center>
70  */
71  void navigate( const mrpt::math::TPoint2D &target,
72  const std::vector<float> &obstacles,
73  double maxRobotSpeed,
74  double &desiredDirection,
75  double &desiredSpeed,
76  CHolonomicLogFileRecordPtr &logRecord );
77 
78  /** The structure used to store a detected gap in obstacles.
79  */
80  struct TGap
81  {
82  unsigned int ini;
83  unsigned int end;
84  double maxDistance;
85  double minDistance;
86  unsigned int representative_sector;
87  };
88 
89  typedef std::vector<TGap> TGapArray;
90 
91  /** The set of posible situations for each trajectory. (mrpt::utils::TEnumType works with this enum)
92  */
94  {
95  SITUATION_TARGET_DIRECTLY = 1,
98  SITUATION_NO_WAY_FOUND
99  };
100 
101  /** Initialize the parameters of the navigator.
102  */
103  void initialize( const mrpt::utils::CConfigFileBase &INI_FILE )
104  {
105  options.loadFromConfigFile(INI_FILE, std::string("ND_CONFIG"));
106  }
107 
108  /** Algorithm options */
110  {
111  double TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT;
112  double RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT;
114  std::vector<double> factorWeights; //!< Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis
115 
116 
118  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
119  void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg ,const std::string &section) const MRPT_OVERRIDE; // See base docs
120  };
121 
122  TOptions options; //!< Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options.loadFromConfigFile(), etc.)
123 
124  private:
126 
127  unsigned int direction2sector(const double a, const unsigned int N);
128 
129  /** Find gaps in the obtacles.
130  */
132  const std::vector<float> & obstacles,
133  const mrpt::math::TPoint2D & in_target,
134  TGapArray & gaps );
135 
136 
137  /** Search the best gap.
138  */
140  const std::vector<float> & in_obstacles,
141  const double in_maxObsRange,
142  const TGapArray & in_gaps,
143  const mrpt::math::TPoint2D & in_target,
144  unsigned int & out_selDirection,
145  double & out_selEvaluation,
146  TSituations & out_situation,
147  double & out_riskEvaluation,
148  CLogFileRecord_NDPtr log);
149 
150  /** Fills in the representative sector field in the gap structure:
151  */
153  TGap & gap,
154  const mrpt::math::TPoint2D & target,
155  const std::vector<float> & obstacles);
156 
157  /** Evaluate each gap:
158  */
160  const std::vector<float> & in_obstacles,
161  const float in_maxObsRange,
162  const TGapArray & in_gaps,
163  const unsigned int TargetSector,
164  const float TargetDist,
165  std::vector<double> & out_gaps_evaluation );
166 
167  }; // end of CHolonomicND
168 
169  /** A class for storing extra information about the execution of
170  * CHolonomicND navigation.
171  * \sa CHolonomicND, CHolonomicLogFileRecord
172  */
174  {
176 
177  public:
178  /** Member data.
179  */
181  std::vector<double> gaps_eval;
182  int32_t selectedSector;
183  double evaluation;
186  };
188 
189  /** @} */
190  } // end namespace
191 
192  // Specializations MUST occur at the same namespace:
193  namespace utils
194  {
195  template <>
196  struct TEnumTypeFiller<nav::CHolonomicND::TSituations>
197  {
199  static void fill(bimap<enum_t,std::string> &m_map)
200  {
201  m_map.insert(nav::CHolonomicND::SITUATION_TARGET_DIRECTLY, "SITUATION_TARGET_DIRECTLY");
202  m_map.insert(nav::CHolonomicND::SITUATION_SMALL_GAP, "SITUATION_SMALL_GAP");
203  m_map.insert(nav::CHolonomicND::SITUATION_WIDE_GAP, "SITUATION_WIDE_GAP");
204  m_map.insert(nav::CHolonomicND::SITUATION_NO_WAY_FOUND, "SITUATION_NO_WAY_FOUND");
205  }
206  };
207  } // End of namespace
208 }
209 
210 
211 #endif
212 
213 
214 
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A base class for holonomic reactive navigation methods.
A base class for log records for different holonomic navigation methods.
An implementation of the holonomic reactive navigation method "Nearness-Diagram".
Definition: CHolonomicND.h:52
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options....
Definition: CHolonomicND.h:122
void initialize(const mrpt::utils::CConfigFileBase &INI_FILE)
Initialize the parameters of the navigator.
Definition: CHolonomicND.h:103
void evaluateGaps(const std::vector< float > &in_obstacles, const float in_maxObsRange, const TGapArray &in_gaps, const unsigned int TargetSector, const float TargetDist, std::vector< double > &out_gaps_evaluation)
Evaluate each gap:
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:89
void searchBestGap(const std::vector< float > &in_obstacles, const double in_maxObsRange, const TGapArray &in_gaps, const mrpt::math::TPoint2D &in_target, unsigned int &out_selDirection, double &out_selEvaluation, TSituations &out_situation, double &out_riskEvaluation, CLogFileRecord_NDPtr log)
Search the best gap.
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:125
void gapsEstimator(const std::vector< float > &obstacles, const mrpt::math::TPoint2D &in_target, TGapArray &gaps)
Find gaps in the obtacles.
CHolonomicND(const mrpt::utils::CConfigFileBase *INI_FILE=NULL)
Initialize the parameters of the navigator, from some configuration file, or default values if set to...
unsigned int direction2sector(const double a, const unsigned int N)
void navigate(const mrpt::math::TPoint2D &target, const std::vector< float > &obstacles, double maxRobotSpeed, double &desiredDirection, double &desiredSpeed, CHolonomicLogFileRecordPtr &logRecord)
This method performs the holonomic navigation itself.
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:94
void calcRepresentativeSectorForGap(TGap &gap, const mrpt::math::TPoint2D &target, const std::vector< float > &obstacles)
Fills in the representative sector field in the gap structure:
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:174
vector_int gaps_ini
Member data.
Definition: CHolonomicND.h:180
std::vector< double > gaps_eval
Definition: CHolonomicND.h:181
CHolonomicND::TSituations situation
Definition: CHolonomicND.h:185
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
std::vector< int32_t > vector_int
Definition: types_simple.h:23
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D point.
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:81
unsigned int representative_sector
Definition: CHolonomicND.h:86
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean),...
Definition: CHolonomicND.h:114
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &section) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CHolonomicND.h:199
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 04:08:57 UTC 2022