Main MRPT website > C++ reference for MRPT 1.5.6
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-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 #ifndef CHolonomicND_H
10 #define CHolonomicND_H
11 
14 
15 namespace mrpt
16 {
17  namespace nav
18  {
19  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CLogFileRecord_ND, CHolonomicLogFileRecord, NAV_IMPEXP)
20  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CHolonomicND, CAbstractHolonomicReactiveMethod, NAV_IMPEXP )
21 
22  /** \addtogroup nav_holo Holonomic navigation methods
23  * \ingroup mrpt_nav_grp
24  * @{ */
25 
26  /** An implementation of the holonomic reactive navigation method "Nearness-Diagram".
27  * The algorithm "Nearness-Diagram" was proposed in:
28  *
29  * Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on
30  * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.
31  *
32  * 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
33  *
34  * \code
35  * # Section name can be changed via setConfigFileSectionName()
36  * [ND_CONFIG]
37  * factorWeights=1.0 0.5 2.0 0.4
38  * // 1: Free space
39  * // 2: Dist. in sectors
40  * // 3: Closer to target (euclidean)
41  * // 4: Hysteresis
42  * WIDE_GAP_SIZE_PERCENT = 0.25
43  * MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25
44  * RISK_EVALUATION_SECTORS_PERCENT = 0.25
45  * RISK_EVALUATION_DISTANCE = 0.15 // In normalized ps-meters [0,1]
46  * TARGET_SLOW_APPROACHING_DISTANCE = 0.60 // For stopping gradually
47  * TOO_CLOSE_OBSTACLE = 0.02 // In normalized ps-meters
48  * \endcode
49  *
50  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
51  */
52  class NAV_IMPEXP CHolonomicND : public CAbstractHolonomicReactiveMethod
53  {
54  DEFINE_SERIALIZABLE( CHolonomicND )
55  public:
56  /** Initialize the parameters of the navigator, from some configuration file, or default values if set to NULL */
57  CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
58 
59  // See base class docs
60  void navigate(const NavInput & ni, NavOutput &no) MRPT_OVERRIDE;
61 
62  /** The structure used to store a detected gap in obstacles. */
63  struct TGap
64  {
65  unsigned int ini;
66  unsigned int end;
67  double maxDistance;
68  double minDistance;
69  unsigned int representative_sector;
70  };
71 
72  typedef std::vector<TGap> TGapArray;
73 
74  /** The set of posible situations for each trajectory. (mrpt::utils::TEnumType works with this enum) */
76  {
77  SITUATION_TARGET_DIRECTLY = 1,
80  SITUATION_NO_WAY_FOUND
81  };
82 
83  /** Initialize the parameters of the navigator. */
84  void initialize(const mrpt::utils::CConfigFileBase &INI_FILE) MRPT_OVERRIDE;
85  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE; // See base class docs
86 
87  /** Algorithm options */
89  {
90  double TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT;
91  double RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT;
93  std::vector<double> factorWeights; //!< Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis
94 
95 
96  TOptions();
97  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
98  void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg ,const std::string &section) const MRPT_OVERRIDE; // See base docs
99  };
100 
101  TOptions options; //!< Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options.loadFromConfigFile(), etc.)
102 
103  double getTargetApproachSlowDownDistance() const MRPT_OVERRIDE { return options.TARGET_SLOW_APPROACHING_DISTANCE; }
104  void setTargetApproachSlowDownDistance(const double dist) MRPT_OVERRIDE { options.TARGET_SLOW_APPROACHING_DISTANCE = dist; }
105 
106  private:
108 
109  unsigned int direction2sector(const double a, const unsigned int N);
110 
111  /** Find gaps in the obtacles.
112  */
113  void gapsEstimator(
114  const std::vector<double> & obstacles,
115  const mrpt::math::TPoint2D & in_target,
116  TGapArray & gaps );
117 
118 
119  /** Search the best gap.
120  */
121  void searchBestGap(
122  const std::vector<double> & in_obstacles,
123  const double in_maxObsRange,
124  const TGapArray & in_gaps,
125  const mrpt::math::TPoint2D & in_target,
126  unsigned int & out_selDirection,
127  double & out_selEvaluation,
128  TSituations & out_situation,
129  double & out_riskEvaluation,
130  CLogFileRecord_NDPtr log);
131 
132  /** Fills in the representative sector field in the gap structure:
133  */
134  void calcRepresentativeSectorForGap(
135  TGap & gap,
137  const std::vector<double> & obstacles);
138 
139  /** Evaluate each gap:
140  */
141  void evaluateGaps(
142  const std::vector<double> & in_obstacles,
143  const double in_maxObsRange,
144  const TGapArray & in_gaps,
145  const unsigned int TargetSector,
146  const float TargetDist,
147  std::vector<double> & out_gaps_evaluation );
148 
149  }; // end of CHolonomicND
151 
152  /** A class for storing extra information about the execution of
153  * CHolonomicND navigation.
154  * \sa CHolonomicND, CHolonomicLogFileRecord
155  */
157  {
159 
160  public:
161  /** Member data.
162  */
163  vector_int gaps_ini,gaps_end;
164  std::vector<double> gaps_eval;
165  int32_t selectedSector;
166  double evaluation;
167  double riskEvaluation;
168  CHolonomicND::TSituations situation;
169  };
171 
172  /** @} */
173  } // end namespace
174 
175  // Specializations MUST occur at the same namespace:
176  namespace utils
177  {
178  template <>
180  {
182  static void fill(bimap<enum_t,std::string> &m_map)
183  {
184  m_map.insert(nav::CHolonomicND::SITUATION_TARGET_DIRECTLY, "SITUATION_TARGET_DIRECTLY");
185  m_map.insert(nav::CHolonomicND::SITUATION_SMALL_GAP, "SITUATION_SMALL_GAP");
186  m_map.insert(nav::CHolonomicND::SITUATION_WIDE_GAP, "SITUATION_WIDE_GAP");
187  m_map.insert(nav::CHolonomicND::SITUATION_NO_WAY_FOUND, "SITUATION_NO_WAY_FOUND");
188  }
189  };
190  } // End of namespace
191 }
192 
193 
194 #endif
195 
196 
197 
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
double getTargetApproachSlowDownDistance() const MRPT_OVERRIDE
Returns the actual value of this parameter [m], as set via the children class options structure...
Definition: CHolonomicND.h:103
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
An implementation of the holonomic reactive navigation method "Nearness-Diagram". ...
Definition: CHolonomicND.h:52
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:107
const GLfloat * c
Definition: glew.h:10088
A base class for holonomic reactive navigation methods.
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:72
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
This class allows loading and storing values and vectors of different types from a configuration text...
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options...
Definition: CHolonomicND.h:101
unsigned int representative_sector
Definition: CHolonomicND.h:69
#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 log records for different holonomic navigation methods.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std::...
Definition: bimap.h:28
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis.
Definition: CHolonomicND.h:93
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CHolonomicND.h:182
void setTargetApproachSlowDownDistance(const double dist) MRPT_OVERRIDE
Sets the actual value of this parameter [m].
Definition: CHolonomicND.h:104
GLenum target
Definition: glew.h:5023
__int32 int32_t
Definition: rptypes.h:48
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:63
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:156
std::vector< int32_t > vector_int
Definition: types_simple.h:23
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Lightweight 2D point.
Output for CAbstractHolonomicReactiveMethod::navigate()
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:75
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



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