MRPT  1.9.9
CHolonomicFullEval.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-2019, 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 
14 
15 namespace mrpt::nav
16 {
17 /** \addtogroup nav_holo Holonomic navigation methods
18  * \ingroup mrpt_nav_grp
19  * @{ */
20 
21 /** Full evaluation of all possible directions within the discrete set of input
22  * directions.
23  *
24  * These are the optional parameters of the method which can be set by means of
25  * a configuration file passed to the constructor or to
26  * CHolonomicFullEval::initialize() or directly in \a
27  * CHolonomicFullEval::options
28  *
29  * \code
30  * # Section name can be changed via setConfigFileSectionName()
31  * [FULL_EVAL_CONFIG]
32  * factorWeights = 1.0 1.0 1.0 0.05 1.0
33  * factorNormalizeOrNot = 0 0 0 0 1
34  * // 0: Clearness in direction
35  * // 1: Closest approach to target along straight line (Euclidean)
36  * // 2: Distance of end collision-free point to target (Euclidean)
37  * // 3: Hysteresis
38  * // 4: Clearness to nearest obstacle along path
39  * TARGET_SLOW_APPROACHING_DISTANCE = 0.20 // Start to reduce speed when
40  * closer than this to target [m]
41  * TOO_CLOSE_OBSTACLE = 0.02 // Directions with collision-free
42  * distances below this threshold are not elegible.
43  * HYSTERESIS_SECTOR_COUNT = 5 // Range of "sectors" (directions)
44  * for hysteresis over successive timesteps
45  * PHASE1_FACTORS = 0 1 2 // Indices of the factors above to
46  * be considered in phase 1
47  * PHASE2_FACTORS = 3 4 // Indices of the factors above to
48  * be considered in phase 2
49  * PHASE1_THRESHOLD = 0.75 // Phase1 scores must be above this
50  * relative range threshold [0,1] to be considered in phase 2 (Default:`0.75`)
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  CHolonomicFullEval(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
62 
63  // See base class docs
64  void navigate(const NavInput& ni, NavOutput& no) override;
65 
66  void initialize(const mrpt::config::CConfigFileBase& INI_FILE)
67  override; // See base class docs
69  const override; // See base class docs
70 
71  /** Algorithm options */
73  {
74  /** Directions with collision-free distances below this threshold are
75  * not elegible. */
76  double TOO_CLOSE_OBSTACLE{0.15};
77  /** Start to reduce speed when closer than this to target [m] */
79  /** Start to reduce speed when clearance is below this value ([0,1]
80  * ratio wrt obstacle reference/max distance) */
82  /** Range of "sectors" (directions) for hysteresis over successive
83  * timesteps */
85  /** See docs above */
86  std::vector<double> factorWeights;
87  /** 0/1 to normalize factors. */
88  std::vector<int32_t> factorNormalizeOrNot;
89  /** Factor indices [0,4] for the factors to consider in each phase
90  * 1,2,...N of the movement decision (Defaults: `PHASE1_FACTORS=0 1 2`,
91  * `PHASE2_FACTORS=`3 4`) */
92  std::vector<std::vector<int32_t>> PHASE_FACTORS;
93  /** Phase 1,2,N-1... scores must be above this relative range threshold
94  * [0,1] to be considered in phase 2 (Default:`0.75`) */
95  std::vector<double> PHASE_THRESHOLDS;
96 
97  /** (default:false, to save space) */
98  bool LOG_SCORE_MATRIX{false};
99 
100  /** Ratio [0,1], times path_count, gives the minimum number of paths at
101  * each side of a target direction to be accepted as desired direction
102  */
104  /** Ratio [0,1], times path_count, gives the minimum gap width to
105  * accept a direct motion towards target. */
107 
108  TOptions();
109  void loadFromConfigFile(
111  const std::string& section) override; // See base docs
112  void saveToConfigFile(
114  const std::string& section) const override; // See base docs
115  };
116 
117  /** Parameters of the algorithm (can be set manually or loaded from
118  * CHolonomicFullEval::initialize or options.loadFromConfigFile(), etc.) */
120 
121  double getTargetApproachSlowDownDistance() const override
122  {
124  }
125  void setTargetApproachSlowDownDistance(const double dist) override
126  {
128  }
129 
130  private:
132  unsigned int direction2sector(const double a, const unsigned int N);
133  /** Individual scores for each direction: (i,j), i (row) are directions, j
134  * (cols) are scores. Not all directions may have evaluations, in which case
135  * a "-1" value will be found. */
137 
138  virtual void postProcessDirectionEvaluations(
139  std::vector<double>& dir_evals, const NavInput& ni,
140  unsigned int trg_idx); // If desired, override in a derived class to
141  // manipulate the final evaluations of each
142  // directions
143 
144  struct EvalOutput
145  {
146  unsigned int best_k;
147  double best_eval{.0};
148  std::vector<std::vector<double>> phase_scores;
149  EvalOutput();
150  };
151 
152  /** Evals one single target of the potentially many of them in NavInput */
153  void evalSingleTarget(
154  unsigned int target_idx, const NavInput& ni, EvalOutput& eo);
155 
157 }; // end of CHolonomicFullEval
158 
159 /** A class for storing extra information about the execution of
160  * CHolonomicFullEval navigation.
161  * \sa CHolonomicFullEval, CHolonomicLogFileRecord
162  */
164 {
166  public:
168 
169  /** Member data */
171  double evaluation{.0};
172  /** Individual scores for each direction: (i,j), i (row) are directions, j
173  * (cols) are scores. Not all directions may have evaluations, in which case
174  * a "-1" value will be found. */
176  /** Normally = 0. Can be >0 if multiple targets passed simultaneously. */
178 
179  const mrpt::math::CMatrixD* getDirectionScores() const override
180  {
181  return &dirs_scores;
182  }
183 };
184 
185 /** @} */
186 } // namespace mrpt::nav
unsigned int direction2sector(const double a, const unsigned int N)
double gap_width_ratio_threshold
Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target...
double getTargetApproachSlowDownDistance() const override
Returns the actual value of this parameter [m], as set via the children class options structure...
virtual void postProcessDirectionEvaluations(std::vector< double > &dir_evals, const NavInput &ni, unsigned int trg_idx)
mrpt::obs::CSinCosLookUpTableFor2DScans m_sincos_lut
A class for storing extra information about the execution of CHolonomicFullEval navigation.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
double TOO_CLOSE_OBSTACLE
Directions with collision-free distances below this threshold are not elegible.
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.
double TARGET_SLOW_APPROACHING_DISTANCE
Start to reduce speed when closer than this to target [m].
std::vector< int32_t > factorNormalizeOrNot
0/1 to normalize factors.
const mrpt::math::CMatrixD * getDirectionScores() const override
double clearance_threshold_ratio
Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction t...
std::vector< double > PHASE_THRESHOLDS
Phase 1,2,N-1...
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicFullEval::initialize or opt...
double OBSTACLE_SLOW_DOWN_DISTANCE
Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max dist...
This class allows loading and storing values and vectors of different types from a configuration text...
const GLubyte * c
Definition: glext.h:6406
A smart look-up-table (LUT) of sin/cos values for 2D laser scans.
A base class for log records for different holonomic navigation methods.
std::vector< double > factorWeights
See docs above.
void evalSingleTarget(unsigned int target_idx, const NavInput &ni, EvalOutput &eo)
Evals one single target of the potentially many of them in NavInput.
CHolonomicFullEval(const mrpt::config::CConfigFileBase *INI_FILE=nullptr)
Initialize the parameters of the navigator, from some configuration file, or default values if set to...
GLsizei const GLchar ** string
Definition: glext.h:4116
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
saves all available parameters, in a forma loadable by initialize()
Full evaluation of all possible directions within the discrete set of input directions.
__int32 int32_t
Definition: rptypes.h:49
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void navigate(const NavInput &ni, NavOutput &no) override
Invokes the holonomic navigation algorithm itself.
mrpt::math::CMatrixD m_dirs_scores
Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores...
void setTargetApproachSlowDownDistance(const double dist) override
Sets the actual value of this parameter [m].
void initialize(const mrpt::config::CConfigFileBase &INI_FILE) override
Initialize the parameters of the navigator, reading from the default section name (see derived classe...
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
double HYSTERESIS_SECTOR_COUNT
Range of "sectors" (directions) for hysteresis over successive timesteps.
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
Output for CAbstractHolonomicReactiveMethod::navigate()
mrpt::math::CMatrixD dirs_scores
Individual scores for each direction: (i,j), i (row) are directions, j (cols) are scores...
std::vector< std::vector< double > > phase_scores
std::vector< std::vector< int32_t > > PHASE_FACTORS
Factor indices [0,4] for the factors to consider in each phase 1,2,...N of the movement decision (Def...
bool LOG_SCORE_MATRIX
(default:false, to save space)
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019