Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
15namespace mrpt
16{
17 namespace nav
18 {
20
21 /** \addtogroup nav_holo Holonomic navigation methods
22 * \ingroup mrpt_nav_grp
23 * @{ */
24
25 /** An implementation of the holonomic reactive navigation method "Nearness-Diagram".
26 * The algorithm "Nearness-Diagram" was proposed in:
27 *
28 * Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on
29 * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.
30 *
31 * These are the optional parameters of the method which can be set by means of a configuration file passed to the constructor or to CHolonomicND::initialize() or directly in \a CHolonomicND::options
32 *
33 * \code
34 * [ND_CONFIG]
35 * factorWeights=1.0 0.5 2.0 0.4
36 * // 1: Free space
37 * // 2: Dist. in sectors
38 * // 3: Closer to target (euclidean)
39 * // 4: Hysteresis
40 * WIDE_GAP_SIZE_PERCENT = 0.25
41 * MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25
42 * RISK_EVALUATION_SECTORS_PERCENT = 0.25
43 * RISK_EVALUATION_DISTANCE = 0.15 // In normalized ps-meters [0,1]
44 * TARGET_SLOW_APPROACHING_DISTANCE = 0.60 // For stopping gradually
45 * TOO_CLOSE_OBSTACLE = 0.02 // In normalized ps-meters
46 * \endcode
47 *
48 * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
49 * \ingroup
50 */
52 {
53 public:
55 public:
56 /** Initialize the parameters of the navigator, from some configuration file, or default values if set to NULL.
57 */
58 CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
59
60 /** This method performs the holonomic navigation itself.
61 * \param target [IN] The relative location (x,y) of target point.
62 * \param obstacles [IN] Distance to obstacles from robot location (0,0). First index refers to -PI direction, and last one to +PI direction. Distances can be dealed as "meters", although they are "pseudometers", see note below.
63 * \param maxRobotSpeed [IN] Maximum robot speed, in "pseudometers/sec". See note below.
64 * \param desiredDirection [OUT] The desired motion direction, in the range [-PI,PI]
65 * \param desiredSpeed [OUT] The desired motion speed in that direction, in "pseudometers"/sec. (See note below)
66 * \param logRecord [IN/OUT] A placeholder for a pointer to a log record with extra info about the execution. Set to NULL if not required. User <b>must free memory</b> using "delete logRecord" after using it.
67 *
68 * NOTE: With "pseudometers" we refer to the distance unit in TP-Space, thus:
69 * <br><center><code>pseudometer<sup>2</sup>= meter<sup>2</sup> + (rad * r)<sup>2</sup></code><br></center>
70 */
71 void navigate( const mrpt::math::TPoint2D &target,
72 const std::vector<float> &obstacles,
73 double maxRobotSpeed,
74 double &desiredDirection,
75 double &desiredSpeed,
76 CHolonomicLogFileRecordPtr &logRecord );
77
78 /** The structure used to store a detected gap in obstacles.
79 */
80 struct TGap
81 {
82 unsigned int ini;
83 unsigned int end;
87 };
88
89 typedef std::vector<TGap> TGapArray;
90
91 /** The set of posible situations for each trajectory. (mrpt::utils::TEnumType works with this enum)
92 */
94 {
95 SITUATION_TARGET_DIRECTLY = 1,
98 SITUATION_NO_WAY_FOUND
99 };
100
101 /** Initialize the parameters of the navigator.
102 */
104 {
105 options.loadFromConfigFile(INI_FILE, std::string("ND_CONFIG"));
106 }
107
108 /** Algorithm options */
110 {
111 double TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT;
112 double RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT;
114 std::vector<double> factorWeights; //!< Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis
115
116
118 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
119 void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg ,const std::string &section) const MRPT_OVERRIDE; // See base docs
120 };
121
122 TOptions options; //!< Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options.loadFromConfigFile(), etc.)
123
124 private:
126
127 unsigned int direction2sector(const double a, const unsigned int N);
128
129 /** Find gaps in the obtacles.
130 */
132 const std::vector<float> & obstacles,
133 const mrpt::math::TPoint2D & in_target,
134 TGapArray & gaps );
135
136
137 /** Search the best gap.
138 */
140 const std::vector<float> & in_obstacles,
141 const double in_maxObsRange,
142 const TGapArray & in_gaps,
143 const mrpt::math::TPoint2D & in_target,
144 unsigned int & out_selDirection,
145 double & out_selEvaluation,
146 TSituations & out_situation,
147 double & out_riskEvaluation,
149
150 /** Fills in the representative sector field in the gap structure:
151 */
153 TGap & gap,
154 const mrpt::math::TPoint2D & target,
155 const std::vector<float> & obstacles);
156
157 /** Evaluate each gap:
158 */
160 const std::vector<float> & in_obstacles,
161 const float in_maxObsRange,
162 const TGapArray & in_gaps,
163 const unsigned int TargetSector,
164 const float TargetDist,
165 std::vector<double> & out_gaps_evaluation );
166
167 }; // end of CHolonomicND
168
169 /** A class for storing extra information about the execution of
170 * CHolonomicND navigation.
171 * \sa CHolonomicND, CHolonomicLogFileRecord
172 */
174 {
176
177 public:
178 /** Member data.
179 */
181 std::vector<double> gaps_eval;
186 };
188
189 /** @} */
190 } // end namespace
191
192 // Specializations MUST occur at the same namespace:
193 namespace utils
194 {
195 template <>
196 struct TEnumTypeFiller<nav::CHolonomicND::TSituations>
197 {
199 static void fill(bimap<enum_t,std::string> &m_map)
200 {
201 m_map.insert(nav::CHolonomicND::SITUATION_TARGET_DIRECTLY, "SITUATION_TARGET_DIRECTLY");
202 m_map.insert(nav::CHolonomicND::SITUATION_SMALL_GAP, "SITUATION_SMALL_GAP");
203 m_map.insert(nav::CHolonomicND::SITUATION_WIDE_GAP, "SITUATION_WIDE_GAP");
204 m_map.insert(nav::CHolonomicND::SITUATION_NO_WAY_FOUND, "SITUATION_NO_WAY_FOUND");
205 }
206 };
207 } // End of namespace
208}
209
210
211#endif
212
213
214
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A base class for holonomic reactive navigation methods.
A base class for log records for different holonomic navigation methods.
An implementation of the holonomic reactive navigation method "Nearness-Diagram".
Definition: CHolonomicND.h:52
TOptions options
Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options....
Definition: CHolonomicND.h:122
void initialize(const mrpt::utils::CConfigFileBase &INI_FILE)
Initialize the parameters of the navigator.
Definition: CHolonomicND.h:103
void evaluateGaps(const std::vector< float > &in_obstacles, const float in_maxObsRange, const TGapArray &in_gaps, const unsigned int TargetSector, const float TargetDist, std::vector< double > &out_gaps_evaluation)
Evaluate each gap:
std::vector< TGap > TGapArray
Definition: CHolonomicND.h:89
void searchBestGap(const std::vector< float > &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_NDPtr log)
Search the best gap.
unsigned int m_last_selected_sector
Definition: CHolonomicND.h:125
void gapsEstimator(const std::vector< float > &obstacles, const mrpt::math::TPoint2D &in_target, TGapArray &gaps)
Find gaps in the obtacles.
CHolonomicND(const mrpt::utils::CConfigFileBase *INI_FILE=NULL)
Initialize the parameters of the navigator, from some configuration file, or default values if set to...
unsigned int direction2sector(const double a, const unsigned int N)
void navigate(const mrpt::math::TPoint2D &target, const std::vector< float > &obstacles, double maxRobotSpeed, double &desiredDirection, double &desiredSpeed, CHolonomicLogFileRecordPtr &logRecord)
This method performs the holonomic navigation itself.
TSituations
The set of posible situations for each trajectory.
Definition: CHolonomicND.h:94
void calcRepresentativeSectorForGap(TGap &gap, const mrpt::math::TPoint2D &target, const std::vector< float > &obstacles)
Fills in the representative sector field in the gap structure:
A class for storing extra information about the execution of CHolonomicND navigation.
Definition: CHolonomicND.h:174
vector_int gaps_ini
Member data.
Definition: CHolonomicND.h:180
std::vector< double > gaps_eval
Definition: CHolonomicND.h:181
CHolonomicND::TSituations situation
Definition: CHolonomicND.h:185
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
std::vector< int32_t > vector_int
Definition: types_simple.h:23
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
struct NAV_IMPEXP CLogFileRecord_NDPtr
Definition: CHolonomicND.h:19
struct NAV_IMPEXP CHolonomicLogFileRecordPtr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
signed long int32_t
Definition: pstdint.h:247
Lightweight 2D point.
The structure used to store a detected gap in obstacles.
Definition: CHolonomicND.h:81
unsigned int representative_sector
Definition: CHolonomicND.h:86
std::vector< double > factorWeights
Vector of 4 weights: [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean),...
Definition: CHolonomicND.h:114
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &section) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CHolonomicND.h:199
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 08:20:48 UTC 2023