CAbstractNavigator.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/nav/reactive/CRobot2NavInterface.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/poses/FrameTransformer.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/typemeta/TEnumType.h>

#include <functional>
#include <list>
#include <memory>  // unique_ptr
#include <mutex>

namespace mrpt::nav
{
/** This is the base class for any reactive/planned navigation system. See
 * derived classes.
 *
 * How to use:
 *  - A class derived from `CRobot2NavInterface` with callbacks must be defined
 * by the user and provided to the constructor.
 *  - `navigationStep()` must be called periodically in order to effectively run
 * the navigation. This method will internally call the callbacks to gather
 * sensor data and robot positioning data.
 *
 * It implements the following state machine (see
 * CAbstractNavigator::getCurrentState() ), taking into account the extensions
 * described in CWaypointsNavigator
 *  \dot
 *  digraph CAbstractNavigator_States {
 *      IDLE; NAVIGATING; SUSPENDED; NAV_ERROR;
 *      IDLE -> NAVIGATING [ label="CAbstractNavigator::navigate()"];
 *      IDLE -> NAVIGATING [ label="CWaypointsNavigator::navigateWaypoints()" ];
 *      NAVIGATING -> IDLE [ label="Final target reached" ];
 *      NAVIGATING -> IDLE [ label="CAbstractNavigator::cancel()" ];
 *      NAVIGATING -> NAV_ERROR [ label="Upon sensor errors, timeout,..." ];
 *      NAVIGATING -> SUSPENDED [ label="CAbstractNavigator::suspend()" ];
 *      SUSPENDED -> NAVIGATING [ label="CAbstractNavigator::resume()" ];
 *      NAV_ERROR -> IDLE [ label="CAbstractNavigator::resetNavError()" ];
 *  }
 *  \enddot
 *
 * \sa CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all
 * children classes
 *  \ingroup nav_reactive
 */
class CAbstractNavigator : public mrpt::system::COutputLogger
{
   public:
    /** ctor */
    CAbstractNavigator(CRobot2NavInterface& robot_interface_impl);
    /** dtor */
    ~CAbstractNavigator() override;

    /** Individual target info in CAbstractNavigator::TNavigationParamsBase and
     * derived classes */
    struct TargetInfo
    {
        /** Coordinates of desired target location. Heading may be ignored by
         * some reactive implementations. */
        mrpt::math::TPose2D target_coords;
        /** (Default="map") Frame ID in which target is given. Optional, use
         * only for submapping applications. */
        std::string target_frame_id;
        /** (Default=0.5 meters) Allowed distance to target in order to end the
         * navigation. */
        float targetAllowedDistance{0.5};
        /** (Default=false) Whether the \a target coordinates are in global
         * coordinates (false) or are relative to the current robot pose (true).
         */
        bool targetIsRelative{false};
        /** (Default=.05) Desired relative speed (wrt maximum speed), in range
         * [0,1], of the vehicle at target. Holonomic nav methods will perform
         * "slow down" approaching target only if this is "==.0". Intermediary
         * values will be honored only by the higher-level navigator, based on
         * straight-line Euclidean distances. */
        double targetDesiredRelSpeed{.05};
        bool targetIsIntermediaryWaypoint{
            false};  // !< (Default=false) If true, event
        // callback
        // `sendWaypointReachedEvent()` will
        // be called instead of
        // `sendNavigationEndEvent()`

        TargetInfo();
        /** Gets navigation params as a human-readable format */
        std::string getAsText() const;
        bool operator==(const TargetInfo& o) const;
        bool operator!=(const TargetInfo& o) const { return !(*this == o); }
    };

    /** Base for all high-level navigation commands. See derived classes */
    struct TNavigationParamsBase
    {
        virtual ~TNavigationParamsBase() = default;
        /** Gets navigation params as a human-readable format */
        virtual std::string getAsText() const = 0;

       protected:
        friend bool operator==(
            const TNavigationParamsBase&, const TNavigationParamsBase&);
        virtual bool isEqual(const TNavigationParamsBase& o) const = 0;
    };

    /** The struct for configuring navigation requests. Used in
     * CAbstractPTGBasedReactive::navigate() */
    struct TNavigationParams : public TNavigationParamsBase
    {
        /** Navigation target */
        TargetInfo target;

        /** Gets navigation params as a human-readable format */
        std::string getAsText() const override;
        virtual std::unique_ptr<TNavigationParams> clone() const
        {
            return std::make_unique<TNavigationParams>(*this);
        }

       protected:
        bool isEqual(const TNavigationParamsBase& o) const override;
    };

    /** \name Navigation control API
     * @{ */

    /** 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. */
    virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
    /** 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. */
    virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;

    /**  Must be called before any other navigation command */
    virtual void initialize() = 0;
    /** This method must be called periodically in order to effectively run the
     * navigation */
    virtual void navigationStep();

    /** Navigation request to a single target location. It starts a new
     * navigation.
     * \param[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.)
     * \note A pointer is used so the passed object can be polymorphic with
     * derived types.
     */
    virtual void navigate(const TNavigationParams* params);

    /** Cancel current navegation. */
    virtual void cancel();
    /** Continues with suspended navigation. \sa suspend */
    virtual void resume();
    /** Suspend current navegation. \sa resume */
    virtual void suspend();
    /** Resets a `NAV_ERROR` state back to `IDLE` */
    virtual void resetNavError();

    /** The different states for the navigation system. */
    enum TState
    {
        IDLE = 0,
        NAVIGATING,
        SUSPENDED,
        NAV_ERROR
    };

    /** Returns the current navigator state. */
    inline TState getCurrentState() const { return m_navigationState; }

    /** Explains the reason for the navigation error. */
    enum TErrorCode
    {
        ERR_NONE = 0,
        ERR_EMERGENCY_STOP,
        ERR_CANNOT_REACH_TARGET,
        ERR_OTHER
    };
    struct TErrorReason
    {
        TErrorReason() = default;

        TErrorCode error_code{ERR_NONE};
        /** Human friendly description of the error */
        std::string error_msg;
    };

    /** In case of state=NAV_ERROR, this returns the reason for the error.
     * Error state is reseted every time a new navigation starts with
     * a call to navigate(), or when resetNavError() is called.
     */
    inline const TErrorReason& getErrorReason() const
    {
        return m_navErrorReason;
    }

    /** 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"`).
     */
    void setFrameTF(
        const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf);

    /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
    std::weak_ptr<mrpt::poses::FrameTransformer<2>> getFrameTF() const
    {
        return m_frame_tf;
    }

    /** By default, error exceptions on navigationStep() will dump an error
     * message to the output logger interface. If rethrow is enabled
     * (default=false), the error message will be reported as well, but
     * exceptions will be re-thrown.
     */
    void enableRethrowNavExceptions(const bool enable)
    {
        m_rethrow_exceptions = enable;
    }
    bool isRethrowNavExceptionsEnabled() const { return m_rethrow_exceptions; }
    /** @}*/

    struct TAbstractNavigatorParams : public mrpt::config::CLoadableOptions
    {
        /** Default value=0, means use the "targetAllowedDistance" passed by the
         * user in the navigation request. */
        double dist_to_target_for_sending_event{0};
        /** navigator timeout (seconds) [Default=30 sec] */
        double alarm_seems_not_approaching_target_timeout{30};
        /** (Default value=0.6) When closer than this distance, check if the
         * target is blocked to abort navigation with an error. */
        double dist_check_target_is_blocked{0.6};
        /** (Default=3) How many steps should the condition for
         * dist_check_target_is_blocked be fulfilled to raise an event */
        int hysteresis_check_target_is_blocked{3};

        void loadFromConfigFile(
            const mrpt::config::CConfigFileBase& c,
            const std::string& s) override;
        void saveToConfigFile(
            mrpt::config::CConfigFileBase& c,
            const std::string& s) const override;
        TAbstractNavigatorParams();
    };

    TAbstractNavigatorParams params_abstract_navigator;

    /** Gives access to a const-ref to the internal time logger used to estimate
     * delays \sa getTimeLogger() in derived classes */
    const mrpt::system::CTimeLogger& getDelaysTimeLogger() const
    {
        return m_timlog_delays;
    }

    /** Publicly available time profiling object. Default: disabled */
    mrpt::system::CTimeLogger m_navProfiler{false,
                                            "mrpt::nav::CAbstractNavigator"};

   private:
    /** Last internal state of navigator: */
    TState m_lastNavigationState{IDLE};
    TErrorReason m_navErrorReason;
    /** Will be false until the navigation end is sent, and it is reset with
     * each new command */
    bool m_navigationEndEventSent{false};
    int m_counter_check_target_is_blocked{0};
    bool m_rethrow_exceptions{false};

    /** Called before starting a new navigation. Internally, it calls to
     * child-implemented onStartNewNavigation() */
    void internal_onStartNewNavigation();

   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. */
    std::list<std::function<void(void)>> m_pending_events;

    void dispatchPendingNavEvents();

    /** To be implemented in derived classes */
    virtual void performNavigationStep() = 0;

    /** Called whenever a new navigation has been started. Can be used to reset
     * state variables, etc. */
    virtual void onStartNewNavigation() = 0;

    /** Called after each call to CAbstractNavigator::navigate() */
    virtual void onNavigateCommandReceived();

    /** 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. */
    virtual void updateCurrentPoseAndSpeeds();

    /** 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()
     */
    virtual void performNavigationStepNavigating(
        bool call_virtual_nav_method = true);

    /** Does the job of navigate(), except the call to
     * onNavigateCommandReceived() */
    virtual void processNavigateCommand(const TNavigationParams* params);

    /** Stops the robot and set navigation state to error */
    virtual void doEmergencyStop(const std::string& msg);

    /** Default: forward call to m_robot.changeSpeed(). Can be overriden. */
    virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd);
    /** Default: forward call to m_robot.changeSpeedsNOP(). Can be overriden. */
    virtual bool changeSpeedsNOP();
    /** Default: forward call to m_robot.stop(). Can be overriden. */
    virtual bool stop(bool isEmergencyStop);

    /** 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).
     */
    virtual bool checkHasReachedTarget(const double targetDist) 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.
     * Default implementation: always returns false. */
    virtual bool checkCollisionWithLatestObstacles(
        const mrpt::math::TPose2D& relative_robot_pose) const;

    /** Current internal state of navigator: */
    TState m_navigationState{IDLE};
    /** Current navigation parameters */
    std::unique_ptr<TNavigationParams> m_navigationParams;

    /** The navigator-robot interface. */
    CRobot2NavInterface& m_robot;

    /** Optional, user-provided frame transformer. */
    std::weak_ptr<mrpt::poses::FrameTransformer<2>> m_frame_tf;

    /** mutex for all navigation methods */
    std::recursive_mutex m_nav_cs;

    struct TRobotPoseVel
    {
        mrpt::math::TPose2D pose;
        mrpt::math::TTwist2D velGlobal, velLocal;
        /** raw odometry (frame does not match to "pose", but is expected to be
         * smoother in the short term). */
        mrpt::math::TPose2D rawOdometry;
        mrpt::system::TTimeStamp timestamp;
        /** map frame ID for `pose` */
        std::string pose_frame_id;
        TRobotPoseVel();
    };

    /** Current robot pose (updated in CAbstractNavigator::navigationStep() ) */
    TRobotPoseVel m_curPoseVel;
    double m_last_curPoseVelUpdate_robot_time{-1e9};
    std::string m_last_curPoseVelUpdate_pose_frame_id;
    /** Latest robot poses (updated in CAbstractNavigator::navigationStep() ) */
    mrpt::poses::CPose2DInterpolator m_latestPoses, m_latestOdomPoses;

    /** Time logger to collect delay-related stats */
    mrpt::system::CTimeLogger m_timlog_delays{
        true, "CAbstractNavigator::m_timlog_delays"};

    /** For sending an alarm (error event) when it seems that we are not
     * approaching toward the target in a while... */
    double m_badNavAlarm_minDistTarget;
    mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime;

   public:
};

bool operator==(
    const CAbstractNavigator::TNavigationParamsBase&,
    const CAbstractNavigator::TNavigationParamsBase&);
}  // namespace mrpt::nav
MRPT_ENUM_TYPE_BEGIN(mrpt::nav::CAbstractNavigator::TState)
using namespace mrpt::nav;
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, IDLE);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, NAVIGATING);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, SUSPENDED);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, NAV_ERROR);
MRPT_ENUM_TYPE_END()