Main MRPT website > C++ reference
MRPT logo
CReactiveNavigationSystem.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 CReactiveNavigationSystem_H
29 #define CReactiveNavigationSystem_H
30 
31 #include <mrpt/maps.h>
32 #include <mrpt/poses.h>
33 #include <mrpt/math.h>
34 #include <mrpt/synch.h>
35 #include <mrpt/reactivenav/link_pragmas.h>
36 
37 #include "CAbstractReactiveNavigationSystem.h"
38 #include "CParameterizedTrajectoryGenerator.h"
39 #include "CLogFileRecord.h"
40 #include "CAbstractHolonomicReactiveMethod.h"
41 #include "CHolonomicVFF.h"
42 #include "CHolonomicND.h"
43 
44 namespace mrpt
45 {
46  /** This namespace contains classes for building a TP-Space Reactive Navigation System.
47  */
48  namespace reactivenav
49  {
50  /** The implemented reactive navigation methods
51  * \ingroup mrpt_reactivenav_grp
52  */
53  enum THolonomicMethod
54  {
55  hmVIRTUAL_FORCE_FIELDS = 0,
56  hmSEARCH_FOR_BEST_GAP = 1
57  };
58 
59  /** Implements a reactive navigation system based on TP-Space, with an arbitrary holonomic
60  * reactive method running on it, and any desired number of PTG for transforming the navigation space.
61  * Both, the holonomic method and the PTGs can be customized by the apropriate user derived classes.
62  *
63  * How to use:
64  * - A class with callbacks must be defined by the user and provided to the constructor.
65  * - loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file).
66  * - navigationStep() must be called periodically in order to effectively run the navigation. This method will internally call the callbacks to gather sensor data and robot positioning data.
67  *
68  * - 17/JUN/2004: First design.
69  * - 16/SEP/2004: Totally redesigned, according to document "MultiParametric Based Space Transformation for Reactive Navigation"
70  * - 29/SEP/2005: Totally rewritten again, for integration into MRPT library and according to the ICRA paper.
71  * - 17/OCT/2007: Whole code updated to accomodate to MRPT 0.5 and make it portable to Linux.
72  *
73  * \sa CAbstractReactiveNavigationSystem, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod
74  * \ingroup mrpt_reactivenav_grp
75  */
76  class REACTIVENAV_IMPEXP CReactiveNavigationSystem : public CAbstractReactiveNavigationSystem
77  {
78  public:
79  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
80  public:
81  /** Constructor
82  * \param configINIFile The file to load the configuration from. See loadConfigFile
83  * \param robotConfigFile The file to load the robot specific configuration from.
84  * \param rmc A set of wrappers that must be filled in.
85  * \param sensors A set of wrappers that must be filled in.
86  * \param dbg A set of wrappers that must be filled in.
87  * \param evnts A set of wrappers that must be filled in.
88  * \param enableConsoleOutput Set to false if console output is not desired.
89  * \param enableLogFile Set to true to enable logging to file.
90  */
91  CReactiveNavigationSystem(
92  CReactiveInterfaceImplementation &react_iterf_impl,
93  bool enableConsoleOutput = true,
94  bool enableLogFile = false);
95 
96  /** Destructor
97  */
98  virtual ~CReactiveNavigationSystem();
99 
100  /** Reload the configuration from a file
101  */
102  void loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni);
103 
104  /** Must be called for loading collision grids, or the first navigation
105  * command may last a long time to be executed.
106  */
107  void initialize();
108 
109  /** Evaluate navigation hardness:
110  */
111  float evaluate( TNavigationParams *params );
112 
113  /** Start navigation:
114  */
115  void navigate( TNavigationParams *params );
116 
117  /** Change current navigation params:
118  */
119  void setParams( TNavigationParams *params );
120 
121  /** Selects which one from the set of available holonomic methods will be used
122  * into transformed TP-Space, and sets its configuration from a configuration file.
123  */
124  void setHolonomicMethod(
125  THolonomicMethod method,
126  const char *config_INIfile = "./CONFIG_ReactiveNavigator.ini");
127 
128  /** Change the robot shape, which is taken into account for collision
129  * grid building.
130  */
131  void changeRobotShape( math::CPolygon &shape );
132 
133  /** Provides a copy of the last log record with information about execution. On any unexpected error "*o" will be NULL.
134  * \param o An object where the log will be stored into.
135  */
136  void getLastLogRecord( CLogFileRecord &o );
137 
138  /** Enables / disables the logging into a file.
139  */
140  void enableLogFile(bool enable);
141 
142  private:
143  // ------------------------------------------------------
144  // PRIVATE DEFINITIONS
145  // ------------------------------------------------------
146  /** The structure used for storing a movement generated by a holonomic-method .
147  */
148  struct THolonomicMovement {
149  CParameterizedTrajectoryGenerator *PTG; /// The associated PTG
150  double direction, speed; /// The holonomic movement
151  double evaluation; /// An evaluation in the range [0,1] for the goodness of the movement.
152  };
153 
154  /** The last log.
155  */
156  CLogFileRecord lastLogRecord;
157 
158  /** For the histeresis: */
159  float last_cmd_v,last_cmd_w;
160 
161  /** Will be false until the navigation end is sent, and it is reset with each new command
162  */
163  bool navigationEndEventSent;
164 
165  /** Critical zones: */
166  synch::CCriticalSection m_critZoneLastLog,m_critZoneNavigating;
167 
168  // ------------------------------------------------------
169  // PRIVATE METHODS
170  // ------------------------------------------------------
171  /** The main method for the navigator */
172  void performNavigationStep( );
173 
174  // ------------------------------------------------------
175  // PRIVATE VARIABLES
176  // ------------------------------------------------------
177 
178  CAbstractHolonomicReactiveMethod *holonomicMethod; //!< The holonomic navigation algorithm.
179  mrpt::utils::CStream *logFile; //!< The current log file stream, or NULL if not being used
180 
181  bool m_enableConsoleOutput; //!< Enables / disables the console debug output.
182 
183  bool m_init_done; //!< Whether \a loadConfigFile() has been called or not.
184 
185  CTicTac timerForExecutionPeriod;
186 
187  // Loaded from INI file:
188  std::string robotName; // El nombre del robot donde estamos
189  float refDistance; // "dmax" in papers.
190  float colGridRes_x,colGridRes_y; // Resolucion de la rejilla de distancias de choque precalculadas
191  float robotMax_V_mps; // Max. vel del robot en m/s
192  float robotMax_W_degps; // Max. vel del robot en rad/s
193  float ROBOTMODEL_TAU,ROBOTMODEL_DELAY; // Params for the motor system modelation
194  std::vector<float> weights; // length: 6 [0,5]
195  float minObstaclesHeight, maxObstaclesHeight; // The range of "z" coordinates for obstacles to be considered
196  float DIST_TO_TARGET_FOR_SENDING_EVENT;
197 
198 
199  unsigned long nIteration; //!< The iteration count.
200  float meanExecutionPeriod; //!< Runtime estimation of execution period of the method.
201 
202 
203  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...
204  */
205  float badNavAlarm_minDistTarget;
206  mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime;
207  float badNavAlarm_AlarmTimeout;
208 
209 
210  /** The robot 2D shape model
211  */
212  math::CPolygon robotShape;
213  bool collisionGridsMustBeUpdated;
214 
215 
216  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
217  @{ */
218  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
219  std::vector<vector_double> TP_Obstacles;
220  std::vector<poses::CPoint2D,Eigen::aligned_allocator<poses::CPoint2D> > TP_Targets; // Target location (x,y) in TP-Space
221  std::vector<THolonomicMovement> holonomicMovements;
222  std::vector<float> times_TP_transformations, times_HoloNav;
223  std::vector<bool> valid_TP;
224  float meanExecutionTime;
225  float meanTotalExecutionTime;
226  int nLastSelectedPTG;
227  int m_decimateHeadingEstimate;
228  /** @} */
229 
230  /** The set of transformations to be used:
231  */
232  std::vector<CParameterizedTrajectoryGenerator*> PTGs;
233 
234 
235  // Steps for the reactive navigation sytem.
236  // ----------------------------------------------------------------------------
237  void STEP1_CollisionGridsBuilder();
238 
239  bool STEP2_Sense(
240  mrpt::slam::CSimplePointsMap &out_obstacles);
241 
242  void STEP3_SpaceTransformer(
243  poses::CPointsMap &in_obstacles,
244  CParameterizedTrajectoryGenerator *in_PTG,
245  vector_double &out_TPObstacles);
246 
247  void STEP4_HolonomicMethod(
248  vector_double &in_Obstacles,
249  poses::CPoint2D &in_Target,
250  float in_maxRobotSpeed,
251  THolonomicMovement &out_selectedMovement,
252  CHolonomicLogFileRecordPtr &in_HLFR );
253 
254  void STEP5_Evaluator(
255  THolonomicMovement &in_holonomicMovement,
256  vector_double &in_TPObstacles,
257  poses::CPoint2D &WS_Target,
258  poses::CPoint2D &TP_Target,
259  bool wasSelectedInLast,
260  CLogFileRecord::TInfoPerPTG &log );
261 
262  void STEP6_Selector(
263  std::vector<THolonomicMovement> &in_holonomicMovements,
264  THolonomicMovement &out_selectedHolonomicMovement,
265  int &out_nSelectedPTG);
266 
267  void STEP7_NonHolonomicMovement(
268  THolonomicMovement &in_movement,
269  float &out_v,
270  float &out_w);
271 
272  //
273  bool m_closing_navigator;
274 
275  /** Stops the robot and set navigation state to error */
276  void doEmergencyStop( const char *msg );
277 
278  };
279  }
280 }
281 
282 
283 #endif
284 
285 
286 
287 
288 


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