Main MRPT website > C++ reference for MRPT 1.5.0
List of all members | Classes | Public Member Functions | Static 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::utils::CConfigFileMemory.

Next we provide a self-documented template config file:

# ------------------------------------------------------------------------------------------
# Example configuration file for MRPT 2.5 Reactive Navigation engine.
# See C++ documentation: http://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
# ------------------------------------------------------------------------------------------

[ROBOT_CONFIG]
Name = MyRobot	#Optional

#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 = 2

#Geometrical description of each level
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.4
LEVEL2_VECTORX = 0.05 0.05 -0.05 -0.05
LEVEL2_VECTORY = -0.1 0.1 0.1 -0.1


# ----------------------------------------------------
#	Parameters for navigation
# ----------------------------------------------------
[ReactiveParams]
# 0 or `hmVIRTUAL_FORCE_FIELDS`: Virtual Force Field
# 1 or `hmSEARCH_FOR_BEST_GAP`: Nearness Diagram (ND)
HOLONOMIC_METHOD = hmSEARCH_FOR_BEST_GAP

weights = 0.5 0.05 0.5 2.0 0.2 0.1
# 1: Free space
# 2: Dist. in sectors			
# 3: Heading toward target
# 4: Closer to target (euclidean)
# 5: Hysteresis
# 6: Security Distance

DIST_TO_TARGET_FOR_SENDING_EVENT = 0.5	# Minimum. distance to target for sending the end event. Set to 0 to send it just on navigation end

robotMax_V_mps = 0.70		# Speed limits - m/s
robotMax_W_degps = 60		# deg/s
ROBOTMODEL_DELAY = 0	# un-used param, must be present for compat. with old mrpt versions 
ROBOTMODEL_TAU = 0		# un-used param, must be present for compat. with old mrpt versions
MAX_REFERENCE_DISTANCE = 2	# Marks the maximum distance regarded by the reactive navigator (m)
SPEEDFILTER_TAU = 0.1	# The 'TAU' time constant of a 1st order lowpass filter for speed commands (s)  

# In normalized distances (distance / maxReference), the start and end of a 
# ramp function that scales the velocity output from the holonomic navigator:
#  velocity scale
#      ^
#      |           _____________
#      |          /
#    1 |         /
#      |        /
#    0 +-------+---|----------------> normalized distance
#           Start
#                 End
secureDistanceStart=0.05	# [0,1] smallest collision-free norm'd dist for which the robot will not move [default: 0.05]
secureDistanceEnd  =0.20	# [0,1] minimum norm'd clearance in current direction for speed to remain same [default:0.20]

USE_DELAYS_MODEL    = false 

# PTGs: See classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( http://reference.mrpt.org/devel/classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html)
# refer to papers for details.
#------------------------------------------------------------------------------
PTG_COUNT      = 3

# 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= 10.0      # Max distance to account for obstacles 
PTG1_num_paths = 121
PTG1_v_max_mps = 1.0
PTG1_w_max_dps = 60
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= 10.0      # Max distance to account for obstacles 
PTG2_num_paths   = 121
PTG2_v_max_mps   = 1.0
PTG2_w_max_dps   = 60
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= 10.0      # Max distance to account for obstacles 
PTG3_num_paths = 121
PTG3_v_max_mps = 1.0
PTG3_w_max_dps = 60
PTG3_K         = -1.0
PTG3_score_priority = 1.0

#	Parameters for the "Nearness diagram" Holonomic method
# --------------------------------------------------------
[ND_CONFIG]
factorWeights = 1.0 2.0 0.5 1.0
# 1: Free space
# 2: Dist. in sectors
# 3: Closer to target (euclidean)
# 4: Hysteresis
WIDE_GAP_SIZE_PERCENT            = 0.25
MAX_SECTOR_DIST_FOR_D2_PERCENT   = 0.25
RISK_EVALUATION_SECTORS_PERCENT  = 0.25
RISK_EVALUATION_DISTANCE         = 0.7  # In normalized ps-meters [0,1]
TARGET_SLOW_APPROACHING_DISTANCE = 0.8	# For stop gradually
TOO_CLOSE_OBSTACLE               = 0.03 # In normalized ps-meters

#	Parameters for the "VFF" Holonomic method
# ----------------------------------------------------
[VFF_CONFIG]
# Used to decrease speed gradually when the target is going to be reached
TARGET_SLOW_APPROACHING_DISTANCE = 0.8	
# Use it to control the relative weight of the target respect to the obstacles
TARGET_ATTRACTIVE_FORCE = 7.5


See Also
CAbstractNavigator, CParameterizedTrajectoryGenerator, CAbstractHolonomicReactiveMethod

Definition at line 68 of file nav/reactive/CReactiveNavigationSystem3D.h.

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

Inheritance diagram for mrpt::nav::CReactiveNavigationSystem3D:
Inheritance graph

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)
 See docs in ctor of base class. More...
 
virtual ~CReactiveNavigationSystem3D ()
 Destructor. More...
 
void changeRobotShape (TRobotShape robotShape)
 Change the robot shape, which is taken into account for collision grid building. More...
 
virtual size_t getPTG_count () const MRPT_OVERRIDE
 Returns the number of different PTGs that have been setup. More...
 
virtual
CParameterizedTrajectoryGenerator
getPTG (size_t i) MRPT_OVERRIDE
 Gets the i'th PTG. More...
 
virtual const
CParameterizedTrajectoryGenerator
getPTG (size_t i) const MRPT_OVERRIDE
 Gets the i'th PTG. More...
 
void initialize () MRPT_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::utils::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 setHolonomicMethod (THolonomicMethod method, const mrpt::utils::CConfigFileBase &cfgBase)
 
void loadHolonomicMethodConfig (const mrpt::utils::CConfigFileBase &ini, const std::string &section)
 Just loads the holonomic method params from the given config source. 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 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::utils::CTimeLoggergetTimeLogger () const
 Gives access to a const-ref to the internal time logger. More...
 
void loadConfigFile (const mrpt::utils::CConfigFileBase &cfg, const std::string &section_prefix="") MRPT_OVERRIDE
 Reload the configuration from a file. More...
 
const
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams
getCurrentRobotSpeedLimits () 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...
 
virtual void navigationStep () MRPT_OVERRIDE
 This method must be called periodically in order to effectively run the navigation. More...
 
virtual void cancel () MRPT_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...
 
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...
 

Static Public Attributes

static mrpt::system::TConsoleColor logging_levels_to_colors [NUMBER_OF_VERBOSITY_LEVELS]
 
static std::string logging_levels_to_names [NUMBER_OF_VERBOSITY_LEVELS]
 Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. Handy for coloring the input based on the verbosity of the message. More...
 

Protected Member Functions

void internal_loadConfigFile (const mrpt::utils::CConfigFileBase &ini, const std::string &section_prefix="") MRPT_OVERRIDE
 Loads derived-class specific parameters. More...
 
virtual void performNavigationStep () MRPT_OVERRIDE
 The main method for the navigator. More...
 
virtual bool impl_waypoint_is_reachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const MRPT_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 STEP5_PTGEvaluator (THolonomicMovement &holonomicMovement, const std::vector< double > &in_TPObstacles, const mrpt::nav::ClearanceDiagram &in_clearance, const mrpt::math::TPose2D &WS_Target, const mrpt::math::TPoint2D &TP_Target, CLogFileRecord::TInfoPerPTG &log, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, const mrpt::poses::CPose2D &relPoseVelCmd_NOP, const unsigned int ptg_idx4weights)
 Scores holonomicMovement. More...
 
virtual void STEP7_GenerateSpeedCommands (const THolonomicMovement &in_movement, mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd)
 
void STEP8_GenerateLogRecord (CLogFileRecord &newLogRec, const mrpt::math::TPose2D &relTarget, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmdPtr &new_vel_cmd, int nPTGs, const bool best_is_NOP_cmdvel, const mrpt::poses::CPose2D &rel_cur_pose_wrt_last_vel_cmd_NOP, const mrpt::poses::CPose2D &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...
 
virtual void onStartNewNavigation () MRPT_OVERRIDE
 Called whenever a new navigation has been started. More...
 
void ptg_eval_target_build_obstacles (CParameterizedTrajectoryGenerator *ptg, const size_t indexPTG, const mrpt::math::TPose2D &relTarget, const mrpt::poses::CPose2D &rel_pose_PTG_origin_wrt_sense, TInfoPerPTG &ipf, THolonomicMovement &holonomicMovement, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, mrpt::nav::CAbstractHolonomicReactiveMethod *holoMethod, const TNavigationParams &navp=TNavigationParams(), const mrpt::poses::CPose2D &relPoseVelCmd_NOP=mrpt::poses::CPose2D())
 
virtual void loadWaypointsParamsConfigFile (const mrpt::utils::CConfigFileBase &cfg, const std::string &sectionName)
 Loads parameters for waypoints navigation. More...
 
void updateCurrentPoseAndSpeeds ()
 Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPose,m_curVel and m_curVelLocal accordingly. More...
 
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(). Can be overriden. More...
 
virtual bool changeSpeedsNOP ()
 Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden. More...
 
virtual bool stop (bool isEmergencyStop)
 Default: forward call to m_robot.stop(). Can be overriden. More...
 

Protected Attributes

std::vector
< CAbstractHolonomicReactiveMethod * > 
m_holonomicMethod
 The holonomic navigation algorithm (one object per PTG, so internal states are maintained) More...
 
mrpt::utils::CStreamm_logFile
 The current log file stream, or NULL if not being used. More...
 
bool m_enableKeepLogRecords
 See enableKeepLogRecords. More...
 
CLogFileRecord lastLogRecord
 The last log. More...
 
mrpt::kinematics::CVehicleVelCmdPtr m_last_vel_cmd
 Last velocity commands. More...
 
mrpt::synch::CCriticalSectionRecursive m_critZoneLastLog
 Critical zones. More...
 
bool m_enableConsoleOutput
 Enables / disables the console debug output. More...
 
bool m_init_done
 Whether loadConfigFile() has been called or not. More...
 
mrpt::utils::CTicTac timerForExecutionPeriod
 
std::string ptg_cache_files_directory
 (Default: ".") More...
 
std::string robotName
 Robot name. More...
 
double refDistance
 "D_{max}" in papers. More...
 
double SPEEDFILTER_TAU
 Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering) More...
 
std::vector< float > weights
 length: 6 [0,5], or empty if using PTG-specific weights. More...
 
std::vector< std::vector< float > > weights4ptg
 
double secureDistanceStart
 In normalized distances, the start and end of a ramp function that scales the velocity output from the holonomic navigator: More...
 
double secureDistanceEnd
 
bool USE_DELAYS_MODEL
 
double MAX_DISTANCE_PREDICTED_ACTUAL_PATH
 for ptg continuation meters More...
 
double MIN_NORMALIZED_FREE_SPACE_FOR_PTG_CONTINUATION
 
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robotVelocityParams
 Params related to speed limits. Loaded during loadConfigFile() More...
 
mrpt::utils::CTimeLogger m_timelogger
 A complete time logger. More...
 
bool m_PTGsMustBeReInitialized
 
bool m_closing_navigator
 Signal that the destructor has been called, so no more calls are accepted from other threads. More...
 
mrpt::system::TTimeStamp m_WS_Obstacles_timestamp
 
mrpt::maps::CPointCloudFilterBasePtr m_WS_filter
 Default: none. More...
 
std::vector< TInfoPerPTGm_infoPerPTG
 Temporary buffers for working with each PTG during a navigationStep() More...
 
mrpt::system::TTimeStamp m_infoPerPTG_timestamp
 
TSentVelCmd m_lastSentVelCmd
 
TWaypointStatusSequence m_waypoint_nav_status
 The latest waypoints navigation command and the up-to-date control status. More...
 
mrpt::synch::CCriticalSectionRecursive m_nav_waypoints_cs
 
double MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT
 In meters. <0: unlimited. More...
 
int MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS
 How many times shall a future waypoint be seen as reachable to skip to it (Default: 1) More...
 
TState m_navigationState
 Current internal state of navigator: More...
 
TNavigationParamsm_navigationParams
 Current navigation parameters. More...
 
CRobot2NavInterfacem_robot
 The navigator-robot interface. More...
 
mrpt::synch::CCriticalSectionRecursive m_nav_cs
 mutex for all navigation methods More...
 
TRobotPoseVel m_curPoseVel
 Current robot pose (updated in CAbstractNavigator::navigationStep() ) More...
 
mrpt::system::TTimeStamp m_last_curPoseVelUpdate_time
 
mrpt::poses::CPose3DInterpolator m_latestPoses
 Latest robot poses and velocities (updated in CAbstractNavigator::navigationStep() ) More...
 
mrpt::utils::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
 
double m_badNavAlarm_AlarmTimeout
 
double DIST_TO_TARGET_FOR_SENDING_EVENT
 Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request. More...
 
VerbosityLevel m_min_verbosity_level
 Provided messages with VerbosityLevel smaller than this value shall be ignored. More...
 
Variables for CReactiveNavigationSystem::performNavigationStep
mrpt::utils::CTicTac totalExecutionTime
 
mrpt::utils::CTicTac executionTime
 
mrpt::utils::CTicTac tictac
 
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
 
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
 
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
 Runtime estimation of execution period of the method. More...
 
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
 
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
 
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
 
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
 

Private Member Functions

virtual void STEP1_InitPTGs () MRPT_OVERRIDE
 
bool implementSenseObstacles (mrpt::system::TTimeStamp &obs_timestamp) MRPT_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::poses::CPose2D &rel_pose_PTG_origin_wrt_sense) MRPT_OVERRIDE
 Builds TP-Obstacles from Workspace obstacles for the given PTG. More...
 
virtual void loggingGetWSObstaclesAndShape (CLogFileRecord &out_log) MRPT_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::CSimplePointsMap
m_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: More...
 

Logging methods

void logStr (const VerbosityLevel level, const std::string &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=NULL) 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. Called upon construction. More...
 
void logRegisterCallback (output_logger_callback_t userFunc, void *userParam=NULL)
 
void logDeregisterCallback (output_logger_callback_t userFunc, void *userParam=NULL)
 
bool logging_enable_console_output
 [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
 [Default=false] Enables storing all messages into an internal list. 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...
 
enum  TState { IDLE =0, NAVIGATING, SUSPENDED, NAV_ERROR }
 The different states for the navigation system. More...
 

Member Enumeration Documentation

The different states for the navigation system.

Enumerator
IDLE 
NAVIGATING 
SUSPENDED 
NAV_ERROR 

Definition at line 92 of file CAbstractNavigator.h.

Constructor & Destructor Documentation

mrpt::nav::CReactiveNavigationSystem3D::CReactiveNavigationSystem3D ( CRobot2NavInterface react_iterf_impl,
bool  enableConsoleOutput = true,
bool  enableLogFile = false 
)

See docs in ctor of base class.

virtual mrpt::nav::CReactiveNavigationSystem3D::~CReactiveNavigationSystem3D ( )
virtual

Destructor.

Member Function Documentation

virtual void mrpt::nav::CWaypointsNavigator::cancel ( )
virtualinherited

Cancel current navegation.

Reimplemented from mrpt::nav::CAbstractNavigator.

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 137 of file CAbstractPTGBasedReactive.h.

void mrpt::nav::CReactiveNavigationSystem3D::changeRobotShape ( TRobotShape  robotShape)

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

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

Default: forward call to m_robot.changeSpeed(). Can be overriden.

virtual bool mrpt::nav::CAbstractNavigator::changeSpeedsNOP ( )
protectedvirtualinherited

Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.

void mrpt::nav::CAbstractNavigator::doEmergencyStop ( const std::string &  msg)
protectedinherited

Stops the robot and set navigation state to error.

void mrpt::utils::COutputLogger::dumpLogToConsole ( ) const
inherited

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

See Also
writeToFile
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 105 of file CAbstractPTGBasedReactive.h.

void mrpt::nav::CAbstractPTGBasedReactive::enableLogFile ( bool  enable)
inherited

Enables/disables saving log files.

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 114 of file CAbstractPTGBasedReactive.h.

References MRPT_UNUSED_PARAM.

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

Get the current, global (honored for all PTGs) robot speed limits.

Definition at line 132 of file CAbstractPTGBasedReactive.h.

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

Returns the current navigator state.

Definition at line 100 of file CAbstractNavigator.h.

void mrpt::nav::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.
void mrpt::utils::COutputLogger::getLogAsString ( std::string &  log_contents) const
inherited

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

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

Get the history of COutputLogger instance in a string representation.

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

Return the last Tmsg instance registered in the logger history.

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

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

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

Return the name of the COutputLogger instance.

See Also
setLoggerName
VerbosityLevel mrpt::utils::COutputLogger::getMinLoggingLevel ( ) const
inlineinherited
virtual CParameterizedTrajectoryGenerator* mrpt::nav::CReactiveNavigationSystem3D::getPTG ( size_t  i)
inlinevirtual

Gets the i'th PTG.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 87 of file nav/reactive/CReactiveNavigationSystem3D.h.

References ASSERT_.

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

Gets the i'th PTG.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 91 of file nav/reactive/CReactiveNavigationSystem3D.h.

References ASSERT_.

virtual size_t mrpt::nav::CReactiveNavigationSystem3D::getPTG_count ( ) const
inlinevirtual

Returns the number of different PTGs that have been setup.

Implements mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 86 of file nav/reactive/CReactiveNavigationSystem3D.h.

References ASSERT_.

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

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

See Also
enableTimeLog

Definition at line 120 of file CAbstractPTGBasedReactive.h.

virtual void mrpt::nav::CWaypointsNavigator::getWaypointNavStatus ( TWaypointStatusSequence out_nav_status) const
virtualinherited

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

virtual bool mrpt::nav::CAbstractPTGBasedReactive::impl_waypoint_is_reachable ( const mrpt::math::TPoint2D wp_local_wrt_robot) const
protectedvirtualinherited

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.

bool mrpt::nav::CReactiveNavigationSystem3D::implementSenseObstacles ( mrpt::system::TTimeStamp obs_timestamp)
privatevirtual

Return false on any fatal error.

Implements mrpt::nav::CAbstractPTGBasedReactive.

void mrpt::nav::CAbstractPTGBasedReactive::initialize ( )
virtualinherited

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.

void mrpt::nav::CReactiveNavigationSystem3D::internal_loadConfigFile ( const mrpt::utils::CConfigFileBase cfg,
const std::string &  section_prefix = "" 
)
protectedvirtual

Loads derived-class specific parameters.

Implements mrpt::nav::CAbstractPTGBasedReactive.

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

Definition at line 159 of file COutputLogger.h.

bool mrpt::nav::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.

void mrpt::nav::CAbstractPTGBasedReactive::loadConfigFile ( const mrpt::utils::CConfigFileBase cfg,
const std::string &  section_prefix = "" 
)
virtualinherited

Reload the configuration from a file.

See details in CReactiveNavigationSystem docs. Section to be read is "{sect_prefix}ReactiveParams".

Implements mrpt::nav::CAbstractNavigator.

void mrpt::nav::CAbstractPTGBasedReactive::loadHolonomicMethodConfig ( const mrpt::utils::CConfigFileBase ini,
const std::string &  section 
)
inherited

Just loads the holonomic method params from the given config source.

See Also
setHolonomicMethod
virtual void mrpt::nav::CWaypointsNavigator::loadWaypointsParamsConfigFile ( const mrpt::utils::CConfigFileBase cfg,
const std::string &  sectionName 
)
protectedvirtualinherited

Loads parameters for waypoints navigation.

void void mrpt::utils::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
void mrpt::utils::COutputLogger::logDeregisterCallback ( output_logger_callback_t  userFunc,
void *  userParam = NULL 
)
inherited
void mrpt::utils::COutputLogger::logFmt ( const VerbosityLevel  level,
const char *  fmt,
  ... 
) const
inherited

Alternative logging method, which mimics the printf behavior.

Handy for not having to first use mrpt::format to pass a std::string message to logStr

// instead of:
logStr(mrpt::format("Today is the %d of %s, %d", 15, "July", 2016));
// one can use:
logFmt("Today is the %d of %s, %d", 15, "July", 2016);
See Also
logStr, logCond

Referenced by mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().

void mrpt::utils::COutputLogger::loggerReset ( )
inherited

Reset the contents of the logger instance. Called upon construction.

virtual void mrpt::nav::CReactiveNavigationSystem3D::loggingGetWSObstaclesAndShape ( CLogFileRecord out_log)
privatevirtual

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

Implements mrpt::nav::CAbstractPTGBasedReactive.

void mrpt::utils::COutputLogger::logRegisterCallback ( output_logger_callback_t  userFunc,
void *  userParam = NULL 
)
inherited
void mrpt::utils::COutputLogger::logStr ( const VerbosityLevel  level,
const std::string &  msg_str 
) const
inherited

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

See Also
logCond, logFmt

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal().

virtual void mrpt::nav::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.

virtual void mrpt::nav::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.
virtual void mrpt::nav::CWaypointsNavigator::navigationStep ( )
virtualinherited

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

Reimplemented from mrpt::nav::CAbstractNavigator.

virtual void mrpt::nav::CAbstractPTGBasedReactive::onStartNewNavigation ( )
protectedvirtualinherited

Called whenever a new navigation has been started.

Can be used to reset state variables, etc.

Reimplemented from mrpt::nav::CWaypointsNavigator.

virtual void mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep ( )
protectedvirtualinherited

The main method for the navigator.

Implements mrpt::nav::CAbstractNavigator.

void mrpt::nav::CAbstractPTGBasedReactive::preDestructor ( )
protectedinherited

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

void mrpt::nav::CAbstractPTGBasedReactive::ptg_eval_target_build_obstacles ( CParameterizedTrajectoryGenerator ptg,
const size_t  indexPTG,
const mrpt::math::TPose2D relTarget,
const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense,
TInfoPerPTG ipf,
THolonomicMovement holonomicMovement,
CLogFileRecord newLogRec,
const bool  this_is_PTG_continuation,
mrpt::nav::CAbstractHolonomicReactiveMethod holoMethod,
const TNavigationParams navp = TNavigationParams(),
const mrpt::poses::CPose2D relPoseVelCmd_NOP = mrpt::poses::CPose2D() 
)
protectedinherited
virtual void mrpt::nav::CAbstractNavigator::resetNavError ( )
virtualinherited

Resets a NAV_ERROR state back to IDLE

virtual void mrpt::nav::CAbstractNavigator::resume ( )
virtualinherited

Continues with suspended navigation.

See Also
suspend
void mrpt::nav::CAbstractPTGBasedReactive::setHolonomicMethod ( const std::string &  method,
const mrpt::utils::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

void mrpt::nav::CAbstractPTGBasedReactive::setHolonomicMethod ( THolonomicMethod  method,
const mrpt::utils::CConfigFileBase cfgBase 
)
inherited
void mrpt::utils::COutputLogger::setLoggerName ( const std::string &  name)
inherited

Set the name of the COutputLogger instance.

See Also
getLoggerName
void mrpt::utils::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

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), and mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().

void mrpt::utils::COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited
virtual void mrpt::nav::CReactiveNavigationSystem3D::STEP1_InitPTGs ( )
privatevirtual
bool mrpt::nav::CAbstractPTGBasedReactive::STEP2_SenseObstacles ( )
protectedinherited
void mrpt::nav::CReactiveNavigationSystem3D::STEP3_WSpaceToTPSpace ( const size_t  ptg_idx,
std::vector< double > &  out_TPObstacles,
mrpt::nav::ClearanceDiagram out_clearance,
const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense 
)
privatevirtual

Builds TP-Obstacles from Workspace obstacles for the given PTG.

"out_TPObstacles" is already initialized to the proper length and maximum collision-free distance for each "k" trajectory index. Distances are in "pseudo-meters". They will be normalized automatically to [0,1] upon return.

Implements mrpt::nav::CAbstractPTGBasedReactive.

void mrpt::nav::CAbstractPTGBasedReactive::STEP5_PTGEvaluator ( THolonomicMovement holonomicMovement,
const std::vector< double > &  in_TPObstacles,
const mrpt::nav::ClearanceDiagram in_clearance,
const mrpt::math::TPose2D WS_Target,
const mrpt::math::TPoint2D TP_Target,
CLogFileRecord::TInfoPerPTG log,
CLogFileRecord newLogRec,
const bool  this_is_PTG_continuation,
const mrpt::poses::CPose2D relPoseVelCmd_NOP,
const unsigned int  ptg_idx4weights 
)
protectedinherited

Scores holonomicMovement.

virtual void mrpt::nav::CAbstractPTGBasedReactive::STEP7_GenerateSpeedCommands ( const THolonomicMovement in_movement,
mrpt::kinematics::CVehicleVelCmdPtr &  new_vel_cmd 
)
protectedvirtualinherited
void mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord ( CLogFileRecord newLogRec,
const mrpt::math::TPose2D relTarget,
int  nSelectedPTG,
const mrpt::kinematics::CVehicleVelCmdPtr &  new_vel_cmd,
int  nPTGs,
const bool  best_is_NOP_cmdvel,
const mrpt::poses::CPose2D rel_cur_pose_wrt_last_vel_cmd_NOP,
const mrpt::poses::CPose2D rel_pose_PTG_origin_wrt_sense_NOP,
const double  executionTimeValue,
const double  tim_changeSpeed,
const mrpt::system::TTimeStamp tim_start_iteration 
)
protectedinherited
virtual bool mrpt::nav::CAbstractNavigator::stop ( bool  isEmergencyStop)
protectedvirtualinherited

Default: forward call to m_robot.stop(). Can be overriden.

virtual void mrpt::nav::CAbstractNavigator::suspend ( )
virtualinherited

Suspend current navegation.

See Also
resume
void mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds ( )
protectedinherited

Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPose,m_curVel and m_curVelLocal accordingly.

void mrpt::utils::COutputLogger::writeLogToFile ( const std::string *  fname_in = NULL) 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

Member Data Documentation

double mrpt::nav::CAbstractNavigator::DIST_TO_TARGET_FOR_SENDING_EVENT
protectedinherited

Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.

Definition at line 150 of file CAbstractNavigator.h.

mrpt::utils::CTicTac mrpt::nav::CAbstractPTGBasedReactive::executionTime
protectedinherited

Definition at line 204 of file CAbstractPTGBasedReactive.h.

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

The last log.

Definition at line 158 of file CAbstractPTGBasedReactive.h.

bool mrpt::utils::COutputLogger::logging_enable_console_output
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 184 of file COutputLogger.h.

bool mrpt::utils::COutputLogger::logging_enable_keep_record
inherited

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

See Also
writeLogToFile, getLogAsString

Definition at line 185 of file COutputLogger.h.

mrpt::system::TConsoleColor mrpt::utils::COutputLogger::logging_levels_to_colors[NUMBER_OF_VERBOSITY_LEVELS]
staticinherited

Definition at line 96 of file COutputLogger.h.

std::string mrpt::utils::COutputLogger::logging_levels_to_names[NUMBER_OF_VERBOSITY_LEVELS]
staticinherited

Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. Handy for coloring the input based on the verbosity of the message.

Map from VerbosityLevels to their corresponding names. Handy for printing the current message VerbosityLevel along with the actual content

Definition at line 97 of file COutputLogger.h.

double mrpt::nav::CAbstractNavigator::m_badNavAlarm_AlarmTimeout
protectedinherited

Definition at line 149 of file CAbstractNavigator.h.

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

Definition at line 148 of file CAbstractNavigator.h.

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 147 of file CAbstractNavigator.h.

bool mrpt::nav::CAbstractPTGBasedReactive::m_closing_navigator
protectedinherited

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

Definition at line 250 of file CAbstractPTGBasedReactive.h.

mrpt::synch::CCriticalSectionRecursive mrpt::nav::CAbstractPTGBasedReactive::m_critZoneLastLog
protectedinherited

Critical zones.

Definition at line 161 of file CAbstractPTGBasedReactive.h.

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

Current robot pose (updated in CAbstractNavigator::navigationStep() )

Definition at line 140 of file CAbstractNavigator.h.

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

Enables / disables the console debug output.

Definition at line 163 of file CAbstractPTGBasedReactive.h.

bool mrpt::nav::CAbstractPTGBasedReactive::m_enableKeepLogRecords
protectedinherited

See enableKeepLogRecords.

Definition at line 157 of file CAbstractPTGBasedReactive.h.

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

The holonomic navigation algorithm (one object per PTG, so internal states are maintained)

Definition at line 155 of file CAbstractPTGBasedReactive.h.

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

Temporary buffers for working with each PTG during a navigationStep()

Definition at line 265 of file CAbstractPTGBasedReactive.h.

mrpt::system::TTimeStamp mrpt::nav::CAbstractPTGBasedReactive::m_infoPerPTG_timestamp
protectedinherited

Definition at line 266 of file CAbstractPTGBasedReactive.h.

bool mrpt::nav::CAbstractPTGBasedReactive::m_init_done
protectedinherited

Whether loadConfigFile() has been called or not.

Definition at line 164 of file CAbstractPTGBasedReactive.h.

mrpt::system::TTimeStamp mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_time
protectedinherited

Definition at line 141 of file CAbstractNavigator.h.

mrpt::kinematics::CVehicleVelCmdPtr mrpt::nav::CAbstractPTGBasedReactive::m_last_vel_cmd
protectedinherited

Last velocity commands.

Definition at line 159 of file CAbstractPTGBasedReactive.h.

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

Definition at line 295 of file CAbstractPTGBasedReactive.h.

mrpt::poses::CPose3DInterpolator mrpt::nav::CAbstractNavigator::m_latestPoses
protectedinherited

Latest robot poses and velocities (updated in CAbstractNavigator::navigationStep() )

Definition at line 142 of file CAbstractNavigator.h.

mrpt::utils::CStream* mrpt::nav::CAbstractPTGBasedReactive::m_logFile
protectedinherited

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

Definition at line 156 of file CAbstractPTGBasedReactive.h.

VerbosityLevel mrpt::utils::COutputLogger::m_min_verbosity_level
protectedinherited

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

Definition at line 207 of file COutputLogger.h.

mrpt::synch::CCriticalSectionRecursive mrpt::nav::CAbstractNavigator::m_nav_cs
protectedinherited

mutex for all navigation methods

Definition at line 130 of file CAbstractNavigator.h.

mrpt::synch::CCriticalSectionRecursive mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs
protectedinherited

Definition at line 58 of file CWaypointsNavigator.h.

TNavigationParams* mrpt::nav::CAbstractNavigator::m_navigationParams
protectedinherited

Current navigation parameters.

Definition at line 126 of file CAbstractNavigator.h.

TState mrpt::nav::CAbstractNavigator::m_navigationState
protectedinherited

Current internal state of navigator:

Definition at line 125 of file CAbstractNavigator.h.

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

The set of PTG-transformations to be used:

Definition at line 123 of file nav/reactive/CReactiveNavigationSystem3D.h.

bool mrpt::nav::CAbstractPTGBasedReactive::m_PTGsMustBeReInitialized
protectedinherited

Definition at line 200 of file CAbstractPTGBasedReactive.h.

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

The navigator-robot interface.

Definition at line 128 of file CAbstractNavigator.h.

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

The robot 3D shape model.

Definition at line 120 of file nav/reactive/CReactiveNavigationSystem3D.h.

mrpt::utils::CTimeLogger mrpt::nav::CAbstractPTGBasedReactive::m_timelogger
protectedinherited

A complete time logger.

See Also
enableTimeLog()

Definition at line 199 of file CAbstractPTGBasedReactive.h.

mrpt::utils::CTimeLogger mrpt::nav::CAbstractNavigator::m_timlog_delays
protectedinherited

Time logger to collect delay-related stats.

Definition at line 144 of file CAbstractNavigator.h.

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

The latest waypoints navigation command and the up-to-date control status.

Definition at line 57 of file CWaypointsNavigator.h.

mrpt::maps::CPointCloudFilterBasePtr mrpt::nav::CAbstractPTGBasedReactive::m_WS_filter
protectedinherited

Default: none.

Definition at line 253 of file CAbstractPTGBasedReactive.h.

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 116 of file nav/reactive/CReactiveNavigationSystem3D.h.

mrpt::system::TTimeStamp mrpt::nav::CAbstractPTGBasedReactive::m_WS_Obstacles_timestamp
protectedinherited

Definition at line 252 of file CAbstractPTGBasedReactive.h.

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

The unsorted set of obstacles from the sensors.

Definition at line 115 of file nav/reactive/CReactiveNavigationSystem3D.h.

double mrpt::nav::CAbstractPTGBasedReactive::MAX_DISTANCE_PREDICTED_ACTUAL_PATH
protectedinherited

for ptg continuation meters

Definition at line 193 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CWaypointsNavigator::MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT
protectedinherited

In meters. <0: unlimited.

Definition at line 60 of file CWaypointsNavigator.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanExecutionPeriod
protectedinherited

Runtime estimation of execution period of the method.

Definition at line 207 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanExecutionTime
protectedinherited

Definition at line 205 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::meanTotalExecutionTime
protectedinherited

Definition at line 206 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CAbstractPTGBasedReactive::MIN_NORMALIZED_FREE_SPACE_FOR_PTG_CONTINUATION
protectedinherited

Definition at line 194 of file CAbstractPTGBasedReactive.h.

int mrpt::nav::CWaypointsNavigator::MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS
protectedinherited

How many times shall a future waypoint be seen as reachable to skip to it (Default: 1)

Definition at line 61 of file CWaypointsNavigator.h.

std::string mrpt::nav::CAbstractPTGBasedReactive::ptg_cache_files_directory
protectedinherited

(Default: ".")

Definition at line 168 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CAbstractPTGBasedReactive::refDistance
protectedinherited

"D_{max}" in papers.

Definition at line 170 of file CAbstractPTGBasedReactive.h.

std::string mrpt::nav::CAbstractPTGBasedReactive::robotName
protectedinherited

Robot name.

Definition at line 169 of file CAbstractPTGBasedReactive.h.

mrpt::kinematics::CVehicleVelCmd::TVelCmdParams mrpt::nav::CAbstractPTGBasedReactive::robotVelocityParams
protectedinherited

Params related to speed limits. Loaded during loadConfigFile()

Definition at line 196 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CAbstractPTGBasedReactive::secureDistanceEnd
protectedinherited

Definition at line 191 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CAbstractPTGBasedReactive::secureDistanceStart
protectedinherited

In normalized distances, the start and end of a ramp function that scales the velocity output from the holonomic navigator:

velocity scale
^
| _____________
| /
1 | /
| /
0 +-------+---|----------------> normalized distance
Start
End

Definition at line 191 of file CAbstractPTGBasedReactive.h.

double mrpt::nav::CAbstractPTGBasedReactive::SPEEDFILTER_TAU
protectedinherited

Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0: no filtering)

Definition at line 171 of file CAbstractPTGBasedReactive.h.

mrpt::utils::CTicTac mrpt::nav::CAbstractPTGBasedReactive::tictac
protectedinherited

Definition at line 204 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::tim_changeSpeed_avr
protectedinherited

Definition at line 208 of file CAbstractPTGBasedReactive.h.

mrpt::utils::CTicTac mrpt::nav::CAbstractPTGBasedReactive::timerForExecutionPeriod
protectedinherited

Definition at line 165 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_curPoseAndSpeed_avr
protectedinherited

Definition at line 208 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_obstacles_avr
protectedinherited

Definition at line 208 of file CAbstractPTGBasedReactive.h.

mrpt::math::LowPassFilter_IIR1 mrpt::nav::CAbstractPTGBasedReactive::timoff_sendVelCmd_avr
protectedinherited

Definition at line 208 of file CAbstractPTGBasedReactive.h.

mrpt::utils::CTicTac mrpt::nav::CAbstractPTGBasedReactive::totalExecutionTime
protectedinherited

Definition at line 204 of file CAbstractPTGBasedReactive.h.

bool mrpt::nav::CAbstractPTGBasedReactive::USE_DELAYS_MODEL
protectedinherited

Definition at line 192 of file CAbstractPTGBasedReactive.h.

std::vector<float> mrpt::nav::CAbstractPTGBasedReactive::weights
protectedinherited

length: 6 [0,5], or empty if using PTG-specific weights.

See Also
weights4ptg

Definition at line 172 of file CAbstractPTGBasedReactive.h.

std::vector<std::vector<float> > mrpt::nav::CAbstractPTGBasedReactive::weights4ptg
protectedinherited

Definition at line 173 of file CAbstractPTGBasedReactive.h.




Page generated by Doxygen 1.8.3.1 for MRPT 1.5.0 Git: 7b95fe2 Thu Jan 19 22:55:15 2017 +0100 at Fri Jan 20 00:00:19 CET 2017 Hosted on:
SourceForge.net Logo