Main MRPT website > C++ reference for MRPT 1.5.6
List of all members | Classes | 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

dot_inline_dotgraph_1.png
See Also
CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all children classes

Definition at line 51 of file CAbstractNavigator.h.

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

Inheritance diagram for mrpt::nav::CAbstractNavigator:
Inheritance graph

Classes

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

Public Member Functions

 CAbstractNavigator (CRobot2NavInterface &robot_interface_impl)
 ctor More...
 
virtual ~CAbstractNavigator ()
 dtor More...
 
const mrpt::utils::CTimeLoggergetDelaysTimeLogger () const
 Gives access to a const-ref to the internal time logger used to estimate delays. More...
 

Public Attributes

TAbstractNavigatorParams params_abstract_navigator
 

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...
 
void processNavigateCommand (const TNavigationParams *params)
 Does the job of navigate(), except the call to onNavigateCommandReceived() More...
 
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...
 
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...
 
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::vector< TPendingEventm_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
 Current internal state of navigator: More...
 
TNavigationParamsm_navigationParams
 Current navigation parameters. More...
 
CRobot2NavInterfacem_robot
 The navigator-robot interface. More...
 
mrpt::poses::FrameTransformer< 2 > * m_frame_tf
 Optional, user-provided frame transformer. More...
 
mrpt::synch::CCriticalSectionRecursive 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
 
std::string m_last_curPoseVelUpdate_pose_frame_id
 
mrpt::poses::CPose2DInterpolator m_latestPoses
 
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
 Latest robot poses (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
 

Private Member Functions

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

Private Attributes

TState m_lastNavigationState
 Last internal state of navigator: More...
 
bool m_navigationEndEventSent
 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
 

Navigation control API

enum  TState { IDLE =0, NAVIGATING, SUSPENDED, NAV_ERROR }
 The different states for the navigation system. More...
 
virtual void loadConfigFile (const mrpt::utils::CConfigFileBase &c)
 Loads all params from a file. More...
 
virtual void saveConfigFile (mrpt::utils::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...
 
void setFrameTF (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...
 
const
mrpt::poses::FrameTransformer< 2 > * 
getFrameTF () const
 Get the current frame tf object (defaults to nullptr) More...
 

Member Enumeration Documentation

The different states for the navigation system.

Enumerator
IDLE 
NAVIGATING 
SUSPENDED 
NAV_ERROR 

Definition at line 119 of file CAbstractNavigator.h.

Constructor & Destructor Documentation

CAbstractNavigator::CAbstractNavigator ( CRobot2NavInterface robot_interface_impl)
CAbstractNavigator::~CAbstractNavigator ( )
virtual

dtor

Definition at line 107 of file CAbstractNavigator.cpp.

References mrpt::mrpt::utils::delete_safe(), and m_navigationParams.

Member Function Documentation

void CAbstractNavigator::cancel ( )
virtual

Cancel current navegation.

Reimplemented in mrpt::nav::CWaypointsNavigator.

Definition at line 115 of file CAbstractNavigator.cpp.

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

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

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

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

Definition at line 402 of file CAbstractNavigator.cpp.

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

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

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

Referenced by performNavigationStepNavigating().

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

Referenced by performNavigationStepNavigating().

void CAbstractNavigator::dispatchPendingNavEvents ( )
protected
void CAbstractNavigator::doEmergencyStop ( const std::string msg)
protected

Stops the robot and set navigation state to error.

Definition at line 277 of file CAbstractNavigator.cpp.

References m_navigationState, MRPT_LOG_ERROR, NAV_ERROR, and stop().

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

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

Returns the current navigator state.

Definition at line 127 of file CAbstractNavigator.h.

const mrpt::utils::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 157 of file CAbstractNavigator.h.

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

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.

void CAbstractNavigator::internal_onStartNewNavigation ( )
private

Called before starting a new navigation.

Internally, it calls to child-implemented onStartNewNavigation()

Definition at line 443 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().

void CAbstractNavigator::loadConfigFile ( const mrpt::utils::CConfigFileBase c)
virtual
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 327 of file CAbstractNavigator.cpp.

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

void CAbstractNavigator::navigationStep ( )
virtual
void CAbstractNavigator::onNavigateCommandReceived ( )
protectedvirtual
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().

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

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

References mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::alarm_seems_not_approaching_target_timeout, 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(), mrpt::nav::CAbstractNavigator::TPendingEvent::event_cannot_get_closer_target, mrpt::nav::CAbstractNavigator::TPendingEvent::event_noargs, mrpt::mrpt::format(), mrpt::nav::CAbstractNavigator::TNavigationParams::getAsText(), mrpt::system::getCurrentTime(), mrpt::nav::CAbstractNavigator::TAbstractNavigatorParams::hysteresis_check_target_is_blocked, IDLE, internal_onStartNewNavigation(), m_badNavAlarm_lastMinDistTime, m_badNavAlarm_minDistTarget, m_counter_check_target_is_blocked, m_curPoseVel, m_lastNavigationState, m_latestPoses, m_navigationEndEventSent, m_navigationParams, m_navigationState, m_pending_events, 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::nav::CAbstractNavigator::TNavigationParams::target, mrpt::nav::CAbstractNavigator::TargetInfo::target_coords, mrpt::nav::CAbstractNavigator::TargetInfo::targetIsIntermediaryWaypoint, mrpt::system::timeDifference(), updateCurrentPoseAndSpeeds(), mrpt::math::TPose2D::x, and mrpt::math::TPose2D::y.

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

void CAbstractNavigator::processNavigateCommand ( const TNavigationParams params)
protected
void CAbstractNavigator::resetNavError ( )
virtual

Resets a NAV_ERROR state back to IDLE

Definition at line 154 of file CAbstractNavigator.cpp.

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

void CAbstractNavigator::resume ( )
virtual

Continues with suspended navigation.

See Also
suspend

Definition at line 127 of file CAbstractNavigator.cpp.

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

void CAbstractNavigator::saveConfigFile ( mrpt::utils::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::CReactiveNavigationSystem3D, mrpt::nav::CWaypointsNavigator, mrpt::nav::CReactiveNavigationSystem, and mrpt::nav::CNavigatorManualSequence.

Definition at line 185 of file CAbstractNavigator.cpp.

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

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

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

Ownership of the pointee object remains belonging to the user, which is responsible of deleting it and ensuring its a valid pointer during the lifetime of this navigator object.

Todo:
[MRPT 2.0: Make this a weak_ptr]

Definition at line 163 of file CAbstractNavigator.cpp.

References m_frame_tf.

bool CAbstractNavigator::stop ( bool  isEmergencyStop)
protectedvirtual
void CAbstractNavigator::suspend ( )
virtual

Suspend current navegation.

See Also
resume

Definition at line 140 of file CAbstractNavigator.cpp.

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

void CAbstractNavigator::updateCurrentPoseAndSpeeds ( )
protected

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

References mrpt::poses::CPoseInterpolatorBase< DIM >::begin(), mrpt::poses::CPoseInterpolatorBase< DIM >::clear(), mrpt::poses::CPoseInterpolatorBase< DIM >::erase(), 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_navigationState, m_robot, m_timlog_delays, 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().

Member Data Documentation

mrpt::system::TTimeStamp mrpt::nav::CAbstractNavigator::m_badNavAlarm_lastMinDistTime
protected
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 259 of file CAbstractNavigator.h.

Referenced by performNavigationStepNavigating(), and processNavigateCommand().

int mrpt::nav::CAbstractNavigator::m_counter_check_target_is_blocked
private

Definition at line 162 of file CAbstractNavigator.h.

Referenced by performNavigationStepNavigating().

TRobotPoseVel mrpt::nav::CAbstractNavigator::m_curPoseVel
protected
mrpt::poses::FrameTransformer<2>* mrpt::nav::CAbstractNavigator::m_frame_tf
protected

Optional, user-provided frame transformer.

Note: We dont have ownership of the pointee object!

Definition at line 237 of file CAbstractNavigator.h.

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

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

Definition at line 253 of file CAbstractNavigator.h.

Referenced by updateCurrentPoseAndSpeeds().

double mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time
protected
TState mrpt::nav::CAbstractNavigator::m_lastNavigationState
private

Last internal state of navigator:

Definition at line 160 of file CAbstractNavigator.h.

Referenced by navigationStep(), and performNavigationStepNavigating().

mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestOdomPoses
protected
mrpt::poses::CPose2DInterpolator mrpt::nav::CAbstractNavigator::m_latestPoses
protected
mrpt::synch::CCriticalSectionRecursive mrpt::nav::CAbstractNavigator::m_nav_cs
protected
bool mrpt::nav::CAbstractNavigator::m_navigationEndEventSent
private

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

Definition at line 161 of file CAbstractNavigator.h.

Referenced by onNavigateCommandReceived(), and performNavigationStepNavigating().

TNavigationParams* mrpt::nav::CAbstractNavigator::m_navigationParams
protected
TState mrpt::nav::CAbstractNavigator::m_navigationState
protected
std::vector<TPendingEvent> 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 187 of file CAbstractNavigator.h.

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

CRobot2NavInterface& mrpt::nav::CAbstractNavigator::m_robot
protected
mrpt::utils::CTimeLogger mrpt::nav::CAbstractNavigator::m_timlog_delays
protected
TAbstractNavigatorParams mrpt::nav::CAbstractNavigator::params_abstract_navigator



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018