Main MRPT website > C++ reference
MRPT logo
CHolonomicND.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CHolonomicND_H
29 #define CHolonomicND_H
30 
31 #include "CAbstractHolonomicReactiveMethod.h"
32 
33 namespace mrpt
34 {
35  namespace reactivenav
36  {
37  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CLogFileRecord_ND, CHolonomicLogFileRecord, REACTIVENAV_IMPEXP)
38 
39  /** An implementation of the holonomic reactive navigation method "Nearness-Diagram".
40  * The algorithm "Nearness-Diagram" was proposed in:
41  *
42  * Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on
43  * Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.
44  *
45  * \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
46  * \ingroup mrpt_reactivenav_grp
47  */
48  class REACTIVENAV_IMPEXP CHolonomicND : public CAbstractHolonomicReactiveMethod
49  {
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52  public:
53  /** Initialize the parameters of the navigator, from some configuration file, or default values if set to NULL.
54  */
55  CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
56 
57  /** This method performs the holonomic navigation itself.
58  * \param target [IN] The relative location (x,y) of target point.
59  * \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.
60  * \param maxRobotSpeed [IN] Maximum robot speed, in "pseudometers/sec". See note below.
61  * \param desiredDirection [OUT] The desired motion direction, in the range [-PI,PI]
62  * \param desiredSpeed [OUT] The desired motion speed in that direction, in "pseudometers"/sec. (See note below)
63  * \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.
64  *
65  * NOTE: With "pseudometers" we refer to the distance unit in TP-Space, thus:
66  * <br><center><code>pseudometer<sup>2</sup>= meter<sup>2</sup> + (rad ยท r)<sup>2</sup></code><br></center>
67  */
68  void navigate( poses::CPoint2D &target,
69  vector_double &obstacles,
70  double maxRobotSpeed,
71  double &desiredDirection,
72  double &desiredSpeed,
73  CHolonomicLogFileRecordPtr &logRecord );
74 
75  /** The structure used to store a detected gap in obstacles.
76  */
77  struct TGap
78  {
79  int ini;
80  int end;
81  double entranceDistance;
82  double maxDistance;
83  int representative_sector;
84  };
85 
86  typedef std::vector<TGap> TGapArray;
87 
88  /** The set of posible situations for each trajectory.
89  */
90  enum TSituations
91  {
92  SITUATION_TARGET_DIRECTLY = 1,
93  SITUATION_SMALL_GAP,
94  SITUATION_WIDE_GAP,
95  SITUATION_NO_WAY_FOUND
96  };
97 
98  /** Initialize the parameters of the navigator.
99  */
100  void initialize( const mrpt::utils::CConfigFileBase &INI_FILE );
101 
102 
103 
104  private:
105  int last_selected_sector;
106 
107  int direction2sector(double a, int N);
108 
109  /** Configuration:
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;
113  double TARGET_SLOW_APPROACHING_DISTANCE;
114 
115  vector_double factorWeights;
116 
117  /** Find gaps in the obtacles.
118  */
119  void gapsEstimator(
120  vector_double &obstacles,
121  poses::CPoint2D &in_target,
122  TGapArray &gaps );
123 
124  /** Search the best gap.
125  */
126  void searchBestGap(
127  vector_double &in_obstacles,
128  double in_maxObsRange,
129  TGapArray &in_gaps,
130  poses::CPoint2D &in_target,
131  int &out_selDirection,
132  double &out_selEvaluation,
133  TSituations &out_situation,
134  double &out_riskEvaluation,
135  CLogFileRecord_NDPtr log);
136 
137  /** Fills in the representative sector field in the gap structure:
138  */
139  void calcRepresentativeSectorForGap(
140  TGap &gap,
141  const poses::CPoint2D &target,
142  const vector_double &obstacles);
143 
144  /** Evaluate each gap:
145  */
146  void evaluateGaps(
147  const vector_double &in_obstacles,
148  const double in_maxObsRange,
149  const TGapArray &in_gaps,
150  const int TargetSector,
151  const double TargetDist,
152  vector_double &out_gaps_evaluation );
153  };
154 
155  /** A class for storing extra information about the execution of
156  * CHolonomicND navigation.
157  * \sa CHolonomicND, CHolonomicLogFileRecord
158  */
159  class CLogFileRecord_ND : public CHolonomicLogFileRecord
160  {
161  DEFINE_SERIALIZABLE( CLogFileRecord_ND )
162 
163  public:
164  /** Member data.
165  */
166  vector_int gaps_ini,gaps_end;
167  vector_double gaps_eval;
168  int32_t selectedSector;
169  double evaluation;
170  double riskEvaluation;
171  CHolonomicND::TSituations situation;
172  };
173 
174  }
175 }
176 
177 
178 #endif
179 
180 
181 


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013