Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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 
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 
117  TOptions();
118  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg ,const std::string &section) const;
119  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section);
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  */
131  void gapsEstimator(
132  const std::vector<float> & obstacles,
133  const mrpt::math::TPoint2D & in_target,
134  TGapArray & gaps );
135 
136 
137  /** Search the best gap.
138  */
139  void searchBestGap(
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  */
152  void calcRepresentativeSectorForGap(
153  TGap & gap,
154  const mrpt::math::TPoint2D & target,
155  const std::vector<float> & obstacles);
156 
157  /** Evaluate each gap:
158  */
159  void evaluateGaps(
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  */
180  vector_int gaps_ini,gaps_end;
181  std::vector<double> gaps_eval;
182  int32_t selectedSector;
183  double evaluation;
184  double riskEvaluation;
185  CHolonomicND::TSituations situation;
186  };
188 
189  /** @} */
190  } // end namespace
191 
192  // Specializations MUST occur at the same namespace:
193  namespace utils
194  {
195  template <>
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 MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
An implementation of the holonomic reactive navigation method "Nearness-Diagram". ...
Definition: CHolonomicND.h:51
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:125
A base class for holonomic reactive navigation methods.
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:89
STL namespace.
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:122
unsigned int representative_sector
Definition: CHolonomicND.h:86
#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:114
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CHolonomicND.h:199
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:80
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:173
std::vector< int32_t > vector_int
Definition: types_simple.h:23
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.
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:93
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void initialize(const mrpt::utils::CConfigFileBase &INI_FILE)
Initialize the parameters of the navigator.
Definition: CHolonomicND.h:103



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:46:27 UTC 2016