125 virtual std::unique_ptr<TNavigationParams>
clone()
const 127 return std::make_unique<TNavigationParams>(*this);
218 std::weak_ptr<mrpt::poses::FrameTransformer<2>>
getFrameTF()
const 313 bool call_virtual_nav_method =
true);
327 virtual bool stop(
bool isEmergencyStop);
TErrorCode
Explains the reason for the navigation error.
double m_badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
std::recursive_mutex m_nav_cs
mutex for all navigation methods
virtual std::unique_ptr< TNavigationParams > clone() const
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
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...
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
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...
~CAbstractNavigator() override
dtor
Base for all high-level navigation commands.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
double dist_check_target_is_blocked
(Default value=0.6) When closer than this distance, check if the target is blocked to abort navigatio...
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
std::string pose_frame_id
map frame ID for pose
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > getFrameTF() const
Get the current frame tf object (defaults to nullptr)
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
virtual bool isEqual(const TNavigationParamsBase &o) const =0
TState m_navigationState
Current internal state of navigator:
Virtual base for velocity commands of different kinematic models of planar mobile robot...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
virtual void performNavigationStep()=0
To be implemented in derived classes.
bool operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
The struct for configuring navigation requests.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
This class allows loading and storing values and vectors of different types from a configuration text...
TState
The different states for the navigation system.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request...
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target...
TState getCurrentState() const
Returns the current navigator state.
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
const mrpt::system::CTimeLogger & getDelaysTimeLogger() const
Gives access to a const-ref to the internal time logger used to estimate delays.
bool m_rethrow_exceptions
std::string target_frame_id
(Default="map") Frame ID in which target is given.
Versatile class for consistent logging and management of output messages.
void enableRethrowNavExceptions(const bool enable)
By default, error exceptions on navigationStep() will dump an error message to the output logger inte...
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
mrpt::system::TTimeStamp timestamp
void internal_onStartNewNavigation()
Called before starting a new navigation.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
#define MRPT_ENUM_TYPE_END()
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
GLsizei const GLchar ** string
virtual void resume()
Continues with suspended navigation.
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
TState m_lastNavigationState
Last internal state of navigator:
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
void dispatchPendingNavEvents()
virtual std::string getAsText() const =0
Gets navigation params as a human-readable format.
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
std::string getAsText() const
Gets navigation params as a human-readable format.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
TAbstractNavigatorParams params_abstract_navigator
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
virtual void initialize()=0
Must be called before any other navigation command.
mrpt::math::TTwist2D velLocal
TAbstractNavigatorParams()
int m_counter_check_target_is_blocked
CRobot2NavInterface & m_robot
The navigator-robot interface.
bool isEqual(const TNavigationParamsBase &o) const override
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)...
TErrorReason m_navErrorReason
virtual void cancel()
Cancel current navegation.
const TErrorReason & getErrorReason() const
In case of state=NAV_ERROR, this returns the reason for the error.
std::string error_msg
Human friendly description of the error.
TargetInfo target
Navigation target.
bool operator==(const TargetInfo &o) const
virtual void suspend()
Suspend current navegation.
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
double m_last_curPoseVelUpdate_robot_time
friend bool operator==(const TNavigationParamsBase &, const TNavigationParamsBase &)
virtual ~TNavigationParamsBase()=default
mrpt::math::TTwist2D velGlobal
bool isRethrowNavExceptionsEnabled() const
int hysteresis_check_target_is_blocked
(Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to rais...
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
std::string m_last_curPoseVelUpdate_pose_frame_id
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
bool m_navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
This is the base class for any reactive/planned navigation system.
GLenum const GLfloat * params
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
std::string getAsText() const override
Gets navigation params as a human-readable format.
bool operator!=(const TargetInfo &o) const
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term)...
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
bool targetIsIntermediaryWaypoint
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, IDLE)
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.