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:
IDLE
status between a waypoint is reached and a new navigation command is issued towards the next waypoint.sendNavigationEndEvent()
will be called only when the last waypoint is reached.sendWaypointReachedEvent()
instead.Definition at line 33 of file CWaypointsNavigator.h.
#include <mrpt/nav/reactive/CWaypointsNavigator.h>
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 () MRPT_OVERRIDE |
This method must be called periodically in order to effectively run the navigation. More... | |
virtual void | cancel () MRPT_OVERRIDE |
Cancel current navegation. More... | |
bool | isRelativePointReachable (const mrpt::math::TPoint2D &wp_local_wrt_robot) const |
Returns true if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range for the existing PTGs, etc. More... | |
virtual void | loadConfigFile (const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE |
Loads all params from a file. More... | |
virtual void | saveConfigFile (mrpt::utils::CConfigFileBase &c) const MRPT_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 () MRPT_OVERRIDE |
Called whenever a new navigation has been started. More... | |
virtual bool | checkHasReachedTarget (const double targetDist) const MRPT_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 | 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 | 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... | |
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... | |
|
inherited |
The different states for the navigation system.
Enumerator | |
---|---|
IDLE | |
NAVIGATING | |
SUSPENDED | |
NAV_ERROR |
Definition at line 120 of file CAbstractNavigator.h.
mrpt::nav::CWaypointsNavigator::CWaypointsNavigator | ( | CRobot2NavInterface & | robot_interface_impl | ) |
ctor
|
virtual |
dtor
|
virtual |
Cancel current navegation.
Reimplemented from mrpt::nav::CAbstractNavigator.
|
protectedvirtualinherited |
Default: forward call to m_robot.changeSpeed(). Can be overriden.
|
protectedvirtualinherited |
Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden.
|
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.
|
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 from mrpt::nav::CAbstractNavigator.
|
protectedinherited |
|
protectedinherited |
Stops the robot and set navigation state to error.
|
inlineinherited |
Returns the current navigator state.
Definition at line 128 of file CAbstractNavigator.h.
|
inlineinherited |
Gives access to a const-ref to the internal time logger used to estimate delays.
Definition at line 158 of file CAbstractNavigator.h.
|
inlineinherited |
Get the current frame tf object (defaults to nullptr)
Definition at line 139 of file CAbstractNavigator.h.
|
virtual |
Get a copy of the control structure which describes the progress status of the waypoint navigation.
|
inline |
Get a copy of the control structure which describes the progress status of the waypoint navigation.
Definition at line 69 of file CWaypointsNavigator.h.
|
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.
|
pure virtualinherited |
Must be called before any other navigation command.
Implemented in mrpt::nav::CAbstractPTGBasedReactive, and mrpt::nav::CNavigatorManualSequence.
bool mrpt::nav::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.
|
virtual |
Loads all params from a file.
To be called before initialize(). Each derived class MUST load its own parameters, and then call ITS PARENT'S overriden method to ensure all params are loaded.
Reimplemented from mrpt::nav::CAbstractNavigator.
Reimplemented in mrpt::nav::CAbstractPTGBasedReactive, mrpt::nav::CReactiveNavigationSystem3D, and mrpt::nav::CReactiveNavigationSystem.
|
virtualinherited |
Navigation request to a single target location.
It starts a new navigation.
[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.) |
Reimplemented in mrpt::nav::CNavigatorManualSequence.
|
virtual |
Waypoint navigation request.
This immediately cancels any other previous on-going navigation.
|
virtual |
This method must be called periodically in order to effectively run the navigation.
Reimplemented from mrpt::nav::CAbstractNavigator.
|
protectedvirtual |
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.
|
protectedpure virtualinherited |
To be implemented in derived classes.
Implemented in mrpt::nav::CAbstractPTGBasedReactive, and mrpt::nav::CNavigatorManualSequence.
|
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()
|
virtualinherited |
Resets a NAV_ERROR
state back to IDLE
|
virtualinherited |
Continues with suspended navigation.
|
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 from mrpt::nav::CAbstractNavigator.
Reimplemented in mrpt::nav::CAbstractPTGBasedReactive, mrpt::nav::CReactiveNavigationSystem3D, and mrpt::nav::CReactiveNavigationSystem.
|
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.
|
protectedvirtualinherited |
Default: forward call to m_robot.stop(). Can be overriden.
|
virtualinherited |
Suspend current navegation.
|
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.
|
inlineprotected |
Definition at line 113 of file CWaypointsNavigator.h.
|
protectedvirtual |
The waypoints-specific part of navigationStep()
|
protectedinherited |
Definition at line 255 of file CAbstractNavigator.h.
|
protectedinherited |
For sending an alarm (error event) when it seems that we are not approaching toward the target in a while...
Definition at line 254 of file CAbstractNavigator.h.
|
protectedinherited |
Current robot pose (updated in CAbstractNavigator::navigationStep() )
Definition at line 246 of file CAbstractNavigator.h.
|
protectedinherited |
Optional, user-provided frame transformer.
Note: We dont have ownership of the pointee object!
Definition at line 232 of file CAbstractNavigator.h.
|
private |
Definition at line 117 of file CWaypointsNavigator.h.
|
private |
Definition at line 118 of file CWaypointsNavigator.h.
|
protectedinherited |
Definition at line 248 of file CAbstractNavigator.h.
|
protectedinherited |
Definition at line 247 of file CAbstractNavigator.h.
|
protectedinherited |
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
Definition at line 249 of file CAbstractNavigator.h.
|
protectedinherited |
Definition at line 249 of file CAbstractNavigator.h.
|
protectedinherited |
mutex for all navigation methods
Definition at line 234 of file CAbstractNavigator.h.
|
protected |
Definition at line 101 of file CWaypointsNavigator.h.
|
protectedinherited |
Current navigation parameters.
Definition at line 226 of file CAbstractNavigator.h.
|
protectedinherited |
Current internal state of navigator:
Definition at line 225 of file CAbstractNavigator.h.
|
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 188 of file CAbstractNavigator.h.
|
protectedinherited |
The navigator-robot interface.
Definition at line 228 of file CAbstractNavigator.h.
|
protectedinherited |
Time logger to collect delay-related stats.
Definition at line 251 of file CAbstractNavigator.h.
|
private |
Whether the last timestep was "is_aligning" in a waypoint with heading.
Definition at line 116 of file CWaypointsNavigator.h.
|
protected |
The latest waypoints navigation command and the up-to-date control status.
Definition at line 100 of file CWaypointsNavigator.h.
|
inherited |
Definition at line 155 of file CAbstractNavigator.h.
TWaypointsNavigatorParams mrpt::nav::CWaypointsNavigator::params_waypoints_navigator |
Definition at line 94 of file CWaypointsNavigator.h.
Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Oct 31 07:27:35 UTC 2017 |