Main MRPT website > C++ reference for MRPT 1.5.3
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-2017, 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 
19 #include <mrpt/utils/CTimeLogger.h>
20 #include <mrpt/system/datetime.h>
21 #include <mrpt/math/filters.h>
23 #include <mrpt/math/CPolygon.h>
25 
26 namespace mrpt
27 {
28  namespace nav
29  {
30  /** Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic
31  * reactive method running on it and any number of PTGs for transforming the navigation space.
32  * Both, the holonomic method and the PTGs can be customized by the apropriate user derived classes.
33  *
34  * How to use:
35  * - Instantiate a reactive navigation object (one of the derived classes of this virtual class).
36  * - A class with callbacks must be defined by the user and provided to the constructor (derived from CRobot2NavInterface)
37  * - loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file).
38  * - 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.
39  *
40  * For working examples, refer to the source code of the apps:
41  * - [ReactiveNavigationDemo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenavigationdemo/)
42  * - [ReactiveNav3D-Demo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenav3d-demo/)
43  *
44  * Publications:
45  * - See derived classes for papers on each specific method.
46  *
47  * Available "variables" or "score names" for each motion candidate (these can be used in runtime-compiled expressions
48  * in the configuration files of motion deciders):
49  *
50  * - `clearance`: Clearance (larger means larger distances to obstacles) for the path from "current pose" up to "end of trajectory".
51  * - `collision_free_distance`: Normalized [0,1] collision-free distance in selected path. For NOP candidates, the traveled distances is substracted.
52  * - `dist_eucl_final`: Euclidean distance (in the real-world WordSpace) between "end of trajectory" and target.
53  * - `eta`: Estimated Time of Arrival at "end of trajectory".
54  * - `hysteresis`: Measure of similarity with previous command [0,1]
55  * - `is_PTG_cont`: 1 (is "NOP" motion command), 0 otherwise
56  * - `is_slowdown`: 1 if PTG returns true in CParameterizedTrajectoryGenerator::supportSpeedAtTarget() for this step.
57  * - `move_cur_d`: Normalized distance already traveled over the selected PTG. Normally 0, unless in a "NOP motion".
58  * - `move_k`: Motion candidate path 0-based index.
59  * - `num_paths`: Number of paths in the PTG
60  * - `original_col_free_dist`: Only for "NOP motions", the collision-free distance when the motion command was originally issued.
61  * - `ptg_idx`: PTG index (0-based)
62  * - `ptg_priority`: Product of PTG getScorePriority() times PTG evalPathRelativePriority()
63  * - `ref_dist`: PTG ref distance [m]
64  * - `robpose_x`, `robpose_y`, `robpose_phi`: Robot pose ([m] and [rad]) at the "end of trajectory": at collision or at target distance.
65  * - `target_d_norm`: Normalized target distance. Can be >1 if distance is larger than ref_distance.
66  * - `target_dir`: Angle of target in TP-Space [rad]
67  * - `target_k`: Same as target_dir but in discrete path 0-based indices.
68  * - `WS_target_x`, `WS_target_y`: Target coordinates in realworld [m]
69  *
70  * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D
71  * \ingroup nav_reactive
72  */
74  {
75  public:
77 
78  /** The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes. */
80  {
81  /** (Default=empty) Optionally, a list of PTG indices can be sent such that
82  * the navigator will restrict itself to only employ those PTGs. */
83  std::vector<size_t> restrict_PTG_indices;
84 
85  virtual std::string getAsText() const MRPT_OVERRIDE;
86  virtual TNavigationParamsBase* clone() const MRPT_OVERRIDE { return new TNavigationParamsPTG(*this); }
87  protected:
88  virtual bool isEqual(const CAbstractNavigator::TNavigationParamsBase& o) const MRPT_OVERRIDE;
89  };
90 
91  /** Constructor.
92  * \param[in] react_iterf_impl An instance of an object that implement all the required interfaces to read from and control a robot.
93  * \param[in] enableConsoleOutput Can be set to false to reduce verbosity.
94  * \param[in] enableLogFile Set to true to enable creation of navigation log files, useful for inspection and debugging.
95  */
97  CRobot2NavInterface &react_iterf_impl,
98  bool enableConsoleOutput = true,
99  bool enableLogFile = false,
100  const std::string &logFileDirectory = std::string("./reactivenav.logs"));
101 
102  virtual ~CAbstractPTGBasedReactive();
103 
104  /** Must be called for loading collision grids, or the first navigation command may last a long time to be executed.
105  * Internally, it just calls STEP1_CollisionGridsBuilder().
106  */
107  void initialize() MRPT_OVERRIDE;
108 
109  /** Selects which one from the set of available holonomic methods will be used
110  * into transformed TP-Space, and sets its configuration from a configuration file.
111  * Available methods: class names of those derived from CAbstractHolonomicReactiveMethod
112  */
113  void setHolonomicMethod(const std::string & method, const mrpt::utils::CConfigFileBase & cfgBase);
114 
115  MRPT_DEPRECATED("Use the signature with a class name string instead of an enum.")
116  void setHolonomicMethod(THolonomicMethod method, const mrpt::utils::CConfigFileBase & cfgBase);
117 
118  /** Provides a copy of the last log record with information about execution.
119  * \param o An object where the log will be stored into.
120  * \note Log records are not prepared unless either "enableLogFile" is enabled in the constructor or "enableLogFile()" has been called.
121  */
122  void getLastLogRecord( CLogFileRecord &o );
123 
124  /** Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() */
125  void enableKeepLogRecords(bool enable=true) { m_enableKeepLogRecords=enable; }
126 
127  /** Enables/disables saving log files. */
128  void enableLogFile(bool enable);
129 
130  /** Changes the prefix for new log files. */
131  void setLogFileDirectory(const std::string &sDir) { m_navlogfiles_dir = sDir; }
132  std::string getLogFileDirectory() const { return m_navlogfiles_dir; }
133 
134 
136  {
137  std::string holonomic_method; //!< C++ class name of the holonomic navigation method to run in the transformed TP-Space
138  std::string motion_decider_method; //!< C++ class name of the motion chooser
139 
140  std::string ptg_cache_files_directory; //!< (Default: ".")
141  double ref_distance; //!< Maximum distance up to obstacles will be considered (D_{max} in papers).
142  double speedfilter_tau; //!< Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering)
143 
144  /** In normalized distances, the start and end of a ramp function that scales the velocity
145  * output from the holonomic navigator:
146  *
147  * \code
148  * velocity scale
149  * ^
150  * | _____________
151  * | /
152  * 1 | /
153  * | /
154  * 0 +-------+---|----------------> normalized distance
155  * Start
156  * End
157  * \endcode
158  *
159  */
160  double secure_distance_start, secure_distance_end;
162  double max_distance_predicted_actual_path; //!< Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05)
163  double min_normalized_free_space_for_ptg_continuation; //!< Min normalized dist [0,1] after current pose in a PTG continuation to allow it.
164 
167  bool evaluate_clearance; //!< Default: false
168  double max_dist_for_timebased_path_prediction; //!< Max dist [meters] to use time-based path prediction for NOP evaluation.
169 
170  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE;
171  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE;
173  };
174 
176 
177  virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE; // See base class docs!
178  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE; // See base class docs!
179 
180 
181  /** Enables/disables the detailed time logger (default:disabled upon construction)
182  * When enabled, a report will be dumped to std::cout upon destruction.
183  * \sa getTimeLogger
184  */
185  void enableTimeLog(bool enable=true) {
186  m_timelogger.enable(enable);
187  }
188 
189  /** Gives access to a const-ref to the internal time logger \sa enableTimeLog */
190  const mrpt::utils::CTimeLogger & getTimeLogger() const { return m_timelogger; }
191 
192  virtual size_t getPTG_count() const = 0; //!< Returns the number of different PTGs that have been setup
193  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0; //!< Gets the i'th PTG
194  virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0; //!< Gets the i'th PTG
195 
196  /** Get the current, global (honored for all PTGs) robot speed limits */
198  return params_abstract_ptg_navigator.robot_absolute_speed_limits;
199  }
200 
201  /** Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits */
203  return params_abstract_ptg_navigator.robot_absolute_speed_limits;
204  }
205 
206  void setTargetApproachSlowDownDistance(const double dist); //!< Changes this parameter in all inner holonomic navigator instances [m].
207  double getTargetApproachSlowDownDistance() const; //!< Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?)
208 
209  protected:
210  /** The main method for the navigator */
211  virtual void performNavigationStep() MRPT_OVERRIDE;
212 
213  std::vector<CAbstractHolonomicReactiveMethod*> m_holonomicMethod; //!< The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
214  mrpt::utils::CStream *m_logFile, *m_prev_logfile; //!< The current log file stream, or NULL if not being used
215  bool m_enableKeepLogRecords; //!< See enableKeepLogRecords
216  CLogFileRecord lastLogRecord; //!< The last log
217  mrpt::kinematics::CVehicleVelCmdPtr m_last_vel_cmd ; //!< Last velocity commands
218 
220 
221  bool m_enableConsoleOutput; //!< Enables / disables the console debug output.
222  bool m_init_done; //!< Whether \a loadConfigFile() has been called or not.
224 
225  mrpt::utils::CTimeLogger m_timelogger; //!< A complete time logger \sa enableTimeLog()
227 
228  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
229  @{ */
230  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
233  mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod; //!< Runtime estimation of execution period of the method.
234  mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr, timoff_obstacles_avr, timoff_curPoseAndSpeed_avr, timoff_sendVelCmd_avr;
235  /** @} */
236 
237  virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const MRPT_OVERRIDE; // See docs in base class
238 
239  // Steps for the reactive navigation sytem.
240  // ----------------------------------------------------------------------------
241  virtual void STEP1_InitPTGs() = 0;
242 
243  /** Return false on any fatal error */
244  virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) = 0;
245  bool STEP2_SenseObstacles();
246 
247  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
248  * "out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index.
249  * Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return. */
250  virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx,std::vector<double> &out_TPObstacles, mrpt::nav::ClearanceDiagram &out_clearance, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, const bool eval_clearance) = 0;
251 
252  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
253  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) = 0;
254 
255  struct PTGTarget
256  {
257  bool valid_TP; //!< For each PTG, whether target falls into the PTG domain.
258  mrpt::math::TPoint2D TP_Target; //!< The Target, in TP-Space (x,y)
259  double target_alpha, target_dist; //!< TP-Target
260  int target_k; //!< The discrete version of target_alpha
261 
262  PTGTarget() : valid_TP(false) {}
263  };
264 
265  /** Scores \a holonomicMovement */
266  void calc_move_candidate_scores(TCandidateMovementPTG & holonomicMovement,
267  const std::vector<double> & in_TPObstacles,
268  const mrpt::nav::ClearanceDiagram & in_clearance,
269  const std::vector<mrpt::math::TPose2D> & WS_Targets,
270  const std::vector<PTGTarget> & TP_Targets,
272  CLogFileRecord & newLogRec,
273  const bool this_is_PTG_continuation,
274  const mrpt::math::TPose2D &relPoseVelCmd_NOP,
275  const unsigned int ptg_idx4weights,
276  const mrpt::system::TTimeStamp tim_start_iteration);
277 
278  /** Return the [0,1] velocity scale of raw PTG cmd_vel */
279  virtual double generate_vel_cmd(const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd );
280  void STEP8_GenerateLogRecord(CLogFileRecord &newLogRec, const std::vector<mrpt::math::TPose2D>& relTargets, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd, int nPTGs, const bool best_is_NOP_cmdvel, const math::TPose2D &rel_cur_pose_wrt_last_vel_cmd_NOP, const math::TPose2D &rel_pose_PTG_origin_wrt_sense_NOP, const double executionTimeValue, const double tim_changeSpeed, const mrpt::system::TTimeStamp &tim_start_iteration);
281  void preDestructor(); //!< To be called during children destructors to assure thread-safe destruction, and free of shared objects.
282  virtual void onStartNewNavigation() MRPT_OVERRIDE;
283 
284  bool m_closing_navigator; //!< Signal that the destructor has been called, so no more calls are accepted from other threads
285 
288 
289  mrpt::nav::CMultiObjectiveMotionOptimizerBasePtr m_multiobjopt;
290 
291 
292  struct TInfoPerPTG
293  {
294  std::vector<PTGTarget> targets;
295  std::vector<double> TP_Obstacles; //!< One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
296  ClearanceDiagram clearance; //!< Clearance for each path
297  };
298 
299  std::vector<TInfoPerPTG> m_infoPerPTG; //!< Temporary buffers for working with each PTG during a navigationStep()
301 
302  void build_movement_candidate(CParameterizedTrajectoryGenerator * ptg,
303  const size_t indexPTG,
304  const std::vector<mrpt::math::TPose2D> &relTargets,
305  const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense,
306  TInfoPerPTG &ipf,
307  TCandidateMovementPTG &holonomicMovement,
308  CLogFileRecord &newLogRec,
309  const bool this_is_PTG_continuation,
311  const mrpt::system::TTimeStamp tim_start_iteration,
312  const TNavigationParams &navp = TNavigationParams(),
313  const mrpt::math::TPose2D &relPoseVelCmd_NOP = mrpt::poses::CPose2D()
314  );
315 
317  {
318  int ptg_index; //!< 0-based index of used PTG
319  int ptg_alpha_index; //!< Path index for selected PTG
320  mrpt::system::TTimeStamp tim_send_cmd_vel; //!< Timestamp of when the cmd was sent
321  TRobotPoseVel poseVel; //!< Robot pose & velocities and timestamp of when it was queried
322  double colfreedist_move_k; //!< TP-Obstacles in the move direction at the instant of picking this movement
324  double speed_scale; //!< [0,1] scale of the raw cmd_vel as generated by the PTG
326 
327  bool isValid() const;
328  void reset();
329  TSentVelCmd();
330  };
331 
333 
334  private:
335  void deleteHolonomicObjects(); //!< Delete m_holonomicMethod
336 
337  std::string m_navlogfiles_dir; //!< Default: "./reactivenav.logs"
338 
339  double m_expr_var_k, m_expr_var_k_target, m_expr_var_num_paths;
340  TNavigationParams *m_copy_prev_navParams; //!< A copy of last-iteration navparams, used to detect changes
341 
342  }; // end of CAbstractPTGBasedReactive
343  }
344 }
345 
346 
347 #endif
Recursive mutex: allow recursive locks by the owner thread.
mrpt::synch::CCriticalSectionRecursive m_critZoneLastLog
Critical zones.
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::CStream * m_prev_logfile
The current log file stream, or NULL if not being used.
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & getCurrentRobotSpeedLimits() const
Get the current, global (honored for all PTGs) robot speed limits.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
Base for all high-level navigation commands.
void setLogFileDirectory(const std::string &sDir)
Changes the prefix for new log files.
mrpt::utils::CTimeLogger m_timelogger
A complete time logger.
A base class for holonomic reactive navigation methods.
bool m_enableConsoleOutput
Enables / disables the console debug output.
std::string holonomic_method
C++ class name of the holonomic navigation method to run in the transformed TP-Space.
Clearance information for one particular PTG and one set of obstacles.
std::vector< double > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
int target_k
The discrete version of target_alpha.
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
Runtime estimation of execution period of the method.
This class extends CAbstractNavigator with the capability of following a list of waypoints.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
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) ...
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
TNavigationParams * m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
double min_normalized_free_space_for_ptg_continuation
Min normalized dist [0,1] after current pose in a PTG continuation to allow it.
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
std::string motion_decider_method
C++ class name of the motion chooser.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
double max_dist_for_timebased_path_prediction
Max dist [meters] to use time-based path prediction for NOP evaluation.
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
mrpt::nav::CMultiObjectiveMotionOptimizerBasePtr m_multiobjopt
1-order low-pass IIR filter.
Definition: filters.h:28
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads...
double secure_distance_start
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
THolonomicMethod
The implemented reactive navigation methods.
double speedfilter_tau
Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0:...
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits
Params related to speed limits.
ClearanceDiagram clearance
Clearance for each path.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CPointCloudFilterBasePtr m_WS_filter
Default: none.
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() ...
#define MRPT_DEPRECATED(msg)
Usage: MRPT_DEPRECATED("Use XX instead") void myFunc(double);.
bool m_init_done
Whether loadConfigFile() has been called or not.
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
Lightweight 2D pose.
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & changeCurrentRobotSpeedLimits()
Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a...
void enableTimeLog(bool enable=true)
Enables/disables the detailed time logger (default:disabled upon construction) When enabled...
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
bool valid_TP
For each PTG, whether target falls into the PTG domain.
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes...
double max_distance_predicted_actual_path
Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05) ...
mrpt::kinematics::CVehicleVelCmdPtr m_last_vel_cmd
Last velocity commands.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
Lightweight 2D point.
mrpt::system::TTimeStamp tim_send_cmd_vel
Timestamp of when the cmd was sent.
Parameters that may be used by cmdVel_limits() in any derived classes.
Dynamic state that may affect the PTG path parameterization.
The structure used to store all relevant information about each transformation into TP-Space...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
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.
Stores a candidate movement in TP-Space-based navigation.



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Oct 31 07:27:35 UTC 2017