class mrpt::nav::CAbstractPTGBasedReactive

Overview

Base class for reactive navigator systems based on TP-Space, with an arbitrary holonomic reactive method running on it and any number of PTGs for transforming the navigation space.

Both, the holonomic method and the PTGs can be customized by the appropriate user derived classes.

How to use:

  • Instantiate a reactive navigation object (one of the derived classes of this virtual class).

  • A class with callbacks must be defined by the user and provided to the constructor (derived from CRobot2NavInterface)

  • loadConfigFile() must be called to set up the bunch of parameters from a config file (could be a memory-based virtual config file).

  • 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.

For working examples, refer to the source code of the apps:

Publications:

  • See derived classes for papers on each specific method.

Available “variables” or “score names” for each motion candidate (these can be used in runtime-compiled expressions in the configuration files of motion deciders):

  • clearance : Clearance (larger means larger distances to obstacles) for the path from “current pose” up to “end of trajectory”.

  • collision_free_distance : Normalized [0,1] collision-free distance in selected path. For NOP candidates, the traveled distances is subtracted.

  • dist_eucl_final : Euclidean distance (in the real-world WordSpace) between “end of trajectory” and target.

  • eta : Estimated Time of Arrival at “end of trajectory”.

  • holo_stage_eval : Final evaluation of the selected direction from inside of the holonomic algorithm.

  • hysteresis : Measure of similarity with previous command [0,1]

  • is_PTG_cont : 1 (is “NOP” motion command), 0 otherwise

  • is_slowdown : 1 if PTG returns true in CParameterizedTrajectoryGenerator::supportSpeedAtTarget() for this step.

  • move_cur_d : Normalized distance already traveled over the selected PTG. Normally 0, unless in a “NOP motion”.

  • move_k : Motion candidate path 0-based index.

  • num_paths : Number of paths in the PTG

  • original_col_free_dist : Only for “NOP motions”, the collision-free distance when the motion command was originally issued.

  • ptg_idx : PTG index (0-based)

  • ptg_priority : Product of PTG getScorePriority() times PTG evalPathRelativePriority()

  • ref_dist : PTG ref distance [m]

  • robpose_x, robpose_y, robpose_phi : Robot pose ([m] and [rad]) at the “end of trajectory”: at collision or at target distance.

  • target_d_norm : Normalized target distance. Can be >1 if distance is larger than ref_distance.

  • target_dir : Angle of target in TP-Space [rad]

  • target_k : Same as target_dir but in discrete path 0-based indices.

  • WS_target_x, WS_target_y : Target coordinates in realworld [m]

See also:

CReactiveNavigationSystem, CReactiveNavigationSystem3D

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

class CAbstractPTGBasedReactive: public mrpt::nav::CWaypointsNavigator
{
public:
    // structs

    struct PTGTarget;
    struct TAbstractPTGNavigatorParams;
    struct TInfoPerPTG;
    struct TNavigationParamsPTG;
    struct TSentVelCmd;

    // construction

    CAbstractPTGBasedReactive(
        CRobot2NavInterface& react_iterf_impl,
        bool enableConsoleOutput = true,
        bool enableLogFile = false,
        const std::string& logFileDirectory = std::string("./reactivenav.logs")
        );

    // methods

    virtual void navigateWaypoints(const TWaypointSequence& nav_request);
    virtual void getWaypointNavStatus(TWaypointStatusSequence& out_nav_status) const;
    TWaypointStatusSequence getWaypointNavStatus() const;
    TWaypointStatusSequence& beginWaypointsAccess();
    void endWaypointsAccess();
    virtual void initialize();
    void setHolonomicMethod(const std::string& method, const mrpt::config::CConfigFileBase& cfgBase);
    void getLastLogRecord(CLogFileRecord& o);
    void enableKeepLogRecords(bool enable = true);
    void enableLogFile(bool enable);
    void setLogFileDirectory(const std::string& sDir);
    virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
    virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;
    void enableTimeLog(bool enable = true);
    const mrpt::system::CTimeLogger& getTimeLogger() const;
    virtual size_t getPTG_count() const = 0;
    virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0;
    virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0;
    const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& getCurrentRobotSpeedLimits() const;
    mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& changeCurrentRobotSpeedLimits();
    void setTargetApproachSlowDownDistance(const double dist);
    double getTargetApproachSlowDownDistance() const;
    virtual void navigationStep();
    virtual void cancel();
    bool isRelativePointReachable(const mrpt::math::TPoint2D& wp_local_wrt_robot) const;
};

// direct descendants

class CReactiveNavigationSystem;
class CReactiveNavigationSystem3D;

Inherited Members

public:
    // enums

    enum TErrorCode;
    enum TState;

    // structs

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

    // fields

    mrpt::system::CTimeLogger m_navProfiler {false, "mrpt::nav::CAbstractNavigator"};
    TWaypointsNavigatorParams params_waypoints_navigator;
    TAbstractNavigatorParams params_abstract_navigator;

    // methods

    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);
    const mrpt::system::CTimeLogger& getDelaysTimeLogger() const;
    bool isRethrowNavExceptionsEnabled() const;
    virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
    virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;

Construction

CAbstractPTGBasedReactive(
    CRobot2NavInterface& react_iterf_impl,
    bool enableConsoleOutput = true,
    bool enableLogFile = false,
    const std::string& logFileDirectory = std::string("./reactivenav.logs")
    )

Constructor.

Parameters:

react_iterf_impl

An instance of an object that implement all the required interfaces to read from and control a robot.

enableConsoleOutput

Can be set to false to reduce verbosity.

enableLogFile

Set to true to enable creation of navigation log files, useful for inspection and debugging.

Methods

virtual void navigateWaypoints(const TWaypointSequence& nav_request)

Waypoint navigation request.

This immediately cancels any other previous on-going navigation.

See also:

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

virtual void getWaypointNavStatus(TWaypointStatusSequence& out_nav_status) const

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

TWaypointStatusSequence getWaypointNavStatus() const

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

getWaypointsRef()

TWaypointStatusSequence& beginWaypointsAccess()

Gets a write-enabled reference to the list of waypoints, simultaneously acquiring the critical section mutex.

Caller must call endWaypointsAccess() when done editing the waypoints.

void endWaypointsAccess()

Must be called after beginWaypointsAccess()

virtual void initialize()

Must be called for loading collision grids, or the first navigation command may last a long time to be executed.

Internally, it just calls STEP1_CollisionGridsBuilder().

void setHolonomicMethod(const std::string& method, const mrpt::config::CConfigFileBase& cfgBase)

Selects which one from the set of available holonomic methods will be used into transformed TP-Space, and sets its configuration from a configuration file.

Available methods: class names of those derived from CAbstractHolonomicReactiveMethod

void getLastLogRecord(CLogFileRecord& o)

Provides a copy of the last log record with information about execution.

Log records are not prepared unless either “enableLogFile” is enabled in the constructor or “enableLogFile()” has been called.

Parameters:

o

An object where the log will be stored into.

 
void enableKeepLogRecords(bool enable = true)

Enables keeping an internal registry of navigation logs that can be queried with getLastLogRecord()

void enableLogFile(bool enable)

Enables/disables saving log files.

void setLogFileDirectory(const std::string& sDir)

Changes the prefix for new log files.

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

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 overridden method to ensure all params are loaded.

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

Saves all current options to a config file.

Each derived class MUST save its own parameters, and then call ITS PARENT’S overridden method to ensure all params are saved.

void enableTimeLog(bool enable = true)

Enables/disables the detailed time logger (default:disabled upon construction) When enabled, a report will be dumped to std::cout upon destruction.

See also:

getTimeLogger

const mrpt::system::CTimeLogger& getTimeLogger() const

Gives access to a const-ref to the internal time logger.

See also:

enableTimeLog

virtual size_t getPTG_count() const = 0

Returns the number of different PTGs that have been setup.

virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0

Gets the i’th PTG.

virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0

Gets the i’th PTG.

const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& getCurrentRobotSpeedLimits() const

Get the current, global (honored for all PTGs) robot speed limits.

mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& changeCurrentRobotSpeedLimits()

Changes the current, global (honored for all PTGs) robot speed limits, via returning a reference to a structure that holds those limits.

void setTargetApproachSlowDownDistance(const double dist)

Changes this parameter in all inner holonomic navigator instances [m].

double getTargetApproachSlowDownDistance() const

Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in all of them?)

virtual void navigationStep()

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

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.