MRPT  2.0.1
CHolonomicND.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-2020, 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 
13 
14 namespace mrpt::nav
15 {
16 class CLogFileRecord_ND;
17 /** \addtogroup nav_holo Holonomic navigation methods
18  * \ingroup mrpt_nav_grp
19  * @{ */
20 
21 /** An implementation of the holonomic reactive navigation method
22  * "Nearness-Diagram".
23  * The algorithm "Nearness-Diagram" was proposed in:
24  *
25  * Nearness diagram (ND) navigation: collision avoidance in troublesome
26  * scenarios, IEEE Transactions on
27  * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp.
28  * 45-59, 2004.
29  *
30  * These are the optional parameters of the method which can be set by means of
31  * a configuration file passed to the constructor or to
32  * 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  */
53 {
55  public:
56  /** Initialize the parameters of the navigator, from some configuration
57  * file, or default values if set to nullptr */
58  CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
59 
60  // See base class docs
61  void navigate(const NavInput& ni, NavOutput& no) override;
62 
63  /** The structure used to store a detected gap in obstacles. */
64  struct TGap
65  {
66  unsigned int ini;
67  unsigned int end;
68  double maxDistance;
69  double minDistance;
70  unsigned int representative_sector;
71  };
72 
73  using TGapArray = std::vector<TGap>;
74 
75  /** The set of posible situations for each trajectory.
76  * (mrpt::typemeta::TEnumType works with this enum) */
78  {
83  };
84 
85  /** Initialize the parameters of the navigator. */
86  void initialize(const mrpt::config::CConfigFileBase& INI_FILE) override;
88  const override; // See base class docs
89 
90  /** Algorithm options */
92  {
98  /** Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors,
99  * [2]=Closer to target (Euclidean), [3]=Hysteresis */
100  std::vector<double> factorWeights;
101 
102  TOptions();
103  void loadFromConfigFile(
104  const mrpt::config::CConfigFileBase& source,
105  const std::string& section) override; // See base docs
106  void saveToConfigFile(
108  const std::string& section) const override; // See base docs
109  };
110 
111  /** Parameters of the algorithm (can be set manually or loaded from
112  * CHolonomicND::initialize or options.loadFromConfigFile(), etc.) */
114 
115  double getTargetApproachSlowDownDistance() const override
116  {
118  }
119  void setTargetApproachSlowDownDistance(const double dist) override
120  {
122  }
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<double>& obstacles,
133  const mrpt::math::TPoint2D& in_target, TGapArray& gaps);
134 
135  /** Search the best gap.
136  */
137  void searchBestGap(
138  const std::vector<double>& in_obstacles, const double in_maxObsRange,
139  const TGapArray& in_gaps, const mrpt::math::TPoint2D& in_target,
140  unsigned int& out_selDirection, double& out_selEvaluation,
141  TSituations& out_situation, double& out_riskEvaluation,
142  CLogFileRecord_ND& log);
143 
144  /** Fills in the representative sector field in the gap structure:
145  */
147  TGap& gap, const mrpt::math::TPoint2D& target,
148  const std::vector<double>& obstacles);
149 
150  /** Evaluate each gap:
151  */
152  void evaluateGaps(
153  const std::vector<double>& in_obstacles, const double in_maxObsRange,
154  const TGapArray& in_gaps, const unsigned int TargetSector,
155  const float TargetDist, std::vector<double>& out_gaps_evaluation);
156 
157 }; // end of CHolonomicND
158 
159 /** A class for storing extra information about the execution of
160  * CHolonomicND navigation.
161  * \sa CHolonomicND, CHolonomicLogFileRecord
162  */
164 {
166 
167  public:
168  /** Member data.
169  */
170  std::vector<int> gaps_ini, gaps_end;
171  std::vector<double> gaps_eval;
172  int32_t selectedSector;
173  double evaluation;
176 };
177 
178 /** @} */
179 } // namespace mrpt::nav
181 using namespace mrpt::nav;
182 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_TARGET_DIRECTLY);
183 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_SMALL_GAP);
184 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_WIDE_GAP);
185 MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_NO_WAY_FOUND);
void initialize(const mrpt::config::CConfigFileBase &INI_FILE) override
Initialize the parameters of the navigator.
void calcRepresentativeSectorForGap(TGap &gap, const mrpt::math::TPoint2D &target, const std::vector< double > &obstacles)
Fills in the representative sector field in the gap structure:
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
An implementation of the holonomic reactive navigation method "Nearness-Diagram". ...
Definition: CHolonomicND.h:52
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:125
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.
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. ...
A base class for holonomic reactive navigation methods.
CHolonomicND::TSituations situation
Definition: CHolonomicND.h:175
void gapsEstimator(const std::vector< double > &obstacles, const mrpt::math::TPoint2D &in_target, TGapArray &gaps)
Find gaps in the obtacles.
void navigate(const NavInput &ni, NavOutput &no) override
Invokes the holonomic navigation algorithm itself.
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:73
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options...
Definition: CHolonomicND.h:113
std::vector< int > gaps_end
Definition: CHolonomicND.h:170
unsigned int representative_sector
Definition: CHolonomicND.h:70
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.
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< int > gaps_ini
Member data.
Definition: CHolonomicND.h:170
A base class for log records for different holonomic navigation methods.
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist.
Definition: CHolonomicND.h:100
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
CHolonomicND(const mrpt::config::CConfigFileBase *INI_FILE=nullptr)
Initialize the parameters of the navigator, from some configuration file, or default values if set to...
void setTargetApproachSlowDownDistance(const double dist) override
Sets the actual value of this parameter [m].
Definition: CHolonomicND.h:119
MRPT_FILL_ENUM_MEMBER(CHolonomicND, SITUATION_TARGET_DIRECTLY)
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:64
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:
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:163
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
std::vector< double > gaps_eval
Definition: CHolonomicND.h:171
Output for CAbstractHolonomicReactiveMethod::navigate()
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:77
unsigned int direction2sector(const double a, const unsigned int N)
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
saves all available parameters, in a forma loadable by initialize()
double getTargetApproachSlowDownDistance() const override
Returns the actual value of this parameter [m], as set via the children class options structure...
Definition: CHolonomicND.h:115



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020