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
21
22namespace 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
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 */
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
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
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 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.
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()
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i'th PTG.
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.5 for MRPT 1.4.0 SVN: at Mon Dec 26 04:51:47 UTC 2022