Main MRPT website > C++ reference for MRPT 1.5.7
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  * - `holo_stage_eval`: Final evaluation of the selected direction from inside of the holonomic algorithm.
55  * - `hysteresis`: Measure of similarity with previous command [0,1]
56  * - `is_PTG_cont`: 1 (is "NOP" motion command), 0 otherwise
57  * - `is_slowdown`: 1 if PTG returns true in CParameterizedTrajectoryGenerator::supportSpeedAtTarget() for this step.
58  * - `move_cur_d`: Normalized distance already traveled over the selected PTG. Normally 0, unless in a "NOP motion".
59  * - `move_k`: Motion candidate path 0-based index.
60  * - `num_paths`: Number of paths in the PTG
61  * - `original_col_free_dist`: Only for "NOP motions", the collision-free distance when the motion command was originally issued.
62  * - `ptg_idx`: PTG index (0-based)
63  * - `ptg_priority`: Product of PTG getScorePriority() times PTG evalPathRelativePriority()
64  * - `ref_dist`: PTG ref distance [m]
65  * - `robpose_x`, `robpose_y`, `robpose_phi`: Robot pose ([m] and [rad]) at the "end of trajectory": at collision or at target distance.
66  * - `target_d_norm`: Normalized target distance. Can be >1 if distance is larger than ref_distance.
67  * - `target_dir`: Angle of target in TP-Space [rad]
68  * - `target_k`: Same as target_dir but in discrete path 0-based indices.
69  * - `WS_target_x`, `WS_target_y`: Target coordinates in realworld [m]
70  *
71  * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D
72  * \ingroup nav_reactive
73  */
75  {
76  public:
78 
79  /** The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes. */
81  {
82  /** (Default=empty) Optionally, a list of PTG indices can be sent such that
83  * the navigator will restrict itself to only employ those PTGs. */
84  std::vector<size_t> restrict_PTG_indices;
85 
86  virtual std::string getAsText() const MRPT_OVERRIDE;
87  virtual TNavigationParams* clone() const MRPT_OVERRIDE { return new TNavigationParamsPTG(*this); }
88  protected:
89  virtual bool isEqual(const CAbstractNavigator::TNavigationParamsBase& o) const MRPT_OVERRIDE;
90  };
91 
92  /** Constructor.
93  * \param[in] react_iterf_impl An instance of an object that implement all the required interfaces to read from and control a robot.
94  * \param[in] enableConsoleOutput Can be set to false to reduce verbosity.
95  * \param[in] enableLogFile Set to true to enable creation of navigation log files, useful for inspection and debugging.
96  */
98  CRobot2NavInterface &react_iterf_impl,
99  bool enableConsoleOutput = true,
100  bool enableLogFile = false,
101  const std::string &logFileDirectory = std::string("./reactivenav.logs"));
102 
103  virtual ~CAbstractPTGBasedReactive();
104 
105  /** Must be called for loading collision grids, or the first navigation command may last a long time to be executed.
106  * Internally, it just calls STEP1_CollisionGridsBuilder().
107  */
108  void initialize() MRPT_OVERRIDE;
109 
110  /** Selects which one from the set of available holonomic methods will be used
111  * into transformed TP-Space, and sets its configuration from a configuration file.
112  * Available methods: class names of those derived from CAbstractHolonomicReactiveMethod
113  */
114  void setHolonomicMethod(const std::string & method, const mrpt::utils::CConfigFileBase & cfgBase);
115 
116  MRPT_DEPRECATED("Use the signature with a class name string instead of an enum.")
117  void setHolonomicMethod(THolonomicMethod method, const mrpt::utils::CConfigFileBase & cfgBase);
118 
119  /** Provides a copy of the last log record with information about execution.
120  * \param o An object where the log will be stored into.
121  * \note Log records are not prepared unless either "enableLogFile" is enabled in the constructor or "enableLogFile()" has been called.
122  */
123  void getLastLogRecord( CLogFileRecord &o );
124 
125  /** Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() */
126  void enableKeepLogRecords(bool enable=true) { m_enableKeepLogRecords=enable; }
127 
128  /** Enables/disables saving log files. */
129  void enableLogFile(bool enable);
130 
131  /** Changes the prefix for new log files. */
132  void setLogFileDirectory(const std::string &sDir) { m_navlogfiles_dir = sDir; }
133  std::string getLogFileDirectory() const { return m_navlogfiles_dir; }
134 
135 
137  {
138  std::string holonomic_method; //!< C++ class name of the holonomic navigation method to run in the transformed TP-Space
139  std::string motion_decider_method; //!< C++ class name of the motion chooser
140 
142  double ref_distance; //!< Maximum distance up to obstacles will be considered (D_{max} in papers).
143  double speedfilter_tau; //!< Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering)
144 
145  /** In normalized distances, the start and end of a ramp function that scales the velocity
146  * output from the holonomic navigator:
147  *
148  * \code
149  * velocity scale
150  * ^
151  * | _____________
152  * | /
153  * 1 | /
154  * | /
155  * 0 +-------+---|----------------> normalized distance
156  * Start
157  * End
158  * \endcode
159  *
160  */
161  double secure_distance_start, secure_distance_end;
163  double max_distance_predicted_actual_path; //!< Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05)
164  double min_normalized_free_space_for_ptg_continuation; //!< Min normalized dist [0,1] after current pose in a PTG continuation to allow it.
165 
168  bool evaluate_clearance; //!< Default: false
169  double max_dist_for_timebased_path_prediction; //!< Max dist [meters] to use time-based path prediction for NOP evaluation.
170 
171  virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s) MRPT_OVERRIDE;
172  virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const MRPT_OVERRIDE;
174  };
175 
177 
178  virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE; // See base class docs!
179  virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE; // See base class docs!
180 
181 
182  /** Enables/disables the detailed time logger (default:disabled upon construction)
183  * When enabled, a report will be dumped to std::cout upon destruction.
184  * \sa getTimeLogger
185  */
186  void enableTimeLog(bool enable=true) {
187  m_timelogger.enable(enable);
188  }
189 
190  /** Gives access to a const-ref to the internal time logger \sa enableTimeLog */
191  const mrpt::utils::CTimeLogger & getTimeLogger() const { return m_timelogger; }
192 
193  virtual size_t getPTG_count() const = 0; //!< Returns the number of different PTGs that have been setup
194  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0; //!< Gets the i'th PTG
195  virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0; //!< Gets the i'th PTG
196 
197  /** Get the current, global (honored for all PTGs) robot speed limits */
199  return params_abstract_ptg_navigator.robot_absolute_speed_limits;
200  }
201 
202  /** Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits */
204  return params_abstract_ptg_navigator.robot_absolute_speed_limits;
205  }
206 
207  void setTargetApproachSlowDownDistance(const double dist); //!< Changes this parameter in all inner holonomic navigator instances [m].
208  double getTargetApproachSlowDownDistance() const; //!< Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?)
209 
210  protected:
211  /** The main method for the navigator */
212  virtual void performNavigationStep() MRPT_OVERRIDE;
213 
214  virtual CAbstractHolonomicReactiveMethod* getHoloMethod(int idx);
215  std::vector<CAbstractHolonomicReactiveMethod*> m_holonomicMethod; //!< The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
216  mrpt::utils::CStream *m_logFile, *m_prev_logfile; //!< The current log file stream, or NULL if not being used
217  bool m_enableKeepLogRecords; //!< See enableKeepLogRecords
218  CLogFileRecord lastLogRecord; //!< The last log
219  mrpt::kinematics::CVehicleVelCmdPtr m_last_vel_cmd ; //!< Last velocity commands
220 
221  mrpt::synch::CCriticalSectionRecursive m_critZoneLastLog; //!< Critical zones
222 
223  bool m_enableConsoleOutput; //!< Enables / disables the console debug output.
224  bool m_init_done; //!< Whether \a loadConfigFile() has been called or not.
225  mrpt::utils::CTicTac timerForExecutionPeriod;
226 
227  mrpt::utils::CTimeLogger m_timelogger; //!< A complete time logger \sa enableTimeLog()
228  bool m_PTGsMustBeReInitialized;
229 
230  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
231  @{ */
232  mrpt::utils::CTicTac totalExecutionTime, executionTime, tictac;
233  mrpt::math::LowPassFilter_IIR1 meanExecutionTime;
234  mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime;
235  mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod; //!< Runtime estimation of execution period of the method.
236  mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr, timoff_obstacles_avr, timoff_curPoseAndSpeed_avr, timoff_sendVelCmd_avr;
237  /** @} */
238 
239  virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const MRPT_OVERRIDE; // See docs in base class
240 
241  // Steps for the reactive navigation sytem.
242  // ----------------------------------------------------------------------------
243  virtual void STEP1_InitPTGs() = 0;
244 
245  /** Return false on any fatal error */
246  virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp) = 0;
247  bool STEP2_SenseObstacles();
248 
249  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
250  * "out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index.
251  * Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return. */
252  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;
253 
254  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep */
255  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log) = 0;
256 
257  struct PTGTarget
258  {
259  bool valid_TP; //!< For each PTG, whether target falls into the PTG domain.
260  mrpt::math::TPoint2D TP_Target; //!< The Target, in TP-Space (x,y)
261  double target_alpha, target_dist; //!< TP-Target
262  int target_k; //!< The discrete version of target_alpha
263 
264  PTGTarget() : valid_TP(false) {}
265  };
266 
267  /** Scores \a holonomicMovement */
268  void calc_move_candidate_scores(TCandidateMovementPTG & holonomicMovement,
269  const std::vector<double> & in_TPObstacles,
270  const mrpt::nav::ClearanceDiagram & in_clearance,
271  const std::vector<mrpt::math::TPose2D> & WS_Targets,
272  const std::vector<PTGTarget> & TP_Targets,
274  CLogFileRecord & newLogRec,
275  const bool this_is_PTG_continuation,
276  const mrpt::math::TPose2D &relPoseVelCmd_NOP,
277  const unsigned int ptg_idx4weights,
278  const mrpt::system::TTimeStamp tim_start_iteration,
279  const mrpt::nav::CHolonomicLogFileRecordPtr &hlfr);
280 
281  /** Return the [0,1] velocity scale of raw PTG cmd_vel */
282  virtual double generate_vel_cmd(const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd );
283  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);
284  void preDestructor(); //!< To be called during children destructors to assure thread-safe destruction, and free of shared objects.
285  virtual void onStartNewNavigation() MRPT_OVERRIDE;
286 
287  bool m_closing_navigator; //!< Signal that the destructor has been called, so no more calls are accepted from other threads
288 
289  mrpt::system::TTimeStamp m_WS_Obstacles_timestamp;
290  mrpt::maps::CPointCloudFilterBasePtr m_WS_filter; //!< Default: none
291 
292  mrpt::nav::CMultiObjectiveMotionOptimizerBasePtr m_multiobjopt;
293 
294 
295  struct TInfoPerPTG
296  {
297  std::vector<PTGTarget> targets;
298  std::vector<double> TP_Obstacles; //!< One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
299  ClearanceDiagram clearance; //!< Clearance for each path
300  };
301 
302  std::vector<TInfoPerPTG> m_infoPerPTG; //!< Temporary buffers for working with each PTG during a navigationStep()
304 
305  void build_movement_candidate(CParameterizedTrajectoryGenerator * ptg,
306  const size_t indexPTG,
307  const std::vector<mrpt::math::TPose2D> &relTargets,
308  const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense,
309  TInfoPerPTG &ipf,
310  TCandidateMovementPTG &holonomicMovement,
311  CLogFileRecord &newLogRec,
312  const bool this_is_PTG_continuation,
314  const mrpt::system::TTimeStamp tim_start_iteration,
315  const TNavigationParams &navp = TNavigationParams(),
316  const mrpt::math::TPose2D &relPoseVelCmd_NOP = mrpt::poses::CPose2D()
317  );
318 
320  {
321  int ptg_index; //!< 0-based index of used PTG
322  int ptg_alpha_index; //!< Path index for selected PTG
323  mrpt::system::TTimeStamp tim_send_cmd_vel; //!< Timestamp of when the cmd was sent
324  TRobotPoseVel poseVel; //!< Robot pose & velocities and timestamp of when it was queried
325  double colfreedist_move_k; //!< TP-Obstacles in the move direction at the instant of picking this movement
327  double speed_scale; //!< [0,1] scale of the raw cmd_vel as generated by the PTG
330 
331  bool isValid() const;
332  void reset();
333  TSentVelCmd();
334  };
335 
337 
338  private:
339  void deleteHolonomicObjects(); //!< Delete m_holonomicMethod
340 
341  std::string m_navlogfiles_dir; //!< Default: "./reactivenav.logs"
342 
343  double m_expr_var_k, m_expr_var_k_target, m_expr_var_num_paths;
344  TNavigationParams *m_copy_prev_navParams; //!< A copy of last-iteration navparams, used to detect changes
345 
346  }; // end of CAbstractPTGBasedReactive
347  }
348 }
349 
350 
351 #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
std::shared_ptr< CPointCloudFilterBase > CPointCloudFilterBasePtr
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
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.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019