Main MRPT website > C++ reference for MRPT 1.9.9
List of all members | Classes | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Attributes
mrpt::nav::CWaypointsNavigator Class Referenceabstract
[mrpt-nav] » Reactive navigation classes

Detailed Description

This class extends CAbstractNavigator with the capability of following a list of waypoints.

By default, waypoints are followed one by one, but, if they are tagged with allow_skip=true and the derived navigator class supports it, the navigator may choose to skip some to make a smoother, safer and shorter navigation.

Waypoints have an optional target_heading field, which will be honored only for waypoints that are skipped, and if the underlying robot interface supports the pure-rotation methods.

Notes on navigation status and event dispatchment:

See also
Base class CAbstractNavigator, CWaypointsNavigator::navigateWaypoints(), and derived classes.

Definition at line 40 of file CWaypointsNavigator.h.

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

Inheritance diagram for mrpt::nav::CWaypointsNavigator:
Inheritance graph

Classes

struct  TNavigationParamsWaypoints
 The struct for configuring navigation requests to CWaypointsNavigator and derived classes. More...
 
struct  TWaypointsNavigatorParams
 

Public Member Functions

 CWaypointsNavigator (CRobot2NavInterface &robot_interface_impl)
 ctor More...
 
virtual ~CWaypointsNavigator ()
 dtor More...
 
virtual void navigationStep () override
 This method must be called periodically in order to effectively run the navigation. More...
 
virtual void cancel () override
 Cancel current navegation. More...
 
bool isRelativePointReachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const
 Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc. More...
 
virtual void loadConfigFile (const mrpt::utils::CConfigFileBase &c) override
 Loads all params from a file. More...
 
virtual void saveConfigFile (mrpt::utils::CConfigFileBase &c) const override
 Saves all current options to a config file. More...
 
const mrpt::utils::CTimeLogger & getDelaysTimeLogger () const
 Gives access to a const-ref to the internal time logger used to estimate delays. More...
 
Waypoint navigation control API
virtual void navigateWaypoints (const TWaypointSequence &nav_request)
 Waypoint navigation request. More...
 
virtual void getWaypointNavStatus (TWaypointStatusSequence &out_nav_status) const
 Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
 
TWaypointStatusSequence getWaypointNavStatus () const
 Get a copy of the control structure which describes the progress status of the waypoint navigation. More...
 

Public Attributes

TWaypointsNavigatorParams params_waypoints_navigator
 
TAbstractNavigatorParams params_abstract_navigator
 

Protected Member Functions

virtual bool impl_waypoint_is_reachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const =0
 Implements the way to waypoint is free function in children classes: true must be returned if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range, etc. More...
 
virtual void onStartNewNavigation () override
 Called whenever a new navigation has been started. More...
 
virtual void onNavigateCommandReceived () override
 Called after each call to CAbstractNavigator::navigate() More...
 
virtual bool checkHasReachedTarget (const double targetDist) const override
 Default implementation: check if target_dist is below the accepted distance. More...
 
virtual void waypoints_navigationStep ()
 The waypoints-specific part of navigationStep() More...
 
bool waypoints_isAligning () const
 
void dispatchPendingNavEvents ()
 
virtual void performNavigationStep ()=0
 To be implemented in derived classes. 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 processNavigateCommand (const TNavigationParams *params)
 Does the job of navigate(), except the call to onNavigateCommandReceived() 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(). 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 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

TWaypointStatusSequence m_waypoint_nav_status
 The latest waypoints navigation command and the up-to-date control status. More...
 
std::recursive_mutex m_nav_waypoints_cs
 
std::vector< 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
 Current internal state of navigator: More...
 
std::unique_ptr< TNavigationParams > m_navigationParams
 Current navigation parameters. More...
 
CRobot2NavInterface & m_robot
 The navigator-robot interface. More...
 
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
 
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::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 Attributes

bool m_was_aligning
 Whether the last timestep was "is_aligning" in a waypoint with heading. More...
 
bool m_is_aligning
 
mrpt::system::TTimeStamp m_last_alignment_cmd
 

Navigation control API

virtual void initialize ()=0
 Must be called before any other navigation command. More...
 
virtual void navigate (const TNavigationParams *params)
 Navigation request to a single target location. More...
 
virtual void resume ()
 Continues with suspended navigation. More...
 
virtual void suspend ()
 Suspend current navegation. More...
 
virtual void resetNavError ()
 Resets a NAV_ERROR state back to IDLE More...
 
TState getCurrentState () const
 Returns the current navigator state. More...
 
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...
 
enum  TState { IDLE = 0, NAVIGATING, SUSPENDED, NAV_ERROR }
 The different states for the navigation system. More...
 

Member Enumeration Documentation

◆ 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

◆ CWaypointsNavigator()

CWaypointsNavigator::CWaypointsNavigator ( CRobot2NavInterface &  robot_interface_impl)

ctor

Definition at line 46 of file CWaypointsNavigator.cpp.

◆ ~CWaypointsNavigator()

CWaypointsNavigator::~CWaypointsNavigator ( )
virtual

dtor

Definition at line 54 of file CWaypointsNavigator.cpp.

Member Function Documentation

◆ cancel()

void CWaypointsNavigator::cancel ( )
overridevirtual

Cancel current navegation.

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 105 of file CWaypointsNavigator.cpp.

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

Here is the call graph for this function:

◆ changeSpeeds()

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

Default: forward call to m_robot.changeSpeed().

Can be overriden.

Definition at line 414 of file CAbstractNavigator.cpp.

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

Referenced by mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and 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 420 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 671 of file CAbstractNavigator.cpp.

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

Here is the caller graph for this function:

◆ checkHasReachedTarget()

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

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

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

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 547 of file CWaypointsNavigator.cpp.

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

Here is the call graph for this function:

◆ dispatchPendingNavEvents()

void CAbstractNavigator::dispatchPendingNavEvents ( )
protectedinherited

Definition at line 251 of file CAbstractNavigator.cpp.

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

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

Here is the caller graph for this function:

◆ doEmergencyStop()

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

Stops the robot and set navigation state to error.

Definition at line 262 of file CAbstractNavigator.cpp.

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

◆ 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::utils::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 229 of file CAbstractNavigator.h.

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

◆ getFrameTF()

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

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

◆ getWaypointNavStatus() [1/2]

void CWaypointsNavigator::getWaypointNavStatus ( TWaypointStatusSequence &  out_nav_status) const
virtual

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

Definition at line 97 of file CWaypointsNavigator.cpp.

References m_waypoint_nav_status.

◆ getWaypointNavStatus() [2/2]

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

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

Definition at line 95 of file CWaypointsNavigator.h.

Referenced by waypoints_navigationStep().

Here is the caller graph for this function:

◆ impl_waypoint_is_reachable()

virtual bool mrpt::nav::CWaypointsNavigator::impl_waypoint_is_reachable ( const mrpt::math::TPoint2D &  wp_local_wrt_robot) const
protectedpure virtual

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

Implemented in mrpt::nav::CAbstractPTGBasedReactive.

Referenced by isRelativePointReachable(), and waypoints_navigationStep().

Here is the caller graph for this function:

◆ initialize()

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

Must be called before any other navigation command.

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

◆ isRelativePointReachable()

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

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

Definition at line 478 of file CWaypointsNavigator.cpp.

References impl_waypoint_is_reachable().

Here is the call graph for this function:

◆ loadConfigFile()

void CWaypointsNavigator::loadConfigFile ( const mrpt::utils::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.

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

Definition at line 484 of file CWaypointsNavigator.cpp.

References mrpt::nav::CAbstractNavigator::loadConfigFile(), mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::loadFromConfigFile(), MRPT_END, MRPT_START, and params_waypoints_navigator.

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

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

◆ navigate()

void CAbstractNavigator::navigate ( const TNavigationParams *  params)
virtualinherited

Navigation request to a single target location.

It starts a new navigation.

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

Reimplemented in mrpt::nav::CNavigatorManualSequence.

Definition at line 316 of file CAbstractNavigator.cpp.

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

Here is the call graph for this function:

◆ navigateWaypoints()

void CWaypointsNavigator::navigateWaypoints ( const TWaypointSequence &  nav_request)
virtual

Waypoint navigation request.

This immediately cancels any other previous on-going navigation.

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

Definition at line 67 of file CWaypointsNavigator.cpp.

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

Here is the call graph for this function:

◆ navigationStep()

void CWaypointsNavigator::navigationStep ( )
overridevirtual

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

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 448 of file CWaypointsNavigator.cpp.

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

Here is the call graph for this function:

◆ onNavigateCommandReceived()

void CWaypointsNavigator::onNavigateCommandReceived ( )
overrideprotectedvirtual

Called after each call to CAbstractNavigator::navigate()

Reimplemented from mrpt::nav::CAbstractNavigator.

Definition at line 55 of file CWaypointsNavigator.cpp.

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

Referenced by navigateWaypoints().

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

◆ onStartNewNavigation()

void CWaypointsNavigator::onStartNewNavigation ( )
overrideprotectedvirtual

Called whenever a new navigation has been started.

Can be used to reset state variables, etc.

Implements mrpt::nav::CAbstractNavigator.

Reimplemented in mrpt::nav::CAbstractPTGBasedReactive.

Definition at line 476 of file CWaypointsNavigator.cpp.

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

Here is the caller graph for this function:

◆ performNavigationStep()

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

To be implemented in derived classes.

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

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

Here is the caller graph for this function:

◆ performNavigationStepNavigating()

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

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

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

Definition at line 496 of file CAbstractNavigator.cpp.

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

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

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

Definition at line 283 of file CAbstractNavigator.cpp.

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

Referenced by mrpt::nav::CAbstractNavigator::navigate(), and 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 146 of file CAbstractNavigator.cpp.

References mrpt::nav::CAbstractNavigator::IDLE, mrpt::nav::CAbstractNavigator::m_nav_cs, mrpt::nav::CAbstractNavigator::m_navigationState, MRPT_LOG_DEBUG, and mrpt::nav::CAbstractNavigator::NAV_ERROR.

◆ resume()

void CAbstractNavigator::resume ( )
virtualinherited

Continues with suspended navigation.

See also
suspend

Definition at line 123 of file CAbstractNavigator.cpp.

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

◆ saveConfigFile()

void CWaypointsNavigator::saveConfigFile ( mrpt::utils::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.

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

Definition at line 494 of file CWaypointsNavigator.cpp.

References params_waypoints_navigator, mrpt::nav::CAbstractNavigator::saveConfigFile(), and mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::saveToConfigFile().

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

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

◆ setFrameTF()

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

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

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

◆ stop()

bool CAbstractNavigator::stop ( bool  isEmergencyStop)
protectedvirtualinherited

Default: forward call to m_robot.stop().

Can be overriden.

Definition at line 422 of file CAbstractNavigator.cpp.

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

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

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

◆ suspend()

void CAbstractNavigator::suspend ( )
virtualinherited

Suspend current navegation.

See also
resume

Definition at line 132 of file CAbstractNavigator.cpp.

References 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 ( )
protectedinherited

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(), mrpt::nav::CRobot2NavInterface::getCurrentPoseAndSpeeds(), mrpt::nav::CRobot2NavInterface::getNavigationTime(), mrpt::poses::CPoseInterpolatorBase< DIM >::insert(), mrpt::nav::CAbstractNavigator::m_curPoseVel, mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_pose_frame_id, mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time, mrpt::nav::CAbstractNavigator::m_latestOdomPoses, mrpt::nav::CAbstractNavigator::m_latestPoses, mrpt::nav::CAbstractNavigator::m_navigationState, mrpt::nav::CAbstractNavigator::m_robot, mrpt::nav::CAbstractNavigator::m_timlog_delays, MRPT_LOG_ERROR, MRPT_LOG_THROTTLE_DEBUG_FMT, mrpt::nav::CAbstractNavigator::NAV_ERROR, mrpt::math::TPose2D::phi, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose, mrpt::nav::CAbstractNavigator::TRobotPoseVel::pose_frame_id, PREVIOUS_POSES_MAX_AGE, mrpt::nav::CAbstractNavigator::TRobotPoseVel::rawOdometry, mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin(), mrpt::math::TTwist2D::rotate(), mrpt::poses::CPoseInterpolatorBase< DIM >::size(), mrpt::nav::CAbstractNavigator::stop(), mrpt::system::timeDifference(), mrpt::nav::CAbstractNavigator::TRobotPoseVel::timestamp, mrpt::nav::CAbstractNavigator::TRobotPoseVel::velGlobal, and mrpt::nav::CAbstractNavigator::TRobotPoseVel::velLocal.

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

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

◆ waypoints_isAligning()

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

Definition at line 168 of file CWaypointsNavigator.h.

References m_is_aligning.

◆ waypoints_navigationStep()

void CWaypointsNavigator::waypoints_navigationStep ( )
protectedvirtual

The waypoints-specific part of navigationStep()

Definition at line 115 of file CWaypointsNavigator.cpp.

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

Referenced by navigationStep().

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

Member Data Documentation

◆ m_badNavAlarm_lastMinDistTime

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

Definition at line 349 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 348 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 337 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 waypoints_navigationStep().

◆ m_frame_tf

mrpt::poses::FrameTransformer<2>* mrpt::nav::CAbstractNavigator::m_frame_tf
protectedinherited

Optional, user-provided frame transformer.

Note: We dont have ownership of the pointee object!

Definition at line 318 of file CAbstractNavigator.h.

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

◆ m_is_aligning

bool mrpt::nav::CWaypointsNavigator::m_is_aligning
private

Definition at line 173 of file CWaypointsNavigator.h.

Referenced by navigationStep(), waypoints_isAligning(), and waypoints_navigationStep().

◆ m_last_alignment_cmd

mrpt::system::TTimeStamp mrpt::nav::CWaypointsNavigator::m_last_alignment_cmd
private

Definition at line 174 of file CWaypointsNavigator.h.

Referenced by waypoints_navigationStep().

◆ m_last_curPoseVelUpdate_pose_frame_id

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

Definition at line 339 of file CAbstractNavigator.h.

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

◆ m_last_curPoseVelUpdate_robot_time

double mrpt::nav::CAbstractNavigator::m_last_curPoseVelUpdate_robot_time
protectedinherited

Definition at line 338 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 341 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 341 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_nav_cs

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

mutex for all navigation methods

Definition at line 321 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::cancel(), mrpt::nav::CAbstractPTGBasedReactive::enableLogFile(), mrpt::nav::CAbstractPTGBasedReactive::initialize(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractNavigator::onNavigateCommandReceived(), mrpt::nav::CAbstractPTGBasedReactive::preDestructor(), mrpt::nav::CAbstractNavigator::processNavigateCommand(), mrpt::nav::CAbstractNavigator::resetNavError(), mrpt::nav::CAbstractNavigator::resume(), mrpt::nav::CAbstractPTGBasedReactive::setHolonomicMethod(), and mrpt::nav::CAbstractNavigator::suspend().

◆ m_nav_waypoints_cs

std::recursive_mutex mrpt::nav::CWaypointsNavigator::m_nav_waypoints_cs
protected

Definition at line 149 of file CWaypointsNavigator.h.

Referenced by cancel(), navigateWaypoints(), onNavigateCommandReceived(), and waypoints_navigationStep().

◆ m_navigationParams

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

Current navigation parameters.

Definition at line 311 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::calc_move_candidate_scores(), 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
protectedinherited

Current internal state of navigator:

Definition at line 309 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::cancel(), mrpt::nav::CAbstractNavigator::doEmergencyStop(), mrpt::nav::CAbstractNavigator::getCurrentState(), navigationStep(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::nav::CAbstractNavigator::processNavigateCommand(), mrpt::nav::CAbstractNavigator::resetNavError(), mrpt::nav::CAbstractNavigator::resume(), mrpt::nav::CAbstractNavigator::suspend(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ m_pending_events

std::vector<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 250 of file CAbstractNavigator.h.

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

◆ m_robot

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

The navigator-robot interface.

Definition at line 314 of file CAbstractNavigator.h.

Referenced by mrpt::nav::CAbstractNavigator::changeSpeeds(), mrpt::nav::CAbstractNavigator::changeSpeedsNOP(), mrpt::nav::CReactiveNavigationSystem::implementSenseObstacles(), mrpt::nav::CReactiveNavigationSystem3D::implementSenseObstacles(), mrpt::nav::CNavigatorManualSequence::initialize(), mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::navigationStep(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), mrpt::nav::CAbstractNavigator::stop(), mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds(), and waypoints_navigationStep().

◆ m_timlog_delays

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

Time logger to collect delay-related stats.

Definition at line 344 of file CAbstractNavigator.h.

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

◆ m_was_aligning

bool mrpt::nav::CWaypointsNavigator::m_was_aligning
private

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

Definition at line 172 of file CWaypointsNavigator.h.

Referenced by onNavigateCommandReceived(), and waypoints_navigationStep().

◆ m_waypoint_nav_status

TWaypointStatusSequence mrpt::nav::CWaypointsNavigator::m_waypoint_nav_status
protected

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

Definition at line 148 of file CWaypointsNavigator.h.

Referenced by cancel(), checkHasReachedTarget(), getWaypointNavStatus(), navigateWaypoints(), onNavigateCommandReceived(), and waypoints_navigationStep().

◆ params_abstract_navigator

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

Definition at line 225 of file CAbstractNavigator.h.

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

◆ params_waypoints_navigator

TWaypointsNavigatorParams mrpt::nav::CWaypointsNavigator::params_waypoints_navigator

Definition at line 138 of file CWaypointsNavigator.h.

Referenced by loadConfigFile(), saveConfigFile(), and waypoints_navigationStep().



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019