class mrpt::nav::CWaypointsNavigator

Overview

Extends CAbstractNavigator with sequential waypoint following.

Accepts a TWaypointSequence and navigates the robot through each waypoint in order, optionally skipping waypoints marked with allow_skip=true if the underlying navigator determines that it is more efficient to do so. Each time a waypoint is reached or skipped, sendWaypointReachedEvent() is called; sendNavigationEndEvent() is called only for the final waypoint.

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:

Navigation state may briefly pass by the 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.
Reaching an intermediary waypoint (or skipping it if considered so by the navigator) generates a call to sendWaypointReachedEvent() instead.

Inherited Members

public:
    // enums

    enum TErrorCode;
    enum TState;

    // structs

    struct TMsg;
    struct TAbstractNavigatorParams;
    struct TErrorReason;
    struct TNavigationParams;
    struct TNavigationParamsBase;
    struct TRobotPoseVel;
    struct TargetInfo;

    // fields

    bool logging_enable_console_output {true};
    bool logging_enable_keep_record {false};
    TAbstractNavigatorParams params_abstract_navigator;
    mrpt::system::CTimeLogger m_navProfiler {false, "mrpt::nav::CAbstractNavigator"};

    // methods

    void logStr(const VerbosityLevel level, std::string_view msg_str) const;
    void logFmt(const VerbosityLevel level, const char* fmt, ...) const;
    void void logCond(const VerbosityLevel level, bool cond, const std::string& msg_str) const;
    void setLoggerName(const std::string& name);
    std::string getLoggerName() const;
    void setVerbosityLevel(const VerbosityLevel level);
    void setVerbosityLevelForCallbacks(const VerbosityLevel level);
    void setMinLoggingLevel(const VerbosityLevel level);
    VerbosityLevel getMinLoggingLevel() const;
    VerbosityLevel getMinLoggingLevelForCallbacks() const;
    bool isLoggingLevelVisible(VerbosityLevel level) const;
    void getLogAsString(std::string& log_contents) const;
    std::string getLogAsString() const;
    void writeLogToFile(const std::optional<std::string>& fname_in = std::nullopt) const;
    void dumpLogToConsole() const;
    std::string getLoggerLastMsg() const;
    void getLoggerLastMsg(std::string& msg_str) const;
    void loggerReset();
    void logRegisterCallback(output_logger_callback_t userFunc);
    bool logDeregisterCallback(output_logger_callback_t userFunc);
    COutputLogger& operator = (const COutputLogger&);
    COutputLogger& operator = (COutputLogger&&);
    static std::array<mrpt::system::ConsoleForegroundColor, NUMBER_OF_VERBOSITY_LEVELS>& logging_levels_to_colors();
    static const std::array<const char*, NUMBER_OF_VERBOSITY_LEVELS>& logging_levels_to_names();
    virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
    virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;
    virtual void initialize() = 0;
    virtual void navigationStep();
    virtual void navigate(const TNavigationParams* params);
    virtual void cancel();
    virtual void resume();
    virtual void suspend();
    virtual void resetNavError();
    TState getCurrentState() const;
    const TErrorReason& getErrorReason() const;
    void setFrameTF(const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf);
    std::weak_ptr<mrpt::poses::FrameTransformer<2>> getFrameTF() const;
    void enableRethrowNavExceptions(const bool enable);
    bool isRethrowNavExceptionsEnabled() const;
    const mrpt::system::CTimeLogger& getDelaysTimeLogger() const;

Construction

CWaypointsNavigator(CRobot2NavInterface& robot_interface_impl)

ctor

Methods

virtual void navigateWaypoints(const TWaypointSequence& nav_request)

Starts navigation through a sequence of waypoints.

Cancels any ongoing navigation immediately. The robot will proceed through the waypoints in order, possibly skipping some if allowed.

Parameters:

nav_request

The waypoint sequence to follow.

See also:

CAbstractNavigator::navigate() for single-target requests.

virtual void getWaypointNavStatus(TWaypointStatusSequence& out_nav_status) const

Returns the current waypoint navigation progress status.

Parameters:

out_nav_status

Filled with the latest waypoint status.

TWaypointStatusSequence getWaypointNavStatus() const

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

getWaypointsRef()

WaypointsAccessGuard getWaypointsAccessGuard()

Returns a RAII guard that holds the waypoints mutex and provides write access to the waypoint status sequence.

auto guard = nav.getWaypointsAccessGuard();
guard.waypoints().waypoints[0].allow_skip = false;

virtual void navigationStep()

Runs one navigation cycle.

Must be called periodically (e.g. at 10-20 Hz) to advance navigation logic.

virtual void cancel()

Cancel current navegation.

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.

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

Loads all navigator parameters from a config file.

Must be called before initialize(). Derived classes must call the parent implementation after loading their own parameters.

Parameters:

c	The configuration source to read from.

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

Saves all current navigator parameters to a config file.

Derived classes must call the parent implementation after saving their own parameters.

Parameters:

c	The configuration file to write to.