Main MRPT website > C++ reference for MRPT 1.5.5
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 TNavigationParams* 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 
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 
219  mrpt::synch::CCriticalSectionRecursive m_critZoneLastLog; //!< Critical zones
220 
221  bool m_enableConsoleOutput; //!< Enables / disables the console debug output.
222  bool m_init_done; //!< Whether \a loadConfigFile() has been called or not.
223  mrpt::utils::CTicTac timerForExecutionPeriod;
224 
225  mrpt::utils::CTimeLogger m_timelogger; //!< A complete time logger \sa enableTimeLog()
226  bool m_PTGsMustBeReInitialized;
227 
228  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
229  @{ */
230  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
231  mrpt::math::LowPassFilter_IIR1 meanExecutionTime;
232  mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime;
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 
286  mrpt::system::TTimeStamp m_WS_Obstacles_timestamp;
287  mrpt::maps::CPointCloudFilterBasePtr m_WS_filter; //!< Default: none
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
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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.
A base class for holonomic reactive navigation methods.
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.
STL namespace.
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.
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...
GLdouble s
Definition: glext.h:3602
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)
The struct for configuring navigation requests.
This is the base class for any user-defined PTG.
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.
const GLubyte * c
Definition: glext.h:5590
stlplus::smart_ptr< CPointCloudFilterBase > CPointCloudFilterBasePtr
std::string motion_decider_method
C++ class name of the motion chooser.
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...
const mrpt::utils::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
GLsizei const GLchar ** string
Definition: glext.h:3919
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
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.
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
#define MRPT_DEPRECATED(msg)
Usage: MRPT_DEPRECATED("Use XX instead") void myFunc(double);.
GLboolean reset
Definition: glext.h:3535
Lightweight 2D pose.
GLuint const GLchar * name
Definition: glext.h:3891
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::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) ...
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.
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
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...
Stores a candidate movement in TP-Space-based navigation.



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019