MRPT  2.0.4
List of all members | Classes | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes
mrpt::nav::CReactiveNavigationSystem3D Class Reference

Detailed Description

See base class CAbstractPTGBasedReactive for a description and instructions of use.

This particular implementation assumes a 3D (or "2.5D") robot shape model, build as a vertical stack of "2D slices".

Paper describing the method:

Class history:

This class requires a number of parameters which are usually provided via an external config (".ini") file. Alternatively, a memory-only object can be used to avoid physical files, see mrpt::config::CConfigFileMemory.

A template config file can be generated at any moment by the user by calling saveConfigFile() with a default-constructed object.

Next we provide a self-documented template config file; or see it online: https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/navigation-ptgs/reactive3d_config.ini

# ------------------------------------------------------------------------------------------
# Example configuration file for MRPT (>=1.5) Reactive Navigation engine.
# See C++ documentation: https://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_reactive_navigation_system3_d.html
# This .ini file is missing information about the equipped sensors and the Short-Term memory
# For more detailed information, please see the App "ReactiveNav3D-Demo" within MRPT
# ------------------------------------------------------------------------------------------

# Max linear vel (m/s):
@define ROBOT_MAX_V  1.0
# Max angular vel (deg/s):
@define ROBOT_MAX_W  60.0
# Max distance to "foresee" obstacles (m):
@define NAV_MAX_REF_DIST   10.0

[CAbstractNavigator]
dist_to_target_for_sending_event                  = 0.000000             // Default value=0, means use the `targetAllowedDistance` passed by the user in the navigation request.
alarm_seems_not_approaching_target_timeout        = 30.000000            // navigator timeout (seconds) [Default=30 sec]
dist_check_target_is_blocked                 = 2.0       // When closer than this distance, check if the target is blocked to abort navigation with an error
hysteresis_check_target_is_blocked           = 3         // How many steps should the condition for dist_check_target_is_blocked be fulfilled to raise an event
enable_time_profiler                         = false


[CWaypointsNavigator]
max_distance_to_allow_skip_waypoint               = -1.000000            // Max distance to `foresee` waypoints [meters]. (<0: unlimited)
min_timesteps_confirm_skip_waypoints              = 1                    // Min timesteps a `future` waypoint must be seen as reachable to become the active one.
waypoint_angle_tolerance                          = 5.0                  // Angular error tolerance for waypoints with an assigned heading [deg]
multitarget_look_ahead   = 0  // >=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance when a waypoint is blocked (Default=0 : none).


[CAbstractPTGBasedReactive]
robotMax_V_mps                                    = ${ROBOT_MAX_V}       // Max. linear speed (m/s) [Default=-1 (not set), will raise exception if needed and not set]
robotMax_W_degps                                  = ${ROBOT_MAX_W}       // Max. angular speed (rad/s) [Default=-1 (not set), will raise exception if needed and not set]
#robotMinCurvRadius                                = -1.000000            // Min. radius of curvature of paths (m) [Default=-1 (not set), will raise exception if needed and not set]

holonomic_method                                  = CHolonomicFullEval   // C++ class name of the holonomic navigation method to run in the transformed TP-Space.
# List of known classes:
# - `CHolonomicFullEval`
# - `CHolonomicND`
# - `CHolonomicVFF`


motion_decider_method                             = CMultiObjMotionOpt_Scalarization // C++ class name of the motion decider method.
# List of known classes:
# - `CMultiObjMotionOpt_Scalarization`


ref_distance                                      = ${NAV_MAX_REF_DIST}   // Maximum distance up to obstacles will be considered (D_{max} in papers).
#speedfilter_tau                                   = 0.000000             // Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering)
secure_distance_start                             = 0.050000             // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity.
secure_distance_end                               = 0.150000             // In normalized distance [0,1], start/end of a ramp function that scales the holonomic navigator output velocity.
use_delays_model                                  = false                // Whether to use robot pose inter/extrapolation to improve accuracy (Default:false)
max_distance_predicted_actual_path                = 0.150000             // Max distance [meters] to discard current PTG and issue a new vel cmd (default= 0.05)
min_normalized_free_space_for_ptg_continuation    = 0.200000             // Min normalized dist [0,1] after current pose in a PTG continuation to allow it.

enable_obstacle_filtering                         = true                 // Enabled obstacle filtering (params in its own section)
evaluate_clearance                                = false


[CPointCloudFilterByDistance]
min_dist                                          = 0.100000
angle_tolerance                                   = 5.000000
too_old_seconds                                   = 1.000000
previous_keyframes                                = 1                    // (Default: 1) How many previous keyframes will be compared with the latest pointcloud.
max_deletion_ratio                                = 0.400000             // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it would be too suspicious and may indicate a failure of this filter.


[CHolonomicFullEval]
# [0]=Free space
# [1]=Dist. in sectors
# [2]=Closer to target (Euclidean)
# [3]=Hysteresis
# [5]=like [2] but without being decimated if path obstructed
# [6]=closeness to target_k
factorWeights                    = [0.25 , 0.1 , 1  , 0.05 , 0.5, 1, 1 ]
factorNormalizeOrNot             = [0    , 0   , 0  , 0    , 1  , 1, 0 ]

TOO_CLOSE_OBSTACLE                                = 0.15                 // Directions with collision-free distances below this threshold are not elegible.
TARGET_SLOW_APPROACHING_DISTANCE                  = 0.300000             // Start to reduce speed when closer than this to target.
OBSTACLE_SLOW_DOWN_DISTANCE                       = 0.150000             // Start to reduce speed when clearance is below this value ([0,1] ratio wrt obstacle reference/max distance)
HYSTERESIS_SECTOR_COUNT                           = 5.000000             // Range of `sectors` (directions) for hysteresis over successive timesteps
LOG_SCORE_MATRIX                                  = false                // Save the entire score matrix in log files

clearance_threshold_ratio                         = 0.10                 // Ratio [0,1], times path_count, gives the minimum number of paths at each side of a target direction to be accepted as desired direction
gap_width_ratio_threshold                         = 0.20                 // Ratio [0,1], times path_count, gives the minimum gap width to accept a direct motion towards target.

PHASE_COUNT                                       = 3                    // Number of evaluation phases to run (params for each phase below)
PHASE1_FACTORS                                    = [6]                // Indices of the factors above to be considered in this phase
PHASE1_THRESHOLD                                  = 0.5                  // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)
PHASE2_FACTORS                                    = [4]                  // Indices of the factors above to be considered in this phase
PHASE2_THRESHOLD                                  = 0.7                  // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)
PHASE3_FACTORS                                    = [0 2]                // Indices of the factors above to be considered in this phase
PHASE3_THRESHOLD                                  = 0.7                  // Phase scores must be above this relative range threshold [0,1] to be considered in next phase (Default:`0.75`)


[CHolonomicND]
WIDE_GAP_SIZE_PERCENT                             = 0.250000
MAX_SECTOR_DIST_FOR_D2_PERCENT                    = 0.250000
RISK_EVALUATION_SECTORS_PERCENT                   = 0.100000
RISK_EVALUATION_DISTANCE                          = 0.400000             // In normalized ps-meters [0,1]
TOO_CLOSE_OBSTACLE                                = 0.150000             // For stopping gradually
TARGET_SLOW_APPROACHING_DISTANCE                  = 0.600000             // In normalized ps-meters
factorWeights                                     = 1.00 0.50 2.00 0.40  // [0]=Free space, [1]=Dist. in sectors, [2]=Closer to target (Euclidean), [3]=Hysteresis


[CHolonomicVFF]
TARGET_SLOW_APPROACHING_DISTANCE                  = 0.100000             // For stopping gradually
TARGET_ATTRACTIVE_FORCE                           = 20.000000            // Dimension-less (may have to be tuned depending on the density of obstacle sampling)

[CMultiObjectiveMotionOptimizerBase]
# Next follows a list of `score%i_{name,formula}` pairs for i=1,...,N
# Each one defines an exprtk formula for one of the scores that will be evaluated for each candidate movement.
# Multiobjective optimizers will then use those scores to select the best candidate,
# possibly using more parameters that follow below.
# See list of all available variables in documentation of mrpt::nav::CAbstractPTGBasedReactive at https://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_abstract_p_t_g_based_reactive.html

score1_name     = target_distance
score1_formula  = \
	var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); \
	if(collision_free_distance>effective_trg_d_norm, \
		1/(1+effective_trg_d_norm^2), \
		0)

score2_name     = collision_free_distance_score
score2_formula  = \
	var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); \
	if (collision_free_distance>(effective_trg_d_norm+0.05), \
		1.0, \
		collision_free_distance)

score3_name     = euclidean_nearness
score3_formula  = 1/(1+dist_eucl_min^2)

score4_name     = hysteresis_score
score4_formula  = hysteresis

score5_name     = path_index_near_target
score5_formula  = \
	var dif:=abs(target_k-move_k); \
	if (dif>(num_paths/2)) \
	{ \
		dif:=num_paths-dif; \
	}; \
	exp(-abs(dif / (num_paths/10.0)));

score6_name        = ptg_priority_score
score6_formula     = ptg_priority


# Next follows a list of `movement_assert%i` exprtk expressions for i=1,...,N
# defining expressions for conditions that any candidate movement must fulfill
# in order to get through the evaluation process. *All* assert conditions must be satisfied.
#movement_assert1 = XXX

# Comma-separated list of scores to normalize so the highest is 1.0.
scores_to_normalize  =
#target_distance

[CMultiObjMotionOpt_Scalarization]
# A formula that takes all/a subset of scores and generates a scalar global score.
scalar_score_formula  = ptg_priority_score*( \
	0.3*target_distance + \
	0.5*collision_free_distance_score + \
	8.0*euclidean_nearness + \
	0.1 * hysteresis_score + \
	0.2*path_index_near_target \
	)


[CReactiveNavigationSystem3D]
min_obstacles_height                              = 0.000000             // Minimum `z` coordinate of obstacles to be considered fo collision checking
max_obstacles_height                              = 10.000000            // Maximum `z` coordinate of obstacles to be considered fo collision checking

#Indicate the geometry of the robot as a set of prisms.
#Format - (LEVELX_HEIGHT, LEVELX_VECTORX, LEVELX_VECTORY)

#Number of height levels used to model the robot
HEIGHT_LEVELS = 3

LEVEL1_HEIGHT = 0.6
LEVEL1_VECTORX = 0.3 0.3 -0.3 -0.3
LEVEL1_VECTORY = -0.3 0.3 0.3 -0.3

LEVEL2_HEIGHT = 0.9
LEVEL2_VECTORX = 0.05 0.05 -0.05 -0.05
LEVEL2_VECTORY = -0.1 0.1 0.1 -0.1

LEVEL3_HEIGHT = 0.6
LEVEL3_VECTORX = 0.30 -0.15 -0.15
LEVEL3_VECTORY = 0 0.2 -0.2


# PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( https://reference.mrpt.org/svn/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html)
# refer to papers for details.
#------------------------------------------------------------------------------
PTG_COUNT      = 1    // Was: 3 (Reduced to speed-up unit tests!)

# C-PTG (circular arcs), driving forward (K=+1)
PTG1_Type      = CPTG_DiffDrive_C
PTG1_resolution = 0.02      # Look-up-table cell size or resolution (in meters)
PTG1_refDistance= ${NAV_MAX_REF_DIST}      # Max distance to account for obstacles
PTG1_num_paths = 41   // Was: 121 (Reduced to speed-up unit tests!)
PTG1_v_max_mps = ${ROBOT_MAX_V}
PTG1_w_max_dps = ${ROBOT_MAX_W}
PTG1_K         = 1.0
PTG1_score_priority = 1.0

# alpha-a PTG
PTG2_Type        = CPTG_DiffDrive_alpha
PTG2_resolution = 0.02      # Look-up-table cell size or resolution (in meters)
PTG2_refDistance= ${NAV_MAX_REF_DIST}      # Max distance to account for obstacles
PTG2_num_paths   = 121
PTG2_v_max_mps   = ${ROBOT_MAX_V}
PTG2_w_max_dps   = ${ROBOT_MAX_W}
PTG2_cte_a0v_deg = 57
PTG2_cte_a0w_deg = 57
PTG2_score_priority = 1.0

# C-PTG (circular arcs), backwards (K=-1)
# lower priority since we prefer driving forward
PTG3_Type      = CPTG_DiffDrive_C
PTG3_resolution = 0.02      # Look-up-table cell size or resolution (in meters)
PTG3_refDistance= ${NAV_MAX_REF_DIST}      # Max distance to account for obstacles
PTG3_num_paths = 121
PTG3_v_max_mps = ${ROBOT_MAX_V}
PTG3_w_max_dps = ${ROBOT_MAX_W}
PTG3_K         = -1.0
PTG3_score_priority = 1.0



# Parameters for the SIMULATOR in the app: ReactiveNav3D-Demo:
# ---------------------------------------------------------------
[LASER_CONFIG]
N_LASERS = 3

# Indicate the lasers parameters. This information must be consistent with the robot shape (height sections).
# Laser pose is relative to the robot coordinate system.
# Information required: 	LASERX_POSE, LASERX_MAX_RANGE, LASERX_APERTURE
# 							LASERX_STD_ERROR, LASERX_LEVEL, LASERX_SEGMENTS
LASER1_POSE = 0 0 0.4 0 0 0
LASER1_MAX_RANGE = 30
LASER1_APERTURE = 3.141592
LASER1_STD_ERROR = 0.02
LASER1_LEVEL = 1
LASER1_SEGMENTS = 181

LASER2_POSE = 0 0 1.1 0 0 0
LASER2_MAX_RANGE = 30
LASER2_APERTURE = 3.141592
LASER2_STD_ERROR = 0.02
LASER2_LEVEL = 2
LASER2_SEGMENTS = 181

LASER3_POSE = 0 0 1.8 0 0 0
LASER3_MAX_RANGE = 30
LASER3_APERTURE = 3.141592
LASER3_STD_ERROR = 0.02
LASER3_LEVEL = 3
LASER3_SEGMENTS = 181


[KINECT_CONFIG]
# Kinects declaration
# Indicate the number of kinects

N_KINECTS = 1

# Indicate the kinect parameters. This information must be consistent with the robot shape (height sections).
# Kinect pose is relative to the robot coordinate system.
# Information required: KINECTX_LEVEL, KINECTX_X, KINECTX_Y, KINECTX_Z, KINECTX_PHI (DEGREES), KINECTX_PITCH_ANGLE (DEGREES),
# 						KINECTX_MINRANGE, KINECTX_MAXRANGE (METERS), KINECTX_ROWS, KINECTX_COLUMNS, KINECTX_STD_ERROR

KINECT1_LEVEL = 2
KINECT1_X = 0
KINECT1_Y = 0
KINECT1_Z = 1
KINECT1_PHI = 0
KINECT1_PITCH = 0
KINECT1_MINRANGE = 0.3
KINECT1_MAXRANGE = 5
KINECT1_FOV_V = 45
KINECT1_FOV_H = 58
KINECT1_ROWS = 21
KINECT1_COLUMNS = 21
KINECT1_STD_ERROR = 0.0


[MAP_CONFIG]
#  The maps are included in the ReactiveNav3D_demo.h file as .xpm files.
#  To modify them change this included files, and the method CMyReactInterface::loadmaps() if necessary
MAP_RESOLUTION = 0.02

#Initial robot position in the simulator
X0 = 2   // [m]
Y0 = 0   // [m]
PHI0 = -90  // [deg]



# Parameters for the Obstacles grid (short term memory),
# ------------------------------------------------------------
[STM_CONFIG]
Stm_active = 1 \n			; Utilize it(1) or not(0)
Obs_grid_length = 2		; (lenght/resolution) has to be integer
Obs_grid_resolution = 0.04
Vision_limit = 0.5			; Min. Limit of vision of the RGBD sensor
Pos_likelihood_incr = 0.8	; Range: 0.51 - 1
Neg_likelihood_incr = 0.4	; Range: 0 - 0.49
Occupancy_threshold = 0.8	; Threshold used to include or not a virtual obstacle
See also
CAbstractNavigator, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod

Definition at line 83 of file CReactiveNavigationSystem3D.h.

#include <mrpt/nav/reactive/CReactiveNavigationSystem3D.h>

Inheritance diagram for mrpt::nav::CReactiveNavigationSystem3D:

Classes

struct  TPTGmultilevel
 A set of PTGs of the same type, one per "height level". More...
 

Public Member Functions

 CReactiveNavigationSystem3D (CRobot2NavInterface &react_iterf_impl, bool enableConsoleOutput=true, bool enableLogFile=false, const std::string &logFileDirectory=std::string("./reactivenav.logs"))
 See docs in ctor of base class. More...
 
 ~CReactiveNavigationSystem3D () override
 Destructor. More...
 
void changeRobotShape (TRobotShape robotShape)
 Change the robot shape, which is taken into account for collision grid building. More...
 
bool checkCollisionWithLatestObstacles (const mrpt::math::TPose2D &relative_robot_pose) const override
 Checks whether the robot shape, when placed at the given pose (relative to the current pose), is colliding with any of the latest known obstacles. More...
 
size_t getPTG_count () const override
 Returns the number of different PTGs that have been setup. More...
 
CParameterizedTrajectoryGeneratorgetPTG (size_t i) override
 Gets the i'th PTG. More...
 
const CParameterizedTrajectoryGeneratorgetPTG (size_t i) const override
 Gets the i'th PTG. More...
 
void loadConfigFile (const mrpt::config::CConfigFileBase &c) override
 Loads all params from a file. More...
 
void saveConfigFile (mrpt::config::CConfigFileBase &c) const override
 Saves all current options to a config file. More...
 
void initialize () override
 Must be called for loading collision grids, or the first navigation command may last a long time to be executed. More...
 
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, and sets its configuration from a configuration file. More...
 
void getLastLogRecord (CLogFileRecord &o)
 Provides a copy of the last log record with information about execution. More...
 
void enableKeepLogRecords (bool enable=true)
 Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord() More...
 
void enableLogFile (bool enable)
 Enables/disables saving log files. More...
 
void setLogFileDirectory (const std::string &sDir)
 Changes the prefix for new log files. More...
 
std::string getLogFileDirectory () const
 
void enableTimeLog (bool enable=true)
 Enables/disables the detailed time logger (default:disabled upon construction) When enabled, a report will be dumped to std::cout upon destruction. More...
 
const mrpt::system::CTimeLoggergetTimeLogger () const
 Gives access to a const-ref to the internal time logger. More...
 
const mrpt::kinematics::CVehicleVelCmd::TVelCmdParamsgetCurrentRobotSpeedLimits () const
 Get the current, global (honored for all PTGs) robot speed limits. More...
 
mrpt::kinematics::CVehicleVelCmd::TVelCmdParamschangeCurrentRobotSpeedLimits ()
 Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits. More...
 
void setTargetApproachSlowDownDistance (const double dist)
 Changes this parameter in all inner holonomic navigator instances [m]. More...
 
double getTargetApproachSlowDownDistance () const
 Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?) More...
 
void navigationStep () override
 This method must be called periodically in order to effectively run the navigation. More...
 
void cancel () override
 Cancel current navegation. More...
 
bool isRelativePointReachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const
 Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc. More...
 
const mrpt::system::CTimeLoggergetDelaysTimeLogger () const
 Gives access to a const-ref to the internal time logger used to estimate delays. More...
 
Waypoint navigation control API
virtual void navigateWaypoints (const TWaypointSequence &nav_request)
 Waypoint navigation request. More...
 
virtual void getWaypointNavStatus (TWaypointStatusSequence &out_nav_status) const
 Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
 
TWaypointStatusSequence getWaypointNavStatus () const
 Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
 

Static Public Member Functions

static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_colors ()
 Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More...
 
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_names ()
 Map from VerbosityLevels to their corresponding names. More...
 

Public Attributes

TAbstractPTGNavigatorParams params_abstract_ptg_navigator
 
TWaypointsNavigatorParams params_waypoints_navigator
 
TAbstractNavigatorParams params_abstract_navigator
 
mrpt::system::CTimeLogger m_navProfiler
 Publicly available time profiling object. More...
 

Protected Member Functions

void performNavigationStep () override
 The main method for the navigator. More...
 
virtual CAbstractHolonomicReactiveMethodgetHoloMethod (int idx)
 
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, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range, etc. More...
 
bool STEP2_SenseObstacles ()
 
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. More...
 
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. More...
 
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)
 
void preDestructor ()
 To be called during children destructors to assure thread-safe destruction, and free of shared objects. More...
 
void onStartNewNavigation () override
 Called whenever a new navigation has been started. More...
 
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 onNavigateCommandReceived () override
 Called after each call to CAbstractNavigator::navigate() More...
 
bool checkHasReachedTarget (const double targetDist) const override
 Default implementation: check if target_dist is below the accepted distance. More...
 
virtual void waypoints_navigationStep ()
 The waypoints-specific part of navigationStep() More...
 
bool waypoints_isAligning () const
 
void dispatchPendingNavEvents ()
 
virtual void updateCurrentPoseAndSpeeds ()
 Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly. More...
 
virtual void performNavigationStepNavigating (bool call_virtual_nav_method=true)
 Factorization of the part inside navigationStep(), for the case of state being NAVIGATING. More...
 
virtual void processNavigateCommand (const TNavigationParams *params)
 Does the job of navigate(), except the call to onNavigateCommandReceived() More...
 
virtual void doEmergencyStop (const std::string &msg)
 Stops the robot and set navigation state to error. More...
 
virtual bool changeSpeeds (const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
 Default: forward call to m_robot.changeSpeed(). More...
 
virtual bool changeSpeedsNOP ()
 Default: forward call to m_robot.changeSpeedsNOP(). More...
 
virtual bool stop (bool isEmergencyStop)
 Default: forward call to m_robot.stop(). More...
 

Protected Attributes

std::vector< CAbstractHolonomicReactiveMethod::Ptrm_holonomicMethod
 The holonomic navigation algorithm (one object per PTG, so internal states are maintained) More...
 
std::unique_ptr< mrpt::io::CStreamm_logFile
 
mrpt::io::CStreamm_prev_logfile {nullptr}
 The current log file stream, or nullptr if not being used. More...
 
bool m_enableKeepLogRecords {false}
 See enableKeepLogRecords. More...
 
CLogFileRecord lastLogRecord
 The last log. More...
 
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
 Last velocity commands. More...
 
std::recursive_mutex m_critZoneLastLog
 Critical zones. More...
 
bool m_enableConsoleOutput
 Enables / disables the console debug output. More...
 
bool m_init_done {false}
 Whether loadConfigFile() has been called or not. More...
 
mrpt::system::CTicTac timerForExecutionPeriod
 
mrpt::system::CTimeLogger m_timelogger {false}
 A complete time logger. More...
 
bool m_PTGsMustBeReInitialized {true}
 
bool m_closing_navigator {false}
 Signal that the destructor has been called, so no more calls are accepted from other threads. More...
 
mrpt::system::TTimeStamp m_WS_Obstacles_timestamp {INVALID_TIMESTAMP}
 
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
 Default: none. More...
 
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
 
std::vector< TInfoPerPTGm_infoPerPTG
 Temporary buffers for working with each PTG during a navigationStep() More...
 
mrpt::system::TTimeStamp m_infoPerPTG_timestamp {INVALID_TIMESTAMP}
 
TSentVelCmd m_lastSentVelCmd
 
TWaypointStatusSequence m_waypoint_nav_status
 The latest waypoints navigation command and the up-to-date control status. More...
 
std::recursive_mutex m_nav_waypoints_cs
 
bool m_was_aligning {false}
 Whether the last timestep was "is_aligning" in a waypoint with heading. More...
 
bool m_is_aligning {false}
 
mrpt::system::TTimeStamp m_last_alignment_cmd
 
std::list< std::function< void(void)> > m_pending_events
 Events generated during navigationStep(), enqueued to be called at the end of the method execution to avoid user code to change the navigator state. More...
 
TState m_navigationState {IDLE}
 Current internal state of navigator: More...
 
std::unique_ptr< TNavigationParamsm_navigationParams
 Current navigation parameters. More...
 
CRobot2NavInterfacem_robot
 The navigator-robot interface. More...
 
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
 Optional, user-provided frame transformer. More...
 
std::recursive_mutex m_nav_cs
 mutex for all navigation methods More...
 
TRobotPoseVel m_curPoseVel
 Current robot pose (updated in CAbstractNavigator::navigationStep() ) More...
 
double m_last_curPoseVelUpdate_robot_time {-1e9}
 
std::string m_last_curPoseVelUpdate_pose_frame_id
 
mrpt::poses::CPose2DInterpolator m_latestPoses
 Latest robot poses (updated in CAbstractNavigator::navigationStep() ) More...
 
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
 
mrpt::system::CTimeLogger m_timlog_delays
 Time logger to collect delay-related stats. More...
 
double m_badNavAlarm_minDistTarget
 For sending an alarm (error event) when it seems that we are not approaching toward the target in a while... More...
 
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
 
VerbosityLevel m_min_verbosity_level {LVL_INFO}
 Provided messages with VerbosityLevel smaller than this value shall be ignored. More...
 
Variables for CReactiveNavigationSystem::performNavigationStep
mrpt::system::CTicTac totalExecutionTime
 
mrpt::system::CTicTac executionTime
 
mrpt::system::CTicTac tictac
 
mrpt::math::LowPassFilter_IIR1 meanExecutionTime {0.7, 1}
 
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime {0.7, 1}
 
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod {0.7, 1}
 Runtime estimation of execution period of the method. More...
 
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr {0.7}
 
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr {0.7}
 
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr {0.7}
 
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr {0.7}
 

Private Member Functions

void STEP1_InitPTGs () override
 
bool implementSenseObstacles (mrpt::system::TTimeStamp &obs_timestamp) override
 Return false on any fatal error. More...
 
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) override
 Transform the obstacle into TP-Obstacles in TP-Spaces. More...
 
void loggingGetWSObstaclesAndShape (CLogFileRecord &out_log) override
 Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep. More...
 

Private Attributes

mrpt::maps::CSimplePointsMap m_WS_Obstacles_unsorted
 The unsorted set of obstacles from the sensors. More...
 
std::vector< mrpt::maps::CSimplePointsMapm_WS_Obstacles_inlevels
 One point cloud per 2.5D robot-shape-slice, coordinates relative to the robot local frame. More...
 
TRobotShape m_robotShape
 The robot 3D shape model. More...
 
std::vector< TPTGmultilevelm_ptgmultilevel
 The set of PTG-transformations to be used: indices are [ptg_index][height_index]. More...
 

Navigation control API

virtual void navigate (const TNavigationParams *params)
 Navigation request to a single target location. More...
 
virtual void resume ()
 Continues with suspended navigation. More...
 
virtual void suspend ()
 Suspend current navegation. More...
 
virtual void resetNavError ()
 Resets a NAV_ERROR state back to IDLE More...
 
TState getCurrentState () const
 Returns the current navigator state. More...
 
const TErrorReasongetErrorReason () const
 In case of state=NAV_ERROR, this returns the reason for the error. More...
 
void setFrameTF (const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &frame_tf)
 Sets a user-provided frame transformer object; used only if providing targets in a frame ID different than the one in which robot odometry is given (both IDs default to "map"). More...
 
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > getFrameTF () const
 Get the current frame tf object (defaults to nullptr) More...
 
void enableRethrowNavExceptions (const bool enable)
 By default, error exceptions on navigationStep() will dump an error message to the output logger interface. More...
 
bool isRethrowNavExceptionsEnabled () const
 
enum  TState { IDLE = 0, NAVIGATING, SUSPENDED, NAV_ERROR }
 The different states for the navigation system. More...
 
enum  TErrorCode { ERR_NONE = 0, ERR_EMERGENCY_STOP, ERR_CANNOT_REACH_TARGET, ERR_OTHER }
 Explains the reason for the navigation error. More...
 

Logging methods

void logStr (const VerbosityLevel level, std::string_view msg_str) const
 Main method to add the specified message string to the logger. More...
 
void logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
 Alternative logging method, which mimics the printf behavior. More...
 
void void logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const
 Log the given message only if the condition is satisfied. More...
 
void setLoggerName (const std::string &name)
 Set the name of the COutputLogger instance. More...
 
std::string getLoggerName () const
 Return the name of the COutputLogger instance. More...
 
void setMinLoggingLevel (const VerbosityLevel level)
 Set the minimum logging level for which the incoming logs are going to be taken into account. More...
 
void setVerbosityLevel (const VerbosityLevel level)
 alias of setMinLoggingLevel() More...
 
VerbosityLevel getMinLoggingLevel () const
 
bool isLoggingLevelVisible (VerbosityLevel level) const
 
void getLogAsString (std::string &log_contents) const
 Fill the provided string with the contents of the logger's history in std::string representation. More...
 
std::string getLogAsString () const
 Get the history of COutputLogger instance in a string representation. More...
 
void writeLogToFile (const std::string *fname_in=nullptr) const
 Write the contents of the COutputLogger instance to an external file. More...
 
void dumpLogToConsole () const
 Dump the current contents of the COutputLogger instance in the terminal window. More...
 
std::string getLoggerLastMsg () const
 Return the last Tmsg instance registered in the logger history. More...
 
void getLoggerLastMsg (std::string &msg_str) const
 Fill inputtted string with the contents of the last message in history. More...
 
void loggerReset ()
 Reset the contents of the logger instance. More...
 
void logRegisterCallback (output_logger_callback_t userFunc)
 
bool logDeregisterCallback (output_logger_callback_t userFunc)
 
bool logging_enable_console_output {true}
 [Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More...
 
bool logging_enable_keep_record {false}
 [Default=false] Enables storing all messages into an internal list. More...
 

Member Enumeration Documentation

◆ TErrorCode

Explains the reason for the navigation error.

Enumerator
ERR_NONE 
ERR_EMERGENCY_STOP 
ERR_CANNOT_REACH_TARGET 
ERR_OTHER 

Definition at line 184 of file CAbstractNavigator.h.

◆ TState

The different states for the navigation system.

Enumerator
IDLE 
NAVIGATING 
SUSPENDED 
NAV_ERROR 

Definition at line 172 of file CAbstractNavigator.h.

Constructor & Destructor Documentation

◆ CReactiveNavigationSystem3D()

CReactiveNavigationSystem3D::CReactiveNavigationSystem3D ( CRobot2NavInterface react_iterf_impl,
bool  enableConsoleOutput = true,
bool  enableLogFile = false,
const std::string &  logFileDirectory = std::string("./reactivenav.logs") 
)

See docs in ctor of base class.

Definition at line 25 of file CReactiveNavigationSystem3D.cpp.

◆ ~CReactiveNavigationSystem3D()

CReactiveNavigationSystem3D::~CReactiveNavigationSystem3D ( )
override

Destructor.

Definition at line 35 of file CReactiveNavigationSystem3D.cpp.

References m_ptgmultilevel, and mrpt::nav::CAbstractPTGBasedReactive::preDestructor().

Here is the call graph for this function:

Member Function Documentation

◆ build_movement_candidate()

void CAbstractPTGBasedReactive::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) 
)
protectedinherited

Definition at line 1538 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CLogFileRecord::additional_debug_msgs, mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index(), ASSERT_, mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::clearance, mrpt::nav::CLogFileRecord::TInfoPerPTG::clearance, mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::clearance, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::colfreedist_move_k, mrpt::containers::copy_container_typecasting(), mrpt::nav::CLogFileRecord::TInfoPerPTG::desiredDirection, mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredDirection, mrpt::nav::CLogFileRecord::TInfoPerPTG::desiredSpeed, mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::desiredSpeed, mrpt::nav::TCandidateMovementPTG::direction, mrpt::nav::CAbstractHolonomicReactiveMethod::enableApproachTargetSlowDown(), mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::evaluate_clearance, mrpt::format(), mrpt::nav::CParameterizedTrajectoryGenerator::getAlphaValuesCount(), mrpt::nav::CParameterizedTrajectoryGenerator::getDescription(), mrpt::nav::CAbstractPTGBasedReactive::getPTG_count(), mrpt::nav::CParameterizedTrajectoryGenerator::getRefDistance(), mrpt::nav::CLogFileRecord::TInfoPerPTG::HLFR, mrpt::hypot_fast(), mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha(), mrpt::nav::CLogFileRecord::infoPerPTG, mrpt::nav::CParameterizedTrajectoryGenerator::initClearanceDiagram(), mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles(), mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP(), mrpt::system::CTimeLogger::isEnabled(), mrpt::nav::CAbstractHolonomicReactiveMethod::NavOutput::logRecord, mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords, mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd, mrpt::nav::CAbstractPTGBasedReactive::m_logFile, mrpt::nav::CAbstractNavigator::m_navProfiler, mrpt::nav::CAbstractPTGBasedReactive::m_timelogger, mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxObstacleDist, mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::maxRobotSpeed, mrpt::nav::CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP(), mrpt::nav::CAbstractHolonomicReactiveMethod::navigate(), mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::obstacles, mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator, mrpt::nav::TCandidateMovementPTG::props, mrpt::nav::TCandidateMovementPTG::PTG, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_alpha_index, mrpt::nav::CLogFileRecord::TInfoPerPTG::PTG_desc, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_index, mrpt::system::CTimeLogger::registerUserMeasure(), mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::secure_distance_end, mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::secure_distance_start, mrpt::nav::TCandidateMovementPTG::speed, mrpt::nav::CAbstractPTGBasedReactive::STEP3_WSpaceToTPSpace(), mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP(), mrpt::system::CTicTac::Tac(), mrpt::nav::CAbstractNavigator::TNavigationParams::target, mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_alpha, mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_dist, mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::target_k, mrpt::nav::CAbstractNavigator::TargetInfo::targetDesiredRelSpeed, mrpt::nav::CAbstractHolonomicReactiveMethod::NavInput::targets, mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::targets, mrpt::system::CTicTac::Tic(), mrpt::nav::CAbstractPTGBasedReactive::tictac, mrpt::nav::CLogFileRecord::TInfoPerPTG::timeForHolonomicMethod, mrpt::nav::CLogFileRecord::TInfoPerPTG::timeForTPObsTransformation, mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Obstacles, mrpt::nav::CAbstractPTGBasedReactive::TInfoPerPTG::TP_Obstacles, mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::TP_Target, mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Targets, mrpt::nav::CParameterizedTrajectoryGenerator::updateClearancePost(), mrpt::nav::CAbstractPTGBasedReactive::PTGTarget::valid_TP, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal, mrpt::math::TTwist2D::vx, mrpt::math::TTwist2D::vy, mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ calc_move_candidate_scores()

void CAbstractPTGBasedReactive::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 
)
protectedinherited

Scores holonomicMovement.

Definition at line 1006 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CLogFileRecord::additional_debug_msgs, mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index(), mrpt::math::angDistance(), ASSERT_, ASSERT_EQUAL_, mrpt::math::TPose2D::asString(), mrpt::nav::TCandidateMovementPTG::direction, mrpt::nav::CParameterizedTrajectoryGenerator::directionToMotionCommand(), mrpt::nav::CParameterizedTrajectoryGenerator::evalClearanceSingleObstacle(), mrpt::nav::CParameterizedTrajectoryGenerator::evalPathRelativePriority(), mrpt::format(), mrpt::nav::ClearanceDiagram::getClearance(), mrpt::nav::CAbstractPTGBasedReactive::getHoloMethod(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathPose(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepDuration(), mrpt::nav::CParameterizedTrajectoryGenerator::getPathStepForDist(), mrpt::nav::CParameterizedTrajectoryGenerator::getRefDistance(), mrpt::nav::CParameterizedTrajectoryGenerator::getScorePriority(), mrpt::nav::CAbstractHolonomicReactiveMethod::getTargetApproachSlowDownDistance(), mrpt::hypot_fast(), mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha(), mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP(), mrpt::nav::CParameterizedTrajectoryGenerator::isBijectiveAt(), mrpt::keep_min(), mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod, mrpt::nav::CAbstractPTGBasedReactive::m_last_vel_cmd, mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd, mrpt::nav::CAbstractNavigator::m_navigationParams, mrpt::nav::CAbstractNavigator::m_navProfiler, mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::max_dist_for_timebased_path_prediction, MRPT_END, MRPT_START, mrpt::system::now(), mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::original_holo_eval, mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator, mrpt::math::TPose2D::phi, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::poseVel, mrpt::nav::TCandidateMovementPTG::props, mrpt::nav::TCandidateMovementPTG::PTG, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_alpha_index, mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry, mrpt::round(), mrpt::nav::TCandidateMovementPTG::speed, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::speed_scale, mrpt::nav::TCandidateMovementPTG::starting_robot_dir, mrpt::nav::TCandidateMovementPTG::starting_robot_dist, mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget(), mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP(), mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::tim_send_cmd_vel, mrpt::system::timeDifference(), mrpt::nav::CLogFileRecord::TInfoPerPTG::TP_Robot, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::was_slowdown, mrpt::math::TPoint2D_data< T >::x, mrpt::math::TPose2D::x, mrpt::math::TPoint2D_data< T >::y, and mrpt::math::TPose2D::y.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ cancel()

void CWaypointsNavigator::cancel ( )
overridevirtualinherited

Cancel current navegation.

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 102 of file CWaypointsNavigator.cpp.

References mrpt::nav::CAbstractNavigator::cancel(), mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs, and mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status.

Here is the call graph for this function:

◆ changeCurrentRobotSpeedLimits()

mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& mrpt::nav::CAbstractPTGBasedReactive::changeCurrentRobotSpeedLimits ( )
inlineinherited

Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits.

Definition at line 271 of file CAbstractPTGBasedReactive.h.

References mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator, and mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::robot_absolute_speed_limits.

◆ changeRobotShape()

void CReactiveNavigationSystem3D::changeRobotShape ( TRobotShape  robotShape)

Change the robot shape, which is taken into account for collision grid building.

Definition at line 46 of file CReactiveNavigationSystem3D.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::m_PTGsMustBeReInitialized, m_robotShape, mrpt::nav::TRobotShape::polygon(), mrpt::nav::TRobotShape::size(), THROW_EXCEPTION, and mrpt::math::CPolygon::verticesCount().

Here is the call graph for this function:

◆ changeSpeeds()

bool CAbstractNavigator::changeSpeeds ( const mrpt::kinematics::CVehicleVelCmd vel_cmd)
protectedvirtualinherited

Default: forward call to m_robot.changeSpeed().

Can be overriden.

Definition at line 421 of file CAbstractNavigator.cpp.

References mrpt::nav::CRobot2NavInterface::changeSpeeds(), and mrpt::nav::CAbstractNavigator::m_robot.

Referenced by mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ changeSpeedsNOP()

bool CAbstractNavigator::changeSpeedsNOP ( )
protectedvirtualinherited

Default: forward call to m_robot.changeSpeedsNOP().

Can be overriden.

Definition at line 427 of file CAbstractNavigator.cpp.

References mrpt::nav::CRobot2NavInterface::changeSpeedsNOP(), and mrpt::nav::CAbstractNavigator::m_robot.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkCollisionWithLatestObstacles()

bool CReactiveNavigationSystem3D::checkCollisionWithLatestObstacles ( const mrpt::math::TPose2D relative_robot_pose) const
overridevirtual

Checks whether the robot shape, when placed at the given pose (relative to the current pose), is colliding with any of the latest known obstacles.

Default implementation: always returns false.

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 332 of file CReactiveNavigationSystem3D.cpp.

References ASSERT_, ASSERT_EQUAL_, mrpt::math::TPose2D::inverseComposePoint(), m_ptgmultilevel, m_robotShape, m_WS_Obstacles_inlevels, MRPT_LOG_WARN, R, mrpt::nav::TRobotShape::size(), mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Here is the call graph for this function:

◆ checkHasReachedTarget()

bool CWaypointsNavigator::checkHasReachedTarget ( const double  targetDist) const
overrideprotectedvirtualinherited

Default implementation: check if target_dist is below the accepted distance.

If true is returned here, the end-of-navigation event will be sent out (only for non-intermediary targets).

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 537 of file CWaypointsNavigator.cpp.

References mrpt::system::dateTimeLocalToString(), mrpt::nav::TWaypointStatus::getAsText(), INVALID_TIMESTAMP, mrpt::nav::CAbstractNavigator::m_navigationParams, mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status, MRPT_LOG_DEBUG_STREAM, and mrpt::nav::TWaypointStatus::reached.

Here is the call graph for this function:

◆ dispatchPendingNavEvents()

void CAbstractNavigator::dispatchPendingNavEvents ( )
protectedinherited

Definition at line 246 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::m_pending_events.

Referenced by mrpt::nav::CWaypointsNavigator::navigationStep(), and mrpt::nav::CAbstractNavigator::navigationStep().

Here is the caller graph for this function:

◆ doEmergencyStop()

void CAbstractNavigator::doEmergencyStop ( const std::string &  msg)
protectedvirtualinherited

Stops the robot and set navigation state to error.

Definition at line 257 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::ERR_EMERGENCY_STOP, mrpt::nav::CAbstractNavigator::ERR_NONE, mrpt::nav::CAbstractNavigator::TErrorReason::error_code, mrpt::nav::CAbstractNavigator::TErrorReason::error_msg, mrpt::nav::CAbstractNavigator::m_navErrorReason, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_ERROR, mrpt::nav::CAbstractNavigator::NAV_ERROR, and mrpt::nav::CAbstractNavigator::stop().

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const
inherited

Dump the current contents of the COutputLogger instance in the terminal window.

See also
writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ enableKeepLogRecords()

void mrpt::nav::CAbstractPTGBasedReactive::enableKeepLogRecords ( bool  enable = true)
inlineinherited

Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()

Definition at line 158 of file CAbstractPTGBasedReactive.h.

References mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords.

◆ enableLogFile()

void CAbstractPTGBasedReactive::enableLogFile ( bool  enable)
inherited

Enables/disables saving log files.

Definition at line 118 of file CAbstractPTGBasedReactive.cpp.

References mrpt::system::createDirectory(), mrpt::system::directoryExists(), mrpt::system::fileExists(), mrpt::format(), mrpt::lockHelper(), mrpt::nav::CAbstractPTGBasedReactive::m_logFile, mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractPTGBasedReactive::m_navlogfiles_dir, MRPT_LOG_DEBUG, MRPT_LOG_DEBUG_FMT, MRPT_LOG_ERROR_FMT, ok, mrpt::io::CFileGZOutputStream::open(), and THROW_EXCEPTION_FMT.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::CAbstractPTGBasedReactive().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ enableRethrowNavExceptions()

void mrpt::nav::CAbstractNavigator::enableRethrowNavExceptions ( const bool  enable)
inlineinherited

By default, error exceptions on navigationStep() will dump an error message to the output logger interface.

If rethrow is enabled (default=false), the error message will be reported as well, but exceptions will be re-thrown.

Definition at line 228 of file CAbstractNavigator.h.

References mrpt::nav::CAbstractNavigator::m_rethrow_exceptions.

◆ enableTimeLog()

void mrpt::nav::CAbstractPTGBasedReactive::enableTimeLog ( bool  enable = true)
inlineinherited

Enables/disables the detailed time logger (default:disabled upon construction) When enabled, a report will be dumped to std::cout upon destruction.

See also
getTimeLogger

Definition at line 246 of file CAbstractPTGBasedReactive.h.

References mrpt::system::CTimeLogger::enable(), and mrpt::nav::CAbstractPTGBasedReactive::m_timelogger.

Here is the call graph for this function:

◆ generate_vel_cmd()

double CAbstractPTGBasedReactive::generate_vel_cmd ( const TCandidateMovementPTG in_movement,
mrpt::kinematics::CVehicleVelCmd::Ptr new_vel_cmd 
)
protectedvirtualinherited

◆ getCurrentRobotSpeedLimits()

const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& mrpt::nav::CAbstractPTGBasedReactive::getCurrentRobotSpeedLimits ( ) const
inlineinherited

◆ getCurrentState()

TState mrpt::nav::CAbstractNavigator::getCurrentState ( ) const
inlineinherited

Returns the current navigator state.

Definition at line 181 of file CAbstractNavigator.h.

References mrpt::nav::CAbstractNavigator::m_navigationState.

◆ getDelaysTimeLogger()

const mrpt::system::CTimeLogger& mrpt::nav::CAbstractNavigator::getDelaysTimeLogger ( ) const
inlineinherited

Gives access to a const-ref to the internal time logger used to estimate delays.

See also
getTimeLogger() in derived classes

Definition at line 262 of file CAbstractNavigator.h.

References mrpt::nav::CAbstractNavigator::m_timlog_delays.

◆ getErrorReason()

const TErrorReason& mrpt::nav::CAbstractNavigator::getErrorReason ( ) const
inlineinherited

In case of state=NAV_ERROR, this returns the reason for the error.

Error state is reseted every time a new navigation starts with a call to navigate(), or when resetNavError() is called.

Definition at line 204 of file CAbstractNavigator.h.

References mrpt::nav::CAbstractNavigator::m_navErrorReason.

◆ getFrameTF()

std::weak_ptr<mrpt::poses::FrameTransformer<2> > mrpt::nav::CAbstractNavigator::getFrameTF ( ) const
inlineinherited

Get the current frame tf object (defaults to nullptr)

See also
setFrameTF

Definition at line 218 of file CAbstractNavigator.h.

References mrpt::nav::CAbstractNavigator::m_frame_tf.

◆ getHoloMethod()

CAbstractHolonomicReactiveMethod * CAbstractPTGBasedReactive::getHoloMethod ( int  idx)
protectedvirtualinherited

Definition at line 2011 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), and mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the caller graph for this function:

◆ getLastLogRecord()

void CAbstractPTGBasedReactive::getLastLogRecord ( CLogFileRecord o)
inherited

Provides a copy of the last log record with information about execution.

Parameters
oAn object where the log will be stored into.
Note
Log records are not prepared unless either "enableLogFile" is enabled in the constructor or "enableLogFile()" has been called.

Definition at line 195 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::lastLogRecord, mrpt::lockHelper(), and mrpt::nav::CAbstractPTGBasedReactive::m_critZoneLastLog.

Here is the call graph for this function:

◆ getLogAsString() [1/2]

void COutputLogger::getLogAsString ( std::string &  log_contents) const
inherited

Fill the provided string with the contents of the logger's history in std::string representation.

Definition at line 154 of file COutputLogger.cpp.

◆ getLogAsString() [2/2]

std::string COutputLogger::getLogAsString ( ) const
inherited

Get the history of COutputLogger instance in a string representation.

Definition at line 159 of file COutputLogger.cpp.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

Here is the caller graph for this function:

◆ getLogFileDirectory()

std::string mrpt::nav::CAbstractPTGBasedReactive::getLogFileDirectory ( ) const
inlineinherited

◆ getLoggerLastMsg() [1/2]

std::string COutputLogger::getLoggerLastMsg ( ) const
inherited

Return the last Tmsg instance registered in the logger history.

Definition at line 195 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::getAsString().

Here is the call graph for this function:

◆ getLoggerLastMsg() [2/2]

void COutputLogger::getLoggerLastMsg ( std::string &  msg_str) const
inherited

Fill inputtted string with the contents of the last message in history.

Definition at line 201 of file COutputLogger.cpp.

◆ getLoggerName()

std::string COutputLogger::getLoggerName ( ) const
inherited

Return the name of the COutputLogger instance.

See also
setLoggerName

Definition at line 143 of file COutputLogger.cpp.

◆ getMinLoggingLevel()

VerbosityLevel mrpt::system::COutputLogger::getMinLoggingLevel ( ) const
inlineinherited

◆ getPTG() [1/2]

CParameterizedTrajectoryGenerator* mrpt::nav::CReactiveNavigationSystem3D::getPTG ( size_t  i)
inlineoverridevirtual

Gets the i'th PTG.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 109 of file CReactiveNavigationSystem3D.h.

References ASSERT_, and m_ptgmultilevel.

◆ getPTG() [2/2]

const CParameterizedTrajectoryGenerator* mrpt::nav::CReactiveNavigationSystem3D::getPTG ( size_t  i) const
inlineoverridevirtual

Gets the i'th PTG.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 117 of file CReactiveNavigationSystem3D.h.

References ASSERT_, and m_ptgmultilevel.

◆ getPTG_count()

size_t mrpt::nav::CReactiveNavigationSystem3D::getPTG_count ( ) const
inlineoverridevirtual

Returns the number of different PTGs that have been setup.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 104 of file CReactiveNavigationSystem3D.h.

References ASSERT_, and m_ptgmultilevel.

◆ getTargetApproachSlowDownDistance()

double CAbstractPTGBasedReactive::getTargetApproachSlowDownDistance ( ) const
inherited

Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?)

Definition at line 2005 of file CAbstractPTGBasedReactive.cpp.

References ASSERT_, and mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod.

◆ getTimeLogger()

const mrpt::system::CTimeLogger& mrpt::nav::CAbstractPTGBasedReactive::getTimeLogger ( ) const
inlineinherited

Gives access to a const-ref to the internal time logger.

See also
enableTimeLog

Definition at line 249 of file CAbstractPTGBasedReactive.h.

References mrpt::nav::CAbstractPTGBasedReactive::m_timelogger.

◆ getWaypointNavStatus() [1/2]

void CWaypointsNavigator::getWaypointNavStatus ( TWaypointStatusSequence out_nav_status) const
virtualinherited

Get a copy of the control structure which describes the progress status of the waypoint navigation.

Definition at line 94 of file CWaypointsNavigator.cpp.

References mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status.

◆ getWaypointNavStatus() [2/2]

TWaypointStatusSequence mrpt::nav::CWaypointsNavigator::getWaypointNavStatus ( ) const
inlineinherited

Get a copy of the control structure which describes the progress status of the waypoint navigation.

Definition at line 93 of file CWaypointsNavigator.h.

Referenced by mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the caller graph for this function:

◆ impl_waypoint_is_reachable()

bool CAbstractPTGBasedReactive::impl_waypoint_is_reachable ( const mrpt::math::TPoint2D wp_local_wrt_robot) const
overrideprotectedvirtualinherited

Implements the way to waypoint is free function in children classes: true must be returned if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range, etc.

Implements mrpt::nav::CWaypointsNavigator.

Definition at line 1464 of file CAbstractPTGBasedReactive.cpp.

References ASSERT_, mrpt::nav::CParameterizedTrajectoryGenerator::getPathCount(), mrpt::nav::CAbstractPTGBasedReactive::getPTG(), mrpt::nav::CAbstractPTGBasedReactive::getPTG_count(), INVALID_TIMESTAMP, mrpt::nav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP(), mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG, mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp, MRPT_END, MRPT_START, mrpt::system::now(), mrpt::system::timeDifference(), mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Here is the call graph for this function:

◆ implementSenseObstacles()

bool CReactiveNavigationSystem3D::implementSenseObstacles ( mrpt::system::TTimeStamp obs_timestamp)
overrideprivatevirtual

◆ initialize()

void CAbstractPTGBasedReactive::initialize ( )
overridevirtualinherited

Must be called for loading collision grids, or the first navigation command may last a long time to be executed.

Internally, it just calls STEP1_CollisionGridsBuilder().

Implements mrpt::nav::CAbstractNavigator.

Definition at line 102 of file CAbstractPTGBasedReactive.cpp.

References ASSERT_, INVALID_TIMESTAMP, mrpt::lockHelper(), mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp, mrpt::nav::CAbstractPTGBasedReactive::m_multiobjopt, mrpt::nav::CAbstractNavigator::m_nav_cs, and mrpt::nav::CAbstractPTGBasedReactive::STEP1_InitPTGs().

Here is the call graph for this function:

◆ isLoggingLevelVisible()

bool mrpt::system::COutputLogger::isLoggingLevelVisible ( VerbosityLevel  level) const
inlineinherited

Definition at line 202 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

Here is the caller graph for this function:

◆ isRelativePointReachable()

bool CWaypointsNavigator::isRelativePointReachable ( const mrpt::math::TPoint2D wp_local_wrt_robot) const
inherited

Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc.

Definition at line 476 of file CWaypointsNavigator.cpp.

References mrpt::nav::CWaypointsNavigator::impl_waypoint_is_reachable().

Here is the call graph for this function:

◆ isRethrowNavExceptionsEnabled()

bool mrpt::nav::CAbstractNavigator::isRethrowNavExceptionsEnabled ( ) const
inlineinherited

◆ loadConfigFile()

void CReactiveNavigationSystem3D::loadConfigFile ( const mrpt::config::CConfigFileBase c)
overridevirtual

◆ logCond()

void COutputLogger::logCond ( const VerbosityLevel  level,
bool  cond,
const std::string &  msg_str 
) const
inherited

Log the given message only if the condition is satisfied.

See also
log, logFmt

Definition at line 131 of file COutputLogger.cpp.

◆ logDeregisterCallback()

bool COutputLogger::logDeregisterCallback ( output_logger_callback_t  userFunc)
inherited
Returns
true if an entry was found and deleted.

Definition at line 291 of file COutputLogger.cpp.

References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.

Here is the call graph for this function:

◆ logFmt()

void COutputLogger::logFmt ( const VerbosityLevel  level,
const char *  fmt,
  ... 
) const
inherited

◆ loggerReset()

void COutputLogger::loggerReset ( )
inherited

Reset the contents of the logger instance.

Called upon construction.

Definition at line 206 of file COutputLogger.cpp.

References mrpt::system::LVL_INFO.

◆ logging_levels_to_colors()

std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_colors ( )
staticinherited

Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.

Handy for coloring the input based on the verbosity of the message

Definition at line 47 of file COutputLogger.cpp.

References logging_levels_to_colors.

Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().

Here is the caller graph for this function:

◆ logging_levels_to_names()

std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_names ( )
staticinherited

Map from VerbosityLevels to their corresponding names.

Handy for printing the current message VerbosityLevel along with the actual content

Definition at line 60 of file COutputLogger.cpp.

References logging_levels_to_names.

Referenced by mrpt::system::COutputLogger::TMsg::getAsString().

Here is the caller graph for this function:

◆ loggingGetWSObstaclesAndShape()

void CReactiveNavigationSystem3D::loggingGetWSObstaclesAndShape ( CLogFileRecord out_log)
overrideprivatevirtual

Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the current timestep.

Generates a pointcloud of obstacles to be saved in the logging record for the current timestep.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 290 of file CReactiveNavigationSystem3D.cpp.

References mrpt::maps::CMetricMap::clear(), mrpt::nav::TRobotShape::getRadius(), mrpt::maps::CPointsMap::insertAnotherMap(), m_robotShape, m_WS_Obstacles_inlevels, mrpt::nav::TRobotShape::polygon(), mrpt::math::CVectorDynamic< T >::resize(), mrpt::nav::CLogFileRecord::robotShape_radius, mrpt::nav::CLogFileRecord::robotShape_x, mrpt::nav::CLogFileRecord::robotShape_y, mrpt::nav::TRobotShape::size(), mrpt::math::CVectorDynamic< T >::size(), mrpt::nav::CLogFileRecord::WS_Obstacles, mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Here is the call graph for this function:

◆ logRegisterCallback()

void COutputLogger::logRegisterCallback ( output_logger_callback_t  userFunc)
inherited

Definition at line 278 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::m_listCallbacks.

◆ logStr()

void COutputLogger::logStr ( const VerbosityLevel  level,
std::string_view  msg_str 
) const
inherited

Main method to add the specified message string to the logger.

See also
logCond, logFmt

Definition at line 72 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.

Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ navigate()

void CAbstractNavigator::navigate ( const TNavigationParams params)
virtualinherited

Navigation request to a single target location.

It starts a new navigation.

Parameters
[in]paramsPointer to structure with navigation info (its contents will be copied, so the original can be freely destroyed upon return if it was dynamically allocated.)
Note
A pointer is used so the passed object can be polymorphic with derived types.

Reimplemented in mrpt::nav::CNavigatorManualSequence.

Definition at line 319 of file CAbstractNavigator.cpp.

References MRPT_END, MRPT_START, mrpt::nav::CAbstractNavigator::onNavigateCommandReceived(), and mrpt::nav::CAbstractNavigator::processNavigateCommand().

Here is the call graph for this function:

◆ navigateWaypoints()

void CWaypointsNavigator::navigateWaypoints ( const TWaypointSequence nav_request)
virtualinherited

Waypoint navigation request.

This immediately cancels any other previous on-going navigation.

See also
CAbstractNavigator::navigate() for single waypoint navigation requests.

Definition at line 66 of file CWaypointsNavigator.cpp.

References ASSERT_, ASSERTMSG_, mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs, mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status, MRPT_END, MRPT_START, mrpt::system::now(), mrpt::nav::CWaypointsNavigator::onNavigateCommandReceived(), mrpt::nav::TWaypointStatusSequence::timestamp_nav_started, mrpt::nav::TWaypointStatusSequence::waypoint_index_current_goal, mrpt::nav::TWaypointSequence::waypoints, and mrpt::nav::TWaypointStatusSequence::waypoints.

Here is the call graph for this function:

◆ navigationStep()

void CWaypointsNavigator::navigationStep ( )
overridevirtualinherited

This method must be called periodically in order to effectively run the navigation.

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 443 of file CWaypointsNavigator.cpp.

References mrpt::nav::CAbstractNavigator::dispatchPendingNavEvents(), mrpt::nav::CWaypointsNavigator::m_is_aligning, mrpt::nav::CAbstractNavigator::m_navigationState, mrpt::nav::CAbstractNavigator::m_navProfiler, MRPT_END, MRPT_START, mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractNavigator::SUSPENDED, and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the call graph for this function:

◆ onNavigateCommandReceived()

void CWaypointsNavigator::onNavigateCommandReceived ( )
overrideprotectedvirtualinherited

Called after each call to CAbstractNavigator::navigate()

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 54 of file CWaypointsNavigator.cpp.

References INVALID_TIMESTAMP, mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs, mrpt::nav::CWaypointsNavigator::m_was_aligning, mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status, mrpt::nav::CAbstractNavigator::onNavigateCommandReceived(), mrpt::nav::TWaypointStatusSequence::timestamp_nav_started, and mrpt::nav::TWaypointStatusSequence::waypoint_index_current_goal.

Referenced by mrpt::nav::CWaypointsNavigator::navigateWaypoints().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onStartNewNavigation()

void CAbstractPTGBasedReactive::onStartNewNavigation ( )
overrideprotectedvirtualinherited

Called whenever a new navigation has been started.

Can be used to reset state variables, etc.

Implements mrpt::nav::CAbstractNavigator.

Definition at line 1510 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time, mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd, mrpt::nav::CWaypointsNavigator::onStartNewNavigation(), and mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::reset().

Here is the call graph for this function:

◆ performNavigationStep()

void CAbstractPTGBasedReactive::performNavigationStep ( )
overrideprotectedvirtualinherited

The main method for the navigator.

The main method: executes one time-iteration of the reactive navigation algorithm.

Allow PTGs to be responsive to target location, dynamics, etc.

Implements mrpt::nav::CAbstractNavigator.

Definition at line 233 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CLogFileRecord::additional_debug_msgs, mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index(), mrpt::serialization::archiveFrom(), ASSERT_, ASSERT_EQUAL_, mrpt::math::TPose2D::asString(), mrpt::poses::CPose2D::asTPose(), mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractNavigator::changeSpeeds(), mrpt::nav::CAbstractNavigator::changeSpeedsNOP(), mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::colfreedist_move_k, mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::curVelLocal, mrpt::nav::TCandidateMovementPTG::direction, mrpt::nav::CAbstractNavigator::doEmergencyStop(), mrpt::nav::CAbstractPTGBasedReactive::executionTime, mrpt::math::LowPassFilter_IIR1::filter(), mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo::final_evaluation, mrpt::format(), mrpt::nav::CAbstractPTGBasedReactive::generate_vel_cmd(), mrpt::nav::CRobot2NavInterface::getEmergencyStopCmd(), mrpt::nav::CAbstractPTGBasedReactive::getHoloMethod(), mrpt::math::LowPassFilter_IIR1::getLastOutput(), mrpt::system::CTimeLogger::getLastTime(), mrpt::nav::CAbstractPTGBasedReactive::getPTG(), mrpt::nav::CAbstractPTGBasedReactive::getPTG_count(), mrpt::nav::CLogFileRecord::infoPerPTG, mrpt::poses::CPoseInterpolatorBase< DIM >::interpolate(), mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::isValid(), mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo::log_entries, mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator, mrpt::nav::CAbstractPTGBasedReactive::m_copy_prev_navParams, mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractPTGBasedReactive::m_enableConsoleOutput, mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords, mrpt::nav::CAbstractNavigator::m_frame_tf, mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod, mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG, mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp, mrpt::nav::CAbstractPTGBasedReactive::m_init_done, mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd, mrpt::nav::CAbstractNavigator::m_latestOdomPoses, mrpt::nav::CAbstractNavigator::m_latestPoses, mrpt::nav::CAbstractPTGBasedReactive::m_logFile, mrpt::nav::CAbstractPTGBasedReactive::m_multiobjopt, mrpt::nav::CAbstractNavigator::m_navigationParams, mrpt::nav::CAbstractNavigator::m_navProfiler, mrpt::nav::CAbstractNavigator::m_pending_events, mrpt::nav::CAbstractPTGBasedReactive::m_prev_logfile, mrpt::nav::CAbstractNavigator::m_robot, mrpt::nav::CAbstractPTGBasedReactive::m_timelogger, mrpt::nav::CAbstractNavigator::m_timlog_delays, mrpt::nav::CAbstractPTGBasedReactive::m_WS_Obstacles_timestamp, mrpt::nav::CParameterizedTrajectoryGenerator::maxTimeInVelCmdNOP(), mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod, mrpt::nav::CAbstractPTGBasedReactive::meanExecutionTime, mrpt::nav::CAbstractPTGBasedReactive::meanTotalExecutionTime, MRPT_LOG_DEBUG, MRPT_LOG_DEBUG_FMT, MRPT_LOG_WARN_FMT, mrpt::nav::CLogFileRecord::navDynState, mrpt::system::now(), mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::original_holo_eval, mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose_frame_id, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::poseVel, mrpt::nav::TCandidateMovementPTG::props, mrpt::nav::TCandidateMovementPTG::PTG, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_alpha_index, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_dynState, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_index, mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry, mrpt::nav::CLogFileRecord::relPoseSense, mrpt::nav::CLogFileRecord::relPoseVelCmd, mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::relTarget, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::reset(), mrpt::io::CMemoryStream::Seek(), mrpt::nav::CRobot2NavInterface::sendApparentCollisionEvent(), mrpt::nav::TCandidateMovementPTG::speed, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::speed_scale, mrpt::nav::CAbstractPTGBasedReactive::STEP1_InitPTGs(), mrpt::nav::CAbstractPTGBasedReactive::STEP2_SenseObstacles(), mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(), mrpt::nav::CAbstractNavigator::stop(), mrpt::nav::CParameterizedTrajectoryGenerator::supportSpeedAtTarget(), mrpt::nav::CParameterizedTrajectoryGenerator::supportVelCmdNOP(), mrpt::system::CTicTac::Tac(), mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState::targetRelSpeed, THROW_EXCEPTION, THROW_EXCEPTION_FMT, mrpt::system::CTicTac::Tic(), mrpt::nav::CAbstractPTGBasedReactive::tim_changeSpeed_avr, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::tim_send_cmd_vel, mrpt::system::timeDifference(), mrpt::nav::CAbstractPTGBasedReactive::timerForExecutionPeriod, mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp, mrpt::system::timestampAdd(), mrpt::nav::CLogFileRecord::timestamps, mrpt::nav::CAbstractPTGBasedReactive::timoff_curPoseAndSpeed_avr, mrpt::nav::CAbstractPTGBasedReactive::timoff_obstacles_avr, mrpt::nav::CAbstractPTGBasedReactive::timoff_sendVelCmd_avr, mrpt::nav::CAbstractPTGBasedReactive::totalExecutionTime, mrpt::nav::CParameterizedTrajectoryGenerator::updateNavDynamicState(), mrpt::nav::CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::use_delays_model, mrpt::nav::CLogFileRecord::values, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal, and mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::was_slowdown.

Here is the call graph for this function:

◆ performNavigationStepNavigating()

void CAbstractNavigator::performNavigationStepNavigating ( bool  call_virtual_nav_method = true)
protectedvirtualinherited

Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.

Performs house-hold tasks like raising events in case of starting/ending navigation, timeout reaching destination, etc. call_virtual_nav_method can be set to false to avoid calling the virtual method performNavigationStep()

Definition at line 499 of file CAbstractNavigator.cpp.

References ASSERT_, mrpt::nav::CAbstractNavigator::checkCollisionWithLatestObstacles(), mrpt::nav::CAbstractNavigator::checkHasReachedTarget(), mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::dist_check_target_is_blocked, mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::dist_to_target_for_sending_event, mrpt::math::TSegment2D::distance(), mrpt::poses::CPoseInterpolatorBase< DIM >::empty(), mrpt::nav::CAbstractNavigator::ERR_CANNOT_REACH_TARGET, mrpt::nav::CAbstractNavigator::ERR_NONE, mrpt::nav::CAbstractNavigator::ERR_OTHER, mrpt::nav::CAbstractNavigator::TErrorReason::error_code, mrpt::nav::CAbstractNavigator::TErrorReason::error_msg, mrpt::format(), mrpt::system::getCurrentTime(), mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::hysteresis_check_target_is_blocked, mrpt::nav::CAbstractNavigator::IDLE, mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation(), mrpt::system::COutputLogger::logFmt(), mrpt::system::LVL_WARN, mrpt::nav::CAbstractNavigator::m_badNavAlarm_lastMinDistTime, mrpt::nav::CAbstractNavigator::m_badNavAlarm_minDistTarget, mrpt::nav::CAbstractNavigator::m_counter_check_target_is_blocked, mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractNavigator::m_lastNavigationState, mrpt::nav::CAbstractNavigator::m_latestPoses, mrpt::nav::CAbstractNavigator::m_navErrorReason, mrpt::nav::CAbstractNavigator::m_navigationEndEventSent, mrpt::nav::CAbstractNavigator::m_navigationParams, mrpt::nav::CAbstractNavigator::m_navigationState, mrpt::nav::CAbstractNavigator::m_pending_events, mrpt::nav::CAbstractNavigator::m_rethrow_exceptions, mrpt::nav::CAbstractNavigator::m_robot, MRPT_LOG_DEBUG, MRPT_LOG_ERROR, MRPT_LOG_ERROR_FMT, MRPT_LOG_INFO, MRPT_LOG_THROTTLE_WARN, MRPT_LOG_WARN, mrpt::nav::CAbstractNavigator::NAV_ERROR, mrpt::nav::CAbstractNavigator::NAVIGATING, mrpt::nav::CAbstractNavigator::params_abstract_navigator, mrpt::nav::CAbstractNavigator::performNavigationStep(), mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin(), mrpt::nav::CRobot2NavInterface::sendNavigationEndEvent(), mrpt::nav::CRobot2NavInterface::sendNavigationStartEvent(), mrpt::nav::CRobot2NavInterface::sendWaySeemsBlockedEvent(), mrpt::poses::CPoseInterpolatorBase< DIM >::size(), mrpt::nav::CAbstractNavigator::stop(), mrpt::system::timeDifference(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

Referenced by mrpt::nav::CAbstractNavigator::navigationStep(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ preDestructor()

void CAbstractPTGBasedReactive::preDestructor ( )
protectedinherited

To be called during children destructors to assure thread-safe destruction, and free of shared objects.

Definition at line 70 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::deleteHolonomicObjects(), mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator, mrpt::nav::CAbstractPTGBasedReactive::m_logFile, mrpt::nav::CAbstractNavigator::m_nav_cs, and mrpt::nav::CAbstractNavigator::stop().

Referenced by mrpt::nav::CAbstractPTGBasedReactive::~CAbstractPTGBasedReactive(), mrpt::nav::CReactiveNavigationSystem::~CReactiveNavigationSystem(), and ~CReactiveNavigationSystem3D().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ processNavigateCommand()

void CAbstractNavigator::processNavigateCommand ( const TNavigationParams params)
protectedvirtualinherited

◆ resetNavError()

void CAbstractNavigator::resetNavError ( )
virtualinherited

Resets a NAV_ERROR state back to IDLE

Definition at line 133 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::IDLE, mrpt::lockHelper(), mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navErrorReason, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_DEBUG, and mrpt::nav::CAbstractNavigator::NAV_ERROR.

Here is the call graph for this function:

◆ resume()

void CAbstractNavigator::resume ( )
virtualinherited

Continues with suspended navigation.

See also
suspend

Definition at line 110 of file CAbstractNavigator.cpp.

References mrpt::lockHelper(), mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_DEBUG, mrpt::nav::CAbstractNavigator::NAVIGATING, and mrpt::nav::CAbstractNavigator::SUSPENDED.

Here is the call graph for this function:

◆ saveConfigFile()

void CReactiveNavigationSystem3D::saveConfigFile ( mrpt::config::CConfigFileBase c) const
overridevirtual

Saves all current options to a config file.

Each derived class MUST save its own parameters, and then call ITS PARENT'S overriden method to ensure all params are saved.

Reimplemented from mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 59 of file CReactiveNavigationSystem3D.cpp.

References m_ptgmultilevel, m_robotShape, MRPT_SAVE_CONFIG_VAR_COMMENT, and mrpt::nav::TRobotShape::size().

Here is the call graph for this function:

◆ setFrameTF()

void CAbstractNavigator::setFrameTF ( const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &  frame_tf)
inherited

Sets a user-provided frame transformer object; used only if providing targets in a frame ID different than the one in which robot odometry is given (both IDs default to "map").

Definition at line 145 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::m_frame_tf.

◆ setHolonomicMethod()

void CAbstractPTGBasedReactive::setHolonomicMethod ( const std::string &  method,
const mrpt::config::CConfigFileBase cfgBase 
)
inherited

Selects which one from the set of available holonomic methods will be used into transformed TP-Space, and sets its configuration from a configuration file.

Available methods: class names of those derived from CAbstractHolonomicReactiveMethod

Definition at line 206 of file CAbstractPTGBasedReactive.cpp.

References ASSERT_, mrpt::nav::CAbstractPTGBasedReactive::deleteHolonomicObjects(), mrpt::nav::CAbstractHolonomicReactiveMethod::Factory(), mrpt::nav::CAbstractPTGBasedReactive::getPTG(), mrpt::nav::CAbstractPTGBasedReactive::getPTG_count(), mrpt::lockHelper(), mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod, mrpt::nav::CAbstractNavigator::m_nav_cs, and THROW_EXCEPTION_FMT.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::loadConfigFile().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setLogFileDirectory()

void mrpt::nav::CAbstractPTGBasedReactive::setLogFileDirectory ( const std::string &  sDir)
inlineinherited

Changes the prefix for new log files.

Definition at line 167 of file CAbstractPTGBasedReactive.h.

References mrpt::nav::CAbstractPTGBasedReactive::m_navlogfiles_dir.

◆ setLoggerName()

void COutputLogger::setLoggerName ( const std::string &  name)
inherited

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel  level)
inherited

Set the minimum logging level for which the incoming logs are going to be taken into account.

String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance

Definition at line 144 of file COutputLogger.cpp.

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ setTargetApproachSlowDownDistance()

void CAbstractPTGBasedReactive::setTargetApproachSlowDownDistance ( const double  dist)
inherited

Changes this parameter in all inner holonomic navigator instances [m].

Definition at line 1996 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod.

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited

◆ STEP1_InitPTGs()

void CReactiveNavigationSystem3D::STEP1_InitPTGs ( )
overrideprivatevirtual

◆ STEP2_SenseObstacles()

bool CAbstractPTGBasedReactive::STEP2_SenseObstacles ( )
protectedinherited

Definition at line 1504 of file CAbstractPTGBasedReactive.cpp.

References mrpt::nav::CAbstractPTGBasedReactive::implementSenseObstacles(), and mrpt::nav::CAbstractPTGBasedReactive::m_WS_Obstacles_timestamp.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ STEP3_WSpaceToTPSpace()

void CReactiveNavigationSystem3D::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 
)
overrideprivatevirtual

Transform the obstacle into TP-Obstacles in TP-Spaces.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 248 of file CReactiveNavigationSystem3D.cpp.

References ASSERT_, ASSERT_EQUAL_, m_ptgmultilevel, m_robotShape, m_WS_Obstacles_inlevels, and mrpt::nav::TRobotShape::size().

Here is the call graph for this function:

◆ STEP8_GenerateLogRecord()

void CAbstractPTGBasedReactive::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 
)
protectedinherited

Definition at line 938 of file CAbstractPTGBasedReactive.cpp.

References mrpt::serialization::archiveFrom(), mrpt::nav::CLogFileRecord::cmd_vel, mrpt::nav::CLogFileRecord::cur_vel, mrpt::nav::CLogFileRecord::cur_vel_local, mrpt::system::CTimeLogger::enter(), mrpt::math::LowPassFilter_IIR1::getLastOutput(), mrpt::system::CTimeLogger::getLastTime(), mrpt::nav::CAbstractPTGBasedReactive::lastLogRecord, mrpt::system::CTimeLogger::leave(), mrpt::lockHelper(), mrpt::nav::CAbstractPTGBasedReactive::loggingGetWSObstaclesAndShape(), mrpt::nav::CAbstractPTGBasedReactive::m_critZoneLastLog, mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd, mrpt::nav::CAbstractPTGBasedReactive::m_logFile, mrpt::nav::CAbstractNavigator::m_navProfiler, mrpt::nav::CAbstractPTGBasedReactive::m_timelogger, mrpt::nav::CAbstractNavigator::m_timlog_delays, mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod, mrpt::nav::CAbstractPTGBasedReactive::meanExecutionTime, mrpt::nav::CLogFileRecord::nPTGs, mrpt::nav::CLogFileRecord::nSelectedPTG, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_alpha_index, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_dynState, mrpt::nav::CAbstractPTGBasedReactive::TSentVelCmd::ptg_index, mrpt::nav::CLogFileRecord::ptg_index_NOP, mrpt::nav::CLogFileRecord::ptg_last_k_NOP, mrpt::nav::CLogFileRecord::ptg_last_navDynState, mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry, mrpt::nav::CLogFileRecord::rel_cur_pose_wrt_last_vel_cmd_NOP, mrpt::nav::CLogFileRecord::rel_pose_PTG_origin_wrt_sense_NOP, mrpt::nav::CLogFileRecord::robotPoseLocalization, mrpt::nav::CLogFileRecord::robotPoseOdometry, mrpt::nav::CAbstractPTGBasedReactive::tim_changeSpeed_avr, mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp, mrpt::nav::CLogFileRecord::timestamps, mrpt::nav::CLogFileRecord::values, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal, and mrpt::nav::CLogFileRecord::WS_targets_relative.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ stop()

bool CAbstractNavigator::stop ( bool  isEmergencyStop)
protectedvirtualinherited

Default: forward call to m_robot.stop().

Can be overriden.

Definition at line 429 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::m_robot, and mrpt::nav::CRobot2NavInterface::stop().

Referenced by mrpt::nav::CAbstractNavigator::cancel(), mrpt::nav::CAbstractNavigator::doEmergencyStop(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::nav::CAbstractPTGBasedReactive::preDestructor(), mrpt::nav::CAbstractNavigator::suspend(), mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ suspend()

void CAbstractNavigator::suspend ( )
virtualinherited

Suspend current navegation.

See also
resume

Definition at line 119 of file CAbstractNavigator.cpp.

References mrpt::lockHelper(), mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_DEBUG, mrpt::nav::CAbstractNavigator::NAVIGATING, mrpt::nav::CAbstractNavigator::stop(), and mrpt::nav::CAbstractNavigator::SUSPENDED.

Here is the call graph for this function:

◆ updateCurrentPoseAndSpeeds()

void CAbstractNavigator::updateCurrentPoseAndSpeeds ( )
protectedvirtualinherited

Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.

If an error is returned by the user callback, first, it calls robot.stop() ,then throws an std::runtime_error exception.

Definition at line 328 of file CAbstractNavigator.cpp.

References mrpt::poses::CPoseInterpolatorBase< DIM >::begin(), mrpt::poses::CPoseInterpolatorBase< DIM >::clear(), mrpt::poses::CPoseInterpolatorBase< DIM >::erase(), mrpt::nav::CAbstractNavigator::ERR_EMERGENCY_STOP, mrpt::nav::CAbstractNavigator::TErrorReason::error_code, mrpt::nav::CAbstractNavigator::TErrorReason::error_msg, mrpt::nav::CRobot2NavInterface::getCurrentPoseAndSpeeds(), mrpt::nav::CRobot2NavInterface::getNavigationTime(), mrpt::poses::CPoseInterpolatorBase< DIM >::insert(), mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_pose_frame_id, mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time, mrpt::nav::CAbstractNavigator::m_latestOdomPoses, mrpt::nav::CAbstractNavigator::m_latestPoses, mrpt::nav::CAbstractNavigator::m_navErrorReason, mrpt::nav::CAbstractNavigator::m_navigationState, mrpt::nav::CAbstractNavigator::m_navProfiler, mrpt::nav::CAbstractNavigator::m_robot, MRPT_LOG_ERROR, MRPT_LOG_THROTTLE_DEBUG_FMT, mrpt::nav::CAbstractNavigator::NAV_ERROR, mrpt::math::TPose2D::phi, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose_frame_id, PREVIOUS_POSES_MAX_AGE, mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry, mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin(), mrpt::math::TTwist2D::rotate(), mrpt::poses::CPoseInterpolatorBase< DIM >::size(), mrpt::nav::CAbstractNavigator::stop(), mrpt::system::timeDifference(), mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal, and mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal.

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::nav::CAbstractNavigator::processNavigateCommand(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ waypoints_isAligning()

bool mrpt::nav::CWaypointsNavigator::waypoints_isAligning ( ) const
inlineprotectedinherited

◆ waypoints_navigationStep()

void CWaypointsNavigator::waypoints_navigationStep ( )
protectedvirtualinherited

The waypoints-specific part of navigationStep()

Definition at line 112 of file CWaypointsNavigator.cpp.

References mrpt::nav::TWaypoint::allowed_distance, mrpt::math::angDistance(), ASSERT_, mrpt::nav::CAbstractNavigator::changeSpeeds(), mrpt::math::TSegment2D::distance(), mrpt::nav::TWaypointStatusSequence::final_goal_reached, mrpt::nav::CRobot2NavInterface::getAlignCmd(), mrpt::nav::CWaypointsNavigator::getWaypointNavStatus(), mrpt::nav::CWaypointsNavigator::impl_waypoint_is_reachable(), mrpt::nav::TWaypoint::INVALID_NUM, mrpt::poses::CPose2D::inverseComposePoint(), mrpt::nav::TWaypointStatusSequence::last_robot_pose, mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CWaypointsNavigator::m_is_aligning, mrpt::nav::CWaypointsNavigator::m_last_alignment_cmd, mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs, mrpt::nav::CAbstractNavigator::m_pending_events, mrpt::nav::CAbstractNavigator::m_robot, mrpt::nav::CAbstractNavigator::m_timlog_delays, mrpt::nav::CWaypointsNavigator::m_was_aligning, mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status, mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::max_distance_to_allow_skip_waypoint, mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::min_timesteps_confirm_skip_waypoints, MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_INFO_FMT, MRPT_LOG_THROTTLE_INFO_FMT, MRPT_START, mrpt::nav::CWaypointsNavigator::TNavigationParamsWaypoints::multiple_targets, mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::multitarget_look_ahead, mrpt::math::TPoint2D_< T >::norm(), mrpt::system::now(), mrpt::nav::CWaypointsNavigator::params_waypoints_navigator, mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::math::TPose2D::phi, mrpt::math::TSegment2D::point1, mrpt::math::TSegment2D::point2, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractNavigator::processNavigateCommand(), mrpt::RAD2DEG(), mrpt::nav::CRobot2NavInterface::sendNewWaypointTargetEvent(), mrpt::nav::CRobot2NavInterface::sendWaypointReachedEvent(), mrpt::nav::TWaypoint::speed_ratio, mrpt::square(), mrpt::nav::CAbstractNavigator::stop(), mrpt::nav::TWaypoint::target, mrpt::nav::CAbstractNavigator::TNavigationParams::target, mrpt::nav::CAbstractNavigator::TargetInfo::target_coords, mrpt::nav::TWaypoint::target_frame_id, mrpt::nav::CAbstractNavigator::TargetInfo::target_frame_id, mrpt::nav::TWaypoint::target_heading, mrpt::nav::CAbstractNavigator::TargetInfo::targetAllowedDistance, mrpt::nav::CAbstractNavigator::TargetInfo::targetDesiredRelSpeed, mrpt::nav::CAbstractNavigator::TargetInfo::targetIsIntermediaryWaypoint, mrpt::nav::CAbstractNavigator::TargetInfo::targetIsRelative, mrpt::system::timeDifference(), mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds(), mrpt::nav::TWaypointStatusSequence::waypoint_index_current_goal, mrpt::nav::TWaypointStatusSequence::waypoints, mrpt::math::TPoint2D_data< T >::x, mrpt::math::TPose2D::x, mrpt::math::TPoint2D_data< T >::y, and mrpt::math::TPose2D::y.

Referenced by mrpt::nav::CWaypointsNavigator::navigationStep().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ writeLogToFile()

void COutputLogger::writeLogToFile ( const std::string *  fname_in = nullptr) const
inherited

Write the contents of the COutputLogger instance to an external file.

Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided

See also
dumpToConsole, getAsString

Definition at line 165 of file COutputLogger.cpp.

References ASSERTMSG_, and mrpt::format().

Here is the call graph for this function:

Member Data Documentation

◆ executionTime

mrpt::system::CTicTac mrpt::nav::CAbstractPTGBasedReactive::executionTime
protectedinherited

◆ lastLogRecord

CLogFileRecord mrpt::nav::CAbstractPTGBasedReactive::lastLogRecord
protectedinherited

◆ logging_enable_console_output

bool mrpt::system::COutputLogger::logging_enable_console_output {true}
inherited

[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.

Definition at line 240 of file system/COutputLogger.h.

◆ logging_enable_keep_record

bool mrpt::system::COutputLogger::logging_enable_keep_record {false}
inherited

[Default=false] Enables storing all messages into an internal list.

See also
writeLogToFile, getLogAsString

Definition at line 243 of file system/COutputLogger.h.

◆ m_badNavAlarm_lastMinDistTime

mrpt::system::TTimeStamp mrpt::nav::CAbstractNavigator::m_badNavAlarm_lastMinDistTime
protectedinherited

◆ m_badNavAlarm_minDistTarget

double mrpt::nav::CAbstractNavigator::m_badNavAlarm_minDistTarget
protectedinherited

For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...

Definition at line 387 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), and mrpt::nav::CAbstractNavigator::processNavigateCommand().

◆ m_closing_navigator

bool mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator {false}
protectedinherited

Signal that the destructor has been called, so no more calls are accepted from other threads.

Definition at line 404 of file CAbstractPTGBasedReactive.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and mrpt::nav::CAbstractPTGBasedReactive::preDestructor().

◆ m_critZoneLastLog

std::recursive_mutex mrpt::nav::CAbstractPTGBasedReactive::m_critZoneLastLog
protectedinherited

◆ m_curPoseVel

TRobotPoseVel mrpt::nav::CAbstractNavigator::m_curPoseVel
protectedinherited

◆ m_enableConsoleOutput

bool mrpt::nav::CAbstractPTGBasedReactive::m_enableConsoleOutput
protectedinherited

◆ m_enableKeepLogRecords

bool mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords {false}
protectedinherited

◆ m_frame_tf

std::weak_ptr<mrpt::poses::FrameTransformer<2> > mrpt::nav::CAbstractNavigator::m_frame_tf
protectedinherited

◆ m_holonomicMethod

std::vector<CAbstractHolonomicReactiveMethod::Ptr> mrpt::nav::CAbstractPTGBasedReactive::m_holonomicMethod
protectedinherited

◆ m_infoPerPTG

std::vector<TInfoPerPTG> mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG
protectedinherited

◆ m_infoPerPTG_timestamp

mrpt::system::TTimeStamp mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp {INVALID_TIMESTAMP}
protectedinherited

◆ m_init_done

bool mrpt::nav::CAbstractPTGBasedReactive::m_init_done {false}
protectedinherited

◆ m_is_aligning

bool mrpt::nav::CWaypointsNavigator::m_is_aligning {false}
protectedinherited

◆ m_last_alignment_cmd

mrpt::system::TTimeStamp mrpt::nav::CWaypointsNavigator::m_last_alignment_cmd
protectedinherited

◆ m_last_curPoseVelUpdate_pose_frame_id

std::string mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_pose_frame_id
protectedinherited

◆ m_last_curPoseVelUpdate_robot_time

double mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time {-1e9}
protectedinherited

◆ m_last_vel_cmd

mrpt::kinematics::CVehicleVelCmd::Ptr mrpt::nav::CAbstractPTGBasedReactive::m_last_vel_cmd
protectedinherited

◆ m_lastSentVelCmd

TSentVelCmd mrpt::nav::CAbstractPTGBasedReactive::m_lastSentVelCmd
protectedinherited

◆ m_latestOdomPoses

mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestOdomPoses
protectedinherited

◆ m_latestPoses

mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestPoses
protectedinherited

◆ m_logFile

std::unique_ptr<mrpt::io::CStream> mrpt::nav::CAbstractPTGBasedReactive::m_logFile
protectedinherited

◆ m_min_verbosity_level

VerbosityLevel mrpt::system::COutputLogger::m_min_verbosity_level {LVL_INFO}
protectedinherited

Provided messages with VerbosityLevel smaller than this value shall be ignored.

Definition at line 253 of file system/COutputLogger.h.

Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().

◆ m_multiobjopt

mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr mrpt::nav::CAbstractPTGBasedReactive::m_multiobjopt
protectedinherited

◆ m_nav_cs

std::recursive_mutex mrpt::nav::CAbstractNavigator::m_nav_cs
protectedinherited

◆ m_nav_waypoints_cs

std::recursive_mutex mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs
protectedinherited

◆ m_navigationParams

std::unique_ptr<TNavigationParams> mrpt::nav::CAbstractNavigator::m_navigationParams
protectedinherited

◆ m_navigationState

TState mrpt::nav::CAbstractNavigator::m_navigationState {IDLE}
protectedinherited

◆ m_navProfiler

mrpt::system::CTimeLogger mrpt::nav::CAbstractNavigator::m_navProfiler
inherited

◆ m_pending_events

std::list<std::function<void(void)> > mrpt::nav::CAbstractNavigator::m_pending_events
protectedinherited

◆ m_prev_logfile

mrpt::io::CStream* mrpt::nav::CAbstractPTGBasedReactive::m_prev_logfile {nullptr}
protectedinherited

The current log file stream, or nullptr if not being used.

Definition at line 294 of file CAbstractPTGBasedReactive.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep().

◆ m_ptgmultilevel

std::vector<TPTGmultilevel> mrpt::nav::CReactiveNavigationSystem3D::m_ptgmultilevel
private

The set of PTG-transformations to be used: indices are [ptg_index][height_index].

Definition at line 160 of file CReactiveNavigationSystem3D.h.

Referenced by checkCollisionWithLatestObstacles(), getPTG(), getPTG_count(), loadConfigFile(), saveConfigFile(), STEP1_InitPTGs(), STEP3_WSpaceToTPSpace(), and ~CReactiveNavigationSystem3D().

◆ m_PTGsMustBeReInitialized

bool mrpt::nav::CAbstractPTGBasedReactive::m_PTGsMustBeReInitialized {true}
protectedinherited

◆ m_robot

CRobot2NavInterface& mrpt::nav::CAbstractNavigator::m_robot
protectedinherited

◆ m_robotShape

TRobotShape mrpt::nav::CReactiveNavigationSystem3D::m_robotShape
private

◆ m_timelogger

mrpt::system::CTimeLogger mrpt::nav::CAbstractPTGBasedReactive::m_timelogger {false}
protectedinherited

◆ m_timlog_delays

mrpt::system::CTimeLogger mrpt::nav::CAbstractNavigator::m_timlog_delays
protectedinherited

◆ m_was_aligning

bool mrpt::nav::CWaypointsNavigator::m_was_aligning {false}
protectedinherited

Whether the last timestep was "is_aligning" in a waypoint with heading.

Definition at line 166 of file CWaypointsNavigator.h.

Referenced by mrpt::nav::CWaypointsNavigator::onNavigateCommandReceived(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

◆ m_waypoint_nav_status

TWaypointStatusSequence mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status
protectedinherited

◆ m_WS_filter

mrpt::maps::CPointCloudFilterBase::Ptr mrpt::nav::CAbstractPTGBasedReactive::m_WS_filter
protectedinherited

◆ m_WS_Obstacles_inlevels

std::vector<mrpt::maps::CSimplePointsMap> mrpt::nav::CReactiveNavigationSystem3D::m_WS_Obstacles_inlevels
private

One point cloud per 2.5D robot-shape-slice, coordinates relative to the robot local frame.

Definition at line 153 of file CReactiveNavigationSystem3D.h.

Referenced by checkCollisionWithLatestObstacles(), implementSenseObstacles(), loggingGetWSObstaclesAndShape(), and STEP3_WSpaceToTPSpace().

◆ m_WS_Obstacles_timestamp

mrpt::system::TTimeStamp mrpt::nav::CAbstractPTGBasedReactive::m_WS_Obstacles_timestamp {INVALID_TIMESTAMP}
protectedinherited

◆ m_WS_Obstacles_unsorted

mrpt::maps::CSimplePointsMap mrpt::nav::CReactiveNavigationSystem3D::m_WS_Obstacles_unsorted
private

The unsorted set of obstacles from the sensors.

Definition at line 150 of file CReactiveNavigationSystem3D.h.

Referenced by implementSenseObstacles().

◆ meanExecutionPeriod

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod {0.7, 1}
protectedinherited

◆ meanExecutionTime

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanExecutionTime {0.7, 1}
protectedinherited

◆ meanTotalExecutionTime

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanTotalExecutionTime {0.7, 1}
protectedinherited

◆ params_abstract_navigator

TAbstractNavigatorParams mrpt::nav::CAbstractNavigator::params_abstract_navigator
inherited

◆ params_abstract_ptg_navigator

TAbstractPTGNavigatorParams mrpt::nav::CAbstractPTGBasedReactive::params_abstract_ptg_navigator
inherited

◆ params_waypoints_navigator

TWaypointsNavigatorParams mrpt::nav::CWaypointsNavigator::params_waypoints_navigator
inherited

◆ tictac

mrpt::system::CTicTac mrpt::nav::CAbstractPTGBasedReactive::tictac
protectedinherited

◆ tim_changeSpeed_avr

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::tim_changeSpeed_avr {0.7}
protectedinherited

◆ timerForExecutionPeriod

mrpt::system::CTicTac mrpt::nav::CAbstractPTGBasedReactive::timerForExecutionPeriod
protectedinherited

◆ timoff_curPoseAndSpeed_avr

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_curPoseAndSpeed_avr {0.7}
protectedinherited

◆ timoff_obstacles_avr

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_obstacles_avr {0.7}
protectedinherited

◆ timoff_sendVelCmd_avr

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_sendVelCmd_avr {0.7}
protectedinherited

◆ totalExecutionTime

mrpt::system::CTicTac mrpt::nav::CAbstractPTGBasedReactive::totalExecutionTime
protectedinherited



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020