Main MRPT website > C++ reference for MRPT 1.4.0
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-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 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 
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.*/
85  THolonomicMethod method,
86  const mrpt::utils::CConfigFileBase & cfgBase);
87 
88  /** Just loads the holonomic method params from the given config source \sa setHolonomicMethod */
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  */
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 */
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 */
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 ptg_cache_files_directory; //!< (Default: ".")
165  std::string robotName; //!< Robot name
166  float refDistance; //!< "D_{max}" in papers.
167  float colGridRes; //!< CollisionGrid resolution
168  float robotMax_V_mps; //!< Max. linear speed (m/s)
169  float robotMax_W_degps; //!< Max. angular speed (deg/s)
170  float SPEEDFILTER_TAU; //!< Time constant for the low-pass filter applied to the speed commands
171  std::vector<float> weights; //!< length: 6 [0,5]
172 
173  /** In normalized distances, the start and end of a ramp function that scales the velocity
174  * output from the holonomic navigator:
175  *
176  * \code
177  * velocity scale
178  * ^
179  * | _____________
180  * | /
181  * 1 | /
182  * | /
183  * 0 +-------+---|----------------> normalized distance
184  * Start
185  * End
186  * \endcode
187  *
188  */
189  float secureDistanceStart,secureDistanceEnd;
190 
191 
193  float meanExecutionPeriod; //!< Runtime estimation of execution period of the method.
194  mrpt::utils::CTimeLogger m_timelogger; //!< A complete time logger \sa enableTimeLog()
195 
196  /** For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... */
200 
202 
203  /** Stops the robot and set navigation state to error */
204  void doEmergencyStop( const char *msg );
205 
206  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
207  @{ */
208  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
209  float meanExecutionTime, meanTotalExecutionTime;
210  /** @} */
211 
212  // Steps for the reactive navigation sytem.
213  // ----------------------------------------------------------------------------
214  virtual void STEP1_CollisionGridsBuilder() = 0;
215 
216  /** Return false on any fatal error */
217  virtual bool STEP2_SenseObstacles() = 0;
218 
219  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
220  * "out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index.
221  * Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return. */
222  virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx,std::vector<float> &out_TPObstacles) = 0;
223 
224  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
225  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) = 0;
226 
227 
228  /** Scores \a holonomicMovement */
230  THolonomicMovement & holonomicMovement,
231  const std::vector<float> & in_TPObstacles,
232  const mrpt::math::TPose2D & WS_Target,
233  const mrpt::math::TPoint2D & TP_Target,
235 
236  virtual void STEP7_GenerateSpeedCommands(const THolonomicMovement &in_movement);
237 
238 
239  void preDestructor(); //!< To be called during children destructors to assure thread-safe destruction, and free of shared objects.
240 
241 
242  private:
243  bool m_closing_navigator; //!< Signal that the destructor has been called, so no more calls are accepted from other threads
244 
245  struct TInfoPerPTG
246  {
247  bool valid_TP; //!< For each PTG, whether the target falls into the PTG domain.
248  mrpt::math::TPoint2D TP_Target; //!< The Target, in TP-Space (x,y)
249  float target_alpha,target_dist; //!< TP-Target
250  int target_k;
251 
252  std::vector<float> TP_Obstacles; //!< One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
253  };
254 
255  std::vector<TInfoPerPTG> m_infoPerPTG; //!< Temporary buffers for working with each PTG during a navigationStep()
256 
257 
258  void deleteHolonomicObjects(); //!< Delete m_holonomicMethod
259 
260 
261  }; // end of CAbstractPTGBasedReactive
262  }
263 }
264 
265 
266 #endif
267 
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
float colGridRes
CollisionGrid resolution.
bool m_enableKeepLogRecords
See enableKeepLogRecords.
bool m_enableConsoleOutput
Enables / disables the console debug output.
virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log)=0
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the c...
float meanExecutionPeriod
Runtime estimation of execution period of the method.
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
std::string ptg_cache_files_directory
(Default: ".")
float badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
bool m_init_done
Whether loadConfigFile() has been called or not.
void performNavigationStep()
The main method for the navigator.
std::vector< CAbstractHolonomicReactiveMethod * > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
virtual void navigate(const TNavigationParams *params)
Start navigation:
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads.
void initialize()
Must be called for loading collision grids, or the first navigation command may last a long time to b...
mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime
void deleteHolonomicObjects()
Delete m_holonomicMethod.
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
CAbstractPTGBasedReactive(CReactiveInterfaceImplementation &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false)
Constructor.
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
void enableLogFile(bool enable)
Enables/disables saving log files.
mrpt::utils::CStream * m_logFile
The current log file stream, or NULL if not being used.
float SPEEDFILTER_TAU
Time constant for the low-pass filter applied to the speed commands.
std::vector< float > weights
length: 6 [0,5]
float robotMax_W_degps
Max. angular speed (deg/s)
virtual void STEP7_GenerateSpeedCommands(const THolonomicMovement &in_movement)
void setHolonomicMethod(THolonomicMethod method, const mrpt::utils::CConfigFileBase &cfgBase)
Selects which one from the set of available holonomic methods will be used into transformed TP-Space,...
virtual void STEP1_CollisionGridsBuilder()=0
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i'th PTG.
virtual bool STEP2_SenseObstacles()=0
Return false on any fatal error.
void enableTimeLog(bool enable=true)
Enables/disables the detailed time logger (default:disabled upon construction) When enabled,...
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
void loadHolonomicMethodConfig(const mrpt::utils::CConfigFileBase &ini, const std::string &section)
Just loads the holonomic method params from the given config source.
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
void doEmergencyStop(const char *msg)
Stops the robot and set navigation state to error.
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
void STEP5_PTGEvaluator(THolonomicMovement &holonomicMovement, const std::vector< float > &in_TPObstacles, const mrpt::math::TPose2D &WS_Target, const mrpt::math::TPoint2D &TP_Target, CLogFileRecord::TInfoPerPTG &log)
Scores holonomicMovement.
virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx, std::vector< float > &out_TPObstacles)=0
Builds TP-Obstacles from Workspace obstacles for the given PTG.
bool navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
This is the base class for any reactive navigation system.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
This is the base class for any user-defined PTG.
The pure virtual class that a user of CAbstractReactiveNavigationSystem-derived classes must implemen...
This class provides simple critical sections functionality.
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
This class implements a high-performance stopwatch.
Definition: CTicTac.h:25
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: CTimeLogger.h:36
THolonomicMethod
The implemented reactive navigation methods.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D point.
Lightweight 2D pose.
The structure used for storing a movement generated by a holonomic-method.
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
double evaluation
An evaluation in the range [0,1] for the goodness of the movement.
bool valid_TP
For each PTG, whether the target falls into the PTG domain.
std::vector< float > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes.
virtual std::string getAsText() const
Gets navigation params as a human-readable format.
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...
The structure used to store all relevant information about each transformation into TP-Space.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Sun Mar 7 18:43:46 UTC 2021