MRPT  1.9.9
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::CAbstractNavigator Class Referenceabstract

Detailed Description

This is the base class for any reactive/planned navigation system.

See derived classes.

How to use:

It implements the following state machine (see CAbstractNavigator::getCurrentState() ), taking into account the extensions described in CWaypointsNavigator

See also
CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all children classes

Definition at line 58 of file CAbstractNavigator.h.

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

Inheritance diagram for mrpt::nav::CAbstractNavigator:

Classes

struct  TAbstractNavigatorParams
 
struct  TargetInfo
 Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes. More...
 
struct  TErrorReason
 
struct  TNavigationParams
 The struct for configuring navigation requests. More...
 
struct  TNavigationParamsBase
 Base for all high-level navigation commands. More...
 
struct  TRobotPoseVel
 

Public Member Functions

 CAbstractNavigator (CRobot2NavInterface &robot_interface_impl)
 ctor More...
 
 ~CAbstractNavigator () override
 dtor More...
 
const mrpt::system::CTimeLoggergetDelaysTimeLogger () const
 Gives access to a const-ref to the internal time logger used to estimate delays. 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

TAbstractNavigatorParams params_abstract_navigator
 
mrpt::system::CTimeLogger m_navProfiler
 Publicly available time profiling object. More...
 

Protected Member Functions

void dispatchPendingNavEvents ()
 
virtual void performNavigationStep ()=0
 To be implemented in derived classes. More...
 
virtual void onStartNewNavigation ()=0
 Called whenever a new navigation has been started. More...
 
virtual void onNavigateCommandReceived ()
 Called after each call to CAbstractNavigator::navigate() More...
 
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...
 
virtual bool checkHasReachedTarget (const double targetDist) const
 Default implementation: check if target_dist is below the accepted distance. More...
 
virtual bool checkCollisionWithLatestObstacles (const mrpt::math::TPose2D &relative_robot_pose) const
 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...
 

Protected Attributes

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...
 

Private Member Functions

void internal_onStartNewNavigation ()
 Called before starting a new navigation. More...
 

Private Attributes

TState m_lastNavigationState {IDLE}
 Last internal state of navigator: More...
 
TErrorReason m_navErrorReason
 
bool m_navigationEndEventSent {false}
 Will be false until the navigation end is sent, and it is reset with each new command. More...
 
int m_counter_check_target_is_blocked {0}
 
bool m_rethrow_exceptions {false}
 

Navigation control API

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...
 
virtual void loadConfigFile (const mrpt::config::CConfigFileBase &c)
 Loads all params from a file. More...
 
virtual void saveConfigFile (mrpt::config::CConfigFileBase &c) const
 Saves all current options to a config file. More...
 
virtual void initialize ()=0
 Must be called before any other navigation command. More...
 
virtual void navigationStep ()
 This method must be called periodically in order to effectively run the navigation. More...
 
virtual void navigate (const TNavigationParams *params)
 Navigation request to a single target location. More...
 
virtual void cancel ()
 Cancel current navegation. 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
 

Logging methods

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...
 
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)
 

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

◆ CAbstractNavigator()

CAbstractNavigator::CAbstractNavigator ( CRobot2NavInterface robot_interface_impl)

ctor

Definition at line 88 of file CAbstractNavigator.cpp.

References mrpt::poses::imLinear2Neig, mrpt::system::LVL_DEBUG, m_latestOdomPoses, m_latestPoses, mrpt::poses::CPoseInterpolatorBase< DIM >::setInterpolationMethod(), and mrpt::system::COutputLogger::setVerbosityLevel().

Here is the call graph for this function:

◆ ~CAbstractNavigator()

CAbstractNavigator::~CAbstractNavigator ( )
overridedefault

dtor

Member Function Documentation

◆ cancel()

void CAbstractNavigator::cancel ( )
virtual

Cancel current navegation.

Reimplemented in mrpt::nav::CWaypointsNavigator.

Definition at line 99 of file CAbstractNavigator.cpp.

References IDLE, m_nav_cs, m_navigationState, MRPT_LOG_DEBUG, and stop().

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

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

◆ changeSpeeds()

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

Default: forward call to m_robot.changeSpeed().

Can be overriden.

Definition at line 418 of file CAbstractNavigator.cpp.

References mrpt::nav::CRobot2NavInterface::changeSpeeds(), and 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 ( )
protectedvirtual

Default: forward call to m_robot.changeSpeedsNOP().

Can be overriden.

Definition at line 424 of file CAbstractNavigator.cpp.

References mrpt::nav::CRobot2NavInterface::changeSpeedsNOP(), and 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 CAbstractNavigator::checkCollisionWithLatestObstacles ( const mrpt::math::TPose2D relative_robot_pose) const
protectedvirtual

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 in mrpt::nav::CReactiveNavigationSystem3D, and mrpt::nav::CReactiveNavigationSystem.

Definition at line 685 of file CAbstractNavigator.cpp.

Referenced by performNavigationStepNavigating().

Here is the caller graph for this function:

◆ checkHasReachedTarget()

bool CAbstractNavigator::checkHasReachedTarget ( const double  targetDist) const
protectedvirtual

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

Definition at line 481 of file CAbstractNavigator.cpp.

Referenced by performNavigationStepNavigating().

Here is the caller graph for this function:

◆ dispatchPendingNavEvents()

void CAbstractNavigator::dispatchPendingNavEvents ( )
protected

Definition at line 243 of file CAbstractNavigator.cpp.

References m_pending_events.

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

Here is the caller graph for this function:

◆ doEmergencyStop()

void CAbstractNavigator::doEmergencyStop ( const std::string msg)
protectedvirtual

Stops the robot and set navigation state to error.

Definition at line 254 of file CAbstractNavigator.cpp.

References ERR_EMERGENCY_STOP, ERR_NONE, mrpt::nav::CAbstractNavigator::TErrorReason::error_code, mrpt::nav::CAbstractNavigator::TErrorReason::error_msg, m_navErrorReason, m_navigationState, MRPT_LOG_ERROR, NAV_ERROR, and 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.

◆ enableRethrowNavExceptions()

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

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 m_rethrow_exceptions.

◆ getCurrentState()

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

Returns the current navigator state.

Definition at line 181 of file CAbstractNavigator.h.

References m_navigationState.

◆ getDelaysTimeLogger()

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

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 m_timlog_delays.

◆ getErrorReason()

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

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

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

Definition at line 204 of file CAbstractNavigator.h.

References m_navErrorReason.

◆ getFrameTF()

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

Get the current frame tf object (defaults to nullptr)

See also
setFrameTF

Definition at line 218 of file CAbstractNavigator.h.

References m_frame_tf.

◆ 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:

◆ 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

◆ initialize()

virtual void mrpt::nav::CAbstractNavigator::initialize ( )
pure virtual

Must be called before any other navigation command.

Implemented in mrpt::nav::CAbstractPTGBasedReactive, and mrpt::nav::CNavigatorManualSequence.

◆ internal_onStartNewNavigation()

void CAbstractNavigator::internal_onStartNewNavigation ( )
private

Called before starting a new navigation.

Internally, it calls to child-implemented onStartNewNavigation()

Definition at line 487 of file CAbstractNavigator.cpp.

References mrpt::poses::CPoseInterpolatorBase< DIM >::clear(), m_latestOdomPoses, m_latestPoses, m_robot, onStartNewNavigation(), and mrpt::nav::CRobot2NavInterface::startWatchdog().

Referenced by performNavigationStepNavigating().

Here is the call graph for this function:
Here is the caller 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:

◆ isRethrowNavExceptionsEnabled()

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

Definition at line 232 of file CAbstractNavigator.h.

References m_rethrow_exceptions.

◆ loadConfigFile()

void CAbstractNavigator::loadConfigFile ( const mrpt::config::CConfigFileBase c)
virtual

Loads all params from a file.

To be called before initialize(). Each derived class MUST load its own parameters, and then call ITS PARENT'S overriden method to ensure all params are loaded.

Reimplemented in mrpt::nav::CAbstractPTGBasedReactive, mrpt::nav::CWaypointsNavigator, mrpt::nav::CReactiveNavigationSystem3D, mrpt::nav::CReactiveNavigationSystem, and mrpt::nav::CNavigatorManualSequence.

Definition at line 149 of file CAbstractNavigator.cpp.

References mrpt::system::CTimeLogger::enable(), mrpt::config::CConfigFileMemory::getContent(), mrpt::system::CTimeLogger::isEnabled(), mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::loadFromConfigFile(), m_navProfiler, MRPT_END, MRPT_LOG_INFO, MRPT_START, params_abstract_navigator, and saveConfigFile().

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

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

◆ 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:

◆ 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)
virtual

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 316 of file CAbstractNavigator.cpp.

References MRPT_END, MRPT_START, onNavigateCommandReceived(), and processNavigateCommand().

Here is the call graph for this function:

◆ navigationStep()

void CAbstractNavigator::navigationStep ( )
virtual

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

Reimplemented in mrpt::nav::CWaypointsNavigator, and mrpt::nav::CNavigatorManualSequence.

Definition at line 176 of file CAbstractNavigator.cpp.

References dispatchPendingNavEvents(), IDLE, m_lastNavigationState, m_nav_cs, m_navigationState, m_navProfiler, m_pending_events, m_robot, MRPT_LOG_ERROR, MRPT_LOG_INFO, NAV_ERROR, NAVIGATING, performNavigationStepNavigating(), mrpt::nav::CRobot2NavInterface::sendNavigationEndDueToErrorEvent(), stop(), mrpt::nav::CRobot2NavInterface::stopWatchdog(), and SUSPENDED.

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

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

◆ onNavigateCommandReceived()

void CAbstractNavigator::onNavigateCommandReceived ( )
protectedvirtual

Called after each call to CAbstractNavigator::navigate()

Reimplemented in mrpt::nav::CWaypointsNavigator.

Definition at line 274 of file CAbstractNavigator.cpp.

References m_nav_cs, m_navigationEndEventSent, and m_navigationParams.

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

Here is the caller graph for this function:

◆ onStartNewNavigation()

virtual void mrpt::nav::CAbstractNavigator::onStartNewNavigation ( )
protectedpure virtual

Called whenever a new navigation has been started.

Can be used to reset state variables, etc.

Implemented in mrpt::nav::CAbstractPTGBasedReactive, mrpt::nav::CWaypointsNavigator, and mrpt::nav::CNavigatorManualSequence.

Referenced by internal_onStartNewNavigation().

Here is the caller graph for this function:

◆ performNavigationStep()

virtual void mrpt::nav::CAbstractNavigator::performNavigationStep ( )
protectedpure virtual

To be implemented in derived classes.

Implemented in mrpt::nav::CAbstractPTGBasedReactive, and mrpt::nav::CNavigatorManualSequence.

Referenced by performNavigationStepNavigating().

Here is the caller graph for this function:

◆ performNavigationStepNavigating()

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

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 496 of file CAbstractNavigator.cpp.

References ASSERT_, checkCollisionWithLatestObstacles(), 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(), ERR_CANNOT_REACH_TARGET, ERR_NONE, 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, IDLE, internal_onStartNewNavigation(), mrpt::system::COutputLogger::logFmt(), mrpt::system::LVL_WARN, m_badNavAlarm_lastMinDistTime, m_badNavAlarm_minDistTarget, m_counter_check_target_is_blocked, m_curPoseVel, m_lastNavigationState, m_latestPoses, m_navErrorReason, m_navigationEndEventSent, m_navigationParams, m_navigationState, m_pending_events, m_rethrow_exceptions, m_robot, MRPT_LOG_DEBUG, MRPT_LOG_ERROR, MRPT_LOG_ERROR_FMT, MRPT_LOG_INFO, MRPT_LOG_THROTTLE_WARN, MRPT_LOG_WARN, NAV_ERROR, NAVIGATING, params_abstract_navigator, 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(), stop(), mrpt::system::timeDifference(), and updateCurrentPoseAndSpeeds().

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

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

◆ processNavigateCommand()

void CAbstractNavigator::processNavigateCommand ( const TNavigationParams params)
protectedvirtual

Does the job of navigate(), except the call to onNavigateCommandReceived()

Definition at line 282 of file CAbstractNavigator.cpp.

References ASSERT_, mrpt::system::getCurrentTime(), m_badNavAlarm_lastMinDistTime, m_badNavAlarm_minDistTarget, m_curPoseVel, m_nav_cs, m_navErrorReason, m_navigationParams, m_navigationState, MRPT_END, MRPT_START, NAVIGATING, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, and updateCurrentPoseAndSpeeds().

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

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

◆ resetNavError()

void CAbstractNavigator::resetNavError ( )
virtual

Resets a NAV_ERROR state back to IDLE

Definition at line 131 of file CAbstractNavigator.cpp.

References IDLE, m_nav_cs, m_navErrorReason, m_navigationState, MRPT_LOG_DEBUG, and NAV_ERROR.

◆ resume()

void CAbstractNavigator::resume ( )
virtual

Continues with suspended navigation.

See also
suspend

Definition at line 108 of file CAbstractNavigator.cpp.

References m_nav_cs, m_navigationState, MRPT_LOG_DEBUG, NAVIGATING, and SUSPENDED.

◆ saveConfigFile()

void CAbstractNavigator::saveConfigFile ( mrpt::config::CConfigFileBase c) const
virtual

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 in mrpt::nav::CAbstractPTGBasedReactive, mrpt::nav::CWaypointsNavigator, mrpt::nav::CReactiveNavigationSystem3D, mrpt::nav::CReactiveNavigationSystem, and mrpt::nav::CNavigatorManualSequence.

Definition at line 170 of file CAbstractNavigator.cpp.

References params_abstract_navigator, and mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::saveToConfigFile().

Referenced by loadConfigFile(), and mrpt::nav::CWaypointsNavigator::saveConfigFile().

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

◆ setFrameTF()

void CAbstractNavigator::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").

Definition at line 143 of file CAbstractNavigator.cpp.

References m_frame_tf.

◆ 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(), and mrpt::apps::CGridMapAlignerApp::run().

Here is the caller graph for this function:

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited

alias of setMinLoggingLevel()

Definition at line 149 of file COutputLogger.cpp.

Referenced by CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::math::ransac_detect_2D_lines(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ stop()

bool CAbstractNavigator::stop ( bool  isEmergencyStop)
protectedvirtual

Default: forward call to m_robot.stop().

Can be overriden.

Definition at line 426 of file CAbstractNavigator.cpp.

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

Referenced by cancel(), doEmergencyStop(), mrpt::nav::CNavigatorManualSequence::navigationStep(), navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), performNavigationStepNavigating(), mrpt::nav::CAbstractPTGBasedReactive::preDestructor(), suspend(), 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 ( )
virtual

Suspend current navegation.

See also
resume

Definition at line 117 of file CAbstractNavigator.cpp.

References m_nav_cs, m_navigationState, MRPT_LOG_DEBUG, NAVIGATING, stop(), and SUSPENDED.

Here is the call graph for this function:

◆ updateCurrentPoseAndSpeeds()

void CAbstractNavigator::updateCurrentPoseAndSpeeds ( )
protectedvirtual

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 325 of file CAbstractNavigator.cpp.

References mrpt::poses::CPoseInterpolatorBase< DIM >::begin(), mrpt::poses::CPoseInterpolatorBase< DIM >::clear(), mrpt::poses::CPoseInterpolatorBase< DIM >::erase(), 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(), m_curPoseVel, m_last_curPoseVelUpdate_pose_frame_id, m_last_curPoseVelUpdate_robot_time, m_latestOdomPoses, m_latestPoses, m_navErrorReason, m_navigationState, m_navProfiler, m_robot, MRPT_LOG_ERROR, MRPT_LOG_THROTTLE_DEBUG_FMT, 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(), stop(), mrpt::system::timeDifference(), mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal, and mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal.

Referenced by performNavigationStepNavigating(), processNavigateCommand(), and mrpt::nav::CWaypointsNavigator::waypoints_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

◆ 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
protected

◆ m_badNavAlarm_minDistTarget

double mrpt::nav::CAbstractNavigator::m_badNavAlarm_minDistTarget
protected

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 performNavigationStepNavigating(), and processNavigateCommand().

◆ m_counter_check_target_is_blocked

int mrpt::nav::CAbstractNavigator::m_counter_check_target_is_blocked {0}
private

Definition at line 278 of file CAbstractNavigator.h.

Referenced by performNavigationStepNavigating().

◆ m_curPoseVel

TRobotPoseVel mrpt::nav::CAbstractNavigator::m_curPoseVel
protected

◆ m_frame_tf

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

Optional, user-provided frame transformer.

Definition at line 356 of file CAbstractNavigator.h.

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

◆ m_last_curPoseVelUpdate_pose_frame_id

std::string mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_pose_frame_id
protected

Definition at line 377 of file CAbstractNavigator.h.

Referenced by updateCurrentPoseAndSpeeds().

◆ m_last_curPoseVelUpdate_robot_time

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

◆ m_lastNavigationState

TState mrpt::nav::CAbstractNavigator::m_lastNavigationState {IDLE}
private

Last internal state of navigator:

Definition at line 273 of file CAbstractNavigator.h.

Referenced by navigationStep(), and performNavigationStepNavigating().

◆ m_latestOdomPoses

mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestOdomPoses
protected

◆ m_latestPoses

mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestPoses
protected

◆ 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_nav_cs

std::recursive_mutex mrpt::nav::CAbstractNavigator::m_nav_cs
protected

◆ m_navErrorReason

TErrorReason mrpt::nav::CAbstractNavigator::m_navErrorReason
private

◆ m_navigationEndEventSent

bool mrpt::nav::CAbstractNavigator::m_navigationEndEventSent {false}
private

Will be false until the navigation end is sent, and it is reset with each new command.

Definition at line 277 of file CAbstractNavigator.h.

Referenced by onNavigateCommandReceived(), and performNavigationStepNavigating().

◆ m_navigationParams

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

◆ m_navigationState

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

◆ m_navProfiler

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

◆ m_pending_events

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

Events generated during navigationStep(), enqueued to be called at the end of the method execution to avoid user code to change the navigator state.

Definition at line 289 of file CAbstractNavigator.h.

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

◆ m_rethrow_exceptions

bool mrpt::nav::CAbstractNavigator::m_rethrow_exceptions {false}
private

◆ m_robot

CRobot2NavInterface& mrpt::nav::CAbstractNavigator::m_robot
protected

◆ m_timlog_delays

mrpt::system::CTimeLogger mrpt::nav::CAbstractNavigator::m_timlog_delays
protected

◆ params_abstract_navigator

TAbstractNavigatorParams mrpt::nav::CAbstractNavigator::params_abstract_navigator



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019