Main MRPT website > C++ reference for MRPT 1.9.9
CHolonomicFullEval.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 #pragma once
10 
14 
15 namespace mrpt
16 {
17 namespace nav
18 {
19 /** \addtogroup nav_holo Holonomic navigation methods
20  * \ingroup mrpt_nav_grp
21  * @{ */
22 
23 /** Full evaluation of all possible directions within the discrete set of input
24  * directions.
25  *
26  * These are the optional parameters of the method which can be set by means of
27  * a configuration file passed to the constructor or to
28  * CHolonomicFullEval::initialize() or directly in \a
29  * CHolonomicFullEval::options
30  *
31  * \code
32  * # Section name can be changed via setConfigFileSectionName()
33  * [FULL_EVAL_CONFIG]
34  * factorWeights = 1.0 1.0 1.0 0.05 1.0
35  * factorNormalizeOrNot = 0 0 0 0 1
36  * // 0: Clearness in direction
37  * // 1: Closest approach to target along straight line (Euclidean)
38  * // 2: Distance of end collision-free point to target (Euclidean)
39  * // 3: Hysteresis
40  * // 4: Clearness to nearest obstacle along path
41  * TARGET_SLOW_APPROACHING_DISTANCE = 0.20 // Start to reduce speed when
42  * closer than this to target [m]
43  * TOO_CLOSE_OBSTACLE = 0.02 // Directions with collision-free
44  * distances below this threshold are not elegible.
45  * HYSTERESIS_SECTOR_COUNT = 5 // Range of "sectors" (directions)
46  * for hysteresis over successive timesteps
47  * PHASE1_FACTORS = 0 1 2 // Indices of the factors above to
48  * be considered in phase 1
49  * PHASE2_FACTORS = 3 4 // Indices of the factors above to
50  * be considered in phase 2
51  * PHASE1_THRESHOLD = 0.75 // Phase1 scores must be above this
52  * relative range threshold [0,1] to be considered in phase 2 (Default:`0.75`)
53  * \endcode
54  *
55  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
56  */
58 {
60  public:
61  /** Initialize the parameters of the navigator, from some configuration
62  * file, or default values if set to nullptr */
63  CHolonomicFullEval(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
64 
65  // See base class docs
66  void navigate(const NavInput& ni, NavOutput& no) override;
67 
68  virtual void initialize(const mrpt::config::CConfigFileBase& INI_FILE)
69  override; // See base class docs
71  const override; // See base class docs
72 
73  /** Algorithm options */
75  {
76  /** Directions with collision-free distances below this threshold are
77  * not elegible. */
79  /** Start to reduce speed when closer than this to target [m] */
81  /** Start to reduce speed when clearance is below this value ([0,1]
82  * ratio wrt obstacle reference/max distance) */
84  /** Range of "sectors" (directions) for hysteresis over successive
85  * timesteps */
87  /** See docs above */
88  std::vector<double> factorWeights;
89  /** 0/1 to normalize factors. */
90  std::vector<int32_t> factorNormalizeOrNot;
91  /** Factor indices [0,4] for the factors to consider in each phase
92  * 1,2,...N of the movement decision (Defaults: `PHASE1_FACTORS=0 1 2`,
93  * `PHASE2_FACTORS=`3 4`) */
94  std::vector<std::vector<int32_t>> PHASE_FACTORS;
95  /** Phase 1,2,N-1... scores must be above this relative range threshold
96  * [0,1] to be considered in phase 2 (Default:`0.75`) */
97  std::vector<double> PHASE_THRESHOLDS;
98 
99  /** (default:false, to save space) */
101 
102  /** Ratio [0,1], times path_count, gives the minimum number of paths at
103  * each side of a target direction to be accepted as desired direction
104  */
106  /** Ratio [0,1], times path_count, gives the minimum gap width to
107  * accept a direct motion towards target. */
109 
110  TOptions();
111  void loadFromConfigFile(
113  const std::string& section) override; // See base docs
114  void saveToConfigFile(
116  const std::string& section) const override; // See base docs
117  };
118 
119  /** Parameters of the algorithm (can be set manually or loaded from
120  * CHolonomicFullEval::initialize or options.loadFromConfigFile(), etc.) */
122 
123  double getTargetApproachSlowDownDistance() const override
124  {
126  }
127  void setTargetApproachSlowDownDistance(const double dist) override
128  {
130  }
131 
132  private:
134  unsigned int direction2sector(const double a, const unsigned int N);
135  /** Individual scores for each direction: (i,j), i (row) are directions, j
136  * (cols) are scores. Not all directions may have evaluations, in which case
137  * a "-1" value will be found. */
139 
140  virtual void postProcessDirectionEvaluations(
141  std::vector<double>& dir_evals, const NavInput& ni,
142  unsigned int trg_idx); // If desired, override in a derived class to
143  // manipulate the final evaluations of each
144  // directions
145 
146  struct EvalOutput
147  {
148  unsigned int best_k;
149  double best_eval;
150  std::vector<std::vector<double>> phase_scores;
151  EvalOutput();
152  };
153 
154  /** Evals one single target of the potentially many of them in NavInput */
155  void evalSingleTarget(
156  unsigned int target_idx, const NavInput& ni, EvalOutput& eo);
157 
159 }; // end of CHolonomicFullEval
160 
161 /** A class for storing extra information about the execution of
162  * CHolonomicFullEval navigation.
163  * \sa CHolonomicFullEval, CHolonomicLogFileRecord
164  */
166 {
168  public:
170 
171  /** Member data */
173  double evaluation;
174  /** Individual scores for each direction: (i,j), i (row) are directions, j
175  * (cols) are scores. Not all directions may have evaluations, in which case
176  * a "-1" value will be found. */
178  /** Normally = 0. Can be >0 if multiple targets passed simultaneously. */
180 
181  const mrpt::math::CMatrixD* getDirectionScores() const override
182  {
183  return &dirs_scores;
184  }
185 };
186 
187 /** @} */
188 } // end namespace
189 }
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 "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:24
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:6313
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:4101
virtual 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:46
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...
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].
virtual 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:4082
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:6279
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019