Main MRPT website > C++ reference for MRPT 1.9.9
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-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 #ifndef CHolonomicND_H
10 #define CHolonomicND_H
11 
14 
15 namespace mrpt
16 {
17 namespace nav
18 {
19 class CLogFileRecord_ND;
20 /** \addtogroup nav_holo Holonomic navigation methods
21  * \ingroup mrpt_nav_grp
22  * @{ */
23 
24 /** An implementation of the holonomic reactive navigation method
25  * "Nearness-Diagram".
26  * The algorithm "Nearness-Diagram" was proposed in:
27  *
28  * Nearness diagram (ND) navigation: collision avoidance in troublesome
29  * scenarios, IEEE Transactions on
30  * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp.
31  * 45-59, 2004.
32  *
33  * These are the optional parameters of the method which can be set by means of
34  * a configuration file passed to the constructor or to
35  * CHolonomicND::initialize() or directly in \a CHolonomicND::options
36  *
37  * \code
38  * # Section name can be changed via setConfigFileSectionName()
39  * [ND_CONFIG]
40  * factorWeights=1.0 0.5 2.0 0.4
41  * // 1: Free space
42  * // 2: Dist. in sectors
43  * // 3: Closer to target (euclidean)
44  * // 4: Hysteresis
45  * WIDE_GAP_SIZE_PERCENT = 0.25
46  * MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25
47  * RISK_EVALUATION_SECTORS_PERCENT = 0.25
48  * RISK_EVALUATION_DISTANCE = 0.15 // In normalized ps-meters [0,1]
49  * TARGET_SLOW_APPROACHING_DISTANCE = 0.60 // For stopping gradually
50  * TOO_CLOSE_OBSTACLE = 0.02 // In normalized ps-meters
51  * \endcode
52  *
53  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
54  */
56 {
58  public:
59  /** Initialize the parameters of the navigator, from some configuration
60  * file, or default values if set to nullptr */
61  CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
62 
63  // See base class docs
64  void navigate(const NavInput& ni, NavOutput& no) override;
65 
66  /** The structure used to store a detected gap in obstacles. */
67  struct TGap
68  {
69  unsigned int ini;
70  unsigned int end;
71  double maxDistance;
72  double minDistance;
73  unsigned int representative_sector;
74  };
75 
76  using TGapArray = std::vector<TGap>;
77 
78  /** The set of posible situations for each trajectory.
79  * (mrpt::typemeta::TEnumType works with this enum) */
81  {
86  };
87 
88  /** Initialize the parameters of the navigator. */
89  void initialize(const mrpt::config::CConfigFileBase& INI_FILE) override;
91  const override; // See base class docs
92 
93  /** Algorithm options */
95  {
100  /** Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors,
101  * [2]=Closer to target (Euclidean), [3]=Hysteresis */
102  std::vector<double> factorWeights;
103 
104  TOptions();
105  void loadFromConfigFile(
107  const std::string& section) override; // See base docs
108  void saveToConfigFile(
110  const std::string& section) const override; // See base docs
111  };
112 
113  /** Parameters of the algorithm (can be set manually or loaded from
114  * CHolonomicND::initialize or options.loadFromConfigFile(), etc.) */
116 
117  double getTargetApproachSlowDownDistance() const override
118  {
120  }
121  void setTargetApproachSlowDownDistance(const double dist) override
122  {
124  }
125 
126  private:
128 
129  unsigned int direction2sector(const double a, const unsigned int N);
130 
131  /** Find gaps in the obtacles.
132  */
133  void gapsEstimator(
134  const std::vector<double>& obstacles,
135  const mrpt::math::TPoint2D& in_target, TGapArray& gaps);
136 
137  /** Search the best gap.
138  */
139  void searchBestGap(
140  const std::vector<double>& in_obstacles, const double in_maxObsRange,
141  const TGapArray& in_gaps, const mrpt::math::TPoint2D& in_target,
142  unsigned int& out_selDirection, double& out_selEvaluation,
143  TSituations& out_situation, double& out_riskEvaluation,
144  CLogFileRecord_ND& log);
145 
146  /** Fills in the representative sector field in the gap structure:
147  */
149  TGap& gap, const mrpt::math::TPoint2D& target,
150  const std::vector<double>& obstacles);
151 
152  /** Evaluate each gap:
153  */
154  void evaluateGaps(
155  const std::vector<double>& in_obstacles, const double in_maxObsRange,
156  const TGapArray& in_gaps, const unsigned int TargetSector,
157  const float TargetDist, std::vector<double>& out_gaps_evaluation);
158 
159 }; // end of CHolonomicND
160 
161 /** A class for storing extra information about the execution of
162  * CHolonomicND navigation.
163  * \sa CHolonomicND, CHolonomicLogFileRecord
164  */
166 {
168 
169  public:
170  /** Member data.
171  */
172  std::vector<int> gaps_ini, gaps_end;
173  std::vector<double> gaps_eval;
175  double evaluation;
178 };
179 
180 /** @} */
181 } // namespace nav
182 } // namespace mrpt
183 
185 using namespace mrpt::nav;
186 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_TARGET_DIRECTLY);
187 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_SMALL_GAP);
188 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_WIDE_GAP);
189 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_NO_WAY_FOUND);
191 
192 #endif
CAbstractHolonomicReactiveMethod.h
mrpt::nav::CHolonomicND::TGap::ini
unsigned int ini
Definition: CHolonomicND.h:69
mrpt::nav::CHolonomicND::TGap::end
unsigned int end
Definition: CHolonomicND.h:70
mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:38
mrpt::nav::CHolonomicND::SITUATION_SMALL_GAP
@ SITUATION_SMALL_GAP
Definition: CHolonomicND.h:83
mrpt::nav::CHolonomicND::TOptions::TARGET_SLOW_APPROACHING_DISTANCE
double TARGET_SLOW_APPROACHING_DISTANCE
Definition: CHolonomicND.h:99
mrpt::nav::CHolonomicND::TOptions::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CHolonomicND.cpp:701
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:74
mrpt::nav::CHolonomicND::TOptions::factorWeights
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist.
Definition: CHolonomicND.h:102
mrpt::nav::CLogFileRecord_ND
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:165
mrpt::nav::CLogFileRecord_ND::riskEvaluation
double riskEvaluation
Definition: CHolonomicND.h:176
mrpt::nav::CHolonomicND::searchBestGap
void searchBestGap(const std::vector< double > &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_ND &log)
Search the best gap.
Definition: CHolonomicND.cpp:294
mrpt::nav::CLogFileRecord_ND::evaluation
double evaluation
Definition: CHolonomicND.h:175
mrpt::nav::CLogFileRecord_ND::gaps_ini
std::vector< int > gaps_ini
Member data.
Definition: CHolonomicND.h:172
c
const GLubyte * c
Definition: glext.h:6313
mrpt::nav::CLogFileRecord_ND::selectedSector
int32_t selectedSector
Definition: CHolonomicND.h:174
mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_SECTORS_PERCENT
double RISK_EVALUATION_SECTORS_PERCENT
Definition: CHolonomicND.h:97
mrpt::nav::CLogFileRecord_ND::gaps_end
std::vector< int > gaps_end
Definition: CHolonomicND.h:172
mrpt::nav::CHolonomicND
An implementation of the holonomic reactive navigation method "Nearness-Diagram".
Definition: CHolonomicND.h:55
mrpt::nav::CHolonomicND::TOptions::MAX_SECTOR_DIST_FOR_D2_PERCENT
double MAX_SECTOR_DIST_FOR_D2_PERCENT
Definition: CHolonomicND.h:98
mrpt::nav::CHolonomicND::setTargetApproachSlowDownDistance
void setTargetApproachSlowDownDistance(const double dist) override
Sets the actual value of this parameter [m].
Definition: CHolonomicND.h:121
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:23
mrpt::nav::CHolonomicND::gapsEstimator
void gapsEstimator(const std::vector< double > &obstacles, const mrpt::math::TPoint2D &in_target, TGapArray &gaps)
Find gaps in the obtacles.
Definition: CHolonomicND.cpp:131
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::nav::CHolonomicND::TOptions::TOptions
TOptions()
Definition: CHolonomicND.cpp:666
mrpt::nav::CHolonomicND::TOptions
Algorithm options.
Definition: CHolonomicND.h:94
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:58
mrpt::nav::CHolonomicND::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CHolonomicND.cpp:678
mrpt::nav::CLogFileRecord_ND::situation
CHolonomicND::TSituations situation
Definition: CHolonomicND.h:177
mrpt::nav::CLogFileRecord_ND::gaps_eval
std::vector< double > gaps_eval
Definition: CHolonomicND.h:173
mrpt::nav::CHolonomicND::direction2sector
unsigned int direction2sector(const double a, const unsigned int N)
Definition: CHolonomicND.cpp:608
mrpt::nav::CHolonomicND::CHolonomicND
CHolonomicND(const mrpt::config::CConfigFileBase *INI_FILE=nullptr)
Initialize the parameters of the navigator, from some configuration file, or default values if set to...
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::nav::CHolonomicND::TGap::maxDistance
double maxDistance
Definition: CHolonomicND.h:71
mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_DISTANCE
double RISK_EVALUATION_DISTANCE
Definition: CHolonomicND.h:98
mrpt::nav::CAbstractHolonomicReactiveMethod
A base class for holonomic reactive navigation methods.
Definition: CAbstractHolonomicReactiveMethod.h:32
mrpt::nav::CHolonomicND::m_last_selected_sector
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:127
mrpt::nav::CHolonomicND::TGap::minDistance
double minDistance
Definition: CHolonomicND.h:72
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_TARGET_DIRECTLY)
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:28
mrpt::nav::CHolonomicND::options
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options....
Definition: CHolonomicND.h:115
mrpt::nav::CHolonomicND::calcRepresentativeSectorForGap
void calcRepresentativeSectorForGap(TGap &gap, const mrpt::math::TPoint2D &target, const std::vector< double > &obstacles)
Fills in the representative sector field in the gap structure:
Definition: CHolonomicND.cpp:445
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:42
mrpt::nav::CHolonomicND::initialize
void initialize(const mrpt::config::CConfigFileBase &INI_FILE) override
Initialize the parameters of the navigator.
Definition: CHolonomicND.cpp:37
mrpt::nav::CHolonomicND::TGap
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:67
int32_t
__int32 int32_t
Definition: rptypes.h:46
CLoadableOptions.h
mrpt::nav::CHolonomicND::SITUATION_NO_WAY_FOUND
@ SITUATION_NO_WAY_FOUND
Definition: CHolonomicND.h:85
mrpt::nav::CHolonomicND::TGapArray
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:76
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:102
mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput
Output for CAbstractHolonomicReactiveMethod::navigate()
Definition: CAbstractHolonomicReactiveMethod.h:65
mrpt::nav::CHolonomicND::evaluateGaps
void evaluateGaps(const std::vector< double > &in_obstacles, const double in_maxObsRange, const TGapArray &in_gaps, const unsigned int TargetSector, const float TargetDist, std::vector< double > &out_gaps_evaluation)
Evaluate each gap:
Definition: CHolonomicND.cpp:508
mrpt::nav::CHolonomicLogFileRecord
A base class for log records for different holonomic navigation methods.
Definition: CHolonomicLogFileRecord.h:27
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::nav::CHolonomicND::saveConfigFile
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
saves all available parameters, in a forma loadable by initialize()
Definition: CHolonomicND.cpp:41
mrpt::nav::CHolonomicND::TOptions::TOO_CLOSE_OBSTACLE
double TOO_CLOSE_OBSTACLE
Definition: CHolonomicND.h:96
mrpt::nav::CHolonomicND::TOptions::WIDE_GAP_SIZE_PERCENT
double WIDE_GAP_SIZE_PERCENT
Definition: CHolonomicND.h:96
mrpt::nav::CHolonomicND::TSituations
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:80
mrpt::nav::CHolonomicND::getTargetApproachSlowDownDistance
double getTargetApproachSlowDownDistance() const override
Returns the actual value of this parameter [m], as set via the children class options structure.
Definition: CHolonomicND.h:117
mrpt::nav::CHolonomicND::SITUATION_TARGET_DIRECTLY
@ SITUATION_TARGET_DIRECTLY
Definition: CHolonomicND.h:82
mrpt::nav::CHolonomicND::TGap::representative_sector
unsigned int representative_sector
Definition: CHolonomicND.h:73
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::nav::CHolonomicND::navigate
void navigate(const NavInput &ni, NavOutput &no) override
Invokes the holonomic navigation algorithm itself.
Definition: CHolonomicND.cpp:49
mrpt::nav::CHolonomicND::SITUATION_WIDE_GAP
@ SITUATION_WIDE_GAP
Definition: CHolonomicND.h:84



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST