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 
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 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 */
229  void STEP5_PTGEvaluator(
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 
CAbstractHolonomicReactiveMethod.h
mrpt::nav::CAbstractPTGBasedReactive::m_logFile
mrpt::utils::CStream * m_logFile
The current log file stream, or NULL if not being used.
Definition: CAbstractPTGBasedReactive.h:152
mrpt::nav::CAbstractPTGBasedReactive::THolonomicMovement::evaluation
double evaluation
An evaluation in the range [0,1] for the goodness of the movement.
Definition: CAbstractPTGBasedReactive.h:137
mrpt::nav::CLogFileRecord
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
Definition: CLogFileRecord.h:30
CParameterizedTrajectoryGenerator.h
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes.
Definition: CAbstractPTGBasedReactive.h:52
mrpt::nav::CAbstractPTGBasedReactive::enableTimeLog
void enableTimeLog(bool enable=true)
Enables/disables the detailed time logger (default:disabled upon construction) When enabled,...
Definition: CAbstractPTGBasedReactive.h:115
mrpt::nav::CAbstractPTGBasedReactive
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
Definition: CAbstractPTGBasedReactive.h:46
mrpt::nav::CAbstractPTGBasedReactive::secureDistanceStart
float secureDistanceStart
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
Definition: CAbstractPTGBasedReactive.h:189
mrpt::nav::CAbstractPTGBasedReactive::navigationEndEventSent
bool navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
Definition: CAbstractPTGBasedReactive.h:156
mrpt::nav::CAbstractPTGBasedReactive::THolonomicMovement::THolonomicMovement
THolonomicMovement()
Definition: CAbstractPTGBasedReactive.h:139
mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords
bool m_enableKeepLogRecords
See enableKeepLogRecords.
Definition: CAbstractPTGBasedReactive.h:153
CHolonomicVFF.h
mrpt::nav::CAbstractPTGBasedReactive::robotMax_V_mps
float robotMax_V_mps
Max. linear speed (m/s)
Definition: CAbstractPTGBasedReactive.h:168
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::valid_TP
bool valid_TP
For each PTG, whether the target falls into the PTG domain.
Definition: CAbstractPTGBasedReactive.h:247
mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod
std::vector< CAbstractHolonomicReactiveMethod * > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
Definition: CAbstractPTGBasedReactive.h:151
mrpt::nav::THolonomicMethod
THolonomicMethod
The implemented reactive navigation methods.
Definition: CAbstractHolonomicReactiveMethod.h:27
mrpt::nav::CAbstractPTGBasedReactive::meanTotalExecutionTime
float meanTotalExecutionTime
Definition: CAbstractPTGBasedReactive.h:209
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG::clone
virtual TNavigationParams * clone() const
Definition: CAbstractPTGBasedReactive.h:61
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
mrpt::nav::CAbstractPTGBasedReactive::new_cmd_w
float new_cmd_w
Speed actual and last commands:
Definition: CAbstractPTGBasedReactive.h:155
mrpt::nav::CAbstractPTGBasedReactive::SPEEDFILTER_TAU
float SPEEDFILTER_TAU
Time constant for the low-pass filter applied to the speed commands.
Definition: CAbstractPTGBasedReactive.h:170
mrpt::nav::CAbstractReactiveNavigationSystem
This is the base class for any reactive navigation system.
Definition: CAbstractReactiveNavigationSystem.h:102
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG::~TNavigationParamsPTG
virtual ~TNavigationParamsPTG()
Definition: CAbstractPTGBasedReactive.h:59
CAbstractReactiveNavigationSystem.h
mrpt::utils::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: CTimeLogger.h:35
mrpt::nav::CAbstractPTGBasedReactive::THolonomicMovement::PTG
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
Definition: CAbstractPTGBasedReactive.h:135
mrpt::nav::CAbstractPTGBasedReactive::getTimeLogger
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
Definition: CAbstractPTGBasedReactive.h:121
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
mrpt::utils::CTicTac
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::nav::CAbstractPTGBasedReactive::m_timelogger
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
Definition: CAbstractPTGBasedReactive.h:194
mrpt::nav::CAbstractPTGBasedReactive::enableKeepLogRecords
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()
Definition: CAbstractPTGBasedReactive.h:106
mrpt::nav::CAbstractPTGBasedReactive::THolonomicMovement
The structure used for storing a movement generated by a holonomic-method.
Definition: CAbstractPTGBasedReactive.h:134
MRPT_MAKE_ALIGNED_OPERATOR_NEW
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
CHolonomicND.h
mrpt::synch::CCriticalSection
This class provides simple critical sections functionality.
Definition: CCriticalSection.h:31
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::target_k
int target_k
Definition: CAbstractPTGBasedReactive.h:250
mrpt::nav::CAbstractPTGBasedReactive::robotName
std::string robotName
Robot name.
Definition: CAbstractPTGBasedReactive.h:165
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:30
mrpt::nav::CAbstractPTGBasedReactive::badNavAlarm_lastMinDistTime
mrpt::system::TTimeStamp badNavAlarm_lastMinDistTime
Definition: CAbstractPTGBasedReactive.h:198
mrpt::nav::CAbstractPTGBasedReactive::robotMax_W_degps
float robotMax_W_degps
Max. angular speed (deg/s)
Definition: CAbstractPTGBasedReactive.h:169
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:148
mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads.
Definition: CAbstractPTGBasedReactive.h:243
CTimeLogger.h
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::TP_Obstacles
std::vector< float > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
Definition: CAbstractPTGBasedReactive.h:252
mrpt::nav::CAbstractPTGBasedReactive::m_collisionGridsMustBeUpdated
bool m_collisionGridsMustBeUpdated
Definition: CAbstractPTGBasedReactive.h:201
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:33
mrpt::nav::CAbstractPTGBasedReactive::totalExecutionTime
mrpt::utils::CTicTac totalExecutionTime
Definition: CAbstractPTGBasedReactive.h:208
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG::TNavigationParamsPTG
TNavigationParamsPTG()
Definition: CAbstractPTGBasedReactive.h:58
mrpt::nav::CAbstractPTGBasedReactive::timerForExecutionPeriod
mrpt::utils::CTicTac timerForExecutionPeriod
Definition: CAbstractPTGBasedReactive.h:161
mrpt::nav::CLogFileRecord::TInfoPerPTG
The structure used to store all relevant information about each transformation into TP-Space.
Definition: CLogFileRecord.h:44
mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod
float meanExecutionPeriod
Runtime estimation of execution period of the method.
Definition: CAbstractPTGBasedReactive.h:193
mrpt::nav::CAbstractPTGBasedReactive::m_enableConsoleOutput
bool m_enableConsoleOutput
Enables / disables the console debug output.
Definition: CAbstractPTGBasedReactive.h:159
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG
Definition: CAbstractPTGBasedReactive.h:245
CLogFileRecord.h
mrpt::nav::CAbstractPTGBasedReactive::colGridRes
float colGridRes
CollisionGrid resolution.
Definition: CAbstractPTGBasedReactive.h:167
mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
Definition: CAbstractPTGBasedReactive.h:255
mrpt::nav::CReactiveInterfaceImplementation
The pure virtual class that a user of CAbstractReactiveNavigationSystem-derived classes must implemen...
Definition: CAbstractReactiveNavigationSystem.h:30
mrpt::nav::CAbstractPTGBasedReactive::ptg_cache_files_directory
std::string ptg_cache_files_directory
(Default: ".")
Definition: CAbstractPTGBasedReactive.h:164
mrpt::nav::CAbstractPTGBasedReactive::m_init_done
bool m_init_done
Whether loadConfigFile() has been called or not.
Definition: CAbstractPTGBasedReactive.h:160
mrpt::nav::CAbstractPTGBasedReactive::m_critZoneNavigating
synch::CCriticalSection m_critZoneNavigating
Critical zones.
Definition: CAbstractPTGBasedReactive.h:157
mrpt::nav::CAbstractPTGBasedReactive::DIST_TO_TARGET_FOR_SENDING_EVENT
float DIST_TO_TARGET_FOR_SENDING_EVENT
Definition: CAbstractPTGBasedReactive.h:192
mrpt::nav::CAbstractPTGBasedReactive::badNavAlarm_AlarmTimeout
float badNavAlarm_AlarmTimeout
Definition: CAbstractPTGBasedReactive.h:199
mrpt::nav::CAbstractPTGBasedReactive::refDistance
float refDistance
"D_{max}" in papers.
Definition: CAbstractPTGBasedReactive.h:166
mrpt::nav::CAbstractPTGBasedReactive::TNavigationParamsPTG::restrict_PTG_indices
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...
Definition: CAbstractPTGBasedReactive.h:56
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::TP_Target
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
Definition: CAbstractPTGBasedReactive.h:248
mrpt::nav::CAbstractReactiveNavigationSystem::TNavigationParams
The struct for configuring navigation requests.
Definition: CAbstractReactiveNavigationSystem.h:106
CCriticalSection.h
mrpt::nav::CAbstractPTGBasedReactive::THolonomicMovement::speed
double speed
The holonomic movement.
Definition: CAbstractPTGBasedReactive.h:136
mrpt::nav::CAbstractPTGBasedReactive::badNavAlarm_minDistTarget
float badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
Definition: CAbstractPTGBasedReactive.h:197
mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::target_dist
float target_dist
TP-Target.
Definition: CAbstractPTGBasedReactive.h:249
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:56
datetime.h
mrpt::nav::CAbstractPTGBasedReactive::lastLogRecord
CLogFileRecord lastLogRecord
The last log.
Definition: CAbstractPTGBasedReactive.h:154
mrpt::nav::CAbstractPTGBasedReactive::weights
std::vector< float > weights
length: 6 [0,5]
Definition: CAbstractPTGBasedReactive.h:171



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:08:25 UTC 2019