Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
20 #include <mrpt/system/datetime.h>
21 #include <mrpt/math/filters.h>
22 #include <mrpt/math/CPolygon.h>
24 #include <memory> // unique_ptr
25 
26 namespace mrpt
27 {
28 namespace nav
29 {
30 /** Base class for reactive navigator systems based on TP-Space, with an
31  * arbitrary holonomic
32  * reactive method running on it and any number of PTGs for transforming the
33  * navigation space.
34  * Both, the holonomic method and the PTGs can be customized by the apropriate
35  * user derived classes.
36  *
37  * How to use:
38  * - Instantiate a reactive navigation object (one of the derived classes of
39  * this virtual class).
40  * - A class with callbacks must be defined by the user and provided to the
41  * constructor (derived from CRobot2NavInterface)
42  * - loadConfigFile() must be called to set up the bunch of parameters from a
43  * config file (could be a memory-based virtual config file).
44  * - navigationStep() must be called periodically in order to effectively run
45  * the navigation. This method will internally call the callbacks to gather
46  * sensor data and robot positioning data.
47  *
48  * For working examples, refer to the source code of the apps:
49  * -
50  * [ReactiveNavigationDemo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenavigationdemo/)
51  * -
52  * [ReactiveNav3D-Demo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenav3d-demo/)
53  *
54  * Publications:
55  * - See derived classes for papers on each specific method.
56  *
57  * Available "variables" or "score names" for each motion candidate (these can
58  * be used in runtime-compiled expressions
59  * in the configuration files of motion deciders):
60  *
61  * - `clearance`: Clearance (larger means larger distances to obstacles) for
62  * the path from "current pose" up to "end of trajectory".
63  * - `collision_free_distance`: Normalized [0,1] collision-free distance in
64  * selected path. For NOP candidates, the traveled distances is substracted.
65  * - `dist_eucl_final`: Euclidean distance (in the real-world WordSpace)
66  * between "end of trajectory" and target.
67  * - `eta`: Estimated Time of Arrival at "end of trajectory".
68  * - `holo_stage_eval`: Final evaluation of the selected direction from inside
69  * of the holonomic algorithm.
70  * - `hysteresis`: Measure of similarity with previous command [0,1]
71  * - `is_PTG_cont`: 1 (is "NOP" motion command), 0 otherwise
72  * - `is_slowdown`: 1 if PTG returns true in
73  * CParameterizedTrajectoryGenerator::supportSpeedAtTarget() for this step.
74  * - `move_cur_d`: Normalized distance already traveled over the selected PTG.
75  * Normally 0, unless in a "NOP motion".
76  * - `move_k`: Motion candidate path 0-based index.
77  * - `num_paths`: Number of paths in the PTG
78  * - `original_col_free_dist`: Only for "NOP motions", the collision-free
79  * distance when the motion command was originally issued.
80  * - `ptg_idx`: PTG index (0-based)
81  * - `ptg_priority`: Product of PTG getScorePriority() times PTG
82  * evalPathRelativePriority()
83  * - `ref_dist`: PTG ref distance [m]
84  * - `robpose_x`, `robpose_y`, `robpose_phi`: Robot pose ([m] and [rad]) at the
85  * "end of trajectory": at collision or at target distance.
86  * - `target_d_norm`: Normalized target distance. Can be >1 if distance is
87  * larger than ref_distance.
88  * - `target_dir`: Angle of target in TP-Space [rad]
89  * - `target_k`: Same as target_dir but in discrete path 0-based indices.
90  * - `WS_target_x`, `WS_target_y`: Target coordinates in realworld [m]
91  *
92  * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D
93  * \ingroup nav_reactive
94  */
96 {
97  public:
99 
100  /** The struct for configuring navigation requests to
101  * CAbstractPTGBasedReactive and derived classes. */
104  {
105  /** (Default=empty) Optionally, a list of PTG indices can be sent such
106  * that
107  * the navigator will restrict itself to only employ those PTGs. */
108  std::vector<size_t> restrict_PTG_indices;
109 
110  virtual std::string getAsText() const override;
111  virtual std::unique_ptr<TNavigationParams> clone() const override
112  {
113  return std::unique_ptr<TNavigationParams>(
114  new TNavigationParamsPTG(*this));
115  }
116 
117  protected:
118  virtual bool isEqual(
119  const CAbstractNavigator::TNavigationParamsBase& o) const override;
120  };
121 
122  /** Constructor.
123  * \param[in] react_iterf_impl An instance of an object that implement all
124  * the required interfaces to read from and control a robot.
125  * \param[in] enableConsoleOutput Can be set to false to reduce verbosity.
126  * \param[in] enableLogFile Set to true to enable creation of navigation
127  * log files, useful for inspection and debugging.
128  */
130  CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput = true,
131  bool enableLogFile = false, const std::string& logFileDirectory =
132  std::string("./reactivenav.logs"));
133 
134  virtual ~CAbstractPTGBasedReactive();
135 
136  /** Must be called for loading collision grids, or the first navigation
137  * command may last a long time to be executed.
138  * Internally, it just calls STEP1_CollisionGridsBuilder().
139  */
140  void initialize() override;
141 
142  /** Selects which one from the set of available holonomic methods will be
143  * used
144  * into transformed TP-Space, and sets its configuration from a
145  * configuration file.
146  * Available methods: class names of those derived from
147  * CAbstractHolonomicReactiveMethod
148  */
149  void setHolonomicMethod(
150  const std::string& method,
151  const mrpt::config::CConfigFileBase& cfgBase);
152 
153  /** Provides a copy of the last log record with information about execution.
154  * \param o An object where the log will be stored into.
155  * \note Log records are not prepared unless either "enableLogFile" is
156  * enabled in the constructor or "enableLogFile()" has been called.
157  */
159 
160  /** Enables keeping an internal registry of navigation logs that can be
161  * queried with getLastLogRecord() */
162  void enableKeepLogRecords(bool enable = true)
163  {
164  m_enableKeepLogRecords = enable;
165  }
166 
167  /** Enables/disables saving log files. */
168  void enableLogFile(bool enable);
169 
170  /** Changes the prefix for new log files. */
172  {
173  m_navlogfiles_dir = sDir;
174  }
177  {
178  /** C++ class name of the holonomic navigation method to run in the
179  * transformed TP-Space */
181  /** C++ class name of the motion chooser */
183 
184  /** (Default: ".") */
186  /** Maximum distance up to obstacles will be considered (D_{max} in
187  * papers). */
188  double ref_distance;
189  /** Time constant (in seconds) for the low-pass filter applied to
190  * kinematic velocity commands (default=0: no filtering) */
192 
193  /** In normalized distances, the start and end of a ramp function that
194  * scales the velocity
195  * output from the holonomic navigator:
196  *
197  * \code
198  * velocity scale
199  * ^
200  * | _____________
201  * | /
202  * 1 | /
203  * | /
204  * 0 +-------+---|----------------> normalized distance
205  * Start
206  * End
207  * \endcode
208  *
209  */
212  /** Max distance [meters] to discard current PTG and issue a new vel cmd
213  * (default= 0.05) */
215  /** Min normalized dist [0,1] after current pose in a PTG continuation
216  * to allow it. */
218 
219  /** Params related to speed limits. */
223  /** Default: false */
225  /** Max dist [meters] to use time-based path prediction for NOP
226  * evaluation. */
228 
229  virtual void loadFromConfigFile(
231  const std::string& s) override;
232  virtual void saveToConfigFile(
234  const std::string& s) const override;
236  };
237 
239 
240  virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c)
241  override; // See base class docs!
243  const override; // See base class docs!
244 
245  /** Enables/disables the detailed time logger (default:disabled upon
246  * construction)
247  * When enabled, a report will be dumped to std::cout upon destruction.
248  * \sa getTimeLogger
249  */
250  void enableTimeLog(bool enable = true) { m_timelogger.enable(enable); }
251  /** Gives access to a const-ref to the internal time logger \sa
252  * enableTimeLog */
254  {
255  return m_timelogger;
256  }
257 
258  /** Returns the number of different PTGs that have been setup */
259  virtual size_t getPTG_count() const = 0;
260  /** Gets the i'th PTG */
261  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0;
262  /** Gets the i'th PTG */
263  virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0;
264 
265  /** Get the current, global (honored for all PTGs) robot speed limits */
268  {
270  }
271 
272  /** Changes the current, global (honored for all PTGs) robot speed limits,
273  * via returning a reference to a structure that holds those limits */
276  {
278  }
279 
280  /** Changes this parameter in all inner holonomic navigator instances [m].
281  */
282  void setTargetApproachSlowDownDistance(const double dist);
283  /** Returns this parameter for the first inner holonomic navigator instances
284  * [m] (should be the same in all of them?) */
285  double getTargetApproachSlowDownDistance() const;
286 
287  protected:
288  /** The main method for the navigator */
289  virtual void performNavigationStep() override;
290 
291  /** The holonomic navigation algorithm (one object per PTG, so internal
292  * states are maintained) */
293  std::vector<CAbstractHolonomicReactiveMethod::Ptr> m_holonomicMethod;
294  std::unique_ptr<mrpt::io::CStream> m_logFile;
295  /** The current log file stream, or nullptr if not being used */
297  /** See enableKeepLogRecords */
299  /** The last log */
301  /** Last velocity commands */
303 
304  /** Critical zones */
305  std::recursive_mutex m_critZoneLastLog;
306 
307  /** Enables / disables the console debug output. */
309  /** Whether \a loadConfigFile() has been called or not. */
312 
313  /** A complete time logger \sa enableTimeLog() */
316 
317  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
318  @{ */
322  /** Runtime estimation of execution period of the method. */
326  /** @} */
327 
328  virtual bool impl_waypoint_is_reachable(
329  const mrpt::math::TPoint2D& wp_local_wrt_robot)
330  const override; // See docs in base class
331 
332  // Steps for the reactive navigation sytem.
333  // ----------------------------------------------------------------------------
334  virtual void STEP1_InitPTGs() = 0;
335 
336  /** Return false on any fatal error */
337  virtual bool implementSenseObstacles(
338  mrpt::system::TTimeStamp& obs_timestamp) = 0;
339  bool STEP2_SenseObstacles();
340 
341  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
342  * "out_TPObstacles" is already initialized to the proper length and
343  * maximum collision-free distance for each "k" trajectory index.
344  * Distances are in "pseudo-meters". They will be normalized automatically
345  * to [0,1] upon return. */
346  virtual void STEP3_WSpaceToTPSpace(
347  const size_t ptg_idx, std::vector<double>& out_TPObstacles,
348  mrpt::nav::ClearanceDiagram& out_clearance,
349  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
350  const bool eval_clearance) = 0;
351 
352  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in
353  * the logging record for the current timestep */
354  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord& out_log) = 0;
355 
356  struct PTGTarget
357  {
358  /** For each PTG, whether target falls into the PTG domain. */
359  bool valid_TP;
360  /** The Target, in TP-Space (x,y) */
362  /** TP-Target */
364  /** The discrete version of target_alpha */
365  int target_k;
366 
367  PTGTarget() : valid_TP(false) {}
368  };
369 
370  /** Scores \a holonomicMovement */
372  TCandidateMovementPTG& holonomicMovement,
373  const std::vector<double>& in_TPObstacles,
374  const mrpt::nav::ClearanceDiagram& in_clearance,
375  const std::vector<mrpt::math::TPose2D>& WS_Targets,
376  const std::vector<PTGTarget>& TP_Targets,
378  const bool this_is_PTG_continuation,
379  const mrpt::math::TPose2D& relPoseVelCmd_NOP,
380  const unsigned int ptg_idx4weights,
381  const mrpt::system::TTimeStamp tim_start_iteration,
383 
384  /** Return the [0,1] velocity scale of raw PTG cmd_vel */
385  virtual double generate_vel_cmd(
386  const TCandidateMovementPTG& in_movement,
389  CLogFileRecord& newLogRec,
390  const std::vector<mrpt::math::TPose2D>& relTargets, int nSelectedPTG,
391  const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd, int nPTGs,
392  const bool best_is_NOP_cmdvel,
393  const math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
394  const math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
395  const double executionTimeValue, const double tim_changeSpeed,
396  const mrpt::system::TTimeStamp& tim_start_iteration);
397 
398  /** To be called during children destructors to assure thread-safe
399  * destruction, and free of shared objects. */
400  void preDestructor();
401  virtual void onStartNewNavigation() override;
402 
403  /** Signal that the destructor has been called, so no more calls are
404  * accepted from other threads */
406 
408  /** Default: none */
410 
412 
413  struct TInfoPerPTG
414  {
415  std::vector<PTGTarget> targets;
416  /** One distance per discretized alpha value, describing the "polar
417  * plot" of TP obstacles. */
418  std::vector<double> TP_Obstacles;
419  /** Clearance for each path */
421  };
422 
423  /** Temporary buffers for working with each PTG during a navigationStep() */
424  std::vector<TInfoPerPTG> m_infoPerPTG;
426 
428  CParameterizedTrajectoryGenerator* ptg, const size_t indexPTG,
429  const std::vector<mrpt::math::TPose2D>& relTargets,
430  const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
431  TInfoPerPTG& ipf, TCandidateMovementPTG& holonomicMovement,
432  CLogFileRecord& newLogRec, const bool this_is_PTG_continuation,
434  const mrpt::system::TTimeStamp tim_start_iteration,
435  const TNavigationParams& navp = TNavigationParams(),
436  const mrpt::math::TPose2D& relPoseVelCmd_NOP =
437  mrpt::math::TPose2D(0, 0, 0));
438 
439  struct TSentVelCmd
440  {
441  /** 0-based index of used PTG */
443  /** Path index for selected PTG */
445  /** Timestamp of when the cmd was sent */
447  /** Robot pose & velocities and timestamp of when it was queried */
448  TRobotPoseVel poseVel;
449  /** TP-Obstacles in the move direction at the instant of picking this
450  * movement */
453  /** [0,1] scale of the raw cmd_vel as generated by the PTG */
454  double speed_scale;
457 
458  bool isValid() const;
459  void reset();
460  TSentVelCmd();
461  };
462 
464 
465  private:
466  /** Delete m_holonomicMethod */
467  void deleteHolonomicObjects();
468 
469  /** Default: "./reactivenav.logs" */
471 
473  /** A copy of last-iteration navparams, used to detect changes */
474  std::unique_ptr<TNavigationParams> m_copy_prev_navParams;
475 
476 }; // end of CAbstractPTGBasedReactive
477 } // namespace nav
478 } // namespace mrpt
479 
480 #endif
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
std::unique_ptr< mrpt::io::CStream > m_logFile
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & getCurrentRobotSpeedLimits() const
Get the current, global (honored for all PTGs) robot speed limits.
void setTargetApproachSlowDownDistance(const double dist)
Changes this parameter in all inner holonomic navigator instances [m].
std::recursive_mutex m_critZoneLastLog
Critical zones.
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
void initialize() override
Must be called for loading collision grids, or the first navigation command may last a long time to b...
Base for all high-level navigation commands.
void setLogFileDirectory(const std::string &sDir)
Changes the prefix for new log files.
A high-performance stopwatch, with typical resolution of nanoseconds.
A base class for holonomic reactive navigation methods.
void enableLogFile(bool enable)
Enables/disables saving log files.
std::vector< CAbstractHolonomicReactiveMethod::Ptr > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained) ...
const mrpt::system::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
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.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Put this macro inside any class with members that require {16,32,64}-byte memory alignment (e...
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...
void setHolonomicMethod(const std::string &method, const mrpt::config::CConfigFileBase &cfgBase)
Selects which one from the set of available holonomic methods will be used into transformed TP-Space...
GLdouble s
Definition: glext.h:3676
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: io/CStream.h:30
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
Last velocity commands.
virtual std::string getAsText() const override
Gets navigation params as a human-readable format.
The struct for configuring navigation requests to CWaypointsNavigator and derived classes...
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
void STEP8_GenerateLogRecord(CLogFileRecord &newLogRec, const std::vector< mrpt::math::TPose2D > &relTargets, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr &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)
The struct for configuring navigation requests.
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
virtual void performNavigationStep() override
The main method for the navigator.
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:6313
virtual double generate_vel_cmd(const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr &new_vel_cmd)
Return the [0,1] velocity scale of raw PTG cmd_vel.
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
std::string motion_decider_method
C++ class name of the motion chooser.
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
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...
virtual void onStartNewNavigation() override
Called whenever a new navigation has been started.
std::unique_ptr< TNavigationParams > m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
1-order low-pass IIR filter.
Definition: filters.h:26
GLsizei const GLchar ** string
Definition: glext.h:4101
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads...
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
Builds TP-Obstacles from Workspace obstacles for the given PTG.
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
double secure_distance_start
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp)=0
Return false on any fatal error.
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.
void enable(bool enabled=true)
ClearanceDiagram clearance
Clearance for each path.
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
mrpt::io::CStream * m_prev_logfile
The current log file stream, or nullptr if not being used.
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
void deleteHolonomicObjects()
Delete m_holonomicMethod.
void build_movement_candidate(CParameterizedTrajectoryGenerator *ptg, const size_t indexPTG, const std::vector< mrpt::math::TPose2D > &relTargets, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, TInfoPerPTG &ipf, TCandidateMovementPTG &holonomicMovement, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, mrpt::nav::CAbstractHolonomicReactiveMethod &holoMethod, const mrpt::system::TTimeStamp tim_start_iteration, const TNavigationParams &navp=TNavigationParams(), const mrpt::math::TPose2D &relPoseVelCmd_NOP=mrpt::math::TPose2D(0, 0, 0))
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() ...
CAbstractPTGBasedReactive(CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs"))
Constructor.
bool m_init_done
Whether loadConfigFile() has been called or not.
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
Lightweight 2D pose.
virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const override
Implements the way to waypoint is free function in children classes: true must be returned if...
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i&#39;th PTG.
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) ...
virtual bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
virtual std::unique_ptr< TNavigationParams > clone() const override
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.
void calc_move_candidate_scores(TCandidateMovementPTG &holonomicMovement, const std::vector< double > &in_TPObstacles, const mrpt::nav::ClearanceDiagram &in_clearance, const std::vector< mrpt::math::TPose2D > &WS_Targets, const std::vector< PTGTarget > &TP_Targets, CLogFileRecord::TInfoPerPTG &log, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, const mrpt::math::TPose2D &relPoseVelCmd_NOP, const unsigned int ptg_idx4weights, const mrpt::system::TTimeStamp tim_start_iteration, const mrpt::nav::CHolonomicLogFileRecord::Ptr &hlfr)
Scores holonomicMovement.
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
The structure used to store all relevant information about each transformation into TP-Space...
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
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.
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
double getTargetApproachSlowDownDistance() const
Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in a...
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
Stores a candidate movement in TP-Space-based navigation.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019