Detailed Description

"Fake navigator" for tests: it just sends out a pre-programmed sequence of commands to the robot.

For a short discussion of the API, see CNavigatorVirtualBase

Definition at line 21 of file CNavigatorManualSequence.h.

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

Inheritance diagram for mrpt::nav::CNavigatorManualSequence:

Classes
struct	TVelCmd

Public Member Functions
	CNavigatorManualSequence (CRobot2NavInterface &react_iterf_impl)

	~CNavigatorManualSequence () override

void	navigationStep () override
	Overriden in this class to ignore the cancel/pause/... More...

const mrpt::system::CTimeLogger &	getDelaysTimeLogger () const
	Gives access to a const-ref to the internal time logger used to estimate delays. More...

Initialization API
void	loadConfigFile (const mrpt::config::CConfigFileBase &c) override
	Loads all params from a file. More...

void	saveConfigFile (mrpt::config::CConfigFileBase &c) const override
	Saves all current options to a config file. More...

void	initialize () override
	Must be called for loading collision grids, etc. 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
std::map< double, TVelCmd >	programmed_orders
	map [time_in_secs_since_beginning] -> orders. More...

TAbstractNavigatorParams	params_abstract_navigator

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

Protected Member Functions
void	onStartNewNavigation () override
	Called whenever a new navigation has been started. More...

void	dispatchPendingNavEvents ()

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< TNavigationParams >	m_navigationParams
	Current navigation parameters. More...

CRobot2NavInterface &	m_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	navigate (const TNavigationParams *params) override
	Navigation request to a single target location. More...

void	performNavigationStep () override
	To be implemented in derived classes. More...

Navigation control API
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 TErrorReason &	getErrorReason () const
	In case of state=NAV_ERROR, this returns the reason for the error. More...

void	setFrameTF (const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &frame_tf)
	Sets a user-provided frame transformer object; used only if providing targets in a frame ID different than the one in which robot odometry is given (both IDs default to `"map"`). More...

std::weak_ptr< mrpt::poses::FrameTransformer< 2 > >	getFrameTF () const
	Get the current frame tf object (defaults to nullptr) More...

void	enableRethrowNavExceptions (const bool enable)
	By default, error exceptions on navigationStep() will dump an error message to the output logger interface. More...

bool	isRethrowNavExceptionsEnabled () const

enum	TState { IDLE = 0, NAVIGATING, SUSPENDED, NAV_ERROR }
	The different states for the navigation system. More...

enum	TErrorCode { ERR_NONE = 0, ERR_EMERGENCY_STOP, ERR_CANNOT_REACH_TARGET, ERR_OTHER }
	Explains the reason for the navigation error. More...

Logging methods
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

enum mrpt::nav::CAbstractNavigator::TErrorCode

inherited

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

enum mrpt::nav::CAbstractNavigator::TState

inherited

The different states for the navigation system.

Enumerator
IDLE
NAVIGATING
SUSPENDED
NAV_ERROR

Definition at line 172 of file CAbstractNavigator.h.

Constructor & Destructor Documentation

◆ CNavigatorManualSequence()

CNavigatorManualSequence::CNavigatorManualSequence ( CRobot2NavInterface & react_iterf_impl )

Definition at line 20 of file CNavigatorManualSequence.cpp.

◆ ~CNavigatorManualSequence()

CNavigatorManualSequence::~CNavigatorManualSequence ( )

overridedefault

Member Function Documentation

◆ cancel()

void CAbstractNavigator::cancel ( )

virtualinherited

Cancel current navegation.

Reimplemented in mrpt::nav::CWaypointsNavigator.

Definition at line 100 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::IDLE, mrpt::lockHelper(), mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_DEBUG, and mrpt::nav::CAbstractNavigator::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 )

protectedvirtualinherited

Default: forward call to m_robot.changeSpeed().

Can be overriden.

Definition at line 421 of file CAbstractNavigator.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ changeSpeedsNOP()

bool CAbstractNavigator::changeSpeedsNOP ( )

protectedvirtualinherited

Default: forward call to m_robot.changeSpeedsNOP().

Can be overriden.

Definition at line 427 of file CAbstractNavigator.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkCollisionWithLatestObstacles()

bool CAbstractNavigator::checkCollisionWithLatestObstacles ( const mrpt::math::TPose2D & relative_robot_pose ) const

protectedvirtualinherited

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

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating().

Here is the caller graph for this function:

◆ checkHasReachedTarget()

bool CAbstractNavigator::checkHasReachedTarget ( const double targetDist ) const

protectedvirtualinherited

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

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating().

Here is the caller graph for this function:

◆ dispatchPendingNavEvents()

void CAbstractNavigator::dispatchPendingNavEvents ( )

protectedinherited

Definition at line 246 of file CAbstractNavigator.cpp.

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

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

Here is the caller graph for this function:

◆ doEmergencyStop()

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

protectedvirtualinherited

Stops the robot and set navigation state to error.

Definition at line 257 of file CAbstractNavigator.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const

inherited

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

See also: writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ enableRethrowNavExceptions()

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

inlineinherited

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

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

Definition at line 228 of file CAbstractNavigator.h.

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

◆ getCurrentState()

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

inlineinherited

Returns the current navigator state.

Definition at line 181 of file CAbstractNavigator.h.

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

◆ getDelaysTimeLogger()

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

inlineinherited

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

See also: getTimeLogger() in derived classes

Definition at line 262 of file CAbstractNavigator.h.

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

◆ getErrorReason()

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

inlineinherited

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

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

Definition at line 204 of file CAbstractNavigator.h.

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

◆ getFrameTF()

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

inlineinherited

Get the current frame tf object (defaults to nullptr)

See also: setFrameTF

Definition at line 218 of file CAbstractNavigator.h.

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

◆ 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

See also: setMinLoggingLevel

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

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

Referenced by mrpt::apps::RawlogGrabberApp::dump_verbose_info(), mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ initialize()

void CNavigatorManualSequence::initialize ( )

overridevirtual

Must be called for loading collision grids, etc.

before invoking any navigation command

Implements mrpt::nav::CAbstractNavigator.

Definition at line 78 of file CNavigatorManualSequence.cpp.

References ASSERT_, mrpt::nav::CAbstractNavigator::m_robot, programmed_orders, and mrpt::nav::CRobot2NavInterface::resetNavigationTimer().

Here is the call graph for this function:

◆ isLoggingLevelVisible()

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

inlineinherited

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

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

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

Here is the caller graph for this function:

◆ isRethrowNavExceptionsEnabled()

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

inlineinherited

Definition at line 232 of file CAbstractNavigator.h.

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

◆ loadConfigFile()

void CNavigatorManualSequence::loadConfigFile ( const mrpt::config::CConfigFileBase & c )

overridevirtual

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

Definition at line 33 of file CNavigatorManualSequence.cpp.

References ASSERTMSG_, mrpt::nav::CNavigatorManualSequence::TVelCmd::cmd_vel, mrpt::kinematics::CVehicleVelCmd_Holo::Create(), mrpt::config::CConfigFileBase::getAllKeys(), programmed_orders, mrpt::config::CConfigFileBase::read_string(), THROW_EXCEPTION, and mrpt::system::tokenize().

Here is the call 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

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

Definition at line 91 of file COutputLogger.cpp.

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::hmtslam::CHMTSLAM::thread_3D_viewer(), mrpt::hmtslam::CHMTSLAM::thread_LSLAM(), mrpt::hmtslam::CHMTSLAM::thread_TBI(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().

Here is the caller graph for this function:

◆ 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 mrpt::nav::CNavigatorManualSequence::navigate ( const TNavigationParams * params )

inlineoverrideprivatevirtual

Navigation request to a single target location.

It starts a new navigation.

Parameters

[in] params Pointer 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 from mrpt::nav::CAbstractNavigator.

Definition at line 56 of file CNavigatorManualSequence.h.

◆ navigationStep()

void CNavigatorManualSequence::navigationStep ( )

overridevirtual

Overriden in this class to ignore the cancel/pause/...

commands

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 85 of file CNavigatorManualSequence.cpp.

References mrpt::nav::CAbstractNavigator::changeSpeeds(), mrpt::nav::CNavigatorManualSequence::TVelCmd::cmd_vel, mrpt::nav::CRobot2NavInterface::getNavigationTime(), mrpt::system::COutputLogger::logFmt(), mrpt::system::LVL_DEBUG, mrpt::system::LVL_ERROR, mrpt::nav::CAbstractNavigator::m_robot, programmed_orders, and mrpt::nav::CAbstractNavigator::stop().

Here is the call graph for this function:

◆ onNavigateCommandReceived()

void CAbstractNavigator::onNavigateCommandReceived ( )

protectedvirtualinherited

Called after each call to CAbstractNavigator::navigate()

Reimplemented in mrpt::nav::CWaypointsNavigator.

Definition at line 277 of file CAbstractNavigator.cpp.

References mrpt::lockHelper(), mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navigationEndEventSent, and mrpt::nav::CAbstractNavigator::m_navigationParams.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onStartNewNavigation()

void mrpt::nav::CNavigatorManualSequence::onStartNewNavigation ( )

inlineoverrideprotectedvirtual

Called whenever a new navigation has been started.

Can be used to reset state variables, etc.

Implements mrpt::nav::CAbstractNavigator.

Definition at line 52 of file CNavigatorManualSequence.h.

◆ performNavigationStep()

void mrpt::nav::CNavigatorManualSequence::performNavigationStep ( )

inlineoverrideprivatevirtual

To be implemented in derived classes.

Implements mrpt::nav::CAbstractNavigator.

Definition at line 57 of file CNavigatorManualSequence.h.

◆ performNavigationStepNavigating()

void CAbstractNavigator::performNavigationStepNavigating ( bool call_virtual_nav_method = true )

protectedvirtualinherited

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

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

Definition at line 499 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ processNavigateCommand()

void CAbstractNavigator::processNavigateCommand ( const TNavigationParams * params )

protectedvirtualinherited

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

Definition at line 285 of file CAbstractNavigator.cpp.

Referenced by mrpt::nav::CAbstractNavigator::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 ( )

virtualinherited

Resets a NAV_ERROR state back to IDLE

Definition at line 133 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

◆ resume()

void CAbstractNavigator::resume ( )

virtualinherited

Continues with suspended navigation.

See also: suspend

Definition at line 110 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

◆ saveConfigFile()

void CNavigatorManualSequence::saveConfigFile ( mrpt::config::CConfigFileBase & c ) const

overridevirtual

Saves all current options to a config file.

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

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 28 of file CNavigatorManualSequence.cpp.

◆ setFrameTF()

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

inherited

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

Definition at line 145 of file CAbstractNavigator.cpp.

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

◆ setLoggerName()

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

inherited

Set the name of the COutputLogger instance.

See also: getLoggerName

Definition at line 138 of file COutputLogger.cpp.

Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base(), mrpt::apps::ICP_SLAM_App_Live::ICP_SLAM_App_Live(), mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog(), mrpt::graphslam::CWindowManager::initCWindowManager(), mrpt::apps::MonteCarloLocalization_Base::MonteCarloLocalization_Base(), mrpt::apps::RBPF_SLAM_App_Base::RBPF_SLAM_App_Base(), and mrpt::apps::RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog().

Here is the caller graph for this function:

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel level )

inherited

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

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

Definition at line 144 of file COutputLogger.cpp.

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

Here is the caller graph for this function:

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel level )

inherited

alias of setMinLoggingLevel()

Definition at line 149 of file COutputLogger.cpp.

Referenced by mrpt::nav::CAbstractNavigator::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 )

protectedvirtualinherited

Default: forward call to m_robot.stop().

Can be overriden.

Definition at line 429 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ suspend()

void CAbstractNavigator::suspend ( )

virtualinherited

Suspend current navegation.

See also: resume

Definition at line 119 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

◆ updateCurrentPoseAndSpeeds()

void CAbstractNavigator::updateCurrentPoseAndSpeeds ( )

protectedvirtualinherited

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

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

Definition at line 328 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

protectedinherited

Definition at line 388 of file CAbstractNavigator.h.

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

◆ m_badNavAlarm_minDistTarget

double mrpt::nav::CAbstractNavigator::m_badNavAlarm_minDistTarget

protectedinherited

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

Definition at line 387 of file CAbstractNavigator.h.

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

◆ m_curPoseVel

TRobotPoseVel mrpt::nav::CAbstractNavigator::m_curPoseVel

protectedinherited

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

Definition at line 375 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CReactiveNavigationSystem::implementSenseObstacles(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::nav::CAbstractNavigator::processNavigateCommand(), mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(), mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

◆ m_frame_tf

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

protectedinherited

Optional, user-provided frame transformer.

Definition at line 356 of file CAbstractNavigator.h.

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

◆ m_last_curPoseVelUpdate_pose_frame_id

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

protectedinherited

Definition at line 377 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ m_last_curPoseVelUpdate_robot_time

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

protectedinherited

Definition at line 376 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::onStartNewNavigation(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ m_latestOdomPoses

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

protectedinherited

Definition at line 379 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ m_latestPoses

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

protectedinherited

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

Definition at line 379 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ 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

protectedinherited

mutex for all navigation methods

Definition at line 359 of file CAbstractNavigator.h.

◆ m_navigationParams

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

protectedinherited

Current navigation parameters.

Definition at line 350 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CWaypointsNavigator::checkHasReachedTarget(), mrpt::nav::CAbstractNavigator::onNavigateCommandReceived(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), and mrpt::nav::CAbstractNavigator::processNavigateCommand().

◆ m_navigationState

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

protectedinherited

Current internal state of navigator:

Definition at line 348 of file CAbstractNavigator.h.

◆ m_navProfiler

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

inherited

Initial value:

{false,

"mrpt::nav::CAbstractNavigator"}

Publicly available time profiling object.

Default: disabled

Definition at line 268 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate(), mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), mrpt::nav::CAbstractNavigator::loadConfigFile(), mrpt::nav::CWaypointsNavigator::navigationStep(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CReactiveNavigationSystem::STEP3_WSpaceToTPSpace(), mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ m_pending_events

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

protectedinherited

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 mrpt::nav::CAbstractNavigator::dispatchPendingNavEvents(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

◆ m_robot

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

protectedinherited

The navigator-robot interface.

Definition at line 353 of file CAbstractNavigator.h.

◆ m_timlog_delays

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

protectedinherited

Initial value:

{

true, "CAbstractNavigator::m_timlog_delays"}

Time logger to collect delay-related stats.

Definition at line 382 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::getDelaysTimeLogger(), mrpt::nav::CReactiveNavigationSystem::implementSenseObstacles(), mrpt::nav::CReactiveNavigationSystem3D::implementSenseObstacles(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(), and mrpt::nav::CWaypointsNavigator::waypoints_navigationStep().

◆ params_abstract_navigator

TAbstractNavigatorParams mrpt::nav::CAbstractNavigator::params_abstract_navigator

inherited

Definition at line 258 of file CAbstractNavigator.h.

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

◆ programmed_orders

std::map<double, TVelCmd> mrpt::nav::CNavigatorManualSequence::programmed_orders

map [time_in_secs_since_beginning] -> orders.

Definition at line 49 of file CNavigatorManualSequence.h.

Referenced by initialize(), loadConfigFile(), and navigationStep().

Detailed Description

Classes

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Member Functions

Navigation control API

Logging methods

Member Enumeration Documentation

◆ TErrorCode

◆ TState

Constructor & Destructor Documentation

◆ CNavigatorManualSequence()

◆ ~CNavigatorManualSequence()

Member Function Documentation

◆ cancel()

◆ changeSpeeds()

◆ changeSpeedsNOP()

◆ checkCollisionWithLatestObstacles()

◆ checkHasReachedTarget()

◆ dispatchPendingNavEvents()

◆ doEmergencyStop()

◆ dumpLogToConsole()

◆ enableRethrowNavExceptions()

◆ getCurrentState()

◆ getDelaysTimeLogger()

◆ getErrorReason()

◆ getFrameTF()

◆ getLogAsString() [1/2]

◆ getLogAsString() [2/2]

◆ getLoggerLastMsg() [1/2]

◆ getLoggerLastMsg() [2/2]

◆ getLoggerName()

◆ getMinLoggingLevel()

◆ initialize()

◆ isLoggingLevelVisible()

◆ isRethrowNavExceptionsEnabled()

◆ loadConfigFile()

◆ logCond()

◆ logDeregisterCallback()

◆ logFmt()

◆ loggerReset()

◆ logging_levels_to_colors()

◆ logging_levels_to_names()

◆ logRegisterCallback()

◆ logStr()

◆ navigate()

◆ navigationStep()

◆ onNavigateCommandReceived()

◆ onStartNewNavigation()

◆ performNavigationStep()

◆ performNavigationStepNavigating()

◆ processNavigateCommand()

◆ resetNavError()

◆ resume()

◆ saveConfigFile()

◆ setFrameTF()

◆ setLoggerName()

◆ setMinLoggingLevel()

◆ setVerbosityLevel()

◆ stop()

◆ suspend()

◆ updateCurrentPoseAndSpeeds()

◆ writeLogToFile()

Member Data Documentation

◆ logging_enable_console_output

◆ logging_enable_keep_record

◆ m_badNavAlarm_lastMinDistTime

◆ m_badNavAlarm_minDistTarget

◆ m_curPoseVel

◆ m_frame_tf

◆ m_last_curPoseVelUpdate_pose_frame_id

◆ m_last_curPoseVelUpdate_robot_time

◆ m_latestOdomPoses

◆ m_latestPoses

◆ m_min_verbosity_level

◆ m_nav_cs

◆ m_navigationParams