MRPT  2.0.1
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-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 
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  * // 5: Like 2, but without being decimated if path to target is obstructed
40  * TARGET_SLOW_APPROACHING_DISTANCE = 0.20 // Start to reduce speed when
41  * closer than this to target [m]
42  * TOO_CLOSE_OBSTACLE = 0.02 // Directions with collision-free
43  * distances below this threshold are not elegible.
44  * HYSTERESIS_SECTOR_COUNT = 5 // Range of "sectors" (directions)
45  * for hysteresis over successive timesteps
46  * PHASE1_FACTORS = 0 1 2 // Indices of the factors above to
47  * be considered in phase 1
48  * PHASE2_FACTORS = 3 4 // Indices of the factors above to
49  * be considered in phase 2
50  * PHASE1_THRESHOLD = 0.75 // Phase1 scores must be above this
51  * relative range threshold [0,1] to be considered in phase 2 (Default:`0.75`)
52  * \endcode
53  *
54  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
55  */
57 {
59  public:
60  /** Initialize the parameters of the navigator, from some configuration
61  * file, or default values if set to nullptr */
62  CHolonomicFullEval(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);
63 
64  // See base class docs
65  void navigate(const NavInput& ni, NavOutput& no) override;
66 
67  void initialize(const mrpt::config::CConfigFileBase& INI_FILE)
68  override; // See base class docs
70  const override; // See base class docs
71 
72  /** Algorithm options */
74  {
75  /** Directions with collision-free distances below this threshold are
76  * not elegible. */
77  double TOO_CLOSE_OBSTACLE{0.15};
78  /** Start to reduce speed when closer than this to target [m] */
80  /** Start to reduce speed when clearance is below this value ([0,1]
81  * ratio wrt obstacle reference/max distance) */
83  /** Range of "sectors" (directions) for hysteresis over successive
84  * timesteps */
86  /** See docs above */
87  std::vector<double> factorWeights;
88  /** 0/1 to normalize factors. */
89  std::vector<int32_t> factorNormalizeOrNot;
90  /** Factor indices [0,4] for the factors to consider in each phase
91  * 1,2,...N of the movement decision (Defaults: `PHASE1_FACTORS=0 1 2`,
92  * `PHASE2_FACTORS=`3 4`) */
93  std::vector<std::vector<int32_t>> PHASE_FACTORS;
94  /** Phase 1,2,N-1... scores must be above this relative range threshold
95  * [0,1] to be considered in phase 2 (Default:`0.75`) */
96  std::vector<double> PHASE_THRESHOLDS;
97 
98  /** (default:false, to save space) */
99  bool LOG_SCORE_MATRIX{false};
100 
101  /** Ratio [0,1], times path_count, gives the minimum number of paths at
102  * each side of a target direction to be accepted as desired direction
103  */
105  /** Ratio [0,1], times path_count, gives the minimum gap width to
106  * accept a direct motion towards target. */
108 
109  TOptions();
110  void loadFromConfigFile(
111  const mrpt::config::CConfigFileBase& source,
112  const std::string& section) override; // See base docs
113  void saveToConfigFile(
115  const std::string& section) const override; // See base docs
116  };
117 
118  /** Parameters of the algorithm (can be set manually or loaded from
119  * CHolonomicFullEval::initialize or options.loadFromConfigFile(), etc.) */
121 
122  double getTargetApproachSlowDownDistance() const override
123  {
125  }
126  void setTargetApproachSlowDownDistance(const double dist) override
127  {
129  }
130 
131  private:
133  unsigned int direction2sector(const double a, const unsigned int N);
134  /** Individual scores for each direction: (i,j), i (row) are directions, j
135  * (cols) are scores. Not all directions may have evaluations, in which case
136  * a "-1" value will be found. */
138 
139  /** If desired, override in a derived class to manipulate the final
140  * evaluations of each directions */
141  virtual void postProcessDirectionEvaluations(
142  std::vector<double>& dir_evals, const NavInput& ni,
143  unsigned int trg_idx);
144 
145  struct EvalOutput
146  {
147  std::vector<std::vector<double>> phase_scores;
149  };
150 
151  /** Evals one single target of the potentially many of them in NavInput */
152  void evalSingleTarget(
153  unsigned int target_idx, const NavInput& ni, EvalOutput& eo);
154 
156 }; // end of CHolonomicFullEval
157 
158 /** A class for storing extra information about the execution of
159  * CHolonomicFullEval navigation.
160  * \sa CHolonomicFullEval, CHolonomicLogFileRecord
161  */
163 {
165  public:
167 
168  /** Member data */
169  int32_t selectedSector{0};
170  double evaluation{.0};
171  /** Individual scores for each direction: (i,j), i (row) are directions, j
172  * (cols) are scores. Not all directions may have evaluations, in which case
173  * a "-1" value will be found. */
175  /** Normally = 0. Can be >0 if multiple targets passed simultaneously. */
176  int32_t selectedTarget{0};
177 
178  const mrpt::math::CMatrixD* getDirectionScores() const override
179  {
180  return &dirs_scores;
181  }
182 };
183 
184 /** @} */
185 } // 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)
If desired, override in a derived class to manipulate the final evaluations of each directions...
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...
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...
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.
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...
double HYSTERESIS_SECTOR_COUNT
Range of "sectors" (directions) for hysteresis over successive timesteps.
#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()
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020