Main MRPT website > C++ reference for MRPT 1.3.2
CAbstractPTGBasedReactive.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-2015, 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 CAbstractPTGBasedReactive_H
10 #define CAbstractPTGBasedReactive_H
11 
18 #include <mrpt/utils/CTimeLogger.h>
19 #include <mrpt/system/datetime.h>
21 
22 namespace mrpt
23 {
24  namespace nav
25  {
26  /** Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic
27  * reactive method running on it and any number of PTGs for transforming the navigation space.
28  * Both, the holonomic method and the PTGs can be customized by the apropriate user derived classes.
29  *
30  * How to use:
31  * - Instantiate a reactive navigation object (one of the derived classes of this virtual class).
32  * - A class with callbacks must be defined by the user and provided to the constructor (derived from CReactiveInterfaceImplementation)
33  * - loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file).
34  * - 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.
35  *
36  * For working examples, refer to the source code of the apps:
37  * - [ReactiveNavigationDemo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenavigationdemo/)
38  * - [ReactiveNav3D-Demo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenav3d-demo/)
39  *
40  * Publications:
41  * - See derived classes for papers on each specific method.
42  *
43  * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D
44  * \ingroup nav_reactive
45  */
47  {
48  public:
50 
51  /** The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes. */
53  {
54  /** (Default=empty) Optionally, a list of PTG indices can be sent such that
55  * the navigator will restrict itself to only employ those PTGs. */
56  std::vector<size_t> restrict_PTG_indices;
57 
59  virtual ~TNavigationParamsPTG() { }
60  virtual std::string getAsText() const;
61  virtual TNavigationParams* clone() const { return new TNavigationParamsPTG(*this); }
62  };
63 
64 
65  /** Constructor.
66  * \param[in] react_iterf_impl An instance of an object that implement all the required interfaces to read from and control a robot.
67  * \param[in] enableConsoleOutput Can be set to false to reduce verbosity.
68  * \param[in] enableLogFile Set to true to enable creation of navigation log files, useful for inspection and debugging.
69  */
71  CReactiveInterfaceImplementation &react_iterf_impl,
72  bool enableConsoleOutput = true,
73  bool enableLogFile = false);
74 
75  virtual ~CAbstractPTGBasedReactive();
76 
77  /** Must be called for loading collision grids, or the first navigation command may last a long time to be executed.
78  * Internally, it just calls STEP1_CollisionGridsBuilder().
79  */
80  void initialize();
81 
82  /** Selects which one from the set of available holonomic methods will be used
83  * into transformed TP-Space, and sets its configuration from a configuration file.*/
84  void setHolonomicMethod(
85  THolonomicMethod method,
86  const mrpt::utils::CConfigFileBase & cfgBase);
87 
88  /** Just loads the holonomic method params from the given config source \sa setHolonomicMethod */
89  void loadHolonomicMethodConfig(
91  const std::string &section );
92 
93 
94  /** Start navigation:
95  * \param[in] params Pointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return.)
96  */
97  virtual void navigate( const TNavigationParams *params );
98 
99  /** Provides a copy of the last log record with information about execution.
100  * \param o An object where the log will be stored into.
101  * \note Log records are not prepared unless either "enableLogFile" is enabled in the constructor or "enableKeepLogRecords()" has been called.
102  */
103  void getLastLogRecord( CLogFileRecord &o );
104 
105  /** Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() */
106  void enableKeepLogRecords(bool enable=true) { m_enableKeepLogRecords=enable; }
107 
108  /** Enables/disables saving log files. */
109  void enableLogFile(bool enable);
110 
111  /** Enables/disables the detailed time logger (default:disabled upon construction)
112  * When enabled, a report will be dumped to std::cout upon destruction.
113  * \sa getTimeLogger
114  */
115  void enableTimeLog(bool enable=true) {
116  MRPT_UNUSED_PARAM(enable);
117  m_timelogger.enable(true);
118  }
119 
120  /** Gives access to a const-ref to the internal time logger \sa enableTimeLog */
121  const mrpt::utils::CTimeLogger & getTimeLogger() const { return m_timelogger; }
122 
123  /** Returns the number of different PTGs that have been setup */
124  virtual size_t getPTG_count() const = 0;
125 
126  /** Gets the i'th PTG */
127  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0;
128 
129  protected:
130  // ------------------------------------------------------
131  // INTERNAL DEFINITIONS
132  // ------------------------------------------------------
133  /** The structure used for storing a movement generated by a holonomic-method. */
135  CParameterizedTrajectoryGenerator *PTG; //!< The associated PTG
136  double direction, speed; //!< The holonomic movement
137  double evaluation; //!< An evaluation in the range [0,1] for the goodness of the movement
138 
139  THolonomicMovement() : PTG(NULL),direction(0),speed(0),evaluation(0) {}
140  };
141 
142  // ------------------------------------------------------
143  // PRIVATE METHODS
144  // ------------------------------------------------------
145  /** The main method for the navigator */
146  void performNavigationStep( );
147 
148  // ------------------------------------------------------
149  // PRIVATE VARIABLES
150  // ------------------------------------------------------
151  std::vector<CAbstractHolonomicReactiveMethod*> m_holonomicMethod; //!< The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
152  mrpt::utils::CStream *m_logFile; //!< The current log file stream, or NULL if not being used
153  bool m_enableKeepLogRecords; //!< See enableKeepLogRecords
154  CLogFileRecord lastLogRecord; //!< The last log
155  float last_cmd_v,last_cmd_w, new_cmd_v, new_cmd_w; //!< Speed actual and last commands:
156  bool navigationEndEventSent; //!< Will be false until the navigation end is sent, and it is reset with each new command
157  synch::CCriticalSection m_critZoneLastLog,m_critZoneNavigating; //!< Critical zones
158 
159  bool m_enableConsoleOutput; //!< Enables / disables the console debug output.
160  bool m_init_done; //!< Whether \a loadConfigFile() has been called or not.
162 
163  // PTG params loaded from INI file:
164  std::string robotName; //!< Robot name
165  float refDistance; //!< "D_{max}" in papers.
166  float colGridRes; //!< CollisionGrid resolution
167  float robotMax_V_mps; //!< Max. linear speed (m/s)
168  float robotMax_W_degps; //!< Max. angular speed (deg/s)
169  float SPEEDFILTER_TAU; //!< Time constant for the low-pass filter applied to the speed commands
170  std::vector<float> weights; //!< length: 6 [0,5]
171 
172  /** In normalized distances, the start and end of a ramp function that scales the velocity
173  * output from the holonomic navigator:
174  *
175  * \code
176  * velocity scale
177  * ^
178  * | _____________
179  * | /
180  * 1 | /
181  * | /
182  * 0 +-------+---|----------------> normalized distance
183  * Start
184  * End
185  * \endcode
186  *
187  */
188  float secureDistanceStart,secureDistanceEnd;
189 
190 
192  float meanExecutionPeriod; //!< Runtime estimation of execution period of the method.
193  mrpt::utils::CTimeLogger m_timelogger; //!< A complete time logger \sa enableTimeLog()
194 
195  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... */
199 
201 
202  /** Stops the robot and set navigation state to error */
203  void doEmergencyStop( const char *msg );
204 
205  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
206  @{ */
207  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
208  float meanExecutionTime, meanTotalExecutionTime;
209  /** @} */
210 
211  // Steps for the reactive navigation sytem.
212  // ----------------------------------------------------------------------------
213  virtual void STEP1_CollisionGridsBuilder() = 0;
214 
215  /** Return false on any fatal error */
216  virtual bool STEP2_SenseObstacles() = 0;
217 
218  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
219  * "out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index.
220  * Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return. */
221  virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx,std::vector<float> &out_TPObstacles) = 0;
222 
223  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
224  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) = 0;
225 
226 
227  /** Scores \a holonomicMovement */
228  void STEP5_PTGEvaluator(
229  THolonomicMovement & holonomicMovement,
230  const std::vector<float> & in_TPObstacles,
231  const mrpt::math::TPose2D & WS_Target,
232  const mrpt::math::TPoint2D & TP_Target,
234 
235  virtual void STEP7_GenerateSpeedCommands(const THolonomicMovement &in_movement);
236 
237 
238  void preDestructor(); //!< To be called during children destructors to assure thread-safe destruction, and free of shared objects.
239 
240 
241  private:
242  bool m_closing_navigator; //!< Signal that the destructor has been called, so no more calls are accepted from other threads
243 
244  struct TInfoPerPTG
245  {
246  bool valid_TP; //!< For each PTG, whether the target falls into the PTG domain.
247  mrpt::math::TPoint2D TP_Target; //!< The Target, in TP-Space (x,y)
248  float target_alpha,target_dist; //!< TP-Target
249  int target_k;
250 
251  std::vector<float> TP_Obstacles; //!< One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
252  };
253 
254  std::vector<TInfoPerPTG> m_infoPerPTG; //!< Temporary buffers for working with each PTG during a navigationStep()
255 
256 
257  void deleteHolonomicObjects(); //!< Delete m_holonomicMethod
258 
259 
260  }; // end of CAbstractPTGBasedReactive
261  }
262 }
263 
264 
265 #endif
266 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
This class provides simple critical sections functionality.
float SPEEDFILTER_TAU
Time constant for the low-pass filter applied to the speed commands.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
mrpt::utils::CStream * m_logFile
The current log file stream, or NULL if not being used.
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
bool m_enableConsoleOutput
Enables / disables the console debug output.
This is the base class for any reactive navigation system.
float new_cmd_w
Speed actual and last commands:
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
This class allows loading and storing values and vectors of different types from a configuration text...
bool valid_TP
For each PTG, whether the target falls into the PTG domain.
mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime
float robotMax_V_mps
Max. linear speed (m/s)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This is the base class for any user-defined PTG.
std::vector< CAbstractHolonomicReactiveMethod * > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained) ...
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
The structure used for storing a movement generated by a holonomic-method.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
double evaluation
An evaluation in the range [0,1] for the goodness of the movement.
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads...
THolonomicMethod
The implemented reactive navigation methods.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
std::vector< float > weights
length: 6 [0,5]
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() ...
float secureDistanceStart
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
bool m_init_done
Whether loadConfigFile() has been called or not.
float robotMax_W_degps
Max. angular speed (deg/s)
bool navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
Lightweight 2D pose.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:35
void enableTimeLog(bool enable=true)
Enables/disables the detailed time logger (default:disabled upon construction) When enabled...
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes...
float badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
The pure virtual class that a user of CAbstractReactiveNavigationSystem-derived classes must implemen...
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
Lightweight 2D point.
std::vector< float > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
The structure used to store all relevant information about each transformation into TP-Space...
float colGridRes
CollisionGrid resolution.
std::vector< size_t > restrict_PTG_indices
(Default=empty) Optionally, a list of PTG indices can be sent such that the navigator will restrict i...
bool m_enableKeepLogRecords
See enableKeepLogRecords.
float meanExecutionPeriod
Runtime estimation of execution period of the method.
synch::CCriticalSection m_critZoneNavigating
Critical zones.



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Mon Oct 3 19:22:36 UTC 2016