Go to the documentation of this file.
9 #ifndef CAbstractPTGBasedReactive_H
10 #define CAbstractPTGBasedReactive_H
111 virtual std::unique_ptr<TNavigationParams>
clone()
const override
113 return std::unique_ptr<TNavigationParams>(
347 const size_t ptg_idx, std::vector<double>& out_TPObstacles,
350 const bool eval_clearance) = 0;
373 const std::vector<double>& in_TPObstacles,
375 const std::vector<mrpt::math::TPose2D>& WS_Targets,
376 const std::vector<PTGTarget>& TP_Targets,
378 const bool this_is_PTG_continuation,
380 const unsigned int ptg_idx4weights,
390 const std::vector<mrpt::math::TPose2D>& relTargets,
int nSelectedPTG,
392 const bool best_is_NOP_cmdvel,
395 const double executionTimeValue,
const double tim_changeSpeed,
429 const std::vector<mrpt::math::TPose2D>& relTargets,
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
Runtime estimation of execution period of the method.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
virtual bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
virtual std::unique_ptr< TNavigationParams > clone() const override
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
virtual ~CAbstractPTGBasedReactive()
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.
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes.
void enableTimeLog(bool enable=true)
Enables/disables the detailed time logger (default:disabled upon construction) When enabled,...
Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive met...
std::shared_ptr< CHolonomicLogFileRecord > Ptr
double ref_distance
Maximum distance up to obstacles will be considered (D_{max} in papers).
1-order low-pass IIR filter.
ClearanceDiagram clearance
Clearance for each path.
virtual std::string getAsText() const override
Gets navigation params as a human-readable format.
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
std::shared_ptr< CPointCloudFilterBase > Ptr
A high-performance stopwatch, with typical resolution of nanoseconds.
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
std::shared_ptr< CMultiObjectiveMotionOptimizerBase > Ptr
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
mrpt::system::TTimeStamp m_infoPerPTG_timestamp
bool m_enableKeepLogRecords
See enableKeepLogRecords.
mrpt::system::TTimeStamp m_WS_Obstacles_timestamp
int target_k
The discrete version of target_alpha.
void initialize() override
Must be called for loading collision grids, or the first navigation command may last a long time to b...
mrpt::system::TTimeStamp tim_send_cmd_vel
Timestamp of when the cmd was sent.
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
double m_expr_var_k_target
double m_expr_var_num_paths
double original_holo_eval
std::vector< CAbstractHolonomicReactiveMethod::Ptr > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
void enableLogFile(bool enable)
Enables/disables saving log files.
Parameters that may be used by cmdVel_limits() in any derived classes.
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
std::vector< PTGTarget > targets
double speedfilter_tau
Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0:...
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,...
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.
int ptg_index
0-based index of used PTG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
The struct for configuring navigation requests to CWaypointsNavigator and derived classes.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
Last velocity commands.
void setLogFileDirectory(const std::string &sDir)
Changes the prefix for new log files.
std::recursive_mutex m_critZoneLastLog
Critical zones.
mrpt::io::CStream * m_prev_logfile
The current log file stream, or nullptr if not being used.
This class extends CAbstractNavigator with the capability of following a list of waypoints.
bool evaluate_clearance
Default: false.
std::string holonomic_method
C++ class name of the holonomic navigation method to run in the transformed TP-Space.
std::vector< double > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits
Params related to speed limits.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
mrpt::system::CTicTac tictac
double secure_distance_end
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.
bool m_PTGsMustBeReInitialized
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...
void setTargetApproachSlowDownDistance(const double dist)
Changes this parameter in all inner holonomic navigator instances [m].
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
The struct for configuring navigation requests.
double target_alpha
TP-Target.
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
void enableKeepLogRecords(bool enable=true)
Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()
A base class for holonomic reactive navigation methods.
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
bool enable_obstacle_filtering
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i'th PTG.
std::string getLogFileDirectory() const
void deleteHolonomicObjects()
Delete m_holonomicMethod.
bool STEP2_SenseObstacles()
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))
mrpt::system::CTicTac totalExecutionTime
This class allows loading and storing values and vectors of different types from a configuration text...
Clearance information for one particular PTG and one set of obstacles.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
Stores a candidate movement in TP-Space-based navigation.
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Dynamic state that may affect the PTG path parameterization.
virtual void performNavigationStep() override
The main method for the navigator.
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads.
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
double max_distance_predicted_actual_path
Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05)
The structure used to store all relevant information about each transformation into TP-Space.
bool m_enableConsoleOutput
Enables / disables the console debug output.
TSentVelCmd m_lastSentVelCmd
double min_normalized_free_space_for_ptg_continuation
Min normalized dist [0,1] after current pose in a PTG continuation to allow it.
std::shared_ptr< CVehicleVelCmd > Ptr
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
std::string ptg_cache_files_directory
(Default: ".")
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)
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
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,...
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.
const mrpt::system::CTimeLogger & getTimeLogger() const
Gives access to a const-ref to the internal time logger.
bool m_init_done
Whether loadConfigFile() has been called or not.
void enable(bool enabled=true)
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
double getTargetApproachSlowDownDistance() const
Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in a...
double max_dist_for_timebased_path_prediction
Max dist [meters] to use time-based path prediction for NOP evaluation.
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
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.
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & getCurrentRobotSpeedLimits() const
Get the current, global (honored for all PTGs) robot speed limits.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Put this macro inside any class with members that require {16,32,64}-byte memory alignment (e....
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...
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
GLsizei const GLchar ** string
std::string motion_decider_method
C++ class name of the motion chooser.
bool valid_TP
For each PTG, whether target falls into the PTG domain.
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & changeCurrentRobotSpeedLimits()
Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a...
TAbstractPTGNavigatorParams()
virtual void STEP1_InitPTGs()=0
std::unique_ptr< TNavigationParams > m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
mrpt::system::CTicTac executionTime
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.
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
int ptg_alpha_index
Path index for selected PTG.
mrpt::system::CTicTac timerForExecutionPeriod
This is the base class for any user-defined PTG.
std::unique_ptr< mrpt::io::CStream > m_logFile
CAbstractPTGBasedReactive(CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs"))
Constructor.
Base for all high-level navigation commands.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
virtual void onStartNewNavigation() override
Called whenever a new navigation has been started.
CLogFileRecord lastLogRecord
The last log.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |